CN112327856A - 一种基于改进A-star算法的机器人路径规划方法 - Google Patents

Abstract

本发明涉及一种基于改进A‑star算法的机器人路径规划方法，包括如下步骤：确定机器人初始点、目标点以及障碍物位置；计算距离代价值；位置更新；搜索初始路径；初始路径的平滑处理；路径规划。本发明利用欧几里得距离规划最小成本路径，并考虑到靠近火灾故障点可能对消防机器人造成损害，对扩展点进行有向选择，逐级扩展确定可行路径；在最小距离成本评价指标的基础上使用插值法对路径进行优化，减少消防机器人转弯移动成本。本发明能够提高消防机器人在行进过程中的安全性，降低路径规划时间并使路径更平滑，适用于火情复杂的环境。

Description

一种基于改进A-star算法的机器人路径规划方法

技术领域

本发明属于机器人自主导航领域，特别是一种基于改进A-star算法的移动机器人路径规划方法。

背景技术

自主导航的好坏是移动机器人的核心指标，而路径规划作为自主导航的关键技术之一受到广泛研究。针对火灾坏境，规划一条从初始点到目标故障点的最优路径，是消防机器人路径规划的主要内容。在构建环境模型中，栅格法具有直观简洁、容易创建和存储等优点，适用于室内环境路径规划地图模型的建立。A-star算法是一种启发式搜索算法，该搜索方法能节省大量搜索空间，提高搜索效率，在一般路径规划中非常流行。但传统A-star算法未能兼顾靠近火灾故障点可能对消防机器人造成的损害，不利于消防机器人在实际火情现场的行进。

发明内容

本发明旨在解决以上现有技术的问题，提出了一种在保证路径长度接近最短的同时，降低消防机器人受火灾故障点损害的一种改进A-star算法的移动消防机器人路径规划方法。改进后算法规划的路径更加平滑，机器人行进效率更高。本发明的技术方案具体如下：

一种基于改进A-star算法的机器人路径规划方法，包括以下步骤：

步骤(1)确定机器人初始点、目标点以及障碍物位置

根据消防机器人所在环境进行栅格地图建模，确定机器人初始点、目标点以及障碍物位置；

步骤(2)计算距离代价值

根据步骤(1)中机器人初始点，对机器人进行八向搜寻，计算八个扩展点到目标点的距离代价值；

步骤(3)位置更新

根据步骤(2)中的距离代价值的基础上，比较障碍物的相对位置，对靠近障碍物的扩展点方向进行剔除，选择距离代价值最小的方向，更新当前位置；步骤(4)搜索初始路径

根据步骤(3)中所述的扩展方向，利用A-star算法在栅格地图中搜索出一条隔离障碍选向之后的初始路径；

步骤(5)初始路径的平滑处理

对步骤(4)的初始路径，采用三次样条插值进行平滑处理；

步骤(6)路径规划

判断环境是否改变，对步骤(5)平滑处理后的始路径进行更新，得到最终规划完成的路径。

进一步地，步骤(1)中，首先初始化算法，建立OPEN表和CLOSE表，根据机器人所在环境建立栅格化地图模型，具体步骤为：

1.1、将地图分割为由若干个大小相同的正方形组成的有界区域，每一个网格称为一个单元，每个单元位置坐标为(xp，yq)，其中p，q为地图横、纵向栅格序数；

1.2、用一条直线在固定方向扫描栅格地图；

1.3、当扫描线扫入时，确认机器人初始位置坐标、目标位置坐标以及障碍物位置坐标；

1.4、初始节点放入OPEN列表中并使CLOSE列表为空。

进一步地，步骤(2)中，根据步骤(1)中所述的机器人初始点，对机器人进行八向搜寻，计算八个扩展点到目标点的路径成本，具体步骤为：

2.1、以机器人初始位置为中心，周围8个方格O_i为当前节点S的扩展点，该当前节点S称为每个对应扩展点O_i的父节点，其中i取值为1，2，…8；

2.2、计算每个扩展点O_i到目标点G的路径成本F(n)

