CN115586769A - 移动机器人路径规划方法和系统 - Google Patents

Abstract

本发明提供移动机器人路径规划方法和系统，包括构建移动机器人工作环境的栅格地图；利用路径规划算法在栅格地图上规划移动机器人路径，获得全局最优路径；确定全局最优路径上的转折点；利用转折点评价函数，计算各转折点的评价值，根据评价值选取关键转折点；将选取出来的关键转折点进行路径平滑处理，获得优化后的全局最优路径。本发明通过选取关键转折点，并将选取的关键转折点做路径平滑处理可以解决机器人行进过程中转折点过多问题；可以缩短机器人的行径路径和行驶时间。

Description

移动机器人路径规划方法和系统

技术领域

本发明属于移动机器人路径规划技术领域，具体涉及移动机器人路径规划。

背景技术

在机器人领域，在有障碍物的环境中，在满足距离、计算时间、通信延迟和能量消耗等优化条件的前提下，寻找从机器人初始位置到期望位置的安全路径。在真实场景下，机器人进行路径规划很重要。

路径规划算法的分类有很多种。根据外部环境信息是否已知，可分为全局最优路径规划算法和局部路径规划算法；而根据算法的搜索方式，也可分为盲目式搜索算法和启发式搜索算法。盲目式搜索注重搜索的过程而不是搜索目标，往往伴随着巨大的搜索空间，导致消耗大量内存资源且效率低下，具体的有广度优先算法、深度优先算法和Dijkstra算法等。启发式搜索在搜索的过程中，根据与问题相关的启发式信息，朝着有利的方向展开搜索，可以避免许多无意义的搜索路径，大大减少搜索范围、降低问题的复杂度，常见的有贪婪算法和A*算法。

但是采用现有的路径规划算法获得路径不一定是最优路径，在机器人行进的路径中常常会遇到许多转折点，导致规划的路径可能更长而且路径不平滑。

发明内容

本发明针对现有技术中存在的在机器人行进的路径中常常会遇到许多转折点，导致规划的路径可能更长而且路径不平滑的技术问题，提供移动机器人路径规划方法和系统。

为实现上述技术目的，本发明采用以下技术方案。

第一方面，本发明提供了移动机器人路径规划方法，包括：

构建移动机器人工作环境的栅格地图；

利用路径规划算法在栅格地图上规划移动机器人路径，获得全局最优路径；

确定全局最优路径上的转折点；利用转折点评价函数，计算各转折点的评价值，根据评价值选取关键转折点；将选取出来的关键转折点进行路径平滑处理，获得优化后的全局最优路径。

进一步地，所述转折点评价函数表示为：

G(x)＝(1-α)d1+αd2,

其中，d1为障碍物中心到起始点与转折点连线的垂直距离,d2为转折点到障碍物所在线段末端的距离,α为可调参数。

进一步地，路径规划算法采用改进的A*算法，所述改进的A*算法全局评价函数中引入障碍物因数，在路障存在的时间段内，障碍物因数为一个无穷大的值，当在路障在路径以外时，障碍物因数为零；所述全局评价函数表示如下：

其中F(x)为全局评价函数，用于评估当前节点到目标点的预估代价；g(x)为真实代价函数；h(x)为启发函数，w(t)为引入障碍物因数。

再进一步地，所述改进的A*算法针对以下两种情况分别确定关键点，从关键点中搜寻全局评价函数值最小的节点；

情况(1)：起始点和目标点在一条直线上且路径中不存在障碍物；

情况(2)：起始点和目标点不共线且路径中存在障碍物；

针对情况(1)，将起始点和目标点连线上的节点都作为关键点；

针对情况(2)，从起始点出发，先按水平方向搜索关键点，当水平方向有障碍物或者搜索不到关键点时，再搜索对角线上的节点，同时还需保证机器人行径路径中不会与障碍物发生碰撞，即只需要满足条件表达式|P1X|+|XP2|>|P1P|+|PP2|，即P为关键点；

其中|P1X|表示起始点P1到节点X之间的距离绝对值，|XP2|表示节点X到目标点P1之间的距离绝对值，|P1P|表示起始点P1到节点P点之间的距离绝对值，|PP2|表示节点P点到目标点P2之间的距离绝对值，节点X为除节点P以外的任意一点。

再进一步地，针对情况(1)删除起始点和目标点连线上的节点中的冗余节点，保留连线上中距离目标点最近的节点作为关键点。

