CN106444740B - 基于mb-rrt的无人机二维航迹规划方法 - Google Patents
基于mb-rrt的无人机二维航迹规划方法 Download PDFInfo
- Publication number
- CN106444740B CN106444740B CN201610563601.2A CN201610563601A CN106444740B CN 106444740 B CN106444740 B CN 106444740B CN 201610563601 A CN201610563601 A CN 201610563601A CN 106444740 B CN106444740 B CN 106444740B
- Authority
- CN
- China
- Prior art keywords
- point
- algorithm
- path
- rrt
- tree
- 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 53
- 230000003044 adaptive effect Effects 0.000 claims abstract description 35
- 238000005070 sampling Methods 0.000 claims abstract description 33
- 230000004888 barrier function Effects 0.000 claims abstract description 24
- 238000003780 insertion Methods 0.000 claims abstract description 21
- 230000037431 insertion Effects 0.000 claims abstract description 20
- 238000001514 detection method Methods 0.000 claims abstract description 15
- 238000012545 processing Methods 0.000 claims abstract description 9
- 238000005457 optimization Methods 0.000 claims abstract description 8
- 230000007613 environmental effect Effects 0.000 claims abstract description 6
- 230000008569 process Effects 0.000 claims description 10
- 230000015572 biosynthetic process Effects 0.000 claims description 4
- 230000012010 growth Effects 0.000 abstract description 24
- 230000006870 function Effects 0.000 description 13
- 238000010586 diagram Methods 0.000 description 8
- 206010051602 Laziness Diseases 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 230000006978 adaptation Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 230000007812 deficiency Effects 0.000 description 2
- 235000013399 edible fruits Nutrition 0.000 description 2
- 230000007773 growth pattern Effects 0.000 description 2
- 238000007689 inspection Methods 0.000 description 2
- 230000035772 mutation Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000012428 routine sampling Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000032823 cell division Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000002068 genetic effect Effects 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000011349 peptide receptor radionuclide therapy Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 230000001902 propagating effect Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 230000000630 rising effect Effects 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
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/0202—Control of position or course in two dimensions specially adapted to aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
Abstract
一种基于MB‑RRT*的无人机二维航迹规划方法，初始化树以及环境信息；导入障碍物信息，并设置迭代次数；判断是否达到迭代次数，如果是，对生成的路径点进行降采样处理并采用插值算法优化生成路径线；否则产生随机采样点，并寻找树中与随机采样点距离最近的点，根据点生成自适应步长，根据步长生成最终插入点，判断插入点到树根的距离是否大于当前的最优路径长，如果否，对路径进行碰撞检测，如果通过将插入点加入树中，并对插入点周围的邻近节点进行优化；如果否，对树进行连接检测并进行连接。本发明收敛速度较快、内存占用空间较小、解决了在障碍物附近生长具有局限性的问题、能直接应用于无人机控制。
Description
技术领域
本发明涉及一种无人机二维航迹规划方法。
背景技术
大路径规划是无人机导航和机器人技术中的一个重要问题。路径规划可以被定义为：给定一个初始状态和目标状态，寻找一个可行路径使无人机无碰撞的从初始状态运行到目标状态。路径规划有广泛的应用场景：GPS导航、无人驾驶汽车、计算机动画、路由问题、制造行业的机械臂运动和生活以及工业领域的很多方面。因此对路径规划问题的研究近年来成为一个热门的研究课题。
无人机二维航迹规划算法根据其感知能力，可以分为局部路径规划和全局路径规划，其中全局路径规划就是在已知环境地图的情况下进行规划，预先知道环境的全局信息；而局部路径规划只需要获得机器人感知范围内的环境信息，主要指障碍物信息，根据局部信息完成规划。全局二维航迹规划算法有很多，人工势场算法是典型的路径规划算法，算法在环境中建立一个人工势场，障碍物和环境边界具有斥力，目标区域具有引力，无人机根据所受力向目标区域靠近。势场法不需要进行复杂的计算，只需要计算出环境的势场即可，然而势场法在复杂环境中容易使飞行器陷入局部最小，并不适合在复杂环境以及狭窄的通道中进行规划。针对势场法的不足，部分学者提出了Dubins曲线算法以及细胞分裂算法和Delaunay三角算法等离散化搜索空间的算法，通过对障碍物或者环境空间进行建模的方法寻找最优路径。同时也有人将进化算法例如遗传算法以及粒子群算法用于解决路径规划问题，利用算法的并行性找到最优路径。然而这类算法的计算开销特别大，算法在复杂的环境以及高维数的环境中需要大量的时间去计算，无法直接应用于无人机的路径规划问题上。
基于采样的路径规划算法已被证明可以高效地解决路径规划问题，概率路线图算法(PRM)和快速扩展随机数算法(RRT)是目前主要的两种采样算法。PRM算法随机在空间生成采样点，并且对这些点进行连接，最后通过图搜索算法找到初始状态到目标区域的路径。与RPM算法相比，RRT算法采用树结构介绍了算法进行碰撞检测的次数，并且树的路径搜索比图的路径搜索更容易实现。然而RRT算法的收敛率太低，即需要通过大量的迭代才能找到最优路径，并且随着迭代次数的上升，算法也需要大量的内存。因此人们目前提出了很多针对RRT算法的变种算法以及改进算法：Nik A将例子滤波与RRT算法结合提出了PRRT算法用于局部路径规划、Stephen R将泰森多边形(Voronoi)引入树的生长中提高算法找到可行解的速度等等，其中应用最广并且效果最好的是Sertac Karaman提出的RRT*算法。RRT*算法在每次迭代后对新加入的节点及其邻近节点进行优化，这一优化操作改善了算法的收敛率，保证了算法的渐近最优性，从而使其广泛应用在路径规划领域并衍生出一系列变种算法。A.H.Qureshi为了加快RRT*算法的收敛速度，在生成随机点的同时将随机点以及目标点和初始位置三个点构成的三角形的内心作为新的随机点加入树中，使随机点一定程度上偏向了目标点；M.Jordan提出了利用两棵树生长寻找路径的方法提高了算法的收敛率。但是这些改进算法中仍然存在着一系列的问题：
(1)算法的收敛速率还有很大的提升空间；
(2)算法寻找最优路径需要进行大量的迭代，因此算法的运行需要大量的内存；
(3)算法的节点基于固定的步长生长，因此树在障碍物附近地生长具有局限性；
(4)由于算法的路径由树节点连接生成，最后生成的路径不够平滑，难以直接应用于无人机中。
发明内容
为了克服已有无人机二维航迹规划方法的收敛速度较慢、内存占用偏大、在障碍物附近生长具有局限性、实用性较差的不足,本发明提供了一种收敛速度较快、内存占用空间较小、解决了在障碍物附近生长具有局限性的问题、能直接应用于无人机控制的基于MB-RRT*的无人机二维航迹规划方法。
本发明解决其技术问题所采用的技术方案是：
一种基于MB-RRT*的无人机二维航迹规划方法，包括如下步骤：
1)初始化树以及环境信息；
2)导入障碍物信息，并设置迭代次数；
3)判断是否达到迭代次数，如果是，对生成的路径点进行降采样处理，并采用插值算法优化生成路径线，结束；否则进入4)；
4)产生随机采样点Xrand，并寻找树中与随机采样点Xrand距离最近的点Xinsert；
5)根据点Xinsert生成自适应步长，根据自适应步长生成最终插入点Xnew；
6)判断插入点Xnew到树根的距离是否大于当前的最优路径长，如果是，进入9)；如果否进入7)；
7)对路径进行碰撞检测，如果未碰撞进入9)，如果碰撞，将插入点加入树中，并对插入点周围的邻近节点进行优化；
8)对树进行连接检测，并进行连接。
9)将当前操作的目标树设置为另一颗树，回到步骤3)。
进一步，所述步骤3)中，所述降采样处理：对生成的轨迹点之间不断地进行碰撞检测，对轨迹不断地截短直至无法截短。
再进一步，所述步骤3)中，采用构建三次贝塞尔曲线对轨迹点进行插值生成最后的路径曲线，三次贝塞尔曲线的方程如下所示：
B(t)＝P0(1-t)3+3P1t(1-t)2+3P2t2(1-t)+P3t3
其中P0为起始点，P3为目标点，P1和P2为控制点，用于控制曲线的方向，对于一系列路径点，只需要根据轨迹点不断计算两个控制点带入方程，即可生成曲线。
更进一步，所述步骤5)中，自适应步长的计算过程如下：首先通过NearestOb(x1,2μMax)寻找并计算出距离点x1最近的障碍物的距离Di，x1为查找的坐标点，μMax为设定的最大步长，μMin为设定的最小步长，根据下列公式计算出自适应步长：
通过这一方法计算出的自适应步长保持在范围μMin＜μ＜μMax，点x1与障碍物之间的距离接近μMax，自适应步长越接近μMin，当以x1为中心半径为2μMax的范围内没有障碍时，自适应步长为μMax。
所述步骤5)中，在采样阶段对采样点进行筛选，即在每次生成插入点xnew后计算插入点到达根节点的长度c(xnew,xinit)，如果这个长度大于当前最优路径的长度就抛弃这个插入点。
本发明的技术构思为：虽然B-RRT*算法利用双向树同时生长提高了算法的收敛率，并且保留了RRT*算法的概率完整性，但是B-RRT*算法仍然有很多缺点需要去改善。首先，B-RRT*算法保留了RRT*算法以固定步长生长树的方式，较大的步长可以较快的生长，然而生长至障碍物附近的节点有较大概率由于无法通过碰撞检测而被抛弃，同时较小的步长虽然在障碍物附近地生长较好，但在空旷的环境地图中相对生长较慢。其次，B-RRT*算法在找到可行路径之后仍然对整个环境空间进行采样，这种采样策略容易产生大量无效节点。最后，B-RRT*算法同时也保留了RRT*算法将最后生成的路径点直接相连的方法生成最终路径，导致最后产生的路径过于粗糙，无法应用于实际情况当中。针对B-RRT*算法的这些问题，本发明提出了MB-RRT*算法，通过引入自适应步长、懒惰采样以及贝塞尔曲线优化等方法对B-RRT*算法进行改进优化。
MB-RRT*算法与B-RRT*算法相比在每次迭代过程中多了两个步骤，并且在算法迭代结束后又加入了两个处理方法。
针对背景技术中的问题(1)、(3)，MB-RRT*算法引入了自适应步长的方法去优化算法的收敛率；针对问题(1)、(2)，MB-RRT*算法引入了懒惰采样的方法在提高算法收敛率的同时减少了算法内存占用；针对问题(4)，MB-RRT*算法抛弃了B-RRT*算法直接连接路径点的过程，引入了降采样和贝塞尔三次插值算法使最终生成的路径更加平滑。
自适应步长：传统的B-RRT*算法以固定的步长μ对树进行生长，μ过小会导致树生长过慢，找到初始可行解的速度降低；μ过大会导致生长过程中的采样点很难通过碰撞检测，使采样效率低下，并且过大的步长树的生长很难通过类似窄道的环境。
针对固定步长对树的生长造成的问题，MB-RRT*算法引入了自适应步长的概念，并通过函数AutoStepSteer(x1,x2)实现了树生长过程中自适应的步长。
传统的Steer(x1,x2)函数以矢量x2-x1为方向，距离为步长μ的位置生成点xnew，而AutoStepSteer(x1,x2)首先通过NearestOb(x1,2μMax)寻找并计算出距离点x1最近的障碍物的距离Di，根据下列公式计算出自适应步长：
通过这一方法计算出的自适应步长保持在范围μMin＜μ＜μMax，点x1与障碍物之间的距离接近μMax，自适应步长越接近μMin，当以x1为中心半径为μMax的范围内没有障碍时，自适应步长为μMax。
图3和图4为节点靠近障碍物情况下的生长，可以看出传统固定步长的生长方法易导致生成的新节点Xnew进入障碍物范围而无法通过障碍检测，而通过本发明提出的自适应步长生长的节点可以在障碍物附近生成有效的采样点。
参照图5和图6，在远离障碍物的情况下，传统固定步长生长的过程由于受限于步长，使其在障碍物空旷的区域生长的较为缓慢(按原有步长生长)。而本发明提出的自适应步长的生长方式按照计算的步长最大值μmax生长，加快了空旷区域探索速度，减少了找到初始可行解的时间。
懒惰采样：传统B-RRT*算法的采样过程保留了RRT*算法的采样过程，对状态空间进行均匀采样，从而继承了RRT*算法的概率完整性。然而B-RRT*算法是采用双向树生长的算法，在算法找到可行解并开始收敛到最优解的过程中，双向树中到各自树根距离大于当前最优解的采样点对最优解的寻找并没有什么意义(算法的最优解不会大于当前最优解)。传统的B-RRT*算法对于这些采样点进行了大量的操作，并且这些采样点也占用了一定量的内存，这在一定程度上减少了算法收敛的速度。
根据B-RRT*算法的这一问题，本发明提出的MB-RRT*算法提出了懒惰采样的方法减少无效采样点。懒惰采样在采样阶段对采样点进行筛选，即在每次生成插入点xnew后计算插入点到达根节点的长度c(xnew,xinit)，如果这个长度大于当前最优路径的长度就抛弃这个插入点。采用懒惰采样可以抛弃了一系列对最优路径搜索没有帮助的新的采样点，因为最优路径节点距离总是小于或等于当前最优路径长，这避免了对无效节点进行碰撞检测，优化等操作，与此同时，抛弃这些采样点可以使算法新生成的采样点收敛在当前最优路径长度的范围内，在不影响算法寻找最优路径的同时减少了树的生长范围。因此，采用懒惰采样这一过程理论上可以在保留算法概率完整性的同时提升算法的收敛速度。
通过进行十次实验取平均值，同样进行20000次迭代，未使用懒惰采样的MB-RRT*算法的平均运行时间为12.20316秒，而使用了懒惰采样的算法的平均运行时间为10.51662秒，加入懒惰采样后的算法的运行效率得到了大幅提升。图7和图8为常规采样的MB-RRT*算法与懒惰采样的MB-RRT*算法在MP3上的运行效果对比图，以初始点为树根生长的树Tinit以及以目标点为树根生长的树Tgoal用黑细线表示。从图中黑色圆圈圈出的区域看出，加入懒惰采样后树Tinit很少在目标点附近采样生长，同时树Tgoal也很少在初始点附近采样生长，这些区域的采样生长对算法收敛到最优解并没有什么帮助，减少这一区域的采样可以树的采样生长点收敛在中间区域。
降采样与曲线拟合：B-RRT*算法对最后生成的最优路径的轨迹点进行直线连接，由于采样的随机性以及树以步长生长的生长方式，算法生成的部分路径轨迹点部分可以进行截短操作从而生成更短的路径，并且直线连接的方式使得算法最终生成一条生硬的折线路径，并不符合实际的运动状况。针对这一情况，MB-RRT*算法在算法迭代完成之后对最后的轨迹点分别进行了降采样处理和曲线拟合处理，使算法得出的最终路径为平滑的曲线。
降采样过程对最终生成的轨迹点之间不断地进行碰撞检测，对最后的轨迹不断地截短直至无法截短。
为了使算法最后生成的路径更加平滑，采用构建三次贝塞尔曲线对轨迹点进行插值生成最后的路径曲线，三次贝塞尔曲线的方程如下所示：
B(t)＝P0(1-t)3+3P1t(1-t)2+3P2t2(1-t)+P3t3
其中P0为起始点，P3为目标点，P1和P2为控制点，用于控制曲线的方向，对于一系列路径点，只需要根据轨迹点不断计算两个控制点带入方程，即可生成曲线。
图9是降采样与曲线拟合示意图，虚线为树生长最后生成的轨迹点，实线连接的端点为经过降采样函数处理后的轨迹点，红色实线为根据降采样之后的端点再进行曲线拟合后生成的最终路径。
在完成曲线拟合后需要对生成的曲线进行碰撞检测，当发现其中的一段轨迹与障碍物发生碰撞，则需要对这段轨迹部分进行插值并重新拟合。插值点可直接选取在碰撞部分的轨迹点的中点，在每次曲线拟合后需要重新进行碰撞检测从而保证最终生成轨迹的安全性。
本发明的有益效果主要表现在：提出了一种MB-RRT*(ModifiedB-RRT*)算法，采用了懒惰采用的方法提升了算法的收敛率并且减少了算法的内存占用；采用自适应步长的方法解决了算法在障碍物附近生长的局限性问题，并且提高了算法找到初始可行解的速度和质量；利用了降采样和三次贝塞尔插值算法实现了曲线拟合的功能，使算法最后生成相对平滑的路径。
附图说明
图1是MB-RRT*算法的流程图。
图2是B-RRT*算法的流程图。
图3是靠近障碍物情况下的固定步长生长的示意图。
图4是靠近障碍物情况下的自适应步长生长的示意图。
图5是远离障碍物情况下的固定步长生长的示意图。
图6是远离障碍物情况下的自适应步长生长的示意图。
图7是常规采样的MB-RRT*算法在MP3上的运行效果图。
图8是懒惰采样的MB-RRT*算法在MP3上的运行效果图。
图9是降采样与曲线拟合示意图。
图10是插值处理示意图，其中，(a)是插值前，(b)是插值后。
具体实施方式
下面结合附图对本发明作进一步描述。
参照图1～图10，一种基于MB-RRT*的无人机二维航迹规划方法，包括如下步骤：
1)初始化树以及环境信息；
2)导入障碍物信息，并设置迭代次数；
3)判断是否达到迭代次数，如果是，对生成的路径点进行降采样处理，并采用插值算法优化生成路径线，结束；否则进入4)；
4)产生随机采样点Xrand，并寻找树中与随机采样点Xrand距离最近的点Xinsert；
5)根据点Xinsert生成自适应步长，根据自适应步长生成最终插入点Xnew；
6)判断插入点Xnew到树根的距离是否大于当前的最优路径长，如果是，进入9)；如果否进入7)；
7)对路径进行碰撞检测，如果未碰撞进入9)，如果碰撞，将插入点加入树中，并对插入点周围的邻近节点进行优化；
8)对树进行连接检测，并进行连接，进入9)。
9)将当前操作的目标树设置为另一颗树，回到步骤3)。
进一步，所述步骤3)中，所述降采样处理：对生成的轨迹点之间不断地进行碰撞检测，对轨迹不断地截短直至无法截短。
再进一步，所述步骤3)中，采用构建三次贝塞尔曲线对轨迹点进行插值生成最后的路径曲线，三次贝塞尔曲线的方程如下所示：
B(t)＝P0(1-t)3+3P1t(1-t)2+3P2t2(1-t)+P3t3
其中P0为起始点，P3为目标点，P1和P2为控制点，用于控制曲线的方向，对于一系列路径点，只需要根据轨迹点不断计算两个控制点带入方程，即可生成曲线。
更进一步，所述步骤5)中，自适应步长的计算过程如下：首先通过NearestOb(x1,2μMax)寻找并计算出距离点x1最近的障碍物的距离Di，根据下列公式计算出自适应步长：
通过这一方法计算出的自适应步长保持在范围μMin＜μ＜μMax，点x1与障碍物之间的距离接近，自适应步长越接近μMin，当以x1为中心半径为2μMax的范围内没有障碍时，自适应步长为μMax。
所述步骤5)中，在采样阶段对采样点进行筛选，即在每次生成插入点xnew后计算插入点到达根节点的长度c(xnew,xinit)，如果这个长度大于当前最优路径的长度就抛弃这个插入点。
本实施例中，在任意环境中，算法在路径可行解存在的情况下，运行迭代次数趋于无穷大的时候能够找到可行，那么称该算法具有概率完备性。RRT*算法已经被证明具有概率完备性，同时RRT算法的双向版本B-RRT算法也被证明了具有概率完备性。本发明提出的MB-RRT*算法使用了新的采样策略，即懒惰采样，由于懒惰采样略过对最优路径没有影响的无效节点，而且算法采用双向树结构生长，保证了树的生长覆盖整个环境空间，而且算法附加的曲线拟合以及降采样操作在算法完成迭代后进行，并不对算法的概率完备性产生影响。因此算法依旧保留着RRT*算法的概率完备性。
渐近最优(Asymptotic optimality)在路径规划问题当中的定义为：另c*为当前环境下路径规划的最优解，为算法ALG在迭代到n次之后产生的最优路径解的长度，算法ALG满足渐近最优性当：
已经知道RRT算法并不具有渐近最优性[]，而Sertac Karaman和Emilio Frazzoli证明了RRT*算法具有渐近最优性，同时Matthew Jordan和Alejandro Perez证明双向树版本的RRT*算法具有渐近最优性当其两棵树的连接过程使用类似于RRT*算法加入新节点的过程，即上文提到的Connect函数。本发明提出的MB-RRT*算法并未对连接过程进行修改，单棵树的生长过程继承RRT*算法的同时引入了自适应步长，而Sertac Karaman和EmilioFrazzoli提出RRT*算法的渐近最优性使用于步长η＞0的任意步长。因此可以得出MB-RRT*算法继承了RRT*算法的渐近最优性。
将MB-RRT*算法的计算复杂度与B-RRT*算法和RRT*的计算复杂度进行对比，分析MB-RRT*算法的计算复杂度。
首先，Sample、CollisionCheck、Steer等函数均可在常数时间内完成，而AutoStepSteer仅仅在传统Steer函数中多调用了一次NearestOb方法，这一方法和CollisionCheck类似，对空间范围内的障碍物搜索，由于障碍物数量是固定的并不随迭代次数增长，所以AutoStepSteer函数也可以在常数时间内完成。其次，Sunil Arya已经证明Nearest函数需要对数的时间进行即Ω(logn)，同时与Nearest函数类似，Near函数作为一个邻近点搜索算法，它的计算时间也为logn[]。最后，MB-RRT*算法的连接过程Connect函数延续了传统B-RRT*算法的连接方法，因此其计算时间和Near函数相同，为logn。根据上述条件可以总结出，存在一个常数φ1，使得
同时结合Yasar Ayaz的定理，可以推论出，存在一个常数φ2，使得
其中x为迭代次数，为算法ALG在x迭代后所执行的步骤数量。
本实施例的MB-RRT*算法与现有的B-RRT*算法和RRT*算法的对比，得出以下结论：
(1)MB-RRT*算法与B-RRT、RRT*和B-RRT*算法相比可以以更少的时间和迭代次数找到初始可行解，并且找到的初始可行解更优；
(2)MB-RRT*算法、B-RRT*算法和RRT*算法的计算复杂度十分接近，MB-RRT*算法的额外步骤没有对计算复杂度造成太大的影响；
(3)MB-RRT*算法与B-RRT*算法和RRT*算法相比有着更快的收敛率，可以更快的收敛到最优解；
(4)MB-RRT*算法最终生成的路径线比B-RRT*算法和RRT*算法更加平滑。
Claims (4)
1.一种基于MB-RRT*的无人机二维航迹规划方法，其特征在于：所述方法包括如下步骤：
1)初始化树以及环境信息；
2)导入障碍物信息，并设置迭代次数；
3)判断是否达到迭代次数，如果是，对生成的路径点进行降采样处理，并采用插值算法优化生成路径线，结束；否则进入4)；
采用插值算法优化生成路径线的过程为：采用构建三次贝塞尔曲线对轨迹点进行插值生成最后的路径曲线，三次贝塞尔曲线的方程如下所示：
B(t)＝P0(1-t)3+3P1t(1-t)2+3P2t2(1-t)+P3t3
其中P0为起始点，P3为目标点，P1和P2为控制点，用于控制曲线的方向，对于一系列路径点，只需要根据轨迹点不断计算两个控制点带入方程，即可生成曲线；
4)产生随机采样点Xrand，并寻找树中与随机采样点Xrand距离最近的点Xinsert；
5)根据点Xinsert生成自适应步长，根据自适应步长生成最终插入点Xnew；
6)判断插入点Xnew到树根的距离是否大于当前的最优路径长，如果是，进入8)；如果否进入7)；
7)对路径进行碰撞检测，如果未碰撞进入8)，如果碰撞，将插入点加入树中，并对插入点周围的邻近节点进行优化；
8)对树进行连接检测，并进行连接。
2.如权利要求1所述的基于MB-RRT*的无人机二维航迹规划方法，其特征在于：所述步骤3)中，所述降采样处理：对生成的轨迹点之间不断地进行碰撞检测，对轨迹不断地截短直至无法截短。
3.如权利要求1或2所述的基于MB-RRT*的无人机二维航迹规划方法，其特征在于：所述步骤5)中，自适应步长的计算过程如下：首先通过NearestOb(x1,2μMax)寻找并计算出距离点x1最近的障碍物的距离Di，x1为查找的坐标点，μMax为设定的最大步长，μMin为设定的最小步长，根据下列公式计算出自适应步长：
通过这一方法计算出的自适应步长保持在范围μMin＜μ＜μMax，点x1与障碍物之间的距离接近μMax，自适应步长越接近μMin，当以x1为中心半径为2μMax的范围内没有障碍时，自适应步长为μMax。
4.如权利要求1或2所述的基于MB-RRT*的无人机二维航迹规划方法，其特征在于：所述步骤5)中，在采样阶段对采样点进行筛选，即在每次生成插入点xnew后计算插入点到达根节点的长度c(xnew,xinit)，如果这个长度大于当前最优路径的长度就抛弃这个插入点。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610563601.2A CN106444740B (zh) | 2016-07-15 | 2016-07-15 | 基于mb-rrt的无人机二维航迹规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610563601.2A CN106444740B (zh) | 2016-07-15 | 2016-07-15 | 基于mb-rrt的无人机二维航迹规划方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106444740A CN106444740A (zh) | 2017-02-22 |
| CN106444740B true CN106444740B (zh) | 2019-03-12 |
Family
ID=58184533
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610563601.2A Active CN106444740B (zh) | 2016-07-15 | 2016-07-15 | 基于mb-rrt的无人机二维航迹规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106444740B (zh) |
Families Citing this family (27)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107085437A (zh) * | 2017-03-20 | 2017-08-22 | 浙江工业大学 | 一种基于eb‑rrt的无人机航迹规划方法 |
| CN108931984A (zh) * | 2017-05-25 | 2018-12-04 | 深圳市天荧智能科技有限公司 | 路径规划方法、控制方法、系统、介质、控制站和无人机 |
| CN108663050B (zh) * | 2018-02-10 | 2020-11-17 | 浙江工业大学 | 一种基于模拟植物生长引导rrt算法的路径规划方法 |
| CN108563237B (zh) * | 2018-03-30 | 2021-03-23 | 北京润科通用技术有限公司 | 一种协同避障方法及装置 |
| CN108458717B (zh) * | 2018-05-07 | 2020-04-07 | 西安电子科技大学 | 一种迭代的快速扩展随机树irrt的无人机路径规划方法 |
| CN108871344B (zh) * | 2018-07-13 | 2022-02-08 | 北京工业大学 | 足球机器人ggrrt路径规划方法 |
| EP3613545A1 (en) * | 2018-08-24 | 2020-02-26 | Siemens Aktiengesellschaft | Simulation assisted planning of motions to lift heavy objects |
| CN109582024A (zh) * | 2018-12-27 | 2019-04-05 | 济南大学 | 一种智能铲运机的路径规划方法 |
| CN110083165B (zh) * | 2019-05-21 | 2022-03-08 | 大连大学 | 一种机器人在复杂狭窄环境下路径规划方法 |
| CN110347151B (zh) * | 2019-05-31 | 2022-07-12 | 河南科技大学 | 一种融合Bezier优化遗传算法的机器人路径规划方法 |
| CN111060125B (zh) * | 2019-12-30 | 2021-09-17 | 深圳一清创新科技有限公司 | 碰撞检测方法、装置、计算机设备和存储介质 |
| CN111189455B (zh) * | 2020-01-14 | 2022-02-01 | 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) | 一种无人机航路规划方法、系统及存储介质 |
| CN111737636B (zh) * | 2020-06-12 | 2024-02-02 | 北京百度网讯科技有限公司 | 路径曲线的生成方法、装置、计算机设备和存储介质 |
| CN111678523B (zh) * | 2020-06-30 | 2022-05-17 | 中南大学 | 一种基于star算法优化的快速bi_rrt避障轨迹规划方法 |
| CN112504279B (zh) * | 2020-11-27 | 2022-12-30 | 上海交通大学 | 适用于无人机的无碰撞路径规划方法、系统及介质 |
| CN113050685B (zh) * | 2021-03-18 | 2023-06-16 | 中煤科工集团上海有限公司 | 一种煤矿井下无人机自主巡检的方法 |
| CN112947582A (zh) * | 2021-03-25 | 2021-06-11 | 成都纵横自动化技术股份有限公司 | 一种航线规划方法及相关装置 |
| CN112947489B (zh) * | 2021-04-08 | 2022-11-22 | 华东理工大学 | 复杂环境下焊接机器人无碰撞路径规划方法及装置 |
| CN112947490B (zh) * | 2021-04-09 | 2023-05-30 | 京东科技信息技术有限公司 | 路径平滑方法、装置、设备、存储介质及产品 |
| CN112987799B (zh) * | 2021-04-16 | 2022-04-05 | 电子科技大学 | 一种基于改进rrt算法的无人机路径规划方法 |
| CN113188562B (zh) * | 2021-07-01 | 2022-03-01 | 新石器慧通（北京）科技有限公司 | 可行驶区域的路径规划方法、装置、电子设备及存储介质 |
| CN113359775B (zh) * | 2021-07-08 | 2022-01-18 | 哈尔滨理工大学 | 一种动态变采样区域rrt无人车路径规划方法 |
| CN115617054A (zh) * | 2021-07-15 | 2023-01-17 | 中移系统集成有限公司 | 路径规划方法和系统、电子设备及可读存储介质 |
| CN113625721B (zh) * | 2021-08-19 | 2023-05-16 | 东北大学 | 一种未知空间自主探索规划方法 |
| CN113741484A (zh) * | 2021-09-30 | 2021-12-03 | 暨南大学 | 一种基于概率模型的路径规划方法、系统及介质 |
| CN114721433B (zh) * | 2022-04-11 | 2024-03-19 | 华南理工大学 | 一种基于无人机的无碰撞多项式轨迹生成方法 |
| CN114578401B (zh) * | 2022-04-29 | 2022-08-30 | 泽景(西安)汽车电子有限责任公司 | 一种车道航迹点的生成方法、装置、电子设备及存储介质 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103365293A (zh) * | 2013-05-08 | 2013-10-23 | 北京大学深圳研究生院 | 一种基于动态区域划分的机器人安全路径规划方法 |
| CN104155974A (zh) * | 2013-07-29 | 2014-11-19 | 深圳信息职业技术学院 | 一种用于机器人快速避碰的路径规划方法及设备 |
| CN104516356A (zh) * | 2015-01-08 | 2015-04-15 | 西北工业大学 | 基于rrt的动态障碍规避算法 |
| CN105353768A (zh) * | 2015-12-08 | 2016-02-24 | 清华大学 | 狭窄空间内基于随机采样的无人机轨迹规划方法 |
| CN107085437A (zh) * | 2017-03-20 | 2017-08-22 | 浙江工业大学 | 一种基于eb‑rrt的无人机航迹规划方法 |
-
2016
- 2016-07-15 CN CN201610563601.2A patent/CN106444740B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103365293A (zh) * | 2013-05-08 | 2013-10-23 | 北京大学深圳研究生院 | 一种基于动态区域划分的机器人安全路径规划方法 |
| CN104155974A (zh) * | 2013-07-29 | 2014-11-19 | 深圳信息职业技术学院 | 一种用于机器人快速避碰的路径规划方法及设备 |
| CN104516356A (zh) * | 2015-01-08 | 2015-04-15 | 西北工业大学 | 基于rrt的动态障碍规避算法 |
| CN105353768A (zh) * | 2015-12-08 | 2016-02-24 | 清华大学 | 狭窄空间内基于随机采样的无人机轨迹规划方法 |
| CN107085437A (zh) * | 2017-03-20 | 2017-08-22 | 浙江工业大学 | 一种基于eb‑rrt的无人机航迹规划方法 |
Non-Patent Citations (6)
| Title |
|---|
| 3D Field D*: Improved Path Planning and Replanning in Three Dimensions;Joseph Carsten 等;《International Conference on Intelligent Robots and Systems》;20061015;全文 * |
| 一种改进的RRT路径规划算法;宋金泽 等;《电子学报》;20100228;第38卷(第2A期);全文 * |
| 基于ＥＢ－ＲＲＴ＊的无人机航迹规划算法研究;陈晋音 等;《计算机科学》;20171130;第44卷(第11A期);全文 * |
| 基于ＭＢ－ＲＲＴ＊的无人机航迹规划算法研究;陈晋音 等;《计算机科学》;20170831;第44卷(第8期);全文 * |
| 基于改进RRT算法的无人机航迹规划;崔挺 等;《电子设计工程》;20130630;第21卷(第12期);全文 * |
| 复杂环境下基于RRT的智能车辆运动规划算法;杜明博 等;《机器人》;20150731;第37卷(第4期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106444740A (zh) | 2017-02-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106444740B (zh) | 基于mb-rrt的无人机二维航迹规划方法 | |
| CN112327885B (zh) | 一种无人船自适应全局-局部混合路径规划的方法 | |
| CN107085437A (zh) | 一种基于eb‑rrt的无人机航迹规划方法 | |
| CN109764886A (zh) | 一种路径规划方法 | |
| CN108681787A (zh) | 基于改进双向快速扩展随机树算法的无人机路径优化方法 | |
| CN112462803B (zh) | 一种基于改进nsga-ii的无人机路径规划方法 | |
| CN111610788B (zh) | 一种分级模糊-人工势场路径规划的方法 | |
| CN112987799B (zh) | 一种基于改进rrt算法的无人机路径规划方法 | |
| CN110262548A (zh) | 一种考虑抵达时间约束的无人机航迹规划方法 | |
| CN109213169A (zh) | 移动机器人的路径规划方法 | |
| CN114115362B (zh) | 一种基于双向apf-rrt*算法的无人机航迹规划方法 | |
| CN109990787A (zh) | 一种机器人在复杂场景中规避动态障碍物的方法 | |
| Xu et al. | Heuristic and random search algorithm in optimization of route planning for Robot’s geomagnetic navigation | |
| Maddern et al. | Towards persistent indoor appearance-based localization, mapping and navigation using cat-graph | |
| CN109211242B (zh) | 一种融合rrt与蚁群算法的三维空间多目标路径规划方法 | |
| CN112344938B (zh) | 基于指向和势场参数的空间环境路径生成及规划方法 | |
| CN112000126B (zh) | 一种基于鲸鱼算法的多无人机协同搜索多动态目标方法 | |
| CN114237302B (zh) | 一种基于滚动时域的三维实时rrt*航路规划方法 | |
| CN114115271A (zh) | 一种改进rrt的机器人路径规划方法和系统 | |
| Wang et al. | Path Planning Based on the Improved RRT* Algorithm for the Mining Truck. | |
| Qiao et al. | Sample-based frontier detection for autonomous robot exploration | |
| CN115826586B (zh) | 一种融合全局算法和局部算法的路径规划方法及系统 | |
| CN109977455B (zh) | 一种适用于带地形障碍三维空间的蚁群优化路径构建方法 | |
| Sun et al. | Optimal UAV flight path planning using skeletonization and particle swarm optimizer | |
| CN114610076B (zh) | 结合人工势场法与A-star算法角度约束的航迹规划方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |