一种基于A*算法和RRT*算法的移动机器人路径规划方法与流程

本申请属于机器人导航技术领域，具体涉及一种基于A*算法和RRT*算法的移动机器人路径规划方法。

背景技术：

移动机器人的路径规划是指机器人在获取到当前环境的地图之后，规划出一条无碰撞的从起点到终点的最优路径。移动机器人的路径规划在各行业的机器人作业中尤为重要。

随着各行业智能化的普及，机器人作业的应用越来越多，因此路径规划问题成为了一个热点探讨问题。常用的路径规划方法有基于采样的规划、图搜索和组合规划等。基于采样的规划中有基于RRT(快速扩展随机树)的路径规划。RRT算法是一种采用增量方式增长的随机采样算法，用于解决有代数约束(障碍带来的)和微分约束(非完整性和动态环境带来的)高维空间问题。

RRT算法的优势在于无需对系统进行建模，无需对搜索区域进行几何划分，在搜索空间的覆盖率高，搜索的范围广，可以尽可能的探索未知区域。但是RRT算法存在计算代价过高，随机性太强等问题，这导致基于RRT的路径规划无法每次均得到最优路径，在路径规划中存在的随机性过强，造成移动机器人作业效率不高、可靠性低，并且路径规划过程过慢，实时性不高。

技术实现要素：

本申请的目的在于提供一种基于A*算法和RRT*算法的移动机器人路径规划方法，有效加速路径规划，并且规划最优路径的可靠性高。

为实现上述目的，本申请所采取的技术方案为：

一种基于A*算法和RRT*算法的移动机器人路径规划方法，所述基于A*算法和RRT*算法的移动机器人路径规划方法，包括：

步骤S1、取移动机器人本次运动的起点坐标和终点坐标，将连接起点坐标和终点坐标的线段离散成N个点；

步骤S2、根据离散得到的N个点进行路径规划，包括：

若N个点均不在障碍物所在范围内，则采用RRT*算法进行全段路径规划；

或者，若N个点中，由起点坐标起的前floor(N/2)个点中存在1个或多个点位于障碍物所在范围内，后N-floor(N/2)个点均不在障碍物所在范围内，则先采用A*算法，以起点坐标作为A*算法的规划起始坐标，以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为A*算法的规划目标坐标进行前段路径规划，后采用RRT*算法，以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为RRT*算法的规划起始坐标，以终点坐标作为RRT*算法的规划目标坐标进行后段路径规划；其中floor()为向下取整函数；

或者，若N个点中，由起点坐标起的前floor(N/2)个点均不在障碍物所在范围内，后N-floor(N/2)个点中存在1个或多个点位于障碍物所在范围内，则先采用RRT*算法，以起点坐标作为RRT*算法的规划起始坐标，以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为RRT*算法的规划目标坐标进行前段路径规划，后采用A*算法，以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为A*算法的规划起始坐标，以终点坐标作为A*算法的规划目标坐标进行后段路径规划；

或者，若N个点中，由起点坐标起的前floor(N/2)个点中存在1个或多点在障碍物所在范围内，后N-floor(N/2)个点中也存在1个或多个点在障碍物所在范围内，则采用A*算法进行全段路径规划；

步骤S3、根据路径规划后得到的路径中的控制节点控制移动机器人由起点坐标运动至终点坐标。

以下还提供了若干可选方式，但并不作为对上述总体方案的额外限定，仅仅是进一步的增补或优选，在没有技术或逻辑矛盾的前提下，各可选方式可单独针对上述总体方案进行组合，还可以是多个可选方式之间进行组合。

作为优选，所述步骤S3，根据路径规划后得到的路径中的控制节点控制移动机器人由起点坐标运动至终点坐标，包括：

步骤S3.1、取全段路径规划得到的全段路径中的控制节点；

或者，整合前段路径规划得到的前段路径和后段路径规划得到的后段路径生成全段路径，取全段路径中的控制节点；

步骤S3.2、将所有控制节点按照由起点坐标至终点坐标的方向，两两以父节点至子节点的顺序保存在一个数组中；

步骤S3.3、取数组中的第一个控制节点作为第一节点，取数组中的最后一个控制节点作为第二节点；

