CN110703768B - 一种改进型动态rrt*的移动机器人运动规划方法 - Google Patents
一种改进型动态rrt*的移动机器人运动规划方法 Download PDFInfo
- Publication number
- CN110703768B CN110703768B CN201911086247.9A CN201911086247A CN110703768B CN 110703768 B CN110703768 B CN 110703768B CN 201911086247 A CN201911086247 A CN 201911086247A CN 110703768 B CN110703768 B CN 110703768B
- Authority
- CN
- China
- Prior art keywords
- new
- point
- node
- algorithm
- cost function
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 40
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 97
- 238000005070 sampling Methods 0.000 claims abstract description 21
- 238000011156 evaluation Methods 0.000 claims abstract description 6
- 238000009826 distribution Methods 0.000 claims description 21
- 230000007613 environmental effect Effects 0.000 claims description 21
- 238000012502 risk assessment Methods 0.000 claims description 18
- 230000008569 process Effects 0.000 claims description 15
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000013459 approach Methods 0.000 claims description 7
- 239000006185 dispersion Substances 0.000 claims description 6
- 230000004888 barrier function Effects 0.000 claims description 5
- 238000005457 optimization Methods 0.000 claims description 5
- 230000003247 decreasing effect Effects 0.000 claims description 4
- 238000001514 detection method Methods 0.000 abstract description 11
- 230000006870 function Effects 0.000 description 55
- 230000003068 static effect Effects 0.000 description 22
- 238000002474 experimental method Methods 0.000 description 15
- 238000004088 simulation Methods 0.000 description 9
- 238000010586 diagram Methods 0.000 description 6
- 241000282414 Homo sapiens Species 0.000 description 2
- 238000012800 visualization Methods 0.000 description 2
- 239000002699 waste material Substances 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000003111 delayed effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 210000003027 ear inner Anatomy 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
- G05D1/0253—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting relative motion information from a plurality of images taken successively, e.g. visual odometry, optical flow
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
- G05D1/028—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using a RF signal
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Optics & Photonics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种改进型动态RRT*的移动机器人运动规划方法。该方法剔除了原RRT*算法中所有的碰撞检测，通过在代价函数中增加碰撞风险评估分量的方法控制代价函数值。当采样点或边与障碍物发生碰撞时，该分量将显著增大，促使代价函数值同步显著增大，抑制了发生碰撞的点和边继续扩张的可能性，从而使算法具有避障的能力。因本发明方法没有碰撞检测，在移动机器人运动规划问题上比RRT*算法收敛的速度更快，效率更高，在越复杂的环境中优势越突出。
Description
技术领域
本发明涉及移动机器人运动规划领域，具体涉及一种改进型动态RRT*的移动机器人运动规划方法。
背景技术
随着人类生活水平的不断提升和工业自动化、智能化程度的迅猛发展，机器人的用途越来越广泛，逐渐成为人类生产生活中必不可少的一部分。不论是工业机器人、服务机器人还是医疗机器人都需要具备能够在复杂多变的工作环境中进行无碰撞运动规划的能力。
机器人的运动规划经过长久的发展，形成了基于人工势场的方法、基于数学模型的方法、基于图搜索的方法、基于节点的方法和基于采样的方法等众多不同的规划方法。为增加机械臂的灵活性和可操纵性，人们发明了冗余机械臂甚至超冗余机械臂，它们的自由度都大于6，由于机械臂运动规划的复杂程度随着机械臂自由度的增加而显著增大，因此冗余机械臂的运动规划通常需要考虑“维度灾难”的问题。基于采样的方法因在多维空间中具有显著优势而在机械臂运动规划领域中得到了广泛的关注。
StevenM.LaValle在1998年提出了RRT算法，该算法倾向于向未探索区域扩张，随着迭代次数的增加，未探索区域会显著减少，因此RRT探索空间的能力强，速度快。RRT算法的缺陷在于其概率完备且不最优，它能很快找到可行路径但该路径通常不是最优路径且包含较多的棱角，也较容易被困于狭窄通道环境中。
Sertac Karaman和Emilio Frazzoli在2010年提出了RRT*算法，针对RRT得到的路径为非最优解的问题，RRT*算法在RRT算法基础上增加了重新布线函数(rewire function)改进了父节点的选择方式，随着迭代次数的增加，由RRT*搜索到的路径逐渐趋近于最优路径，因此RRT*是渐进最优的。RRT*算法的缺陷在于增加的重新布线函数使得算法的扩张速度及收敛速度变慢，且同RRT算法一样较容易被困于狭窄通道环境中。
R.Bohlin和L.E.Kavraki在2000年时第一次提出Lazy思想，利用延迟碰撞检测的方法减少大量不必要的碰撞检测从而提高路径规划效率，获得较快的规划速度。Lazy-PRM算法的缺陷在于其概率完备且不最优，它无法找到最优路径且随着采样点数量的增多所需时间显著增加。
Kris Hauser在2015年提出了Lazy-PRM*算法，Lazy-PRM*算法仅在Lazy-PRM算法上做了微小的改动，将对点的碰撞检测提前到采样时进行，但保持了边的延迟碰撞检测。Lazy-PRM*算法的缺陷在于其概率完备且不最优，采样点数量较少时，可以在较短的时间内获得较好的路径，但随着采样点数量的增多所需时间显著增加但路径质量并没有明显提高。
Luigi Palmieri和Sven Koenig等人在2016年提出了Theta*-RRT算法，该算法是RRT的变体，将离散的任意角度搜索与连续的非完整轮式机器人的运动规划结合起来，在不损失平滑度的情况下相比RRT的速度更快、路径质量更高，能够迅速的生成第一个轨迹。Theta*-RRT算法的缺陷在于同RRT一样是具有概率完备性但无法找到最优路径的算法。
Adnan Tahirovic和Mina Ferizbegovic在2018年提出了RRV(Rapidly-exploringRandom Vines)算法，该算法能解决经典RRT算法在狭窄通道环境中效率低的问题。通过对节点周围的空间进行适当的采样，能够有效地扩张障碍物周围和狭窄通道中的树。RRV算法的缺陷在于其只在处理狭窄通道环境中表现良好，在没有狭窄通道的环境中，RRV算法与RRT算法的运行情况相似，甚至在具有许多障碍物但没有狭窄通道的环境中表现得比RRT稍差。
发明内容
本发明的目的在于提供一种改进型动态RRT*的移动机器人运动规划方法，该方法克服现有运动规划算法的不足，算法收敛速度快于RRT*，路径质量优于RRT，即使在狭窄通道或迷宫等复杂环境中依然具有优势。
为实现上述目的，本发明的技术方案是：一种改进型动态RRT*的移动机器人运动规划方法，包括如下步骤：
步骤S1、获取移动机器人当前周围环境信息：通过激光雷达或深度相机获取移动机器人半径范围内的环境信息，未被扫描到的位置呈现未知状态，扫描到的无障碍物位置呈现自由状态，扫描到的有障碍物位置呈现占用状态；在机器人根据规划的路径移动的过程中，不断获取新位置周围的环境信息，并与之前的环境信息相结合得到机器人走过路径的周围所有环境信息；
步骤S2、算法初始化：设定所需规划的起始位置、目标位置和总迭代次数N，设置当前迭代次数k等于0，设置当前规划时间t等于0，将起始位置xinit加入节点集合V，将起始位置加入搜索树G，并设置父节点集合E为空集；
步骤S3、障碍物离散化：将环境中所有的已知障碍物进行离散化，即用有限数量的间隔均匀的点集合表示连续的障碍物，并假设点的数量为M，组成集合ξkn，即
步骤S4、随机均匀采样，在环境地图中的随机位置加入一个采样点xrand，并将当前迭代次数k加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，设置当前迭代次数k等于0，设置当前规划时间t等于0，将起始位置xinit加入节点集合V，将起始位置加入搜索树G，并设置父节点集合E为空集；
步骤S3、障碍物离散化：将环境中所有的已知障碍物进行离散化，即用有限数量的间隔均匀的点集合表示连续的障碍物，并假设点的数量为M，组成集合ξkn，即
如图2b所示。
步骤S4、随机均匀采样(以函数Sample(i)表示)，在环境地图中的随机位置加入一个采样点xrand，并将当前迭代次数k加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所示，黑色点表示算法的采样点，蓝色线段表示算法的搜索树，黑色矩形表示障碍物，绿色线条表示算法探索到的可行路径。在障碍物中的长度越长，碰撞风险评估分量将越大，代价函数显著增大，每个节点都会选择代价函数值最小的连接方式，因此红色实线框内的树呈现对称状断开，不会彼此相连，使得从起始点到目标点生长的搜索树将不能够从障碍物上通过，从而实现了算法避免碰撞的功能。
本实施例中，采用的算法的完整伪代码，具体如下：
其中，Algorithm 1中的第2条，Algorithm 2中的第7-10、13为本发明采用的改进的RRT*算法相比于原来的RRT*算法的改进部分。
以下用具体的实验对本发明的实施方式进行详细说明，本发明提供一种概率完备且渐进最优的运动规划算法，主要通过仿真实验和真实实验来验证其有效性。具体实验设置如下：
仿真实验：
仿真实验在MATLAB R2017b软件中进行。
(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、ROS Kinetic，使用搭载Kinect深度相机的Turtlebot2及Intel Core i7-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、ROS Kinetic，使用搭载Rplidar激光雷达的Turtlebot2及Intel Core i7-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里程计真实位置输出。
以上所述仅为本发明的较佳实施例，对于本领域的普通技术人员而言，根据本发明的教导，设计出不同形式的运动规划算法并不需要创造性的劳动，在不脱离本发明的原理和精神的情况下凡依本发明申请专利范围所做的均等变化、修改、替换和变型，皆应属本发明的涵盖范围。
Claims (5)
1.一种改进型动态RRT*的移动机器人运动规划方法，其特征在于，包括如下步骤：
步骤S1、获取移动机器人当前周围环境信息：通过激光雷达或深度相机获取移动机器人半径范围内的环境信息，未被扫描到的位置呈现未知状态，扫描到的无障碍物位置呈现自由状态，扫描到的有障碍物位置呈现占用状态；在机器人根据规划的路径移动的过程中，不断获取新位置周围的环境信息，并与之前的环境信息相结合得到机器人走过路径的周围所有环境信息；
步骤S2、算法初始化：设定所需规划的起始位置、目标位置和总迭代次数N，设置当前迭代次数k等于0，设置当前规划时间t等于0，将起始位置xinit加入节点集合V，将起始位置加入搜索树G，并设置父节点集合E为空集；
步骤S3、障碍物离散化：将环境中所有的已知障碍物进行离散化，即用有限数量的间隔均匀的点集合表示连续的障碍物，并假设点的数量为M，组成集合ξkn，即
步骤S4、随机均匀采样，在环境地图中的随机位置加入一个采样点xrand，并将当前迭代次数k加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*算法得到的代价函数值相同。
2.根据权利要求1所述的一种改进型动态RRT*的移动机器人运动规划方法，其特征在于，所述σ为标准差描述数据分布的离散程度，是高斯分布的形状参数；σ越大，数据分布越分散，曲线越扁平，障碍物的影响范围越大；σ越小，数据分布越集中，曲线越瘦高，障碍物的影响范围仅局限在障碍物本身尺寸内；当σ增大，会增大障碍物对周边环境的影响；在需要快速搜寻到一条可行路径时可以通过减少ξkn的大小，即减小M同时增大σ，扩大障碍物的影响范围来降低算法的运算时间。
3.根据权利要求2所述的一种改进型动态RRT*的移动机器人运动规划方法，其特征在于，所述步骤S9的具体实现方式为：将点xnew附近节点集合Xnear中各个节点的代价函数值Cost(xnear)加上该节点到点xnew的欧式距离c(Line(xnear,xnew))，再加上该节点到点xnew对应的碰撞风险评估分量kα*c”得到点xnew所有可能的代价函数值c'，并将计算所得的代价函数值中的最小值min(c′)设为点xnew的代价函数值Cost(xnew)，将最小代价函数值对应的节点设置为点xnew的父节点xmin。
4.根据权利要求3所述的一种改进型动态RRT*的移动机器人运动规划方法，其特征在于，所述步骤S10的具体实现方式为：将节点集合Xnear中除去点xnew的父节点xmin外所有节点对应的代价函数值同点xnew的代价函数值加上各个节点到点xnew的欧式距离，再加上各个节点到点xnew对应的碰撞风险评估分量进行一一对比：如前者数值大，则找出该节点的父节点xparent，并将其修改为点xnew，以缩小该节点的代价函数值；如后者数值大，则保持该节点的父节点不变。
5.根据权利要求4所述的一种改进型动态RRT*的移动机器人运动规划方法，其特征在于，所述步骤S11的具体实现方式为：当点xnew和目标点xgoal的距离小于目标偏差容忍度Rfind且点xnew的代价函数值小于一个设定阈值时，该阈值大小受分量系数kα影响，算法规划出可行路径，算法终止，输出搜索树图及可行路径；或当前迭代次数等于总迭代次数时，即使未找到可行路径算法也将终止运算并回到步骤S2的算法初始化，重新进行路径规划。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911086247.9A CN110703768B (zh) | 2019-11-08 | 2019-11-08 | 一种改进型动态rrt*的移动机器人运动规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911086247.9A CN110703768B (zh) | 2019-11-08 | 2019-11-08 | 一种改进型动态rrt*的移动机器人运动规划方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110703768A CN110703768A (zh) | 2020-01-17 |
| CN110703768B true CN110703768B (zh) | 2021-07-13 |
Family
ID=69204753
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911086247.9A Active CN110703768B (zh) | 2019-11-08 | 2019-11-08 | 一种改进型动态rrt*的移动机器人运动规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110703768B (zh) |
Families Citing this family (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111412920B (zh) * | 2020-04-08 | 2022-02-25 | 广东博智林机器人有限公司 | 移动设备朝向转向路径的处理方法和装置 |
| CN111397598B (zh) * | 2020-04-16 | 2022-02-01 | 苏州大学 | 人机共融环境中移动型机器人路径规划采样方法及系统 |
| CN111580548B (zh) * | 2020-04-17 | 2021-10-19 | 中山大学 | 一种基于样条-rrt和速度障碍的无人机避障方法 |
| CN111824131B (zh) * | 2020-07-10 | 2021-10-12 | 广州小鹏自动驾驶科技有限公司 | 一种自动泊车的方法和车辆 |
| CN111752306B (zh) * | 2020-08-14 | 2022-07-12 | 西北工业大学 | 一种基于快速扩展随机树的无人机航路规划方法 |
| CN112338903A (zh) * | 2020-10-21 | 2021-02-09 | 西安工程大学 | 一种基于模型设计的机械臂控制方法 |
| CN112734619B (zh) * | 2020-12-21 | 2022-11-01 | 上海理工大学 | 一种面向检测特征扫描不确定度的自由曲面覆盖视点自动采样方法 |
| CN113282077B (zh) * | 2021-04-08 | 2022-06-17 | 浙江工业大学 | 基于正态分布和雅可比矩阵的同心管机器人运动规划方法 |
| CN113359717B (zh) * | 2021-05-26 | 2022-07-26 | 浙江工业大学 | 一种基于深度强化学习的移动机器人导航避障方法 |
| CN113359796B (zh) * | 2021-06-08 | 2022-09-06 | 同济大学 | 一种地下多分支洞穴的无人机探寻方法 |
| CN113485356B (zh) * | 2021-07-27 | 2022-06-21 | 西北工业大学 | 一种机器人快速运动规划方法 |
| CN113485241B (zh) * | 2021-07-28 | 2022-11-01 | 华南理工大学 | 基于线结构光传感器的焊接机器人离线扫描路径规划方法 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106444769A (zh) * | 2016-10-31 | 2017-02-22 | 湖南大学 | 一种室内移动机器人增量式环境信息采样的最优路径规划方法 |
| CN110221614A (zh) * | 2019-06-14 | 2019-09-10 | 福州大学 | 一种基于快速探索随机树的多机器人地图探索方法 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108983780A (zh) * | 2018-07-24 | 2018-12-11 | 武汉理工大学 | 一种基于改进rrt*算法的移动机器人路径规划方法 |
| US11530921B2 (en) * | 2018-09-28 | 2022-12-20 | Intel Corporation | Method of generating a collision free path of travel and computing system |
-
2019
- 2019-11-08 CN CN201911086247.9A patent/CN110703768B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106444769A (zh) * | 2016-10-31 | 2017-02-22 | 湖南大学 | 一种室内移动机器人增量式环境信息采样的最优路径规划方法 |
| CN110221614A (zh) * | 2019-06-14 | 2019-09-10 | 福州大学 | 一种基于快速探索随机树的多机器人地图探索方法 |
Non-Patent Citations (5)
| Title |
|---|
| 【规划】 RRT*算法图解;笑杨轩逸;《CSDN》;20190327;第21-32页 * |
| BI2RRT*: An Efficient Sampling-Based Path Planning Framework;Felix Burget 等;《2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)》;20160814;全文 * |
| Bidirectional Potential Guided RRT*;WANG XINYU 等;《IEEE Access》;20190801;全文 * |
| 基于改进 A* 算法的移动机器人路径规划;赵晓 等;《机器人 ROBOT》;20181130;全文 * |
| 局部环境增量采样的服务机器人路径规划;陈彦杰 等;《仪器仪表学报》;20170531;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110703768A (zh) | 2020-01-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110703768B (zh) | 一种改进型动态rrt*的移动机器人运动规划方法 | |
| Chen et al. | Path planning for manipulators based on an improved probabilistic roadmap method | |
| WO2018176596A1 (zh) | 基于权重改进粒子群算法的无人自行车路径规划方法 | |
| CN109542117B (zh) | 基于改进rrt的水下航行器滚动规划算法 | |
| Sudhakara et al. | Trajectory planning of a mobile robot using enhanced A-star algorithm | |
| CN110986953B (zh) | 路径规划方法、机器人及计算机可读存储介质 | |
| Gan et al. | Research on robot motion planning based on RRT algorithm with nonholonomic constraints | |
| CN110162029A (zh) | 一种基于规划路径的运动控制方法及装置、机器人 | |
| CN113253744B (zh) | 多机器人协同轨迹规划方法、装置、电子设备和存储介质 | |
| CN114460965B (zh) | 一种基于改进人工势场法的无人机三维避障方法 | |
| CN110954124A (zh) | 一种基于a*-pso算法的自适应路径规划方法及系统 | |
| Ma et al. | Efficient reciprocal collision avoidance between heterogeneous agents using ctmat | |
| Nam et al. | Fast and resilient manipulation planning for object retrieval in cluttered and confined environments | |
| CN113858205A (zh) | 一种基于改进rrt*的七轴冗余机械臂避障算法 | |
| US11813756B2 (en) | Disassembly based assembly planning | |
| Wang et al. | Research on AGV task path planning based on improved A* algorithm | |
| Diaz de Leon S et al. | Automatic path planning for a mobile robot among obstacles of arbitrary shape | |
| CN116852367A (zh) | 一种基于改进RRTstar的六轴机械臂避障路径规划方法 | |
| CN116520852A (zh) | 一种局部信息下的群机器人围捕多目标方法、装置及设备 | |
| Ho et al. | Simulated annealing based algorithm for smooth robot path planning with different kinematic constraints | |
| Dang | Autonomous mobile robot path planning based on enhanced A* algorithm integrating with time elastic band | |
| Charalampous et al. | Real-Time Robot Path Planning for Dynamic Obstacle Avoidance. | |
| Nametala et al. | A new distance diffusion algorithm for a path-planning model based on cellular automata | |
| Huang et al. | Multi-robot collision avoidance based on buffered voronoi diagram | |
| Wang et al. | Dynamic path planning algorithm for autonomous vehicles in cluttered environments |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |