CN112147998B - 一种基于区域生长法的移动机器人路径规划方法 - Google Patents

Abstract

本发明涉及一种基于区域生长法的移动机器人路径规划方法，包括步骤：读取工作环境代价地图，在可行域内随机选择一系列坐标点；采用区域生长法，以随机选择的坐标点为中心，分别向代价地图的左上、左下、右上和右下四个方向以三角形形状生长，对可行域内的坐标点划分区域，获取划分后的区域地图，并重新计算每个区域的区域中心点坐标；根据处理结果确定移动机器人当前位置所属的区域，并确定目标点所属的区域，采用改进的A*算法规划路径；将起点坐标和终点坐标加入规划路径中；采用theta*算法对路径进行优化，获取最终优化路径。与现有技术相比，本发明可有效降低机器人跟随路径时与障碍物发生碰撞的风险。

Description

一种基于区域生长法的移动机器人路径规划方法

技术领域

本发明涉及机器人路径规划技术领域，尤其是涉及一种基于区域生长法的移动机器人路径规划方法。

背景技术

路径规划是移动机器人运动的基础，是保障移动机器人在不同位置之间正确、安全移动的必要环节。如何基于地图快速地得到从移动机器人当前位置到目标点的最优路径是移动机器人领域的一个研究热点。现有移动机器人路径规划方法主要有以下几种：

图搜索法：如Dijkstra算法、A*算法等，通过地图建立节点之间的连接关系，一般地图上可行域内的每一个像素点都是一个节点，从理论上能够保证找到任意起点到任意终点的路径；虽然在实际工作环境的应用中，图搜索法具备较好的实时性，但得到的路径并不一定是最优路径，例如采用Dijkstra算法或A*算法在搜索路径过程中往往倾向于朝目标点方向移动，如果遇到障碍物则会采取贴着障碍物绕行的策略，这样得到的路径不仅不是最优路径，且控制移动机器人沿着障碍物的轮廓移动是不可取的。对于传统的A*算法规划路径会贴近障碍物的问题，现有技术通常选择增大地图的膨胀半径，然而这种方式会导致机器人无法通过较为狭窄的区域，例如门等区域。膨胀半径的增大会减少可行区域的宽度，极有可能出现可行域宽度小于机器人自身宽度从而导致机器人无法通过。

人工势场法：该方法包括基本人工势场法和各种改进版的人工势场法，通过仿照自然地势变化将移动机器人与周围环境的关系抽象成引力场和斥力场的关系，从引力和斥力的角度规划路径，人工势场法计算简单、结构简单、实时性高且易于实现，其规划出的路径一般较为平滑，因而能够得到广泛应用；然而当移动机器人受到的引力和斥力大小相等、方向相反时，移动机器人所受合力为0，即移动机器人陷入局部最小值或震荡；当移动机器人与目标位置相距较远时，移动机器人所受引力过大，可能导致移动机器人与障碍物相撞，且目标点靠近障碍物时，移动机器人所受斥力可能大于目标位置产生的引力从而导致移动机器人无法到达终点。

启发式方法：如蚁群算法，鱼群算法等，主要仿照自然界生物的习性建立搜索算法；启发式方法往往无法得到移动机器人当前位置到目标位置的最优路径，甚至得到无法使用的路径，且一般算法耗时较长，在很多情况下难以收敛。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于区域生长法的移动机器人路径规划方法，该方法可降低移动机器人跟随路径时与障碍物发生碰撞的风险，并有效缓解增大膨胀半径导致移动机器人无法通过较为狭窄的区域的缺陷。

本发明的目的可以通过以下技术方案来实现：

一种基于区域生长法的移动机器人路径规划方法，该方法包括如下步骤：

S1：读取建立的工作环境的代价地图，在可行域内随机选择一系列坐标点。

进一步地，在可行域内随机选择的坐标点的数量为当前建立的工作环境的代价地图的像素总数的1/150。

S2：采用区域生长法，以随机选择的坐标点为中心，分别向代价地图的左上、左下、右上和右下四个方向以三角形形状生长，四个方向的区域生长相互独立，通过对生长区域判断是否存在障碍物，对可行域内的坐标点划分区域，获取划分后的区域地图，并重新计算每个区域的区域中心点坐标；具体地：

遍历随机选择的每一个坐标点，对每一个坐标点以其自身为中心，分别向代价地图的左上、左下、右上和右下四个方向以三角形形状生长，判断各三角形形状生长所构成的三角形区域是否碰触到障碍物，若触碰到障碍物，则停止生长，四个方向的生长均结束后，当前构成的各三角形区域的集合为划分区域，随后进行下一个随机选择的每一个坐标点的区域生长，直至所有随机选择的坐标点的区域划分结束后，更新区域中心点坐标。

S3：对可行域内无法通过区域生长法划分区域的部分孤立点进行单独区域划分处理，处理结束后，得到划分后的区域地图，并重新计算每个区域的中心点坐标；具体地：

遍历每一个孤立点，判断该孤立点与各区域中心点之间的直线上是否存在障碍物，若存在，则判定该孤立点与对应区域中心点的距离为无穷大，随后判定与该孤立点最近的区域中心点为该孤立点的中心点；若不存在障碍物，则计算该孤立点与各区域中心点之间的欧式距离，随后判定与该孤立点最近的区域中心点为该孤立点的中心点，直至所有孤立点遍历结束。

S4：保存步骤S2、步骤S3处理后的各区域中心点坐标、划分区域后的区域地图以及区域联通关系矩阵；将每一个区域视作一个节点，所述区域联通关系矩阵为各节点之间的连接关系。

S5：根据步骤S2、步骤S3处理后的结果确定移动机器人当前位置所属的区域，并确定目标点所属的区域，以每一个区域为一个节点，采用改进的A*算法规划路径。采用改进的A*算法规划路径的具体步骤包括：

51)根据划分后的区域地图，确定移动机器人当前位置所属节点，将其视为起始节点，并确定移动机器人的目标位置所属节点，将其视为目标节点；

52)将列表closelist和poplist初始化为空；

53)将起始节点设为当前所需处理节点；

54)将起点和起始节点放入closelist。其中closelist存放规划出的路径，poplist存放算法执行过程中需要排除的节点；

55)判断当前所需处理节点是否为目标节点，若是则停止执行，将目标节点和目标位置放入closelist中，路径搜索完成，否则，基于区域联通关系矩阵获取当前所需处理节点的所有邻接节点；

56)遍历所有邻接节点，判断邻接节点是否包含在closelist列表或poplist列表中，若是，则从相应列表中删除该邻接节点后，判断邻接节点是否为空，若为空，则进行列表更新步骤，若不为空，则执行下一步；

57)计算所有邻接节点的代价函数值，表达式为：

F＝G+H

式中，G为当前所需处理节点到邻接节点的欧氏距离，H为邻接节点到目标节点的欧氏距离。

58)将代价函数值最小的邻接节点放入到closelist列表中，并将其设为当前所需处理节点，返回至步骤55)中继续执行。

进一步地，步骤56)中，列表更新步骤的具体内容为：

若判断邻接节点包含在closelist列表或poplist列表中，则从相应列表中删除该邻接节点后，判断邻接节点列表是否为空，对于列表为空的情况，结束算法；若邻接节点列表为空且closelist列表中未包含目标节点，则判定当前所需处理节点为死角，清空poplist列表，将当前所需处理节点放入poplist列表中，并对closelist列表删除最后一个节点，将最新作为closelist的最后节点设为当前所需处理节点，跳转至步骤55)继续执行。

进一步地，在步骤56)之前，还包括以下步骤内容：

采用改进的A*算法规划路径时，每次只考虑一个节点的邻域，若当前所需处理节点不存在符合条件的邻域，则标记当前所需处理节点为死角节点，并删除openlist列表和closelist列表中的死角节点后，退回至上一节点规划路径。

S6：将起点坐标和终点坐标加入步骤S5规划的路径中，得到以起点-起始节点中心点-起始节点到目标节点的路径-目标节点中心点-目标位置排列的路径。

S7：基于Bresenham算法，计算步骤S6得到的路径上所有相邻区域中心点构成的线段上的所有坐标点，通过对坐标点在区域地图上的值是否为障碍物对应的值，将该路径以区域中心点的连接关系扩充为像素点的连接关系，并采用theta*算法对路径进行优化，获取最终优化路径。

采用theta*算法对路径进行优化具体包括下列步骤：

71)选取步骤S6中规划的路径的第一个坐标点和第二个坐标点，将其分别命名为一号节点和二号节点；