步骤S3.4、判断第一节点和第二节点的连线是否穿过障碍物所在范围，若连线未穿过障碍物所在范围，则仅保留第一节点和第二节点，丢弃第一节点和第二节点之间的其余控制节点，并执行步骤S3.5；若连线穿过障碍物所在范围，则找到第二节点的父节点作为新的第二节点，并重新执行步骤S3.4；

步骤S3.5、判断当前找到的第二节点是否是数组中的最后一个控制节点，若是则执行步骤S3.6；否则以当前找到的第二节点作为新的第一节点，以数组中的最后一个控制节点作为新的第二节点重新执行步骤S3.4；

步骤S3.6、取最终保留的控制节点，作为移动机器人运动中的局部目标点，控制移动机器人由起点坐标运动至终点坐标。

作为优选，所述A*算法，包括：

1)将规划起始坐标作为起始节点a，将规划目标坐标作为终节点b，从起始节点a开始，把起始节点a作为待处理的方格，存入一个open表中，open表存放的是待检查的节点；

2)寻找起始节点a相邻并可以直接到达的节点，将寻找到的节点都放入open表中，并将寻找到的节点的父节点设置为起始节点a；

3)从open表中删除起始节点a，并将起始节点a加入close表中，close表中存放的是不需要再次检查的节点；

4)从open表中找出F值最小的节点c，其中F值的计算如下：

F＝G+H

其中G表示从起始节点a移动到当前计算节点的移动代价，H表示从当前计算节点到终节点的估值代价；

5)把节点c从open表中删除，放入close表中；

6)检查节点c所有相邻并且可以直接到达的节点，若所述节点还不在open表中，则将所述节点加入open表，并将所述节点的父节点设置为节点c；

7)如果节点c所有相邻并且可以直接到达的节点中的部分节点已经在open表中，则计算已经在open表中的节点新的路径的G值，若G值更低则将当前计算节点的父节点设置为节点c，并重新计算当前计算节点的F值；否则不作处理；

8)重复步骤4)～7)直至将终节点加入open表，说明路径已经找到，路径规划完成；或者重复步骤4)～7)直至查找终节点失败，并且open表为空，表明不存在路径，路径规划完成。

作为优选，所述RRT*算法，包括：

1)在规划起始坐标和规划目标坐标之间基于RRT算法找到一条可行的初始路径；

2)以规划起始坐标和规划目标坐标作为椭圆的两个焦点，以所述初始路径的总长度为椭圆上的点到两个焦点的距离之和，构建一个椭圆；

3)在所述椭圆中进行重采样，并对重采样得到的节点重新选择父节点；

4)为随机树进行重新布线。

作为优选，所述在椭圆中进行重采样，包括：

3.1)连接椭圆的两个顶点得到线段l1，将线段l1进行M等分，记M-1个等分点为m0,m1,…mM-3,mM-2；

3.2)分别过各等分点做线段l1的垂线，记垂线交于椭圆的上半部分的交点为并保存在数组v1中，记垂线交于椭圆的下半部分的交点为并保存在数组v2中；

3.3)判断数组v1和v2中的交点在障碍物所在范围内的情况，选取数组v1和v2中交点在障碍物所在范围内较少的一组所对应的一半椭圆作为重采样区域，在所述重采样区域中进行重采样。

本申请提供的基于A*算法和RRT*算法的移动机器人路径规划方法，根据障碍物所在范围，采用A*算法和RRT*算法RRT*算法融合的方式，极大的降低了RRT*算法计算代价过高，且随机性过强的问题，有效加速路径规划，并且规划最优路径的可靠性高。

附图说明

图1为本申请的基于A*算法和RRT*算法的移动机器人路径规划方法的流程图；

图2为本申请的根据离散得到的N个点进行路径规划的流程图；

图3为本申请的RRT*算法中在椭圆中进行重采样过程的示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是在于限制本申请。

其中一个实施例中，提供一种基于A*算法(A星算法)和RRT*算法(RRT星算法)的移动机器人路径规划方法，将A*算法在遇到障碍物时能够快速绕开的特性和RRT*算法在自由空间中能够快速搜索的特性相结合，通过判断起点和终点之间的障碍物情况选择合适的路径规划算法，从而有效加速路径规划，提供规划最优路径的可靠性。

