一种改进型动态RRT*的移动机器人运动规划方法与流程

本发明涉及移动机器人运动规划领域，具体涉及一种改进型动态rrt*的移动机器人运动规划方法。

背景技术：

随着人类生活水平的不断提升和工业自动化、智能化程度的迅猛发展，机器人的用途越来越广泛，逐渐成为人类生产生活中必不可少的一部分。不论是工业机器人、服务机器人还是医疗机器人都需要具备能够在复杂多变的工作环境中进行无碰撞运动规划的能力。

机器人的运动规划经过长久的发展，形成了基于人工势场的方法、基于数学模型的方法、基于图搜索的方法、基于节点的方法和基于采样的方法等众多不同的规划方法。为增加机械臂的灵活性和可操纵性，人们发明了冗余机械臂甚至超冗余机械臂，它们的自由度都大于6，由于机械臂运动规划的复杂程度随着机械臂自由度的增加而显著增大，因此冗余机械臂的运动规划通常需要考虑“维度灾难”的问题。基于采样的方法因在多维空间中具有显著优势而在机械臂运动规划领域中得到了广泛的关注。

stevenm.lavalle在1998年提出了rrt算法，该算法倾向于向未探索区域扩张，随着迭代次数的增加，未探索区域会显著减少，因此rrt探索空间的能力强，速度快。rrt算法的缺陷在于其概率完备且不最优，它能很快找到可行路径但该路径通常不是最优路径且包含较多的棱角，也较容易被困于狭窄通道环境中。

sertackaraman和emiliofrazzoli在2010年提出了rrt*算法，针对rrt得到的路径为非最优解的问题，rrt*算法在rrt算法基础上增加了重新布线函数(rewirefunction)改进了父节点的选择方式，随着迭代次数的增加，由rrt*搜索到的路径逐渐趋近于最优路径，因此rrt*是渐进最优的。rrt*算法的缺陷在于增加的重新布线函数使得算法的扩张速度及收敛速度变慢，且同rrt算法一样较容易被困于狭窄通道环境中。

r.bohlin和l.e.kavraki在2000年时第一次提出lazy思想，利用延迟碰撞检测的方法减少大量不必要的碰撞检测从而提高路径规划效率，获得较快的规划速度。lazy-prm算法的缺陷在于其概率完备且不最优，它无法找到最优路径且随着采样点数量的增多所需时间显著增加。

krishauser在2015年提出了lazy-prm*算法，lazy-prm*算法仅在lazy-prm算法上做了微小的改动，将对点的碰撞检测提前到采样时进行，但保持了边的延迟碰撞检测。lazy-prm*算法的缺陷在于其概率完备且不最优，采样点数量较少时，可以在较短的时间内获得较好的路径，但随着采样点数量的增多所需时间显著增加但路径质量并没有明显提高。

luigipalmieri和svenkoenig等人在2016年提出了theta*-rrt算法，该算法是rrt的变体，将离散的任意角度搜索与连续的非完整轮式机器人的运动规划结合起来，在不损失平滑度的情况下相比rrt的速度更快、路径质量更高，能够迅速的生成第一个轨迹。theta*-rrt算法的缺陷在于同rrt一样是具有概率完备性但无法找到最优路径的算法。

adnantahirovic和minaferizbegovic在2018年提出了rrv(rapidly-exploringrandomvines)算法，该算法能解决经典rrt算法在狭窄通道环境中效率低的问题。通过对节点周围的空间进行适当的采样，能够有效地扩张障碍物周围和狭窄通道中的树。rrv算法的缺陷在于其只在处理狭窄通道环境中表现良好，在没有狭窄通道的环境中，rrv算法与rrt算法的运行情况相似，甚至在具有许多障碍物但没有狭窄通道的环境中表现得比rrt稍差。

技术实现要素：

本发明的目的在于提供一种改进型动态rrt*的移动机器人运动规划方法，该方法克服现有运动规划算法的不足，算法收敛速度快于rrt*，路径质量优于rrt，即使在狭窄通道或迷宫等复杂环境中依然具有优势。

