CN112034836A - 一种改进a*算法的移动机器人路径规划方法 - Google Patents

Abstract

本发明涉及一种改进A*算法的移动机器人路径规划方法，其包括步骤：采用栅格法建立移动机器人工作环境的三维地图模型；在三维地图模型中，针对栅格存在部分障碍物的情况采用改进A*算法进行邻域搜索，以提高生成路径的合理性；同时，通过引入坡度信息的改进代价函数计算从起始节点开始到其每一个邻域节点的代价值，使路径更平缓，进而在保证搜索效率的前提下得到最优路径。本发明改善了移动机器人易与障碍物边缘发生碰撞的问题，解决了算法生成路径在复杂地形中过于陡峭的问题。

Description

一种改进A*算法的移动机器人路径规划方法

技术领域

本发明涉及一种机器人路径规划领域，特别是关于一种改进A*算法的移动机器人路径规划方法。

背景技术

随着社会发展，机器人的应用更为广泛，移动机器人的路径规划问题更是目前的一个研究热点。该问题可以描述为移动机器人按照某种性能指标在有障碍物的环境里从起点至终点间找到一条最短或最佳的安全无碰撞路径。常用的路径规划算法有RRT算法、蚁群算法、人工势场算法、SFLA、A*算法等。RRT算法搜索效率高，但得到的路径往往和最优路径相差甚远。蚁群算法的收敛速度相对较慢且容易陷入局部最优状态。人工势场法虽然结构简单易于实现，但仍存在目标不可达，障碍物前产生震荡，规划路径不可达等问题。相较于其他路径规划算法，A* 算法构建模型简单，搜索效率高，针对静态场景所得到的解接近最优解，对不同的场景具有很强的扩展性和适应性。A*算法同样存在很多的不足之处，如生成路径拐点较多，路径不够平滑，大范围搜索情况下计算量大等问题。针对这些问题，国内外研究人员从不同角度提出不同的改进方案。Duchoň总结了Basic theta*、 Phi*、JumpPoint Search这三种对A*算法的改进方法，Basic theta*和Phi*解决了A*算法不能全方向搜索的问题，最终得到的路径长度更接近最优路径，但计算效率低；Jump Point Search能够提高搜索效率，但搜索方向仍受到限制，得到的路径相较于另外两种搜索方法更长。

A*算法的改进方法有很多，但在移动机器人路径规划中仍存在以下问题没有很好的解决方案：没有充分考虑移动机器人的体积而造成的路径安全性问题；复杂地形环境下算法生成路径合理性问题。

发明内容

针对在栅格地图中移动机器人规划路径问题，传统A*算法容易忽略机器人实际大小，生成的路径存在与障碍物发生碰撞的可能性问题，本发明的目的是提供一种改进A*算法的移动机器人路径规划方法，其改善了移动机器人易与障碍物边缘发生碰撞的问题。解决了算法生成路径在复杂地形中过于陡峭的问题。

为实现上述目的，本发明采取以下技术方案：一种改进A*算法的移动机器人路径规划方法，其包括以下步骤：S1、采用栅格法建立移动机器人工作环境的三维地图模型；S2、在三维地图模型中，针对栅格存在部分障碍物的情况采用改进A*算法进行邻域搜索，以提高生成路径的合理性；同时，通过引入坡度信息的改进代价函数计算从起始节点开始到其每一个邻域节点的代价值，使路径更平缓，进而在保证搜索效率的前提下得到最优路径。

进一步，所述步骤S1中，设置栅格地图时，选取机器人投影面积和每一个栅格大小之比为2:3的比例构建取n×n的平面栅格地图作为平面环境地图模型，每一个栅格是一个节点，再将每一个栅格划分为j×j的小栅格，进而得到一个p×p 的二维平面地图模型，在此基础上在matlab中生成一个p×p高度的信息矩阵，用mesh函数生成一个三维地图模型；其中，j、n均为大于1的自然数，p＝j×n。

进一步，所述步骤S2中，将栅格地图中的每个节点划分为j＝4阶的障碍物节点，并根据每个节点内障碍物的分布情况划分为完全障碍物节点、部分障碍物节点和无障碍节点。

进一步，选用j＝4阶矩阵A来表示每一个栅格的障碍物信息并采用如下规则对节点进行分类：

a)若矩阵A中不存在非0点则认为是无障碍节点；

b)若矩阵A中主对角线或次对角线上存在两个及以上非0点则认为该节点为完全障碍物节点；

c)若矩阵A中第2、3列或第2、3行存在两个及以上非0点则认为该节点为完全障碍物节点；

d)除上述情况以外的节点认为是不完全障碍节点。

进一步，在每一次邻域搜索时，如果当前节点为对于完全障碍物节点或无障碍节点，则采用现有A*算法进行邻域搜索，对栅格存在不完全障碍物的情况采用改进A*算法进行邻域搜索。

进一步，对栅格存在不完全障碍物的情况采用改进A*算法进行邻域搜索，并根据节点中障碍物分布的情况判断目标邻域节点是否满足可通行条件，具体方法如下：

S21、建立节点拓展4阶障碍物矩阵时障碍物节点序号表N_obs，已知每个节点都存在8个邻域节点，用坐标的方式来表示，将当前点视为[0,0]，设定当前点0 度的方向为[1,0]；

S22、每个邻域搜索方向都有不同的判断条件，判断当前节点在其邻域搜索方向上是否满足通行条件；

S23、引入坡度信息的改进代价函数计算从起始节点开始到其每一个邻域节点的代价值。

进一步，所述障碍物节点序号表N_obs为：

障碍物节点序号N_obs

进一步，所述每个节点的8个邻域节点的坐标及忽略项表示方法如下表所示：

节点自身障碍物矩阵忽略项

进一步，所述S22中，判断方法如下：

S221、在进行邻域搜索时，对应搜索目标邻域需满足除对应忽略项外其余项均为0，认定当前节点在该方向上可以进行搜索；

S222、在满足节点自身能够搜索该邻域的条件时，判断目标邻域节点是否满足可通行条件：

当目标邻域节点边缘存在障碍物时，需要判断目标邻域节点的障碍物分布情况，当目标邻域节点为右上节点时，考虑机器人在栅格地图中移动的轨迹，将满足可通行条件的障碍物情况分为两种：

a)目标邻域节点障碍物分布为左上侧时：即N_obs中1，2，5区域的任意区域存在障碍，其余区域均不存在障碍时，将轨迹中心点进行偏移以获取可通行的路径；

b)目标邻域节点障碍物分布为右下侧时：即N_obs中12，15，16区域的任意区域存在障碍，其余区域均不存在障碍时，将轨迹中心点进行偏移以获取可通行的路径。

进一步，所述S23中，具体计算方法为：

S231、确定在山地行走时速度和坡度的关系为：

1/V＝0.75+14.6slope²

式中，Slope为坡度信息，Slope＝H/L，H为当前节点中最高的高度信息值,L 为水平宽度；

S232、对于存在坡度的路面计算其等效平面距离S_eq，根据坡度和速度的关系，取其等效平面距离S_eq为：

S_eq＝(1+Slope_n ²)×ρ

Slope_n表示当前节点的坡度,ρ为两个节点间的平面欧式距离；

可得从起始节点到当前节点n的实际代价G(n)为：

G_n＝G_n-1+(1+Slope_n ²)×S_eq；

S233、在已知当前节点和目标点之间的平面欧式距离的基础上，假设从当前节点到目标节点的坡度信息均为Slope_n，得到其等效平面距离S_eq’：

其中，Goal_x表示x方向的最后终点，Goal_y表示y方向的最后终点；

在此基础上再考虑目标点的坡度信息和当前节点坡度信息的差的等效平面距离，可得从当前节点n到目标点的代价估计值H(n)的计算方式：

H_n＝[(Slope_n-Slope_goal)²+1]×S_eq’。