F(n)＝G(n)+H(n)；

其中，G(n)是扩展点O_i移动到指定方格的移动路径成本，H(n)为从指定方格移动到目标点的路径估算成本。

进一步地，步骤(3)的具体步骤如下：

3.1、计算当前节点与障碍物节点的相对位置，若当前节点的扩展点O_i紧邻障碍物节点，则剔除该扩展点；

3.2、选择剩下的扩展点中路径成本F(n)最小的为扩展方向，并更新当前位置S为路径成本F(n)最小的扩展点。

进一步地，步骤(4)中，在所述扩展点选向基础上，利用A-star算法获取行驶路径，具体步骤包括：

4.1、初始化算法，确认机器人初始位置、目标位置以及障碍物位置，建立OPEN表和CLOSE表；

4.2、把初始节点放入OPEN列表中并使CLOSE列表为空；

4.3、利用扩展点选向的原理，更新当前位置节点；

4.4、判断当前位置节点的每个扩展节点O_i是否具有下一跳扩展节点M_ij；j的取值范围是1到p；p表示当前节点的下一跳扩展节点的总个数；若不存在下一跳扩展节点M_ij，则算法结束，若存在扩展节点M_ij，则进行以下过程：若存在若干个下一跳扩展节点M_ij包含目标节点，则计算扩展节点M_ij对应的每个扩展节点O_i的路径成本函数F(n)，并选择路径成本函数F(n)值最小的扩展节点O_i并标记为S；

4.5、把初始节点S从OPEN列表中移除，把初始节点S放入CLOSE列表中并冠以编号N，返回A-4，遍历图中的所有扩展节点O_i；

4.6、若OPEN列表为空，算法结束，若不为空，判断所有扩展节点O_i中是否存在目标节点，若所有扩展节点O_i存在目标节点，将是目标节点的扩展节点O_i放入CLOSE列表中，算法结束。

进一步地，步骤(5)中，具体过程如下：

5.1、取出A-star算法路径的n+1个数据节点，标记节点坐标分别为(x₀，y₀)，(x₁，y₁)，(x₂，y₂)，...，(x_n，y_n)；在每个子区间x_i≤x≤x_i+1中，创建三次样条差值方程：g_i(x)＝a_i+b_i(x-x_i)+c_i(x-x_i)^2+d_i(x-x_i)^3；其中a_i，b_i，c_i，d_i为样条曲线的系数；5.2、选取步长h_i＝x_i+1-x_i，i＝0，1，...，n-1；将数据节点和端点条件M₀＝0，M_n＝0带入如下矩阵方程：

其中，M_i表示样条差值方程二次微分值；求解矩阵方程，得到样条差值方程二次微分值M_i，i＝0，1，...，n；

5.3、计算样条曲线的系数a_i，b_i，c_i，d_i，公式如下：a_i＝y_i，

求解出每段样条差值方程中的系数，即可得到每段曲线的具体表达式。

进一步地，步骤(6)中，判断环境是否改变主要是判别规划路径中当前位置的下一段路径周围是否有障碍物变化。

与现有技术相比，本发明的优点及有益效果如下：

本发明提出的改进A-star算法考虑到靠近火灾故障点可能对消防机器人造成损害，对机器人传统八向搜索进行了改进，并对传统A-star算法路径进行三次样条插值平滑优化。减少了路径转折点的同时提高了机器人在复杂火情环境中的行进安全性，此外还减小了路径总长度，使整体路径更加平滑，使得机器人能更好地绕过障碍物，提高了规划效率。

附图说明

图1为本发明的方法的流程图；

图2为本发明的路径八向搜索示意图；

图3为本发明提供的隔离障碍物方向选择示意图；

图4为传统A-star算法路径规划示意图；

图5为本发明实施例的改进A-star算法路径规划示意图；

图6为本发明实施例的改进A-star算法路最终路径规划示意图。

具体实施方式