为实现上述目的，本发明的技术方案是：一种改进型动态rrt*的移动机器人运动规划方法，包括如下步骤：

步骤s1、获取移动机器人当前周围环境信息：通过激光雷达或深度相机获取移动机器人半径范围内的环境信息，未被扫描到的位置呈现未知状态，扫描到的无障碍物位置呈现自由状态，扫描到的有障碍物位置呈现占用状态；在机器人根据规划的路径移动的过程中，不断获取新位置周围的环境信息，并与之前的环境信息相结合得到机器人走过路径的周围所有环境信息；

步骤s2、算法初始化：设定所需规划的起始位置、目标位置和总迭代次数n，设置当前迭代次数i等于0，设置当前规划时间t等于0，将起始位置xinit加入节点集合v，将起始位置加入搜索树g，并设置父节点集合e为空集；

步骤s3、障碍物离散化：将环境中所有的已知障碍物进行离散化，即用有限数量的间隔均匀的点集合表示连续的障碍物，并假设点的数量为m，组成集合ξkn，即

步骤s4、随机均匀采样，在环境地图中的随机位置加入一个采样点xrand，并将当前迭代次数i加1；

步骤s5、找到搜索树g中离采样点xrand距离最近的节点xnearest；

步骤s6、在点xnearest朝向点xrand方向上加相应增量得点xnew；

步骤s7、找到以点xnew为中心，rnear为半径范围内搜索树g上所有节点，组成集合xnear，以函数near(g,xnew,|v|)表示；

步骤s8、计算点xnew附近节点集合xnear的碰撞风险评估函数，作为代价函数的分量，使算法具有避障的能力；

步骤s9、计算点xnew附近节点集合xnear的代价函数值，得到点xnew的最小代价函数值及其父节点；

步骤s10、修改点xnew附近节点集合xnear的父节点，使搜索树的各个节点得到最小代价函数值，从而实现算法的渐进最优；

步骤s11、算法终止；

步骤s12、移动机器人按照规划路径移动：将规划完成的可行路径变换成线速度和角速度发布给移动机器人的底盘，移动机器人将按照接收的速度进行移动和转向；在朝向目标位置移动的过程中，如果环境信息未发生变化，则移动机器人继续按照当前规划路径移动，当环境信息发生变化时，算法将进行新一次路径规划，即回到步骤s2的算法初始化；

步骤s13、路径规划结束：当移动机器人按照算法规划的路径到达设定的目标位置时，任务完成，路径规划结束。

在本发明一实施例中，所步骤s8具体实现方式为：将点xnew附近节点集合xnear中所有节点分别带入碰撞风险评估函数gauss(xnear,xnew)，得到点xnew与附近所有节点之间的代价函数的碰撞风险评估分量c”，用于判断点xnew及其与周围节点的连线是否会同障碍物发生碰撞，从而决定点xnew的父节点并判断点xnew是否能够作为附近节点的父节点。

在本发明一实施例中，所述将点xnew附近节点集合xnear中所有节点分别带入碰撞风险评估函数gauss(xnear,xnew)，得到点xnew与附近所有节点之间的代价函数的碰撞风险评估分量c”的具体计算过程如下：

假设p为以点xnear和xnew为两端点的线段line(xnear,xnew)表示上的点，由公式(1)可得点p至所有障碍物点obsi的欧式距离d：

d＝||p,obsi||2,i＝1,...m(1)

将d带入高斯分布公式，即公式(2)可得fi(p)：

公式(2)中σ为标准差描述数据分布的离散程度，是高斯分布的形状参数；如fi(p)趋近于1，可知点p位于障碍物中，如fi(p)趋近于0，可知点p位于自由空间中；将点p同m个障碍物点求得m个fi(p)带入公式(3)中得到f(p)：

在line(xnear,xnew)上均匀取n个与点p相同的点，并作相同运算后带入公式(4)中得line(xnear,xnew)的代价函数分量gauss(xnear,xnew)：