再进一步地，利用改进后的全局最优路径规划A*算法在栅格地图上规划全局最优路径，具体为：

步骤1：设定open列表和close列表，分别存储没有访问过的节点信息和已经访问过的节点信息；将起始点添加到open列表；

步骤2：重复以下步骤：

a)寻找open列表中全局评价函数值最小的节点，作为当前节点；

b)将当前节点从open列表中移除，并添加进close列表；

c)检查当前节点是否是目标点，如果是，停止搜索，跳转到步骤3；如果不是，搜寻关键点，如果关键点已在close列表中，忽略它，反之如下：

如果关键点不在open列表中，将其添加进去，把当前节点作为它的父节点，同时更新关键点的全局评价函数值；

如果关键点已经在open列表中，通过判断沿当前节点到它的路径的全局评价函数值是否更小，若更小，则将当前节点作为它的父节点，同时更新它的全局评价函数值否则，不做任何操作；

d)当目标点已添加到close列表时，全局最优路径已被找到；或者没有找到目标点，并且open列表已为空，则全局最优路径不存在；以上两种情况结束循环；

步骤3：从目标点开始，沿着每一个节点的父节点移动至起始点，形成的路径即为所求全局最优路径。

进一步地，所述构建移动机器人工作环境的栅格地图，包括：依次计算移动机器人工作环境中各个障碍物的面积，获得障碍物的总面积；

根据障碍物的总面积及设定栅格的边长确定栅格的密度，表达式如下：

其中L为栅格的密度，S_total为栅格的总面积S_tota＝∑i∈PS，S为障碍物的总面积，p为障碍物集合，i为其中某一障碍物，d为对应栅格的边长。

进一步地，确定各个障碍物的面积，包括：判断该障碍物的外形，若该障碍物为凸形则将该障碍物换分为多个互不相交的三角形，根据三角形计算公式计算障碍物面积；若障碍物为其他形状则计算该障碍物的最大顶点和最小顶点，并将最大顶点和最小顶点作为矩形对角线，通过矩形面积计算出障碍物的面积。

第二方面，本发明还提供了移动机器人路径规划系统，包括：

栅格地图构建模块，用于构建移动机器人工作环境的栅格地图；

全局最优路径规划模块，用于利用路径规划算法在栅格地图上规划移动机器人路径，获得全局最优路径；

转折点确定模块，用于确定全局最优路径上的转折点；

关键转折点选取模块，用于利用评价函数，计算各转折点的评价值，根据评价值选取关键转折点；

路径优化模块，用于将选取出来的关键转折点进行路径平滑处理，获得优化后的全局最优路径。

本发明所取得的有益技术效果：

本发明通过选取关键转折点，并将选取的关键转折点做路径平滑处理可以解决机器人行进过程中转折点过多问题；可以缩短机器人的行径路径和行驶时间；

在采用改进的A*算法做路径规划时，对全局评价函数做个改进并加入了关键点的选取，确保机器人行进过程中不会触碰到障碍物，同时计算减小了计算量，缓解了内存消耗严重的问题。

本发明在路径规划过程中确定关键点时删除冗余节点，进一步减少了计算量，提高路径规划的效率。

附图说明

图1为实施例中机器人移动环境示意图；

图2为实施例中机器人行进路径图；

图3为实施例中A*算法关键点图；

图4为实施例中关键点选取图一；

图5为实施例中关键点选取图二；

图6为实施例中有障碍物情况图一；

图7为实施例中有障碍物情况图二；

图8为实施例中转折点计算图；

图9为实施例中路径转折点处理图；

图10为传统A*算法路径图；

图11为采用本发明实施例提供的移动机器人路径规划方法获得的路径图。

具体实施方式

下面结合说明书附图和具体实施例对本发明作详细描述。

实施例1：

移动机器人路径规划方法，其特征在于，包括：

构建移动机器人工作环境的栅格地图；

利用路径规划算法在栅格地图上规划移动机器人路径，获得全局最优路径；

确定全局最优路径上的转折点；利用转折点评价函数，计算各转折点的评价值，根据评价值选取关键转折点；将选取出来的关键转折点进行路径平滑处理，获得优化后的全局最优路径。

在机器人行径过程中常常会遇到许多转折点，转折点的确定方法采用现有技术实现，如基于获取的初始全局最优路径中所有节点，从起点开始，利用面积法依次判断相邻三个节点是否共线，从而找到路径上的所有转折点，即拐点。