本部分将详细描述本发明的具体实施例，本发明实施例在附图中示出，附图的作用在于用图形补充说明书文字部分的描述，使人能够直观地、形象地理解本发明的每个技术特征和整体技术方案。在本发明的描述中，需要理解的是，涉及到方位描述，例如上、下、左、右等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述。下面结合附图和具体实施例对本发明作进一步说明。

如图1所示，本实施例的基于改进A-star算法的机器人路径规划方法，包括以下步骤：

步骤(1)根据消防机器人所在环境进行栅格地图建模，确定机器人初始点、目标点以及障碍物位置；

首先初始化算法，建立OPEN表和CLOSE表，根据机器人所在环境建立栅格化地图模型，具体步骤为：

1.1、将地图分割为由若干个大小相同的正方形组成的有界区域，每一个网格称为一个单元，每个单元位置坐标为(x_p，y_q)，其中p，q为地图横、纵向栅格序数。

1.2、用一条直线在固定方向(横向或者纵向)扫描栅格地图。

1.3、当扫描线扫入时，确认机器人初始位置坐标、目标位置坐标以及障碍物位置坐标。

1.4、初始节点S放入OPEN列表中并使CLOSE列表为空。

步骤(2)根据步骤(1)中所述的机器人初始点S，对机器人进行八向搜寻，计算八个扩展点到目标点的距离代价值，即路径成本，具体步骤为：

2.1、以机器人初始位置S(当前节点)为中心，周围8个方格O_i为当前节点S的扩展点，该当前节点S称为每个对应扩展点O_i的父节点，其中i取值为1，2，…8。

2.2、计算每个扩展点O_i到目标点G的路径成本F(n)。

F(n)＝G(n)+H(n)；

其中，G(n)是扩展点O_i移动到指定方格的移动路径成本，H(n)为从指定方格移动到目标点的路径估算成本。

利用欧几里得距离规划最小成本路径，欧几里得距离启发式函数算法原理可表述为：设搜索起点A和目标点B，AB两点的坐标均为所在栅格地图网格的中心，分别记为(x_i，y_i)和(x_j，y_j)；计算两单元的中心之间的曼哈顿距离，计算公式如下：d(i，j)＝sqrt((x_i-x_j)^2+(y_i-y_j)^2)，此距离作为两点路径的代价值。

步骤(3)根据步骤(2)中所述的距离代价值的基础上，比较障碍物的相对位置，对靠近障碍物的扩展点方向进行剔除，选择距离代价值最小的方向，更新当前位置，具体步骤为：

3.1、计算当前节点S与障碍物节点的相对位置，若当前节点S的扩展点O_i紧邻障碍物节点，则剔除该扩展点。

如图2所示，灰色方格为当前节点S，黑色方格a，b，c为障碍节点，白色方格为可行驶路径节点，由于扩展点1至5分别与障碍节点a，b，c紧邻，故剔除扩展方向为1至5的扩展点。

3.2、选择剩下的扩展点中路径成本F(n)最小的为扩展方向，并更新当前位置S为路径成本F(n)最小的扩展点。

步骤(4)根据步骤(3)中所述的扩展方向，利用A-star算法在栅格地图中搜索出一条隔离障碍选向之后的初始路径，如图3所示；

4.1、初始化算法，确认机器人初始位置、目标位置以及障碍物位置，建立OPEN表和CLOSE表；

4.2、把初始节点S放入OPEN列表中并使CLOSE列表为空；

4.3、利用扩展点选向的原理，更新当前位置节点S；

4.4、判断当前位置节点的每个扩展节点O_i是否具有下一跳扩展节点M_ij；j的取值范围是1到p；p表示当前节点的下一跳扩展节点的总个数；若不存在下一跳扩展节点M_ij，则算法结束，若存在扩展节点M_ij，则进行以下过程：若存在若干个下一跳扩展节点M_ij包含目标节点，则计算扩展节点M_ij对应的每个扩展节点O_i的路径成本函数F(n)，并选择路径成本函数F(n)值最小的扩展节点O_i并标记为S；