72)判断二号节点是否为终点，若是，则路径优化完成，停止本算法，否则通过Bresenham算法计算一号节点和二号节点构成的线段上的所有坐标点，从区域地图中读取计算的坐标点对应的值；

73)判断读取的坐标点对应的值是否均为可行域对应的数值，若是，则更新二号节点，选取路径列表中的下一个节点作为二号节点，并跳转至步骤72)继续执行；若至少存在一个坐标点在区域地图上对应的值为障碍物所对应的值，则将一号节点到二号节点的前一个节点之间的所有点设为新路径，将一号节点更新为二号节点的前一个节点，并令二号节点保持不变，跳转至步骤72)继续执行。

本发明提供的基于区域生长法的移动机器人路径规划方法，相较于现有技术至少包括如下有益效果：

一、现有技术大多以代价地图中的每一个像素点为节点，出现在障碍物边界的节点因过于靠近障碍物而增加了机器人移动到该节点时与障碍物发生碰撞的几率，本发明以每一个子区域为一个节点，以区域中心点坐标代表该区域，每一个节点的中心点与都不会过于贴近障碍物，从而使得规划出来的路径不会贴近障碍物边界，极大的降低了机器人跟随路径时与障碍物发生碰撞的风险；

二、传统A*算法规划路径会贴近障碍物，为了解决这一问题，通常选择增大地图的膨胀半径，但是如门等较为狭窄的区域，膨胀半径的增大会减少可行区域的宽度，极有可能出现可行域宽度小于机器人自身宽度从而导致机器人无法通过，本发明提出的路径规划方法以区域为节点，以节点中心点为代表而节点中心点不会过于靠近障碍物，控制机器人移动时机器人会沿着节点中心点构成的线段移动，因此得到的路径不会靠近障碍物边界，有效的缓解增大膨胀半径导致机器人无法通过较为狭窄的区域的缺陷；

三、本发明提出的路径规划方法以区域为节点，以区域中心点坐标代表该区域，每一个节点的中心点与都不会过于贴近障碍物，从而使得规划出来的路径不会贴近障碍物边界，规划路径时，每次只考虑一个节点的邻域，并及时删除死角节点，且规划的路径采用theta*算法进行优化，方法简单，数据处理量小，耗时短，能达到很好的收敛效果。

附图说明

图1为实施例中基于区域生长法的移动机器人路径规划方法的流程示意图；

图2为实施例中使用区域生长法为可行域划分区域的流程示意图；

图3为实施例中对每一个中心点应用区域生长法的具体流程示意图；

图4为实施例中孤立点的处理流程示意图；

图5为实施例中规划路径的流程示意图；

图6为实施例中基于Bresenham的theta*算法优化路径示意图；

图7a)为实施例中工作环境地图；图7b)为实施例中区域划分效果图；

图8为实施例中基于区域生长法的移动机器人路径规划方法在不同起点和终点的规划效果图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。

实施例

如图1所示，本发明涉及一种基于区域生长法的移动机器人路径规划方法，该方法包括以下步骤：

步骤一、读取建立的工作环境的代价地图，在可行域内随机选择一系列坐标点。

工作环境的代价地图为通过其他建图算法(如覆盖栅格建图算法、计数建图算法、TSDF建图算法等)得到的工作环境的二维地图经过膨胀处理得到的地图(costmap)。提取可行域时，所有经过膨胀处理覆盖的区域均认为是障碍物区域，随机选择的点均为可行域内的点。本发明将在可行域内随机选择的坐标点的数量设置为当前地图像素总数的150分之一。，对于同一个工作环境，对其代价地图可行域的子区域划分和孤立点处理过程只需要执行一次，保存处理结果，每次路径规划无需重复进行。

步骤二、通过区域生长法以这些随机点为中心分别向左上、左下、右上、右下四个方向以三角形形状生长，四个方向的区域生长相互独立，以是否碰到障碍物为条件判断区域生长是否停止，从而对可行域内大部分点划分区域，划分结束后重新计算每个区域的区域中心点坐标。具体地：

遍历步骤一得到的每一个随机点，将随机点的邻域沿水平方向和竖直方向分成四个部分：左上、左下、右上、右下，每个部分均以三角形形状独立生长，当三角形区域碰触到障碍物时停止生长，四个方向的生长均结束后，当前节点的区域划分结束，则进行下一个随机点的区域生长，直到所有随机点的区域划分结束后，更新区域中心点坐标。区域生长法的流程如图2所示，对每一个随机点进行区域生长法划分区域的流程如图3所示。