假如line(xnear,xnew)与障碍物间发生碰撞长度越长，gauss(xnear,xnew)的值越大，与分量系数kα相乘后加入原rrt*算法的代价函数公式中，将会得到足够大的代价函数值使得与障碍物发生碰撞的采样点和边停止生长搜索树；假如line(xnear,xnew)未与障碍物发生碰撞，gauss(xnear,xnew)的值趋近于0，求得的代价函数值与原rrt*算法得到的代价函数值相同。

在本发明一实施例中，所述σ为标准差描述数据分布的离散程度，是高斯分布的形状参数；σ越大，数据分布越分散，曲线越扁平，障碍物的影响范围越大；σ越小，数据分布越集中，曲线越瘦高，障碍物的影响范围仅局限在障碍物本身尺寸内；当σ增大，会增大障碍物对周边环境的影响；在需要快速搜寻到一条可行路径时可以通过减少ξkn的大小，即减小m同时增大σ，扩大障碍物的影响范围来降低算法的运算时间。

在本发明一实施例中，所述步骤s9的具体实现方式为：将点xnew附近节点集合xnear中各个节点的代价函数值cost(xnear)加上该节点到点xnew的欧式距离c(line(xnear,xnew))，再加上该节点到点xnew对应的碰撞风险评估分量kα*c”得到点xnew所有可能的代价函数值c'，并将计算所得的代价函数值中的最小值min(c′)设为点xnew的代价函数值cost(xnew)，将最小代价函数值对应的节点设置为点xnew的父节点xmin。

在本发明一实施例中，所述步骤s10的具体实现方式为：将节点集合xnear中除去点xnew的父节点xmin外所有节点对应的代价函数值同点xnew的代价函数值加上各个节点到点xnew的欧式距离，再加上各个节点到点xnew对应的碰撞风险评估分量进行一一对比：如前者数值大，则找出该节点的父节点xparent，并将其修改为点xnew，以缩小该节点的代价函数值；如后者数值大，则保持该节点的父节点不变。

在本发明一实施例中，所述步骤s11的具体实现方式为：当点xnew和目标点xgoal的距离小于目标偏差容忍度rfind且点xnew的代价函数值小于一个设定阈值时，该阈值大小受分量系数kα影响，算法规划出可行路径，算法终止，输出搜索树图及可行路径；或当前迭代次数等于总迭代次数时，即使未找到可行路径算法也将终止运算并回到步骤s2的算法初始化，重新进行路径规划。

相较于现有技术，本发明具有以下有益效果：

相比于rrt、rrt*、lazy-prm、lazy-prm*、theta*-rrt、rrv算法，本发明提出的算法有如下优势：

(1)本发明提出的算法是概率完备且渐进最优的算法；

(2)碰撞检测的计算代价相对较为昂贵，在多数运动规划算法中，大量的计算时间都花费在碰撞检测上。当环境越复杂、障碍物越多，碰撞检测的计算代价就越高，算法效率大幅降低。本发明提出的算法剔除了所有的碰撞检测，因此在运动规划问题上收敛速度较快；

(3)在有狭窄通道的环境中寻找路径是rrt和rrt*共同的弱点。因为狭窄通道面积小，被碰到的概率低，rrt和rrt*时常会被困在由障碍物包围的环境中，只能在被包围的环境中不断的生长搜索树，需要较长时间才能扩张到障碍物外部，效率较低。而本发明提出的算法在狭窄通道环境中不会因找不到出口而浪费大量时间在被障碍物包围的环境中盲目搜索，能够在寻找出口的同时先将搜索树扩张到整个地图环境中，避免了节点大量聚集在障碍物包围的环境中所产生浪费，提高了算法的效率。且本发明提出的算法不仅仅在狭窄通道环境中具有优势，在迷宫等其他复杂环境中算法执行速度也相对其他算法较快，路径质量较好。

附图说明

图1是本发明的流程图；

图2是本发明的算法原理说明图；

图3是本发明的不同σ时障碍物的影响范围图；

图4是本发明的算法避障示意图；

图5是本发明的静态场景一仿真结果图；