本发明由于采取以上技术方案，其具有以下优点：1、本发明在设定机器人投影和每一个栅格大小比例关系的基础上，将每一个栅格拓展为j阶障碍物矩阵，针对栅格存在部分障碍物的情况对邻域搜索方式进行改进以提高生成路径的合理性。同时针对在复杂地形(如山地)条件下生成路径较陡峭的问题，通过引入坡度信息改进代价函数计算方式，使路径更平缓。本发明能够通过拓展节点障碍物邻域的方法有效避免与障碍物发生碰撞，在复杂地形条件下生成的路径也更为合理。2、本发明通过在存在障碍物的栅格中拓展障碍物矩阵的方法改善移动机器人易与障碍物边缘发生碰撞的问题。3、本发明引入坡度信息改进代价函数计算方式，考虑到地形坡度对移动机器人通过性的影响，解决算法生成路径在复杂地形中过于陡峭的问题。

附图说明

图1是本发明实施例中的环境三维地图模型示意图。

图2是目标邻域节点为无障碍节点时不同起始点的轨迹示意图；其中，图a 表示默认情况即认定没有障碍的情况下当前节点的起始点，图b是向左偏移了一格的初始点，图c是向下偏移了一格的初始点，图d是向右偏移了一格的初始点，图e是向上偏移了一格的初始点，即图b至图e是上一节点向当前节点拓展时遇到不完全障碍节点时，可能会产生的四种偏移方向。

图3是目标邻域节点障碍分布为“左上侧”示意图；其中，图a是默认情况即认定没有障碍的情况下当前节点的起始点，图b是向下偏移了一格的初始点，图c是向左偏移了一格的初始点，图d是向上偏移了一格的初始点，图e是向右偏移了一格的初始点。

图4是目标邻域节点障碍分布为“右下侧”示意图；其中，图a表示默认情况即认定没有障碍的情况下当前节点的起始点，图b是向左偏移了一格的初始点，图c是向上偏移了一格的初始点，图d是向下偏移了一格的初始点，图e是向右偏移了一格的初始点。

图5是实施例中改进算法生成路径示意图。

图6是实施例中拓展障碍物矩阵方法生成路径示意图。

图7a是实施例中在水平方向拓展时遇到障碍物时算法生成的路径示意图。

图7b是实施例中方向为斜向时遇到障碍物时算法生成的路径示意图。

图8是实施例中存在不完全障碍物节点情况路径示意图。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。

本发明提供一种改进A*算法的移动机器人路径规划方法，其包括以下步骤：

S1、采用栅格法建立移动机器人工作环境的三维地图模型。

复杂环境下，移动机器人的路径规划问题更侧重于如何得到一条可以安全通过的路径，考虑到机器人自身的体积问题，建立环境的栅格地图时选取比移动机器人自身体积稍大的栅格地图。同一环境下，构建相对更大的栅格地图会减少算法中搜索邻域节点的计算量，提高运行效率。但栅格过大会影响搜索效果，到达目标点的误差也会更大。因此，在本实施例中，设置栅格地图时，选取机器人投影面积和每一个栅格大小之比为2:3的比例构建取n×n的平面栅格地图作为平面环境地图模型，认为每一个栅格是一个节点，再将每一个栅格划分为j×j的小栅格，进而得到一个p×p的二维平面地图模型，在此基础上在matlab中生成一个p ×p高度的信息矩阵，用mesh函数生成一个三维地图模型。其中，j、n均为大于1的自然数，p＝j×n。

例如，选取25×25的平面栅格地图作为平面环境地图模型，认为每一个栅格是一个节点，再将每一个栅格划分为4×4的小栅格，由此方法划分完成，得到一个100×100的二维平面地图模型，在此基础上在matlab中生成一个100×100的高度信息矩阵，用mesh函数生成一个三维地图模型，如图1所示。