步骤三、对于可行域内无法通过区域生长法划分区域的部分孤立点，对这些孤立点进行单独的处理，根据孤立点到各个区域中心点的距离以及孤立点与区域中心点之间是否有障碍物为依据，为这些孤立点划分区域，孤立点处理结束后，重新计算每个区域的中心点坐标。

孤立点到个区域中心点的距离计算需要考虑孤立点与区域中心点构成的线段是否经过障碍物，距离的计算公式为：

其中x_i是第i个孤立点，x_c是第c个区域中心点，通过本距离计算方法可以保证不会将障碍物包含到区域内部。对孤立点的处理过程如图4所示。具体地：

遍历每一个孤立点，判断该直线上是否有障碍物，如有，则判定孤立点到个区域中心点的距离为无穷大，然后判定与该孤立点最近的中心点为该孤立点的中心点；若直线上没有障碍物，则按照上式计算孤立点到个区域中心点的欧式距离，然后判定与该孤立点最近的中心点为该孤立点的中心点；按照上述过程遍历每一个孤立点，直至所有孤立点处理结束。

步骤二对代价地图可行域的子区域划分和步骤三对孤立点处理过程直接影响后续规划的路径的质量，规划出的路径质量较差或者从起点到终点确实存在路径却没有规划出路径时，应当重新对可行域进行区域划分。

步骤四、经过区域生长法和孤立点处理过程，保存处理得到的结果供后续规划路径使用，保存的数据有：

1、各区域中心点坐标。

2、划分好区域之后的区域地图，同一个区域内的点用同一数值标记，不同区域的点在地图上的数值不同。

3、区域联通关系矩阵，一个区域视为一个节点，联通关系矩阵保存节点间的连接关系：相邻或不相邻。

步骤五、读取预处理过程得到的结果，确定移动机器人当前位置所属的区域，确定目标点所属的区域，以每一个区域为一个节点，使用改进的A*算法规划路径。

使用A*算法规划路径，以每一个区域为一个节点，由于每个区域大小和形状都不相同，所以每个节点的邻接节点数量也不同，另外针对“U”型障碍物对A*算法进行了改进。使用A*算法规划从起始节点到目标节点的路径，A*算法所涉及的节点之间距离的计算均适用两节点的中心点的欧氏距离。

本发明所使用的A*算法的步骤具体包括：

1、借助划分好区域后的区域地图，确定移动机器人当前位置所属的节点(称为起始节点)和目标位置所属的节点(称为目标节点)。

2、初始化列表closelist和poplist为空，其中closelist存放规划出的路径，poplist存放算法执行过程中需要排除的节点。

3、把起始节点设为当前要处理的节点(称为当前节点)。

4、把起点和起始节点放入closelist。

5、如果当前节点是目标节点则停止算法，将目标节点和目标位置放入到closelist中，路径搜索完成停止本算法。否则基于区域联通关系矩阵得到当前节点的所有邻接节点。

6、遍历所有邻接节点列表，如果邻接节点包含在closelist或poplist中则从列表中删除该邻接节点，如果邻接节点列表变为空则无法得到起始节点到目标节点的路径，算法结束。

7、如果邻接节点列表为空且closelist未包含目标节点，说明当前节点为死角，清空poplist列表，将当前节点放入到poplist中，closelist删除最后一个节点，将新成为closelist的最后节点设为当前节点跳转到第5步继续执行；如果邻接列表不为空则继续执行下一步。

8、计算所有邻接节点的代价函数值(F＝G+H)，其中G为当前节点到邻接节点的欧氏距离，H为邻接节点到目标节点的欧氏距离。

9、将代价函数值最小的邻接节点放入到closelist中并将其设为当前节点；回到上述第5步继续执行。

步骤六、将起点坐标、步骤五得到的以区域中心点为代表的路径，终点坐标依顺序排列即为得到的路径。得到的路径为：起点-起始节点中心点-起始节点到目标节点的路径-目标节点中心点-目标位置。

步骤七、因步骤六得到的路径点并不从像素层面上相邻，故使用基于Bresenham算法的theta*算法对步骤六得到的路径进行优化。具体地：