图6是本发明的静态场景二仿真结果图；

图7是本发明的静态场景三仿真结果图；

图8是本发明的静态真实实验图；

图9是本发明的静态真实实验里程计位置输出图；

图10是本发明的动态真实实验图；

图11是本发明的动态真实实验里程计位置输出图。

具体实施方式

为让本发明的上述特征和优点能更明显易懂，下文特举实施例，并配合附图，作详细说明如下，但本发明并不限于此。

参考图1至图11。

本发明提出了一种改进型动态rrt*的移动机器人运动规划方法，包括如下步骤：

步骤s1、获取移动机器人当前周围环境信息：通过激光雷达或深度相机获取移动机器人半径范围内的环境信息，未被扫描到的位置呈现未知状态，扫描到的无障碍物位置呈现自由状态，扫描到的有障碍物位置呈现占用状态；在机器人根据规划的路径移动的过程中，不断获取新位置周围的环境信息，并与之前的环境信息相结合得到机器人走过路径的周围所有环境信息；如图2a所示。

步骤s2、算法初始化：设定所需规划的起始位置、目标位置和总迭代次数n，设置当前迭代次数i等于0，设置当前规划时间t等于0，将起始位置xinit加入节点集合v，将起始位置加入搜索树g，并设置父节点集合e为空集；

步骤s3、障碍物离散化：将环境中所有的已知障碍物进行离散化，即用有限数量的间隔均匀的点集合表示连续的障碍物，并假设点的数量为m，组成集合ξkn，即如图2b所示。

步骤s4、随机均匀采样(以函数sample(i)表示)，在环境地图中的随机位置加入一个采样点xrand，并将当前迭代次数i加1；如图2c所示。

步骤s5、找到搜索树g中离采样点xrand距离最近的节点xnearest；如图2d所示。

步骤s6、在点xnearest朝向点xrand方向上加相应增量得点xnew(以函数steer(xnearest,x)表示)；如图2e所示。

步骤s7、找到以点xnew为中心，rnear为半径范围内搜索树g上所有节点，组成集合xnear，以函数near(g,xnew,|v|)表示；如图2f所示。

步骤s8、计算点xnew附近节点集合xnear的碰撞风险评估函数，作为代价函数的分量，使算法具有避障的能力；

步骤s9、计算点xnew附近节点集合xnear的代价函数值，得到点xnew的最小代价函数值及其父节点；

步骤s10、修改点xnew附近节点集合xnear的父节点，使搜索树的各个节点得到最小代价函数值，从而实现算法的渐进最优；

步骤s11、算法终止；

步骤s12、移动机器人按照规划路径移动：将规划完成的可行路径变换成线速度和角速度发布给移动机器人的底盘，移动机器人将按照接收的速度进行移动和转向；在朝向目标位置移动的过程中，如果环境信息未发生变化，则移动机器人继续按照当前规划路径移动，当环境信息发生变化时，算法将进行新一次路径规划，即回到步骤s2的算法初始化；

步骤s13、路径规划结束：当移动机器人按照算法规划的路径到达设定的目标位置时，任务完成，路径规划结束。

本发明的具体流程图如图1所示。

本实施例中，所步骤s8具体实现方式为：将点xnew附近节点集合xnear中所有节点分别带入碰撞风险评估函数gauss(xnear,xnew)，得到点xnew与附近所有节点之间的代价函数的碰撞风险评估分量c”，用于判断点xnew及其与周围节点的连线是否会同障碍物发生碰撞，从而决定点xnew的父节点并判断点xnew是否能够作为附近节点的父节点。如图2g所示。

本实施例中，所述将点xnew附近节点集合xnear中所有节点分别带入碰撞风险评估函数gauss(xnear,xnew)，得到点xnew与附近所有节点之间的代价函数的碰撞风险评估分量c”的具体计算过程如下：

假设p为以点xnear和xnew为两端点的线段line(xnear,xnew)表示上的点，由公式(1)可得点p至所有障碍物点obsi的欧式距离d：

d＝||p,obsi||2,i＝1,...m(1)