S2、在三维地图模型中，针对栅格存在部分障碍物的情况采用改进A*算法进行邻域搜索，以提高生成路径的合理性；同时，通过引入坡度信息的改进代价函数计算从起始节点开始到其每一个邻域节点的代价值，使路径更平缓，进而在保证搜索效率的前提下得到最优路径。

在本步骤中，将移动机器人在前进过程中与周边障碍发生碰撞的问题进行简化，将机器人自身体积在平面坐标系的投影视作一个正方形，因此，仅需要考虑正方形的四个角与周边环境是否发生碰撞来进行判断。为解决传统路径规划算法产生的路径会导致移动机器人和障碍物发生碰撞的问题，本发明将栅格地图中的每个节点划分为j阶的障碍物节点，并根据每个节点内障碍物的分布情况划分为完全障碍物节点、不完全障碍物节点(即部分障碍物节点)和无障碍节点：

由于选取机器人投影面积和每一个栅格比例为2:3，为了便于计算移动机器人是否与障碍物边缘存在碰撞可能性，选用j＝4阶矩阵A来表示每一个栅格的障碍物信息并采用如下规则对节点进行分类：

a)若矩阵A中不存在非0点则认为是无障碍节点。

b)若矩阵A中主对角线或次对角线上存在两个及以上非0点则认为该节点为完全障碍物节点。

c)若矩阵A中第2、3列或第2、3行存在两个及以上非0点则认为该节点为完全障碍物节点。

d)除上述情况以外的节点认为是不完全障碍节点。

在每一次邻域搜索时，如果当前节点为对于完全障碍物节点或无障碍节点，则采用现有A*算法进行邻域搜索，对栅格存在不完全障碍物的情况采用改进A*算法进行邻域搜索。

其中，A*算法是一种启发式搜索算法，是通过一个代价函数计算从起始节点开始到其每一个邻域节点的代价值来确搜索方向。用G(n)表示从起始节点到当前节点n的实际代价，H(n)表示从当前节点n到目标点的代价估计值，可得代价函数的计算方式为：

f(n)＝g(n)+h(n)。 (1)

A*算法的计算步骤为：

a)初始化一个OPEN表和一个CLOSE表用于存储节点信息；

b)将起点放入OPEN表中，设其f(n)值为0；

c)判断OPEN表中存在节点则选取f(n)值最小节点N；

d)若节点N是终点，从终点开始追溯每一个节点的父节点直到起点，取出所有父节点即为路径

e)若节点N不是终点，将节点N从OPEN表中取出加入CLOSE表，遍历N的邻域节点，若邻域节点M不在CLOSE表中且不在OPEN表中，计算M的f(n)值，并将节点M加入OPEN表。

在每一次邻域搜索时，对栅格存在不完全障碍物的情况采用改进A*算法进行邻域搜索，并根据节点中障碍物分布的情况判断目标邻域节点是否满足可通行条件，具体方法如下：

S21、建立节点拓展4阶障碍物矩阵时障碍物节点序号表N_obs(如表1所示)，已知每个节点都存在8个邻域节点，用坐标的方式来表示，将当前点视为[0，0]，设定当前点0度的方向为[1，0]，则其8个邻域节点的坐标及忽略项表示方法如表 2所示；

表1障碍节点序号N_obs

1	2	3	4
				5	6	7	8
9	10	11	12
				13	14	15	16

表2节点自身障碍物矩阵忽略项

S22、每个邻域搜索方向都有不同的判断条件，判断当前节点在其邻域搜索方向上是否满足通行条件；判断方法如下：

S221、在进行邻域搜索时，对应搜索目标邻域需满足除表2中对应忽略项外其余项均为0，可认定当前节点在该方向上可以进行搜索。

S222、在满足节点自身能够搜索该邻域的条件时，判断目标邻域节点是否满足可通行条件。