路径规划算法采用传统A*算法，确定的全局最优路径中的转折点如图10所示，可将路径中的转折点记录在一个集合P中，从起始点开始依次连接各个转折点。转折点过多，路径不平滑，且极易与障碍物发生接触。

本实施例中定义转折点评价函数G(x),将其作为最优转折点(即关键转折点)的选择依据,转折点评价函数G(x)＝(1-α)d1+αd2,如图8所示，其中:d1为障碍物中心到起始点与转折点连线的垂直距离,d2为转折点到障碍物所在线段末端的距离,其中系数α为可调参数。

通过计算各个节点的评价值，选取其中评价值最小的作为关键转折点。

将选取出来的转折点进行路径平滑处理如图9所示，设MN点的坐标为(X1,Y1)，(X2,Y2)，OP点的坐标为(X3,Y3)，(X4,Y4)，A点的坐标为(a,b)，其中BM与BN分别垂直于OA和OM，两条垂直线相交于B点，原A*算法需要经过的长度为L＝L1+L2，

经过算法改进后由原来的直线改为平滑曲线后L的距离发生变化，假设B点的坐标为(X,Y)，通过联立方程组可求解出圆的半径R。如图9可知α角为π-θ，通过公式计算可得出半径

及

最后通过计算弧长MN及

通过比较LMN的与NA和MA距离的和，可以发现路径长度有明显的减小。

在机器人移动路线规划优化的过程中，进行移动机器人工作环境的地图建立，这是路径优化的第一步，地图环境的建立指的是通过多个环境传感器获得机器人移动的环境信息，将多传感器检测到的信息融合在一起，形成最终的抽象地图模型，并将环境信息用栅格单位的形式来描述，以此来建立机器人移动的工作环境模型，如图1所示。

如图1所述可将机器人运行环境按照栅格进行划分，将其划分为可通过区域和不可通过区域，对每个单元格进行编码以此来表示单元格可通行的状态，黑色代表不可通行也就是存在障碍物，白色代表可通行，并将环境模型存储在机器人存储器中，沿X轴右方向为正，沿y轴上方向为正。

在构建栅格时其精度也会对机器人行径路径造成一定的影响，栅格精度越细其存储的环境量信息越多，越容易干扰机器人的行径，反之栅格精度越低，存储的环境量信息就越少，机器人越容易碰到障碍物，因此可以通过引入栅格精度进行更有效的路径规划。

在具体实施例中，可选地，构建栅格地图包括：

(1)获取环境中的某一障碍物信息。

(2)判断该障碍物的外形，若该障碍物为凸形可将该障碍物换分为多个互不相交的三角形，若该障碍物为其他形状则计算该障碍物的最大和最小顶点，并将最大和最小顶点作为矩形对角线可通过计算该矩形面积计算出障碍物的面积。

(3)根据面积计算公式计算出障碍物的面积。

(4)观察换分的栅格中是否还存在其他障碍物，若存在则返回至步骤(1)。

(5)计算栅格的密度

其中Stotal为栅格的总面积Stotal＝∑_i∈PS，S为障碍物的面积，p为障碍物集合，i为其中某一障碍物，d为对应栅格的边长。

实施例2：

在实施例1的基础上，本实施例中路径规划算法采用改进的A*算法，所述改进的A*算法全局评价函数中引入障碍物因数，在路障存在的时间段内，障碍物因数为一个无穷大的值，当在路障在路径以外时，障碍物因数为零；所述全局评价函数表示如下：

其中F(x)为全局评价函数，用于评估当前节点到目标点的预估代价；g(x)为真实代价函数；h(x)为启发函数，w(t)为引入障碍物因数。

A*算法是一种启发式的搜索算法。在算法实现过程中会对考虑节点的信息。对其中的每一个节点进行计算，再从中选取计算节点最小的为后续的搜索节点。传统的A^*算法在运行过程中，需要对机器人行径过程中的路径进行全方位的计算如图2所示，

区域为A^*算法需要计算的点。由图2可以看出，

区域为A*在寻路过程中需要计算的节点，但大部分节点都与生成的路径无关，造成机器人在巡检过程中存在计算量过大、内存消耗严重的问题。在规划好的路径中存在着许多冗余节点，例如点P1可以直接到点P2。并且传统A*算法规划好的路径转折点居多，不利于机器人正常巡检。

从图3可以看出P1点直接到P点的直线距离为最短距离，其只需要计算P1点到P点的距离，规划路径中由于存在过多的转折点也会对机器人巡线造成影响。本文可通过寻找关键点进行优化。