将d带入高斯分布公式，即公式(2)可得fi(p)：

公式(2)中σ为标准差描述数据分布的离散程度，是高斯分布的形状参数；如fi(p)趋近于1，可知点p位于障碍物中，如fi(p)趋近于0，可知点p位于自由空间中；将点p同m个障碍物点求得m个fi(p)带入公式(3)中得到f(p)：

在line(xnear,xnew)上均匀取n个与点p相同的点，并作相同运算后带入公式(4)中得line(xnear,xnew)的代价函数分量gauss(xnear,xnew)：

假如line(xnear,xnew)与障碍物间发生碰撞长度越长，gauss(xnear,xnew)的值越大，与分量系数kα相乘后加入原rrt*算法的代价函数公式中，将会得到足够大的代价函数值使得与障碍物发生碰撞的采样点和边停止生长搜索树；假如line(xnear,xnew)未与障碍物发生碰撞，gauss(xnear,xnew)的值趋近于0，求得的代价函数值与原rrt*算法得到的代价函数值相同。

本实施例中，所述σ为标准差描述数据分布的离散程度，是高斯分布的形状参数；σ越大，数据分布越分散，曲线越扁平，障碍物的影响范围越大；σ越小，数据分布越集中，曲线越瘦高，障碍物的影响范围仅局限在障碍物本身尺寸内；如图3所示，在检测半径为64m的范围内设置四个完全相同的静态障碍物，障碍物尺寸均为40m*40m，各个障碍物坐标位置如图3a所示。图3b为σ＝0.95时障碍物的影响范围，图3c为σ＝5时障碍物的影响范围，可见当σ增大，会增大障碍物对周边环境的影响；在需要快速搜寻到一条可行路径时可以通过减少ξkn的大小，即减小m同时增大σ，扩大障碍物的影响范围来降低算法的运算时间。

本实施例中，所述步骤s9的具体实现方式为：将点xnew附近节点集合xnear中各个节点的代价函数值cost(xnear)加上该节点到点xnew的欧式距离c(line(xnear,xnew))，再加上该节点到点xnew对应的碰撞风险评估分量kα*c”得到点xnew所有可能的代价函数值c'，如图2h所示，并将计算所得的代价函数值中的最小值min(c′)设为点xnew的代价函数值cost(xnew)，将最小代价函数值对应的节点设置为点xnew的父节点xmin。如图2i所示。

本实施例中，所述步骤s10的具体实现方式为：将节点集合xnear中除去点xnew的父节点xmin外所有节点对应的代价函数值同点xnew的代价函数值加上各个节点到点xnew的欧式距离，再加上各个节点到点xnew对应的碰撞风险评估分量进行一一对比：如前者数值大，则找出该节点的父节点xparent，并将其修改为点xnew，以缩小该节点的代价函数值；如后者数值大，则保持该节点的父节点不变。如图2j所示。

本实施例中，所述步骤s11的具体实现方式为：当点xnew和目标点xgoal的距离小于目标偏差容忍度rfind且点xnew的代价函数值小于一个设定阈值时，该阈值大小受分量系数kα影响，算法规划出可行路径，算法终止，输出搜索树图及可行路径，如图2k所示；或当前迭代次数等于总迭代次数时，即使未找到可行路径算法也将终止运算并回到步骤s2的算法初始化，重新进行路径规划。如图2l所示。

本实例中的算法具有避障能力，具体的因代价函数中增加了碰撞风险评估分量，落在障碍物内部的采样点不再将欧式距离的大小作为选择父节点的主要判断因素，如图4所示，黑色点表示算法的采样点，蓝色线段表示算法的搜索树，黑色矩形表示障碍物，绿色线条表示算法探索到的可行路径。在障碍物中的长度越长，碰撞风险评估分量将越大，代价函数显著增大，每个节点都会选择代价函数值最小的连接方式，因此红色实线框内的树呈现对称状断开，不会彼此相连，使得从起始点到目标点生长的搜索树将不能够从障碍物上通过，从而实现了算法避免碰撞的功能。