以目标邻域节点为右上节点时为例，N₀为当前节点的轨迹起始点，N_i为搜索过程中出现的下一目标点；在目标邻域节点不存在障碍物的情况下存在如图2所示5种情况即图2中的图a至图e(5种情况为：如果将第一种作为默认情况即认定没有障碍的情况下当前节点的起始点，其余四种分别为向上、向下、向左和向右偏移了一格的初始点，即上一节点向当前节点拓展时遇到不完全障碍时，可能会产生的四种偏移方向)，此时除考虑N₀和当前N_i的障碍物分布，同时也需要考虑其相邻邻域的障碍物分布情况是否满足可通行条件，即如图2中三条实线构成的区域其障碍物矩阵项的值均为0。

当目标邻域节点边缘存在障碍物时，需要判断目标邻域节点的障碍物分布情况，由图2中图a至图e可知，当目标邻域节点为右上节点时，考虑机器人在栅格地图中移动的轨迹，将满足可通行条件的障碍物情况分为两种：

a)目标邻域节点障碍物分布为左上侧时：即N_obs中1，2，5区域的任意区域存在障碍，其余区域均不存在障碍时，可将轨迹中心点进行偏移以获取可通行的路径。

b)如图4所示，目标邻域节点障碍物分布为右下侧时：即N_obs中12，15，16 区域的任意区域存在障碍，其余区域均不存在障碍时，可将轨迹中心点进行偏移以获取可通行的路径，如图4中的图a至图e所示。

如图3所示，为节点左上侧存在障碍区域，N_i的障碍物矩阵中1，2，5为不可通行区域，而其余区域满足可通行的条件，进而再判断相邻邻域节点是否具有可通行的条件，即N₀右侧和上方的节点的障碍物矩阵中被虚线划过的区域均不存在障碍物，称其满足“借道条件”，则将目标邻域节点的“中心点”偏移，避开边角存在的障碍物(如图3中的图a至图e所示)，从而取得一条从起始点到终点代价较低的安全路径。

S23、引入坡度信息的改进代价函数计算从起始节点开始到其每一个邻域节点的代价值；具体计算方法为：

S231、确定在山地行走时速度和坡度的关系为：

1/V＝0.75+14.6slope² (2)

式中，Slope为坡度信息，其计算公式为：

Slope＝H/L (3)

式中，H为当前节点中最高的高度信息值,L为水平宽度。

S232、对代价函数中G(n)和H(n)的计算方式进行改进，仍选取欧式距离的计算方法：

式中，n_x表示x方向的当前节点，n_x-1表示x方向的上一个节点，n_y表示y 方向的当前节点，n_y-1表示y方向的上一个节点，ρ为两个节点间的平面欧式距离，对于存在坡度的路面计算其等效平面距离S_eq，根据式(2)中坡度和速度的关系，取其等效平面距离S_eq为：

S_eq＝(1+Slope_n ²)×ρ (5)

Slope_n表示当前节点的坡度；

可得G(n)的计算公式为：

G_n＝G_n-1+(1+Slope_n ²)×S_eq (6)

S233、相应的H(n)需要考虑和终点间的估计代价值，仍选取欧式距离作为代价函数的计算方式，在已知当前节点和目标点之间的平面欧式距离的基础上，假设从当前节点到目标节点的坡度信息均为Slope_n，根据公式(5)得到其等效平面距离S_eq’：

其中，Goal_x表示x方向的最后终点，Goal_y表示y方向的最后终点；

在此基础上再考虑目标点的坡度信息和当前节点坡度信息的差的等效平面距离，可得H(n)的计算方式：

H_n＝[(Slope_n-Slope_goal)²+1]×S_eq’。 (8)

实施例：

在无障碍情况下，在不考虑高度信息的情况下由传统A*算法得到的最短路径如图5虚线所示，这条路径虽然是理论上存在的最短路径，但没有考虑复杂地形对于移动机器人行动的影响，在此种地形环境中，路径规划问题应更多考虑到实际地形因素。如图5实线所示为改进了代价函数计算方式的A*算法相较于前者，对地形条件的考虑更为充分，虽然路径更为长，但生成的路径更为平缓，通过性更好。