4.5、把初始节点S从OPEN列表中移除，把初始节点S放入CLOSE列表中并冠以编号N，返回4.4，遍历图中的所有扩展节点O_i；

4.6、若OPEN列表为空，算法结束，若不为空，判断所有扩展节点O_i中是否存在目标节点，若所有扩展节点O_i存在目标节点，将是目标节点的扩展节点O_i放入CLOSE列表中，算法结束。

步骤(5)对步骤(4)的初始路径，采用三次样条插值进行平滑处理，具体步骤为：

5.1、取出A-star算法路径的n+1个数据节点，标记节点坐标分别为(x₀，y₀)，(x₁，y₁)，(x₂，y₂)，...，(x_n，y_n)；在每个子区间x_i≤x≤x_i+1中，创建三次样条差值方程：g_i(x)＝a_i+b_i(x-x_i)+c_i(x-x_i)^2+d_i(x-x_i)^3；其中a_i，b_i，c_i，d_i为样条曲线的系数；

5.2、选取步长h_i＝x_i+1-x_i，i＝0，1，...，n-1；将数据节点和端点条件M₀＝0，M_n＝0带入如下矩阵方程：

其中，M_i表示样条差值方程二次微分值；求解矩阵方程，得到样条差值方程二次微分值M_i，i＝0，1，...，n；

5.3、计算样条曲线的系数a_i，b_i，c_i，d_i，公式如下：

a_i＝y_i，

求解出每段样条差值方程中的系数，根据插值方程平滑处理路径。

步骤(6)判断环境是否改变，对步骤(5)平滑处理后的始路径进行更新，得到最终规划完成的路径。

判断环境是否改变，主要是判别规划路径中当前位置的下一段路径周围是否有障碍物变化。如步骤3.1所描述的，路径规划中会对靠近障碍物的扩展点方向进行剔除，如图3所示，若机器人行进中出现新的故障点，将根据步骤3.1的原理调整路径，以保证机器人在行进中离开障碍物一段距离，从而减少因靠近故障点可能对消防机器人造成损害。以上过程与图1对应。

图5为是本发明提供实施例的传统A-star算法路径规划示意图，图6为本发明提供实施例的最终改进A-star算法路径规划示意图，由图6可知，本实施例方法对机器人传统八向搜索进行了改进，机器人路径与障碍物之间留有一定距离，减少了消防机器人因靠近火灾故障点而可能造成的损害。此外对传统A*算法路径进行三次样条插值平滑优化，使整体路径更加平滑，使得机器人能更好地绕过障碍物，使机器人具有更高的智能化，图4为传统A-star算法路径规划示意图。计算结果如表1所示：

表1实施例1计算结果

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于改进A-star算法的机器人路径规划方法，其特征在于：包括以下步骤：

步骤（1）确定机器人初始点、目标点以及障碍物位置

根据消防机器人所在环境进行栅格地图建模，确定机器人初始点、目标点以及障碍物位置；

步骤（2）计算距离代价值

根据步骤（1）中机器人初始点，对机器人进行八向搜寻，计算八个扩展点到目标点的距离代价值；

步骤（3）位置更新

根据步骤（2）中的距离代价值的基础上，比较障碍物的相对位置，对靠近障碍物的扩展点方向进行剔除，选择距离代价值最小的方向，更新当前位置；

步骤（4）搜索初始路径

根据步骤（3）中所述的扩展方向，利用A-star算法在栅格地图中搜索出一条隔离障碍选向之后的初始路径；

步骤（5）初始路径的平滑处理

对步骤（4）的初始路径，采用三次样条插值进行平滑处理；

步骤（6）路径规划

判断环境是否改变，对步骤（5）平滑处理后的始路径进行更新，得到最终规划完成的路径。

2.根据权利要求1所述的方法，其特征在于：步骤（1）中，首先初始化算法，建立OPEN表和CLOSE表，根据机器人所在环境建立栅格化地图模型，具体步骤为：

1.1、将地图分割为由若干个大小相同的正方形组成的有界区域，每一个网格称为一个单元，每个单元位置坐标为（xp，yq），其中p，q为地图横、纵向栅格序数；