如图1所示，本实施例中基于A*算法和RRT*算法的移动机器人路径规划方法，包括以下步骤：

步骤S1、取移动机器人本次运动的起点坐标和终点坐标，将连接起点坐标和终点坐标的线段离散成N个点。

在路径规划之前通过前期的定位和建图获得当前环境的地图，对周边的障碍物进行膨胀后得到正确的地图。在地图上指定移动机器人本次运动的终点坐标，同时在机器人启动底盘驱动和开启导航功能时，获取本次运动的起点坐标。

容易理解的是，本申请重点在于路径规划，对于路径规划前期的环境地图建模、障碍物膨胀等操作不进行限制，基于现有技术实现即可。

在离散线段时，可以设置固定的离散点的个数，也可以根据线段的长度决定具体的离散点的个数，例如移动机器人底盘直径d＝0.4米，起点坐标和终点坐标之间的距离为Dis(start,goal)，则离散点的个数可以是其中Dis()为距离函数，ceil()为向上取整函数。根据线段长度确定离散点的个数的方法可得到适宜的离散点，有利于后期路径规划时确定最佳的规划算法。

需要说明的是，判断点是否在障碍物所在范围内为避障领域较为常规的技术，本实施例基于现有的判断方法实现，例如将机器人所在的代价地图中的坐标转换成栅格地图中的坐标，栅格地图中的每个坐标保存了该栅格的占据情况，只要判断离散点在栅格地图中占据情况就能判断出该离散点是否在障碍物中。当然也可以采用其他现有方式进行判断，这里就不进行限制。

步骤S2、根据离散得到的N个点进行路径规划，如图2所示，包括：

1)若N个点均不在障碍物所在范围内，则采用RRT*算法进行全段路径规划。规划时使用机器人的最大安全距离作为RRT*算法的生长步长，取较大的贪心程度p＝0.8，这样既满足了RRT算法的快速搜索效率，又保留了探索未知区域的可能性。

2)或者，若N个点中，由起点坐标起的前floor(N/2)个点中存在1个或多个点位于障碍物所在范围内，后N-floor(N/2)个点均不在障碍物所在范围内。

该情况下认定机器人的起始位置距障碍物较近，则先采用A*算法，以起点坐标作为A*算法的规划起始坐标，以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为A*算法的规划目标坐标进行前段路径规划，以“逃离”障碍物；后采用RRT*算法，以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为RRT*算法的规划起始坐标，以终点坐标作为RRT*算法的规划目标坐标进行后段路径规划，以快速规划出后半段路径；其中floor()为向下取整函数。

3)或者，若N个点中，由起点坐标起的前floor(N/2)个点均不在障碍物所在范围内，后N-floor(N/2)个点中存在1个或多个点位于障碍物所在范围内。

该情况下认定机器人的起始位置距离障碍物较远，但是后半程中有障碍物阻挡，则先采用RRT*算法，以起点坐标作为RRT*算法的规划起始坐标，以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为RRT*算法的规划目标坐标进行前段路径规划，以快速完成前半段路径规划，快速靠近障碍物；后采用A*算法，以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为A*算法的规划起始坐标，以终点坐标作为A*算法的规划目标坐标进行后段路径规划，以绕开障碍物后向终点移动。

4)或者，若N个点中，由起点坐标起的前floor(N/2)个点中存在1个或多点在障碍物所在范围内，后N-floor(N/2)个点中也存在1个或多个点在障碍物所在范围内。

该情况下认定此次路线中障碍物较多，使用RRT*算法会消耗更多的时间在新生成节点的障碍物判断中，因此该情况下宜采用A*算法进行全段路径规划，在保证躲避障碍物的前提下，规划出一条最优路径。

本实施例相比于传统的只采用一种路径规划算法，结合了A*算法中能找到最优路径、避障能力强等优点和RRT*算法中搜索效率高、覆盖空间广等优点，即满足了快速搜索路径的要求，又保留了很强的避障能力。

步骤S3、根据路径规划后得到的路径中的控制节点控制移动机器人由起点坐标运动至终点坐标。

在找到一条从起点坐标到终点坐标的路径之后，不管是A*算法还是RRT*算法，都包含很多的控制节点。在机器人的实际控制中，是将这些控制节点当作一个个局部目标点，计算出机器人的运行速度和角度，发送给底盘。