首先使用Bresenham算法将步骤六得到的路径上所有相邻区域中心点构成的线段上的所有点的坐标计算出，这些点均为路径上的点，从而使得路径上的点在像素层面上相邻；得到的路径一般存在较多的转折点，使用基于Bresenham的theta*算法优化路径，规划路径过程如图5所示，路径优化过程简化为图6所示。图中黑色填充框为障碍物，白色填充框为可行域，实折线为初步规划出的路径示意，数字①到⑤所示的虚线按数值依次递增为路径优化过程示意)。

本发明所使用theta*算法优化路径的步骤包括：

1、分别取已经规划出的路径的第一个坐标点和第二个坐标点命名为1号节点和二号节点；

2、如果2号节点即为终点，则路径优化完成，停止本算法，否则通过Bresenham算法计算1号节点和2号节点构成的线段上的所有坐标点，从区域地图数据中读取这些坐标点对应的值；

3、如果所有的值均为可行域对应的数值，则更新2号节点，取路径列表中的下一个节点作为2号节点，跳转到第2步继续执行；如果至少存在一个坐标点在地图上的值为障碍物所对应的值，则将从1号节点到2号节点的前一个节点之间的所有点设为新的路径，将1号节点更新为2号节点的前一个节点，2号节点保持不变，跳转到第2步继续执行。

实际的代价地图区域划分与路径规划效果如图7和图8所示。图7a)为工作环境地图；图7b)为区域划分效果图，图7b)中的各多边形状为按照上述方法所划分的各个区域。图8为基于区域生长法的移动机器人路径规划方法在不同起点和终点的规划效果图，可见规划的路径不会贴近障碍物边界，有效避开了包括狭窄区域在内的障碍物。

本发明提供的移动机器人路径规划方法与现有技术中的路径规划方法相比，最大的创新点有两点：第一点在于本发明以每一个子区域为一个节点，以区域中心点坐标代表该区域，每一个节点的中心点与都不会过于贴近障碍物，从而使得规划出来的路径不会贴近障碍物边界，极大的降低了移动机器人跟随路径时与障碍物发生碰撞的风险。第二点在于传统的A*算法规划路径会贴近障碍物，为了解决这一问题，通常选择增大地图的膨胀半径，但是这会导致移动机器人无法通过如门等较为狭窄的区域，本发明提出的路径规划方法有效的缓解了这一矛盾。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的工作人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (9)

1.一种基于区域生长法的移动机器人路径规划方法，其特征在于，包括下列步骤：

1)读取建立的工作环境的代价地图，在可行域内随机选择一系列坐标点；

2)采用区域生长法，以随机选择的坐标点为中心，分别向代价地图的左上、左下、右上和右下四个方向以三角形形状生长，四个方向的区域生长相互独立，通过对生长区域判断是否存在障碍物，对可行域内的坐标点划分区域，获取划分后的区域地图，并重新计算每个区域的区域中心点坐标；

3)对可行域内无法通过区域生长法划分区域的部分孤立点进行单独区域划分处理，处理结束后，得到划分后的区域地图，并重新计算每个区域的中心点坐标；

4)保存步骤2)、步骤3)处理后的各区域中心点坐标、划分区域后的区域地图以及区域联通关系矩阵；

5)根据步骤2)、步骤3)处理后的结果确定移动机器人当前位置所属的区域，并确定目标点所属的区域，以每一个区域为一个节点，采用改进的A*算法规划路径；

6)将起点坐标和终点坐标加入步骤5)规划的路径中，得到以起点-起始节点中心点-起始节点到目标节点的路径-目标节点中心点-目标位置排列的路径；

7)基于Bresenham算法，计算步骤6)得到的路径上所有相邻区域中心点构成的线段上的所有坐标点，通过对坐标点在区域地图上的值是否为障碍物对应的值，将该路径以区域中心点的连接关系扩充为像素点的连接关系，并采用theta*算法对路径进行优化，获取最终优化路径。

2.根据权利要求1所述的一种基于区域生长法的移动机器人路径规划方法，其特征在于，步骤2)的具体内容为：

遍历步骤1)随机选择的每一个坐标点，对每一个坐标点以其自身为中心，分别向代价地图的左上、左下、右上和右下四个方向以三角形形状生长，判断各三角形形状生长所构成的三角形区域是否碰触到障碍物，若触碰到障碍物，则停止生长，四个方向的生长均结束后，当前构成的各三角形区域的集合为划分区域，随后进行下一个随机选择的每一个坐标点的区域生长，直至所有随机选择的坐标点的区域划分结束后，更新区域中心点坐标。