在改进代价函数的基础上，进一步验证规划在考虑移动机器人本身体积的情况下在该路径下是否和障碍物发生碰撞的问题，在目标邻域节点为水平方向和对角线方向时进行讨论，图6为存在障碍物情况下的路径规划情况，虚线表示改进代价函数的A*算法的避开障碍物的路径，实线为拓展障碍物矩阵方法改进后生成的路径，由结果分析，如果不考虑移动机器人和障碍物体积而得到的规划路径是一条危险路径，且在斜方向为目标邻域节点的情况下，移动机器人会与障碍物发生碰撞，现实情况中移动机器人的路径规划问题通常会采用局部路径规划和全局路径规划相结合的方式进行实时避障，在遇到存在无法通过的障碍物的情况时会进行局部的二次规划，为减少重复规划的次数，在进行全局的路径规划时应得到一条当前静态环境下更为安全的路径。

如图7a、图7b所示，采用为节点拓展障碍物矩阵的方法之后生成的路径和障碍物之间留有足够的安全距离按照设定移动机器人本体大小和栅格地图比例为 2:3，路径为移动机器人中心点的轨迹，图中路径与障碍物的最短距离为1个网格长度，而机器人大小所占据面积的1/2长度小于1个网格距离，得改进后的算法能够解决在目标邻域节点为对角线方向时，生成的路径会与障碍物的边界发生碰撞的问题。

为进一步验证本发明在遇到栅格边缘存在障碍物的情况，在原有路径上添加不完全障碍物节点，如图8所示：实线部分为原路径即不存在该不完全障碍物节点时所生成的路径，虚线为添加不完全障碍物节点的情况下生成的路径，相较于不存在该障碍物的情况下向下偏移一个网格的距离，使得生成路径的安全性更高。

综上，本发明通过拓展栅格地图节点的障碍物矩阵的方法解决了因忽略移动机器人自身体积和栅格地图划分栅格大小造成的生成路径安全性问题，且生成的路径更为合理。同时针对在复杂地形条件下，采用了新的代价函数计算方法，引入坡度信息，将陡坡上移动的距离简化为平面等效距离计算每一个节点总的代价值，使得生成路径更趋于平缓。本发明能够在复杂地形条件下得到一条平缓的路径且通过拓展节点障碍物矩阵的方法能够有效减少移动机器人与障碍物发生碰撞的概率。

上述各实施例仅用于说明本发明，各个步骤都是可以有所变化的，在本发明技术方案的基础上，凡根据本发明原理对个别步骤进行的改进和等同变换，均不应排除在本发明的保护范围之外。

Claims (10)

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

S1、采用栅格法建立移动机器人工作环境的三维地图模型；

S2、在三维地图模型中，针对栅格存在部分障碍物的情况采用改进A*算法进行邻域搜索，以提高生成路径的合理性；同时，通过引入坡度信息的改进代价函数计算从起始节点开始到其每一个邻域节点的代价值，使路径更平缓，进而在保证搜索效率的前提下得到最优路径。

2.如权利要求1所述移动机器人路径规划方法，其特征在于：所述步骤S1中，设置栅格地图时，选取机器人投影面积和每一个栅格大小之比为2:3的比例构建取n×n的平面栅格地图作为平面环境地图模型，每一个栅格是一个节点，再将每一个栅格划分为j×j的小栅格，进而得到一个p×p的二维平面地图模型，在此基础上在matlab中生成一个p×p高度的信息矩阵，用mesh函数生成一个三维地图模型；其中，j、n均为大于1的自然数，p＝j×n。

3.如权利要求1所述移动机器人路径规划方法，其特征在于：所述步骤S2中，将栅格地图中的每个节点划分为j＝4阶的障碍物节点，并根据每个节点内障碍物的分布情况划分为完全障碍物节点、部分障碍物节点和无障碍节点。

4.如权利要求3所述移动机器人路径规划方法，其特征在于，选用j＝4阶矩阵A来表示每一个栅格的障碍物信息并采用如下规则对节点进行分类：

a)若矩阵A中不存在非0点则认为是无障碍节点；