若将路径规划得到的所有控制节点均作为一个个局部目标点，那将导致移动机器人的运动控制十分没有效率，而且机器人很容易出现卡顿的情况。

因此本申请在一实施例中采用节点回溯的方式，剔除冗余的点，只保留最终路径中的个别控制节点，作为机器人的局部目标点，具体过程如下：

步骤S3.1、取全段路径规划得到的全段路径中的控制节点；

或者，整合前段路径规划得到的前段路径和后段路径规划得到的后段路径生成全段路径，取全段路径中的控制节点。

步骤S3.2、将所有控制节点按照由起点坐标至终点坐标的方向，两两以父节点至子节点的顺序保存在一个数组中。

步骤S3.3、取数组中的第一个控制节点作为第一节点，取数组中的最后一个控制节点作为第二节点。

步骤S3.4、判断第一节点和第二节点的连线是否穿过障碍物所在范围，若连线未穿过障碍物所在范围，则仅保留第一节点和第二节点，丢弃第一节点和第二节点之间的其余控制节点，并执行步骤S3.5；若连线穿过障碍物所在范围，则找到第二节点的父节点作为新的第二节点，并重新执行步骤S3.4。

步骤S3.5、判断当前找到的第二节点是否是数组中的最后一个控制节点，若是则执行步骤S3.6；否则以当前找到的第二节点作为新的第一节点，以数组中的最后一个控制节点作为新的第二节点重新执行步骤S3.4。

步骤S3.6、取最终保留的控制节点，作为移动机器人运动中的局部目标点，控制移动机器人由起点坐标运动至终点坐标。

该方法大大的删减了冗余的控制节点，简化整个运动路径为一个或多个线段，便于运动控制的实现，同时也提高了机器人移动的流畅度，降低机器人的运动损耗。

在本申请的整个控制过程中，结合了A*算法和RRT*算法，取两者算法之所长，以实现机器人高效、可靠的路径规划。并且本申请并非简单组合两种算法，而是结合实际障碍点的分布情况，在整个路径规划中融合两种算法，既凸显了每一算法的优势，又可根据实时分析的障碍物位置分布，提升两种算法的融合度，避免算法切换规划节点处出现路径突变或断层的现象。其中A*算法和RRT*算法可采用现有的算法实现。

其中一个实施例中，采用的A*算法包括以下步骤：

1)将规划起始坐标作为起始节点a，将规划目标坐标作为终节点b，从起始节点a开始，把起始节点a作为待处理的方格，存入一个open表中，open表存放的是待检查的节点。

2)寻找起始节点a相邻并可以直接到达的节点，将寻找到的节点都放入open表中，并将寻找到的节点的父节点设置为起始节点a。

3)从open表中删除起始节点a，并将起始节点a加入close表中，close表中存放的是不需要再次检查的节点。

4)从open表中找出F值最小的节点c，其中F值的计算如下：

F＝G+H

其中G表示从起始节点a移动到当前计算节点的移动代价，H表示从当前计算节点到终节点的估值代价。

5)把节点c从open表中删除，放入close表中。

6)检查节点c所有相邻并且可以直接到达的节点，若所述节点还不在open表中，则将所述节点加入open表，并将所述节点的父节点设置为节点c。

7)如果节点c所有相邻并且可以直接到达的节点中的部分节点已经在open表中，则计算已经在open表中的节点新的路径的G值，若G值更低则将当前计算节点的父节点设置为节点c，并重新计算当前计算节点的F值；否则不作处理。

8)重复步骤4)～7)直至将终节点加入open表，说明路径已经找到，路径规划完成；或者重复步骤4)～7)直至查找终节点失败，并且open表为空，表明不存在路径，路径规划完成。

本实施例中的A*算法在路径规划过程中能够快速绕开障碍物，加速路径规划速度。关于本实施例A*算法中为具体展开说明的部分参考现有技术实现即可，例如F值、G值、H值的计算过程等。

其中，RRT*算法为基本的RRT算法的一种改进，对于现有的RRT算法而言，其执行步骤如下：

1)在状态空间中随机选择一个采样点Xrand。

2)在随机树中找到距采样点Xrand最近的节点Xnearest。