本实施例中，采用的算法的完整伪代码，具体如下：

其中，algorithm1中的第2条，algorithm2中的第7-10、13为本发明采用的改进的rrt*算法相比于原来的rrt*算法的改进部分。

以下用具体的实验对本发明的实施方式进行详细说明，本发明提供一种概率完备且渐进最优的运动规划算法，主要通过仿真实验和真实实验来验证其有效性。具体实验设置如下：

仿真实验：

仿真实验在matlabr2017b软件中进行。

(1)静态场景一

仿真静态场景地图大小为x＝y＝500m，所有障碍物皆为已知静态障碍物。起始点为(50，450)，目标点为(30，30)，标准差σ＝0.95，分量系数kα＝500。如图5所示，为静态场景一的仿真地图，玫红色点表示起始点，绿色点表示目标点，绿色线段表示节点数为20000时本发明提出的算法规划的路径。

(2)静态场景二

仿真静态场景地图大小为x＝y＝500m，所有障碍物皆为已知静态障碍物。起始点为(175，150)，目标点为(330，350)，标准差σ＝0.95，分量系数kα＝500。如图6所示，为静态场景二的仿真地图，玫红色点表示起始点，绿色点表示目标点，绿色线段表示节点数为20000时本发明提出的算法规划的路径。

(3)静态场景三

仿真静态场景地图大小为x＝y＝520m，所有障碍物皆为已知静态障碍物。起始点为(490，490)，目标点为(25，25)，标准差σ＝0.95，分量系数kα＝1000。如图7所示，为静态场景三的仿真地图，玫红色点表示起始点，绿色点表示目标点，绿色线段表示节点数为20000时本发明提出的算法规划的路径。

真实实验：

本发明可以直接用于移动机器人的路径规划、机械臂的运动规划还有无人机的轨迹规划中，在ros操作系统中编写一个新的全局路径规划器并将其作为插件添加到ros操作系统中，使用时通过move_base包调用即可。

静态真实实验环境为ubuntu16.04、roskinetic，使用搭载kinect深度相机的turtlebot2及intelcorei7-7700，2.80ghz，内存为8gb的笔记本电脑。场景地图大小为x＝y＝12m，所有障碍物皆为已知静态障碍物。起始点为(1.5，0.5)，目标点为(10.5，9)，标准差σ＝0.95，分量系数kα＝2000。如图8所示，为静态真实实验过程，左侧图为实验过程实拍，右侧图为实验过程中本发明提出的算法规划路径时rviz可视化界面。

turtlebot2设定最大速度0.3m/s，从起始位置到达目标位置耗时79s，路径长度21.937m。如图9所示，绿色点表示turtlebot2的起始位置，红色点表示turtlebot2的目标位置，黑色矩形表示障碍物，蓝色点及线段为实验时turtlebot2里程计真实位置输出。

动态真实实验环境为ubuntu16.04、roskinetic，使用搭载rplidar激光雷达的turtlebot2及intelcorei7-7700，2.80ghz，内存为32gb的笔记本电脑。场景地图大小为x＝y＝12m，所有障碍物皆为未知静态障碍物。起始点为(1.5，1)，目标点为(10，8)，标准差σ＝0.95，分量系数kα＝2000。如图10所示，为动态真实实验过程，左侧图为实验过程实拍，右侧图为实验过程中本发明提出的算法规划路径时rviz可视化界面。

turtlebot2设定最大速度0.3m/s，从起始位置到达目标位置耗时148s，路径长度27.198m。如图11所示，绿色点表示turtlebot2的起始位置，红色点表示turtlebot2的目标位置，黑色矩形表示障碍物，蓝色点及线段为实验时turtlebot2里程计真实位置输出。

以上所述仅为本发明的较佳实施例，对于本领域的普通技术人员而言，根据本发明的教导，设计出不同形式的运动规划算法并不需要创造性的劳动，在不脱离本发明的原理和精神的情况下凡依本发明申请专利范围所做的均等变化、修改、替换和变型，皆应属本发明的涵盖范围。