为解决A*算法内存消耗过大，本实施例在选择关键点时，尽量不选择靠近障碍物的点避免机器人在前进过程中发生碰撞；通过关键点的选取降低机器人内存的消耗。

如图3所示，P1为起点，P2为终点。按照传统的A*算法，应首先计算(1,2)，(2,1)，(2,2)这三个点的代价值，然后选择代价最小的点(2，2)点作为下一个父节点，并计算(2,2)周边的所有点的代价值，再选择代价最小的路径，即图3中带*号的方格代表代价最小的节点集。如图3可以看出P1到P2存在一条最短从路径如下图3所示。及只需要计算(2,2)，(3,3)，(4,4)，(5,5)这些点的代价,本文通过选取关键点的方法可以大大降低内存的消耗问题。

如图4、图5所示，可将关键点选取划分为以下两种情况(1)两点在一条直线上且路径中不存在障碍物；(2)两点不共线且路径中存在障碍物。

(1)P1P2两点位于同一条直线，或P1P2两点位于对角线上，其中P,q为除P1P2以外的任意一个点，如图4所示P1到P2可以由P1->p->q->P2，则p、q都可选取为关键点。

进一步可选地，由于机器人在行径路径并未发生方向的改变，且后一个节点与前一个节点运动方向一致，则认为节点p为冗余节点删除节点p只计算节点q，依次遍历机器人行径路径中的所有节点并删除冗余节点寻找出关键点。

(2)如下图6、图7所示，该情况表示两点不共线且行径路径中存在障碍物，当节点P1和节点P2的路径中存在障碍物，如图5所示，先按水平方向搜索关键点，当水平方向有障碍物或者搜索不到关键点，时再搜索对角线上的点，同时还需保证机器人行径路径中不会与障碍物发生碰撞，如下所示只需计算

区域的点即可。即只需要保证该关键点|P1X|+|XP2|>|P1P|+|PP2|即P为关键点，其中X点为除P以为的任意一点。

为了验证算法的准确性及可靠性，本文通过仿真实验的方法将采用本发明实施例2提供的路径规划方法与传统的A*算法做了一个比较，仿真在Matlab上进行仿真实验，本文通过采用栅格法在二维平面进行实验，以二维平面作为实际情况，通过坐标系构建现场环境情况，通过仿真得出机器人行径路线。通过从机器人行径路径长度、转折点个数、距离障碍物最近距离、时间4个方面进行对比,结果如表1所示。

表1传统A*与本发明实施例比较

如图10所示为传统A*算法机器人行径路线，可以看出机器人行径路线转折点过多，路径不平滑，且极易与障碍物发生接触，经过本文处理的关键点及平滑处理后，机器人行径路径发生了如下变化，如图11所示。

通过如上实验可以看出，传统A*算法规划车辆行程路径未考虑车辆和障碍物的距离，本发明的结果与其相比路径更短，转折点少路径更平滑，且发明中根据参数α的不同可在行径路径，障碍物距离与运行时间之间具有一定的关系，相较于传统A*算法有一定的提升。

与以上实施例提供的移动机器人路径规划方法相对应地，本发明实施例还提供了移动机器人路径规划系统，包括：

栅格地图构建模块，用于构建移动机器人工作环境的栅格地图；

全局最优路径规划模块，用于利用路径规划算法在栅格地图上规划移动机器人路径，获得全局最优路径；

转折点确定模块，用于确定全局最优路径上的转折点；

关键转折点选取模块，用于利用评价函数，计算各转折点的评价值，根据评价值选取关键转折点；

路径优化模块，用于将选取出来的关键转折点进行路径平滑处理，获得优化后的全局最优路径。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

以上结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。

Claims (9)

1.移动机器人路径规划方法，其特征在于，包括：

构建移动机器人工作环境的栅格地图；

利用路径规划算法在栅格地图上规划移动机器人路径，获得全局最优路径；

确定全局最优路径上的转折点；利用转折点评价函数，计算各转折点的评价值，根据评价值选取关键转折点；将选取出来的关键转折点进行路径平滑处理，获得优化后的全局最优路径。

2.根据权利要求1所述的移动机器人路径规划方法，其特征在于，所述转折点评价函数表示为：

G(x)＝(1-α)d1+αd2；

其中，d1为障碍物中心到起始点与转折点连线的垂直距离,d2为转折点到障碍物所在线段末端的距离,α为可调参数。