3)从节点Xnearest开始，向着采样点Xrand的方向生长一个步长stepsize的距离，得到一个新的节点Xnew。如果节点Xnew与障碍物发生碰撞，则放弃这次生长，否则将节点Xnew加入随机树中。

4)重复上述步骤直到节点Xnearest与终点Xgoal距离小于一个阈值，代表随机树到达了终点，算法结束。

由于RRT算法采样的随机性，导致最终生成的路径往往只是可行路径而不是最优路径，因此本实施例不直接采用RRT算法，而是采用规划性能更优的RRT*算法来得到更好的规划路径，其中RRT*算法主要包括：

1)在规划起始坐标和规划目标坐标之间基于RRT算法找到一条可行的初始路径。

2)以规划起始坐标和规划目标坐标作为椭圆的两个焦点，以所述初始路径的总长度为椭圆上的点到两个焦点的距离之和，构建一个椭圆。

3)在所述椭圆中进行重采样，并对重采样得到的节点(即节点Xnew)重新选择父节点。

4)为随机树进行重新布线。

RRT*算法主要基于RRT算法实现，在RRT算法的基础上增加了重新为节点Xnew选择父节点的过程和重布线随机数的过程，因此弱化了RRT算法的随机性，提高路径规划可靠性。

RRT*算法也是路径规划领域较为成熟的技术，本申请可基于现有的RRT*算法实现，例如基于S.Karaman和E.Frazzoli于2011年提出的RRT*算法。该RRT*算法的过程如下：

1)在规划起始坐标和规划目标坐标之间基于RRT算法找到一条可行的初始路径。

2)以规划起始坐标和规划目标坐标作为椭圆的两个焦点，以所述初始路径的总长度为椭圆上的点到两个焦点的距离之和，构建一个椭圆。

3)在所述椭圆中进行重采样，产生一个新的采用点Xrand

4)在随机树中找到距采样点Xrand最近的节点Xnearest。

5)连接Xrand与Xnearest。

6)以Xrand为中心，ri＝1.5倍的stepsize为半径，在随机树上搜索节点，找出潜在的父节点集合Xpotential_parent，目的是更新Xrand，找到是否有比其更好的父节点

7)从某一个潜在的父节点Xpotential_parent开始考虑，计算出Xparent作为父节点的代价。

8)先不进行碰撞检测，而是将Xpotential_parent与Xrand连接起来，计算出连线路径的代价。

9)将新的这条连线路径的代价与原路径的代价进行比较，如果新的这连线条路径的代价更小则进行碰撞检测，如果新的这条连线路径代价更大则换为下一个潜在的父节点。

10)如果碰撞检测失败，Xpotential_parent与Xrand连线之间有障碍物，则该潜在父节点不作为新的父节点。

11)如果碰撞检测成功，在随机树中将之前的边删掉，将新的边添加进去，将Xpotential_parent作为Xparent。

12)遍历所有的潜在父节点，得到更新后的随机树。

RRT*算法是渐进最优的，只要给足够的运行时间，一定可以收敛到最优解。但是得到最优解所花时间较长，不满足机器人路径规划的实时性。因此本实施例为了进一步提升RRT算法的路径规划速度，加速了RRT*算法种的重采样的过程，在一实施例中，在所述椭圆中进行重采样的过程如下：

3.1)连接椭圆的两个顶点得到线段l1，将线段l1进行M等分，记M-1个等分点为m0,m1,…mM-3,mM-2。

3.2)分别过各等分点做线段l1的垂线，记垂线交于椭圆的上半部分的交点为并保存在数组v1中，记垂线交于椭圆的下半部分的交点为并保存在数组v2中；

3.3)判断数组v1和v2中的交点在障碍物所在范围内的情况，选取数组v1和v2中交点在障碍物所在范围内较少的一组所对应的一半椭圆作为重采样区域，在所述重采样区域中进行重采样。

本实施例通过判断初始规划的路径所在的椭圆范围内的障碍物情况，缩小了重采样的区域，直接将重采样区域从整个椭圆减少为一半椭圆，并且能够保持重采样的效果，加快收敛到最优路径的速度，即在不影响得到路径规划最优解的前提下，极大程度上的提升了路径规划效率。本申请的路径规划方法对环境适应性强，难度低，易于实施。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。