b)若矩阵A中主对角线或次对角线上存在两个及以上非0点则认为该节点为完全障碍物节点；

c)若矩阵A中第2、3列或第2、3行存在两个及以上非0点则认为该节点为完全障碍物节点；

d)除上述情况以外的节点认为是不完全障碍节点。

5.如权利要求4所述移动机器人路径规划方法，其特征在于，在每一次邻域搜索时，如果当前节点为对于完全障碍物节点或无障碍节点，则采用现有A*算法进行邻域搜索，对栅格存在不完全障碍物的情况采用改进A*算法进行邻域搜索。

6.如权利要求5所述移动机器人路径规划方法，其特征在于，对栅格存在不完全障碍物的情况采用改进A*算法进行邻域搜索，并根据节点中障碍物分布的情况判断目标邻域节点是否满足可通行条件，具体方法如下：

S21、建立节点拓展4阶障碍物矩阵时障碍物节点序号表N_obs，已知每个节点都存在8个邻域节点，用坐标的方式来表示，将当前点视为[0,0]，设定当前点0度的方向为[1,0]；

S22、每个邻域搜索方向都有不同的判断条件，判断当前节点在其邻域搜索方向上是否满足通行条件；

S23、引入坡度信息的改进代价函数计算从起始节点开始到其每一个邻域节点的代价值。

7.如权利要求6所述移动机器人路径规划方法，其特征在于，所述障碍物节点序号表N_obs为：

障碍物节点序号N_obs

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16

。

8.如权利要求7所述移动机器人路径规划方法，其特征在于，所述每个节点的8个邻域节点的坐标及忽略项表示方法如下表所示：

节点自身障碍物矩阵忽略项

9.如权利要求6所述移动机器人路径规划方法，其特征在于，所述S22中，判断方法如下：

S221、在进行邻域搜索时，对应搜索目标邻域需满足除对应忽略项外其余项均为0，认定当前节点在该方向上可以进行搜索；

S222、在满足节点自身能够搜索该邻域的条件时，判断目标邻域节点是否满足可通行条件：

当目标邻域节点边缘存在障碍物时，需要判断目标邻域节点的障碍物分布情况，当目标邻域节点为右上节点时，考虑机器人在栅格地图中移动的轨迹，将满足可通行条件的障碍物情况分为两种：

a)目标邻域节点障碍物分布为左上侧时：即N_obs中1，2，5区域的任意区域存在障碍，其余区域均不存在障碍时，将轨迹中心点进行偏移以获取可通行的路径；

b)目标邻域节点障碍物分布为右下侧时：即N_obs中12，15，16区域的任意区域存在障碍，其余区域均不存在障碍时，将轨迹中心点进行偏移以获取可通行的路径。

10.如权利要求6所述移动机器人路径规划方法，其特征在于，所述S23中，具体计算方法为：

S231、确定在山地行走时速度和坡度的关系为：

1/V＝0.75+14.6slope²

式中，Slope为坡度信息，Slope＝H/L，H为当前节点中最高的高度信息值,L为水平宽度；

S232、对于存在坡度的路面计算其等效平面距离S_eq，根据坡度和速度的关系，取其等效平面距离S_eq为：

S_eq＝(1+Slope_n ²)×ρ

Slope_n表示当前节点的坡度,ρ为两个节点间的平面欧式距离；

可得从起始节点到当前节点n的实际代价G(n)为：

G_n＝G_n-1+(1+Slope_n ²)×S_eq；

S233、在已知当前节点和目标点之间的平面欧式距离的基础上，假设从当前节点到目标节点的坡度信息均为Slope_n，得到其等效平面距离S_eq’：

其中，Goal_x表示x方向的最后终点，Goal_y表示y方向的最后终点；

在此基础上再考虑目标点的坡度信息和当前节点坡度信息的差的等效平面距离，可得从当前节点n到目标点的代价估计值H(n)的计算方式：

H_n＝[(Slope_n-Slope_goal)²+1]×S_eq’。

Images