2024, Vol. 46
快速搜索随机树*（Rapidly-Exploring Random Tree-star，RRT*）算法是由快速扩展随机树算法演变而来的一类全局路径规划算法。该算法与RRT算法同样具备概率完备性，在使用欧几里得距离作为价值函数的情况下，随着迭代次数的增加，RRT*算法求解的最优路径可以无限逼近理论中的最短路径，RRT*算法通过2种方式改变了RRT的路径规划结构，从而实现了获取最优路径的方式。通过一定范围内的重选父节点，然后重新编排树结构，RRT*每次树结构的延伸都保证了局部最优的扩展结果，从而经过不断迭代，可以获得全局最优的路径规划结果。
然而，RRT*算法有着先天的缺陷。首先，RRT*算法规划路径收敛时间长。作为一种基于采样的路径规划算法，不同于传统的RRT算法搜索到终点位置就停止迭代的特点，RRT*算法需要通过不断迭代采样。然后，通过其重选父节点以及重新编排局部树结构才能逐渐收敛到最优路径，然而对于全空间选点来说，RRT*算法向最优路径的逼近速度还是相当缓慢。同时，RRT*算法规划的路径难以满足运动载体的运动约束。无论对于陆上、水面以及水下的智能载体，其运动过程总会面临由于载体自生设计或者动力布置导致的运动学/动力学约束问题，然而在仅考虑运动路线问题的常规RRT*算法中，其路径规划的结果只具备理论意义。
针对RRT*算法存在的缺陷，并保留算法在迭代中趋近最优路径的特点，进而衍生出多种类型的改进RRT*算法。
针对RRT*算法规划路径时间长的问题，Nasir等[1]采用RRT*-Smart算法进行改进，该算法通过RRT*产生初始路径，通过对初始路径的优化处理，形成多折线形优化路径后，再通过信标节点对路径价值函数进行优化；Gammell等[2 − 3]采取双向搜索方案，通过单次采样同时从规划路径起点和终点同时进行树节点的扩展，从而加快搜索速度，同时在采样点的选择上，对采样点的采样范围进行了相应的约束，避免大范围随机采样导致无效节点的产生；张兰勇等[4− 5]在进行RRT*的优化中采用了目标偏置采样原则，使得算法在随机点的选择上更加具有方向性，从而加快算法的收敛速度。
针对RRT*算法规划路径未考虑载体运动/动力约束的问题，Alejandro等[6]使用LQR-RRT*算法来解决路径规划中的动力学约束问题，通过线性化的目标模型，通过LQR方法建立基于能耗最优的新“伪距离”计算方法作为树节点间判断距离的价值函数，从而保证RRT*路径规划中规划处满足控制量约束且能量最优的路径。然而该方式即使在线性模型的情况下也无法保证规划路径收敛于目标位置点，规划路径存在固有次优的缺陷；而且对于水下无人潜航器这种多维强非线性载体来说，该方法存在显著的劣势。
Webb等[7]针对RRT*路径规划中的多约束问题提出了Kinodynamic RRT*的解决方案，该方案不仅仅只考虑随机树节点的位置信息，在规划时还兼顾了每个节点的速度信息，从而重新定义树节点中的“最近”问题，通过节点的位置和速度状态，获得比欧氏距离更加符合机器人运动规律的价值行数设置方式。该方法对于线性多维载体的多约束路径规划问题有着很大的优势，对于弱非线性模型通过一阶泰勒展开的线性化方法也能达到不错的效果，但是对于强非线性及强耦合模型来说，在RRT*存在大量计算的同时，变节点位置的实时线性化会占用大量的计算时间；获得的规划路径由于存在控制对象速度维度的模拟，出现仿真意义上的“伪优”绕行路径，对实际的欠驱动潜航器循迹产生危险引导。
针对传统RRT*算法在寻优过程中的收敛速度慢以及规划路径不符合欠驱动水下潜航器运动约束等问题，设计了改进的RRT*算法。算法通过自适应目标趋向以及随机树拓展过程中的约束控制，加快随机树搜索过程中最优路径趋近速度的同时使得设计路径满足欠驱动无人潜航器运动约束，并利用Matlab仿真对传统RRT*算法和改进算法效果进行对比。
1 研究对象本文的研究对象是近海灵捷型水下无人潜航器，其外形为回转体结构，操纵系统为尾部十字舵，动力系统为尾部的导管桨驱动，是典型的欠驱动型水下潜航器。由于该潜航器的设计特点，需要着重考虑在路径规划中存在的潜航器运动约束问题，包括潜航器的转向角度以及俯仰角度约束。
2 优化算法考虑到研究对象的欠驱动特性，针对经典RRT*算法的运行特点，进行了包括增加随机树拓展过程的目标趋向以及随机树生长及迭代过程中的约束考虑。
2.1 自适应目标趋向算法传统的RRT*算法和RRT算法采用相同的取点方式，即对整个研究对象作业空间进行选点。这种随机树生长方式保证了RRT*算法规划路径不存在局部最优，然而这种随机生长的树节点拓展方式会使得随机树存在大量的冗余节点，在搜寻路径的过程中会极大限制算法寻优的速度。在限定搜索次数的随机树算法中，较小的搜索次数甚至无法给出一条可行路径。
为了加快随机树的生长速度，同时保留随机取点的全局空间扩展能力，本文提出树节点自适应目标趋向算法。自适应目标趋向算法的数学表达式如下：
| $ \left\{ {\begin{array}{*{20}{c}} {{x_{{\mathrm{rand}}}},p \geqslant {a_{THR}}} \\ {{x_{{\mathrm{goal}}}},p < {a_{THR}}} \end{array}} \right.,{a_{THR}} = \left(0.4 + 0.2\times\frac{1}{{1 + {e^{ - 2n}}}}\right)。$ | (1) |
其中：n为变化因子；
|
图 1 变化因子n更新逻辑 Fig. 1 Change factor n update logic |
自适应目标趋向算法通过设定判断阈值
这种方式给RRT*算法增加了扩展指向性，并且在扩展过程中随着随机树的拓展情况自适应改变指向性扩展概率。当随机树延伸失败或者受阻，该算法会根据失败次数减少指向性扩展概率，将当前随机树拓展的扩展方式从趋于指向性扩展转换成趋于随机扩展，扩展成功反之。2种方式都会随着随机树扩展失败/成功次数进一步扩大当前自身趋势，并在出现扩展情况转变时及时恢复初始扩展概率。
2.2 树节点拓展约束方法传统的RRT*算法使用欧式距离作为价值函数判定最近节点，这种判定方式保证了算法最终规划路径最终会收敛于路径长度最短。然而对于欠驱动水下无人潜航器，这种长度最短路径并不能符合潜航器的实际运动约束，导致潜航器在实际循迹中存在过多的机动点，使得潜航器偏离预定航迹的概率加大，对潜航器安全运行带来威胁。为了保证规划路径的有效性，必须在路径规划过程中考虑潜航器运动中的约束问题。
传统RRT*算法中，树节点的拓展是从全局随机点选取开始。通过在状态空间的随机点
本算法在随机树拓展时，对随机拓展过程施加角度约束
| $ \left\{ {\begin{aligned} \overrightarrow {{x_{{\mathrm{near}}}}{x_{{\mathrm{new}}}}} = & d\cdot \frac{{\overrightarrow {{x_{{\mathrm{near}}}}{x_{{\mathrm{rand}}}}} }}{{\left| {\overrightarrow {{x_{{\mathrm{near}}}}{x_{{\mathrm{rand}}}}} } \right|}},\\ &\left| {\frac{{\overrightarrow {{x_{{\mathrm{near}}}}{x_{{\mathrm{rand}}}}} }}{{\left| {\overrightarrow {{x_{{\mathrm{near}}}}{x_{{\mathrm{rand}}}}} } \right|}} - \frac{{\overrightarrow {{x_{{\mathrm{near}}}}{x_i}} }}{{\left| {\overrightarrow {{x_{{\mathrm{near}}}}{x_i}} } \right|}}} \right| \leqslant {\theta _{\max }}，\\ \overrightarrow {{x_{{\mathrm{near}}}}{x_{{\mathrm{new}}}}} = &{\text{d}}\left(\frac{{\overrightarrow {{x_{{\mathrm{near}}}}{x_i}} }}{{\left| {\overrightarrow {{x_{{\mathrm{near}}}}{x_i}} } \right|}} \pm \measuredangle {\theta _{\max }}\right),\\ &\left| {\frac{{\overrightarrow {{x_{{\mathrm{near}}}}{x_{{\mathrm{rand}}}}} }}{{\left| {\overrightarrow {{x_{{\mathrm{near}}}}{x_{{\mathrm{rand}}}}} } \right|}} - \frac{{\overrightarrow {{x_{{\mathrm{near}}}}{x_i}} }}{{\left| {\overrightarrow {{x_{{\mathrm{near}}}}{x_i}} } \right|}}} \right| > {\theta _{\max }} 。\end{aligned}} \right. $ | (2) |
当
作为区别RRT和RRT*算法的重要特征，重选父节点和重新布线是RRT*算法规划寻优的重要环节。传统的RRT*算法在重选父节点和重新布线的过程中仅仅以欧式距离作为价值函数，即使在随机扩展过程中考虑了潜航器运动约束，但最终规划路径仍然会由于重选父节点和重新布线的环节导致规划路径存在不合理的转向路径点等情况。针对迭代次数有限的情况下仍然保证规划路径能够满足潜航器的运动约束，本优化算法对原重选父节点和重新布线环节进行改进。
设定重选父节点和重新布线的价值函数如下：
| $ {\rm cos}t = \left\{ {\begin{aligned} & {\sum\limits_{i = 1}^n {\left| {{x_i}{x_{i - 1}}} \right|,{\rm if} \displaystyle\sum\limits_{i = 1}^n {\left| {{x_i}{x_{i - 1}}} \right| > thr{d_{{\text{dist}}}}} } }, \\ & {\sum\limits_{i = 1}^n {\left| {\measuredangle \overrightarrow {{x_i}{x_{i - 1}}} \overrightarrow {{x_{i - 1}}{x_{i - 2}}} } \right|,{\rm if}\displaystyle\sum\limits_{i = 1}^n {\left| {{x_i}{x_{i - 1}}} \right| \leqslant thr{d_{{\text{dist}}}}} } }。\end{aligned}} \right. $ | (3) |
在
重新布线环节也采用重选父节点过程中使用的判断条件来判断是否采用以
根据上述优化过程，改进的RRT*算法运行步骤为：首先，根据自适应参数值选择随机点的生成形式，根据设定步长确定随机点的位置；确定与随机点的欧式距离最近的树节点，以此树节点为原点根据设定的运动角度约束进行随机树的扩展，在满足碰撞条件的基础上，确保随机树的扩展满足设定的运动角度约束；在设定范围内选择重选父节点的树节点集合，根据树节点间的欧式距离选择不同的价值函数筛选新树节点的父节点，在满足碰撞检测条件后，即可确定新的父节点，并以相同的价值函数进行树节点的重新布线。按照以上步骤进行循环迭代，在到达设定的迭代次数时停止迭代，对树节点进行父节点回溯，从而获得规划路径。
3 算法仿真设定RRT*算法的步长为5 m，迭代仿真迭代次数为2 000次，俯仰角度变化阈值为15°，偏航角度变化约束为35°。设定潜航器起始位置为（5，5，5），目标位置为（95，95，95），障碍物选择使用球状障碍物作为不规则障碍物的膨胀状态，障碍物的位置和大小使用随机值生成。将传统RRT*算法与改进RRT*算法进行三维路径规划仿真对比，仿真结果如图2所示。
|
图 2 RRT*和改进RRT*算法仿真结果图 Fig. 2 Simulation results of RRT* and rmprovedRRT* algorithms |
从2种算法的随机树图来说，传统的RRT*算法的全局随机拓展使得系统存在很多的无效分支，而采用改进算法的随机树图，其自适应的目标趋向算法是得随机树的拓展更加具有方向性，因此相同迭代次数的情况下，改进算法的无效分支更少。
由图3和图4可知，传统的RRT*算法规划路径出现大角度的偏航和抬升，对比本次研究的欠驱动无人潜航器来说，这种大角度的偏转和抬升动作是无法完成的；在相同迭代次数过程中，改进算法规划路径相对平滑，最大偏航角度33.7°和最大抬升角度11.2°能够满足设定的35°和15°的范围要求，且规划的路径长度更加接近理论最小值（见表1）。
|
图 3 潜航器水平方向运动规划轨迹图 Fig. 3 Horizontal motion planning trajectory of submarine |
|
图 4 潜航器深度运动规划轨迹图 Fig. 4 Motion planning trajectory of depth of submersible |
|
表 1 RRT*和改进RRT*算法规划路径结果对比 Tab.1 Comparison of Path Planning Results between RRT * and Improved RRT * Algorithms |
改进RRT*算法在相同的迭代次数下的规划路径能够在满足约束条件下更接近最优路径，规划的路径更加符合本次研究对象的欠驱动特性。然而，在实际的仿真算法设计中，相比于传统的RRT*算法，改进算法需要计算机的算力更多，且相同迭代次数的情况下，寻找到全局可行路径的概率有所降低。根据实际路径规划算法的应用情况，将该算法实时性转化，将全局规划目标改为局部规划目标有助于该算法的实际应用。
| [1] |
NASIR J , ISLAM F , MALIK U , et al. RRT*-Smart: A rapid convergence implementation of RRT*[J]. International Journal of Advanced Robotic Systems, 2013, 10(7): 299-310. |
| [2] |
GAMMELL J D, SRINIVASA S S, BARFOOT T D. Informed RRT*: optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems, 2014: 2997−3004.
|
| [3] |
MASHAYEKHI R, IDRIS M, ANISI M H, et al. Informed RRT*-connect: an asymptotically optimal single-query path planning method[J]. IEEE Access, 2020(8): 1.
|
| [4] |
张兰勇, 韩宇. 基于改进的RRT*算法的AUV集群路径规划研究[J]. 中国舰船研究, 2023, 18(1): 43-51. ZHANG L Y, HAN Y. Research on AUV cluster path planning based on improved RRT* algorithm[J]. Chinese Journal of Ship Research, 2023, 18(1): 43-51. |
| [5] |
许万, 杨晔, 余磊涛, 等. 一种基于改进RRT~*的全局路径规划算法[J]. 控制与决策, 2022, 37(4): 829-838. XU W, YANG H, YU L T, et al. A global path planning algorithm based on improved RRT~*[J]. Control and Decision-making, 2022, 37(4): 829-838. |
| [6] |
PEREZ A , JR R P , KONIDARIS G , et al. LQR-RRT*: Optimal sampling-based motion planning with automatically derived extension heuristics[C]// IEEE International Conference on Robotics & Automation. IEEE, 2012: 2537−2542.
|
| [7] |
WEBB D J , BERG J . Kinodynamic RRT*: optimal motion planning for systems with linear differential constraints: 10.48550/arXiv. 1205.5088[P]. 2012.
|