1.2、用一条直线在固定方向扫描栅格地图；

1.3、当扫描线扫入时，确认机器人初始位置坐标、目标位置坐标以及障碍物位置坐标；

1.4、初始节点放入OPEN列表中并使CLOSE列表为空。

3.根据权利要求2所述的方法，其特征在于：步骤（2）中，根据步骤（1）中所述的机器人初始点，对机器人进行八向搜寻，计算八个扩展点到目标点的路径成本，具体步骤为：

2.1、以机器人初始位置为中心，周围8个方格O_i为当前节点S的扩展点，该当前节点S称为每个对应扩展点O_i的父节点，其中i取值为1，2，…8；

2.2、计算每个扩展点O_i到目标点G的路径成本F(n)

F(n)= G(n)+ H(n)；

其中，G(n)是扩展点O_i移动到指定方格的移动路径成本，H(n)为从指定方格移动到目标点的路径估算成本。

4.根据权利要求3所述的方法，其特征在于：步骤（3）的具体步骤如下：

3.1、计算当前节点与障碍物节点的相对位置，若当前节点的扩展点O_i紧邻障碍物节点，则剔除该扩展点；

3.2、选择剩下的扩展点中路径成本F(n)最小的为扩展方向，并更新当前位置S为路径成本F(n)最小的扩展点。

5.根据权利要求1所述的方法，其特征在于：步骤（4）中，在所述扩展点选向基础上，利用A-star算法获取行驶路径，具体步骤包括：

4.1、初始化算法，确认机器人初始位置、目标位置以及障碍物位置，建立OPEN表和CLOSE表；

4.2、把初始节点放入OPEN列表中并使CLOSE列表为空；

4.3、利用扩展点选向的原理，更新当前位置节点；

4.4、判断当前位置节点的每个扩展节点O_i是否具有下一跳扩展节点M_ij；j的取值范围是1到p；p表示当前节点的下一跳扩展节点的总个数；若不存在下一跳扩展节点M_ij，则算法结束，若存在扩展节点M_ij，则进行以下过程：若存在若干个下一跳扩展节点M_ij包含目标节点，则计算扩展节点M_ij对应的每个扩展节点O_i的路径成本函数F(n)，并选择路径成本函数F(n)值最小的扩展节点O_i并标记为S；

4.5、把初始节点S从OPEN列表中移除，把初始节点S放入CLOSE列表中并冠以编号N，返回A-4，遍历图中的所有扩展节点O_i；

4.6、若OPEN列表为空，算法结束，若不为空，判断所有扩展节点O_i中是否存在目标节点，若所有扩展节点O_i存在目标节点，将是目标节点的扩展节点O_i放入CLOSE列表中，算法结束。

6.根据权利要求5所述的方法，其特征在于：步骤（5）中，具体过程如下：

5.1、取出A-star算法路径的n+1个数据节点，标记节点坐标分别为(x₀，y₀)，(x₁，y₁)，(x₂，y₂)，...，(x_n，y_n)；在每个子区间x_i≤x≤x_i+1中，创建三次样条差值方程：

g_i(x)＝a_i+b_i(x-x_i)+c_i(x-x_i)^2+d_i(x-x_i)^3 ；其中a_i，b_i，c_i，d_i为样条曲线的系数；

5.2、选取步长h_i＝x_i+1-x_i，i＝0，1，...，n-1；将数据节点和端点条件M₀＝0，M_n＝0带入如下矩阵方程：

其中，M_i表示样条差值方程二次微分值；求解矩阵方程，得到样条差值方程二次微分值M_i，i＝0，1，...，n；

5.3、计算样条曲线的系数a_i，b_i，c_i，d_i，公式如下：

求解出每段样条差值方程中的系数，即可得到每段曲线的具体表达式。

7.根据权利要求5所述的方法，其特征在于：步骤（6）中，判断环境是否改变主要是判别规划路径中当前位置的下一段路径周围是否有障碍物变化。

Images