3.根据权利要求1所述的移动机器人路径规划方法，其特征在于，路径规划算法采用改进的A*算法，所述改进的A*算法全局评价函数中引入障碍物因数，在路障存在的时间段内，障碍物因数为一个无穷大的值，当在路障在路径以外时，障碍物因数为零；所述全局评价函数表示如下：

其中F(x)为全局评价函数，用于评估当前节点到目标点的预估代价；g(x)为真实代价函数；h(x)为启发函数，w(t)为引入障碍物因数。

4.根据权利要求3所述的移动机器人路径规划方法，其特征在于，所述改进的A*算法针对以下两种情况分别确定关键点，从关键点中搜寻当前全局评价函数值最小的节点；

情况(1)：起始点和目标点在一条直线上且路径中不存在障碍物；

情况(2)：起始点和目标点不共线且路径中存在障碍物；

针对情况(1)，将起始点和目标点连线上的节点都作为关键点；

针对情况(2)，从起始点出发，先按水平方向搜索关键点，当水平方向有障碍物或者搜索不到关键点时，再搜索对角线上的节点，同时还需保证机器人行径路径中不会与障碍物发生碰撞，即只需要满足条件表达式|P1X|+|XP2|>|P1P|+|PP2|，即P为关键点；

其中|P1X|表示起始点P1到节点X之间的距离绝对值，|XP2|表示节点X到目标点P1之间的距离绝对值，|P1P|表示起始点P1到节点P点之间的距离绝对值，|PP2|表示节点P点到目标点P2之间的距离绝对值，节点X为除节点P以外的任意一点。

5.根据权利要求4所述的移动机器人路径规划方法，其特征在于，针对情况(1)删除起始点和目标点连线上的节点中的冗余节点，保留连线上距离目标点最近的节点作为关键点。

6.根据权利要求4所述的移动机器人路径规划方法，其特征在于，利用改进后的全局最优路径规划A*算法在栅格地图上规划全局最优路径，具体为：

步骤1：设定open列表和close列表，分别存储没有访问过的节点信息和已经访问过的节点信息；将起始点添加到open列表；

步骤2：重复以下步骤：

a)寻找open列表中全局评价函数值最小的节点，作为当前节点；

b)将当前节点从open列表中移除，并添加进close列表；

c)检查当前节点是否是目标点，如果是，停止搜索，跳转到步骤3；如果不是，搜寻关键点，如果关键点已在close列表中，忽略它，反之如下：

如果关键点不在open列表中，将其添加进去，把当前节点作为它的父节点，同时更新关键点的全局评价函数值；

如果关键点已经在open列表中，通过判断沿当前节点到它的路径的全局评价函数值是否更小，若更小，则将当前节点作为它的父节点，同时更新它的全局评价函数值；否则，不做任何操作；

d)当目标点已添加到close列表时，全局最优路径已被找到；或者

没有找到目标点，并且open列表已为空，则不存在可行路径；以上两种情况结束循环；

步骤3：从目标点开始，沿着每一个节点的父节点移动至起始点，形成的路径即为所求全局最优路径。

7.根据权利要求1所述的移动机器人路径规划方法，其特征在于，所述构建移动机器人工作环境的栅格地图，包括：

依次计算移动机器人工作环境中各个障碍物的面积，获得障碍物的总面积；

根据障碍物的总面积及设定栅格的边长确定栅格的密度，表达式如下：

其中L为栅格的密度，S_total为栅格的总面积S_tota＝∑_i∈PS，S为障碍物的总面积，p为障碍物集合，i为其中某一障碍物，d为对应栅格的边长。

8.根据权利要求7所述的移动机器人路径规划方法，其特征在于，确定各个障碍物的面积，包括：判断该障碍物的外形，若该障碍物为凸形则将该障碍物换分为多个互不相交的三角形，根据三角形计算公式计算障碍物面积；若障碍物为其他形状则计算该障碍物的最大顶点和最小顶点，并将最大顶点和最小顶点作为矩形对角线，通过矩形面积计算出障碍物的面积。

9.移动机器人路径规划系统，其特征在于，包括：

栅格地图构建模块，用于构建移动机器人工作环境的栅格地图；

全局最优路径规划模块，用于利用路径规划算法在栅格地图上规划移动机器人路径，获得全局最优路径；

转折点确定模块，用于确定全局最优路径上的转折点；

关键转折点选取模块，用于利用评价函数，计算各转折点的评价值，根据评价值选取关键转折点；

路径优化模块，用于将选取出来的关键转折点进行路径平滑处理，获得优化后的全局最优路径。

Images