3.根据权利要求1所述的一种基于区域生长法的移动机器人路径规划方法，其特征在于，步骤3)的具体内容为：

遍历每一个孤立点，判断该孤立点与各区域中心点之间的直线上是否存在障碍物，若存在，则判定该孤立点与对应区域中心点的距离为无穷大，随后判定与该孤立点最近的区域中心点为该孤立点的中心点；若不存在障碍物，则计算该孤立点与各区域中心点之间的欧式距离，随后判定与该孤立点最近的区域中心点为该孤立点的中心点，直至所有孤立点遍历结束。

4.根据权利要求1所述的一种基于区域生长法的移动机器人路径规划方法，其特征在于，将每一个区域视作一个节点，所述区域联通关系矩阵为各节点之间的连接关系。

5.根据权利要求1所述的一种基于区域生长法的移动机器人路径规划方法，其特征在于，步骤5)中，采用改进的A*算法规划路径的具体步骤包括：

51)根据划分后的区域地图，确定移动机器人当前位置所属节点，将其视为起始节点，并确定移动机器人的目标位置所属节点，将其视为目标节点；

52)将用以存放规划路径的列表closelist以及用以存放算法执行过程中需要排除的节点列表poplist初始化为空；

53)将起始节点设为当前所需处理节点；

54)将起点和起始节点放入closelist中；

55)判断当前所需处理节点是否为目标节点，若是则停止执行，将目标节点和目标位置放入closelist中，路径搜索完成，否则，基于区域联通关系矩阵获取当前所需处理节点的所有邻接节点；

56)遍历所有邻接节点，判断邻接节点是否包含在closelist或poplist中，若是，则从相应列表中删除该邻接节点后，判断邻接节点是否为空，若为空，则进行列表更新步骤，若不为空，则执行下一步；

57)计算所有邻接节点的代价函数值；

58)将代价函数值最小的邻接节点放入到closelist中，并将其设为当前所需处理节点，返回至步骤55)中继续执行。

6.根据权利要求5所述的一种基于区域生长法的移动机器人路径规划方法，其特征在于，步骤56)中，列表更新步骤的具体内容为：

若判断邻接节点包含在closelist或poplist中，则从相应列表中删除该邻接节点后，判断邻接节点列表是否为空，对于列表为空的情况，结束算法；若邻接节点列表为空且closelist中未包含目标节点，则判定当前所需处理节点为死角，清空poplist，将当前所需处理节点放入poplist中，并对closelist删除最后一个节点，将最新作为closelist的最后节点设为当前所需处理节点，跳转至步骤55)继续执行。

7.根据权利要求5所述的一种基于区域生长法的移动机器人路径规划方法，其特征在于，邻接节点的代价函数值F的表达式为：

F＝G+H

式中，G为当前所需处理节点到邻接节点的欧氏距离，H为邻接节点到目标节点的欧氏距离。

8.根据权利要求1所述的一种基于区域生长法的移动机器人路径规划方法，其特征在于，步骤7)中，采用theta*算法对路径进行优化具体包括下列步骤：

71)选取步骤6)中规划的路径的第一个坐标点和第二个坐标点，将其分别命名为一号节点和二号节点；

72)判断二号节点是否为终点，若是，则路径优化完成，停止本算法，否则通过Bresenham算法计算一号节点和二号节点构成的线段上的所有坐标点，从区域地图中读取计算的坐标点对应的值；

73)判断读取的坐标点对应的值是否均为可行域对应的数值，若是，则更新二号节点，选取路径列表中的下一个节点作为二号节点，并跳转至步骤72)继续执行；若至少存在一个坐标点在区域地图上对应的值为障碍物所对应的值，则将一号节点到二号节点的前一个节点之间的所有点设为新路径，将一号节点更新为二号节点的前一个节点，并令二号节点保持不变，跳转至步骤72)继续执行。

9.根据权利要求1所述的一种基于区域生长法的移动机器人路径规划方法，其特征在于，步骤1)中，在可行域内随机选择的坐标点的数量为当前建立的工作环境的代价地图的像素总数的1/150。

Images