基于改进RRT⁃Connect的快速路径规划算法

基于改进RRT⁃Connect的快速路径规划算法 Faster Path Planning Based on Improved RRT⁃Connect Algorithm 1 first-author 王 坤，男，硕士生，主要从事机器人控制、路径规划算法方面的研究。 E⁃mail：M020217119@sues.edu.cn 王 坤，男，硕士生，主要从事机器人控制、路径规划算法方面的研究。 E⁃mail：M020217119@sues.edu.cn M020217119@sues.edu.cn 1 2 corresp E⁃mail：huangbosues@sues.edu.cn E⁃mail：huangbosues@sues.edu.cn huangbosues@sues.edu.cn 1 3 上海工程技术大学 电子电气工程学院，上海 201620 上海工程技术大学 电子电气工程学院，上海 201620 School of Electronic and Electrical Engineering， Shanghai University of Engineering Science， Shanghai 201620， China School of Electronic and Electrical Engineering， Shanghai University of Engineering Science， Shanghai 201620， China 江西省经济犯罪侦查与防控技术协同创新中心，江西 南昌 330103 江西省经济犯罪侦查与防控技术协同创新中心，江西 南昌 330103 Center of Economic Crime Detection and Prevention and Control Technology Collaborative Innovation，Nanchang 330103，Jiangxi，China Center of Economic Crime Detection and Prevention and Control Technology Collaborative Innovation，Nanchang 330103，Jiangxi，China 上海应用技术大学 电子与电气工程学院，上海 200235 上海应用技术大学 电子与电气工程学院，上海 200235 College of Electrical and Electronic Engineering， Shanghai Institute of Technology， Shanghai 200235， China College of Electrical and Electronic Engineering， Shanghai Institute of Technology， Shanghai 200235， China 针对双向快速扩展随机树（RRT⁃Connect）算法的路径规划效率较低且采样具有随机性，提出了基于RRT⁃Connect的改进算法（DRRT⁃Connect）。该算法在起始点与目标点中间选取一个第三节点作为扩展点，使算法可以同时从起始点、目标点和第三节点生成四棵随机树；同时在改进算法中引入自适应步长调节函数，当探索无障碍空间时，算法使用步长调节函数增大扩展步长，从而提高随机树探索空间的速度；在RRT⁃Connect算法的基础上引入目标偏置策略，使DRRT⁃Connect在探索无障碍空间时可以朝目标点进行快速扩展，在探索障碍物空间时则调用随机采样函数，使算法可以快速摆脱障碍物，防止陷入局部最优。将DRRT⁃Connect算法分别与RRT、RRT⁃Connect、RRT*算法进行仿真对比，结果表明DRRT⁃Connect在路径规划效率与迭代次数上均明显优于其他对比算法，其中相较于RRT⁃Connect算法，DRRT⁃Connect在路径规划速度上提高了50%，迭代次数上降低了32.3%。 An improved RRT⁃Connect algorithm(DRRT⁃Connect), based on RRT⁃Connect was proposed to overcome the low efficiency and randomness in path planning of rapidly⁃exploring random tree connect (RRT⁃Connect) algorithm. Firstly, the algorithm selected a third node between the initial node and goal node as the extension node, so that the algorithm can generate four random trees from the initial node, goal node and the third node at the same time. In addition, the adaptive step⁃size adjustment function was introduced into the improved algorithm. The algorithm used the step⁃size adjustment function to increase the extended step size when exploring the free space. The function improved the exploring speed of random tree. Finally, a target⁃oriented strategy was introduced on the basis of RRT⁃Connect algorithm, which enabled the improved algorithm to expand rapidly towards the target node when exploring the free space, while the random sampling function was called when exploring the obstacle space, so that the algorithm could get rid of the obstacle quickly and avoid falling into local optimization. The simulation comparison of DRRT⁃Connect algorithm with RRT, RRT⁃Connect and RRT* algorithms showed that the improved algorithm was significantly better than other algorithms in path planning efficiency and iteration times. The improved algorithm increased the path planning speed by 50% and reduces the iteration times by 32.3% compared with RRT⁃Connect algorithm. 路径规划 快速扩展随机树 RRT⁃Connect算法 自适应步长 目标偏置策略 path planning rapidly⁃exploring random tree RRT⁃Connect algorithm adaptive step⁃size biased strategy 王坤,黄勃,曾国辉, 等.基于改进RRT⁃Connect的快速路径规划算法[J].武汉大学学报（理学版）,2019,65(3):283-289. DOI:10.14188/j.1671-8836.2019.03.008. 0　 引 言 1 路径规划是移动机器人领域的一项热门研究，目前已在自动驾驶 ［ 1 1 ］ 、自主探测 ［ 2 2 ］ 、计算生物学 ［ 3 3 ］ 、图形动画学 ［ 4 4 ］ 等领域获得广泛应用。路径规划的最终目的是寻找到一条可以从起始点到目标点的无碰撞路径 ［ 5 5 ］ ，该路径可以保证移动机器人在移动过程中不会与障碍物发生碰撞，同时这条路径需要满足距离最近、用时最短或耗能最低等指标中的一种或几种。目前在路径规划方面已经出现了众多可行的算法 ［ 6 6 ~ 12 12 6~12 ］ 。在低维空间中，这些算法尚有一定的优势，但在高维空间中，由于计算的复杂性，其生成路径的效率较低。 为解决高维空间中的路径规划问题，基于采样的路径规划算法被提出并得到发展，其代表算法是LaValle等提出的RRT ［ 13 13 ］ 算法，该算法不需要对环境进行建模，只需要通过随机采样生成随机树的方式来探索整个空间，随机点的选取保证了搜索路径的全局性，从而使算法具有概率完备 ［ 14 14 ］ 的特点。由于该算法可以大幅降低高维空间路径规划的计算量，在提出后受到了众多学者的关注。但该算法存在不足：1） 由于扩展的随机性，使得算法收敛速度缓慢，路径生成效率低；2） 生成的路径只是可行路径，而非最优或次优 ［ 15 15 ］ ；3） 路径生成中没有采用平滑处理，导致生成的路径较为粗糙，不适合机器人的实际移动。 针对RRT算法存在的不足，国内外学者纷纷提出了一系列的改进算法，其中由Kuffner等提出的RRT⁃Connect算法 ［ 16 16 ］ 在规划效率上取得明显进步。该算法通过在起始点和目标点同时生成两棵随机树的方式，用以加速算法的收敛。本文基于RRT⁃Connect算法，通过在起始点和目标点中间选取第三节点以生成四棵探索树的方式来提高算法的收敛速度，同时在算法扩展过程中，加入启发函数，使随机树在扩展中可以自适应调节步长，并通过引入目标偏置策略，使改进后的算法在无障碍空间中能够以最快速度探索未知区域，增加扩展效率。 1　 RRT与RRT⁃Connect算法 1 RRT算法作为单一查询算法，不需要预先对整个构型空间进行采样与存储，而是通过函数迭代的方式探索整个状态空间 ［ 17 17 ］ 。 图1 图1 中， <math id="M1"><mi>n</mi><mmultiscripts><mrow/><mprescripts/><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi></mrow><mrow/></mmultiscripts></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206713&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206710&type= 5.33399963 3.72533321 、 <math id="M2"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">l</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206720&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206716&type= 5.41866684 3.72533321 分别为路径的起始点和目标点， <math id="M3"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206725&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206722&type= 5.58799982 3.72533321 为状态空间 C <math id="M4"><mo>∈</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206732&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206728&type= 2.70933342 3.13266659 <math id="M5"><msup><mrow><mi mathvariant="normal">ℝ</mi></mrow><mrow><mi>n</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206737&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206734&type= 3.55599999 2.96333337 中的随机采样点， <math id="M6"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">t</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206740&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206739&type= 7.70466709 3.72533321 为随机树中离随机采样点 <math id="M7"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206747&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206744&type= 5.58799982 3.72533321 最近的节点， <math id="M8"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206752&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206749&type= 5.41866684 3.72533321 为 <math id="M9"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206759&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206755&type= 5.58799982 3.72533321 方向上随机树下一步扩展点， <math id="M10"><mi>ε</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206765&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206762&type= 1.43933344 2.79399991 为随机树每一步的扩展步长。 当随机采样函数通过对空间随机采样获得 <math id="M11"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206789&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206784&type= 5.58799982 3.72533321 后，RRT算法便以该点为下一步扩展方向向外扩展，扩展过程中有以下几种情况：1） 当 <math id="M12"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206798&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206793&type= 5.58799982 3.72533321 与 <math id="M13"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">t</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206805&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206802&type= 7.70466709 3.72533321 之间的距离大于 <math id="M14"><mi>ε</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206814&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206810&type= 1.43933344 2.79399991 时，则在 <math id="M15"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206822&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206818&type= 5.58799982 3.72533321 与 <math id="M16"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">t</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206832&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206827&type= 7.70466709 3.72533321 之间的连线上取一点 <math id="M17"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206842&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206836&type= 5.41866684 3.72533321 插入到树中，记为Reached；2） 当 <math id="M18"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206848&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206844&type= 5.58799982 3.72533321 与 <math id="M19"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">t</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206858&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206853&type= 7.70466709 3.72533321 之间的距离小于 <math id="M20"><mi>ε</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206870&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206863&type= 1.43933344 2.79399991 时，则直接将 <math id="M21"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206879&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206875&type= 5.58799982 3.72533321 插入到树中，记为Advanced；3） 在扩展中遇障，则舍弃原先的扩展方式，并重新生成随机点 <math id="M22"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206890&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206886&type= 5.58799982 3.72533321 ，记为Trapped。 RRT算法由RRT建树（BU_RRT（ <math id="M23"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206900&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206894&type= 4.65666676 3.72533321 ））和RRT扩展（EXT（ T ， <math id="M24"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206910&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206906&type= 5.58799982 3.72533321 ））两部分组成。RRT建树过程如下： 2 BU_RRT( <math id="M25"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi></mrow></msub></math> )//RRT建树 1 T .init( <math id="M26"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi></mrow></msub></math> )； 2 for k = 1 to K do 3 <math id="M27"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> <math id="M28"><mo>←</mo></math> SampleNode(); 4 EXT( T , <math id="M29"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> ); 5 Return T RRT扩展如下： 2 EXT( T , <math id="M30"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> )// RRT扩展 1 <math id="M31"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">t</mi></mrow></msub></math> <math id="M32"><mo>←</mo></math> NearestNode( <math id="M33"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> , T ); 2 if NewNode( <math id="M34"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> ， <math id="M35"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">t</mi></mrow></msub></math> ， <math id="M36"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> ) 3 then 4 T . V <math id="M37"><mo>←</mo></math> <math id="M38"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> ; 5 T . E <math id="M39"><mo>←</mo></math> ( <math id="M40"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> ， <math id="M41"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">t</mi></mrow></msub></math> ); 6 if dist( <math id="M42"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> ， <math id="M43"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">t</mi></mrow></msub></math> )) < <math id="M44"><mi>ε</mi></math> 7 then 8 <math id="M45"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> <math id="M46"><mo>←</mo></math> <math id="M47"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> ; 9 Return Reached; 10 else 11 Return Advanced; 12 Return Trapped; 其中， T 表示随机树， T . V 表示随机树中的节点， T . E 表示随机树顶点之间的路径。BU_RRT函数在状态空间中采样随机点，确定扩展方向。通过循环调用EXT函数来扩展未知空间，最终返回生成的搜索树。EXT函数主要用于扩展随机树。根据设定步长判断 <math id="M48"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206939&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206933&type= 5.41866684 3.72533321 节点的位置，并将新节点添加到树中。 由于RRT算法在扩展中的随机性，导致其存在收敛速度缓慢，为了提高RRT算法的收敛速度，Kuffner和LaValle提出了RRT⁃Connect算法 ［ 16 16 ］ ，该算法分别在起始点和目标点构建两棵并行的随机树，并在每次迭代过程中，使两棵树分别朝着彼此的方向交替扩展，且在扩展中引入了具有贪婪特性的CON函数，实现随机树的快速扩展。 RRT⁃Connect建树过程如下： 2 BU_RRT⁃Con( <math id="M49"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi></mrow></msub></math> , <math id="M50"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">l</mi></mrow></msub></math> ) 1 <math id="M51"><msub><mrow><mi>T</mi></mrow><mrow><mn>1</mn></mrow></msub><mo>.</mo><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi><mo stretchy="false">(</mo><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi></mrow></msub><mo stretchy="false">)</mo></math> ; <math id="M52"><msub><mrow><mi>T</mi></mrow><mrow><mn>2</mn></mrow></msub><mo>.</mo><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi><mo stretchy="false">(</mo><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">l</mi></mrow></msub><mo stretchy="false">)</mo></math> ; 2 for k = 1 to K do 3 <math id="M53"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> <math id="M54"><mo>←</mo></math> SampleNode(); 4 if (EXT( <math id="M55"><msub><mrow><mi>T</mi></mrow><mrow><mn>1</mn></mrow></msub></math> , <math id="M56"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> ) ≠ Trapped ) 5 then 6 if (CON( <math id="M57"><msub><mrow><mi>T</mi></mrow><mrow><mn>2</mn></mrow></msub></math> , <math id="M58"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> )=Reached) 7 then 8 Return PATH( <math id="M59"><msub><mrow><mi>T</mi></mrow><mrow><mn>1</mn></mrow></msub></math> , <math id="M60"><msub><mrow><mi>T</mi></mrow><mrow><mn>2</mn></mrow></msub></math> ); 9 SWAP( <math id="M61"><msub><mrow><mi>T</mi></mrow><mrow><mn>1</mn></mrow></msub></math> , <math id="M62"><msub><mrow><mi>T</mi></mrow><mrow><mn>2</mn></mrow></msub></math> ); 10 Return Failed； 其中： <math id="M63"><msub><mrow><mi>T</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mi mathvariant="normal">、</mi><msub><mrow><mi>T</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206957&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206953&type= 9.56733322 3.97933340 表示分别从起始点 <math id="M64"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206967&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206962&type= 4.65666676 3.72533321 和目标点 <math id="M65"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">l</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206976&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206972&type= 5.41866684 3.72533321 生成的随机树：CON函数如下： 2 CON( T , <math id="M66"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> ) 1 repeat 2 S ←EXT(T , <math id="M67"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> ); 3 until ( S ≠Advanced); 4 Return S ; 该算法首先调用EXT函数，从起始点 <math id="M68"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206997&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4206993&type= 4.65666676 3.72533321 出发，通过EXT扩展生成节点 <math id="M69"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207005&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207000&type= 5.41866684 3.72533321 。此后， <math id="M70"><msub><mrow><mi>T</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207012&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207008&type= 3.72533321 3.72533321 开始以 <math id="M71"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207022&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207017&type= 5.41866684 3.72533321 为方向进行扩展，在 <math id="M72"><msub><mrow><mi>T</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207031&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207027&type= 3.72533321 3.72533321 扩展中，如果没有发生碰撞或者 <math id="M73"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207040&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207036&type= 5.41866684 3.72533321 与当前扩展节点之间的距离不小于步长 <math id="M74"><mi>ε</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207048&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207045&type= 1.43933344 2.79399991 时， <math id="M75"><msub><mrow><mi>T</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207057&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207052&type= 3.72533321 3.72533321 通过调用CON函数来实现快速向 <math id="M76"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207066&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207062&type= 5.41866684 3.72533321 扩展，当扩展过程中出现障碍物时， <math id="M77"><msub><mrow><mi>T</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207074&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207070&type= 3.72533321 3.72533321 停止扩展， <math id="M78"><msub><mrow><mi>T</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207082&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207078&type= 3.72533321 3.72533321 以当前 <math id="M79"><msub><mrow><mi>T</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207090&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207086&type= 3.72533321 3.72533321 的终止点为目标点开始扩展，通过SWAP函数使两棵树交替扩展，最终相遇并连接。 2　 DRRT⁃Connect算法 1 RRT⁃Connect算法虽对探索树的扩展速度有很大幅度的提高，但在如防反弹袭击、无人机避障等一些需要高灵敏度的领域仍存在规划效率低的问题。基于上述原因，本文提出的DRRT⁃Connect，对RRT⁃Connect算法做出以下两点改进：首先，由于RRT⁃Connect只能同时生成两棵树，所以DRRT⁃Connect算法在RRT⁃Connect算法的基础上引入第三节点，使算法可以同时生成四棵树，以达到加倍扩展RRT⁃Connect的效果；其次，DRRT⁃Connect在RRT⁃Connect算法的基础上加入可自适应调节步长的启发函数，使随机树在探索无障碍空间时的扩展效率更高，同时在算法执行时，引入目标偏置策略，使算法的扩展更具方向性，进一步加速随机树的扩展，减少迭代次数，以下对算法改进原则做详细介绍。 2.1　第三节点选择方式 2 为使DRRT⁃Connect算法可以实现四棵树同时扩展，需要选择一个第三节点。为提高算法效率，本文以起始点与目标点连线的中心点作为第三节点，假设起始点在构型空间中的坐标为 <math id="M80"><mo stretchy="false">(</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>,</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207096&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207093&type= 11.09133339 4.74133348 ，目标点的坐标为 <math id="M81"><mo stretchy="false">(</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo>,</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207104&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207100&type= 11.09133339 4.74133348 ，则求取第三节点 <math id="M82"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">c</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">r</mi></mrow></msub><mo stretchy="false">(</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub><mo>,</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207113&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207108&type= 18.71133423 4.74133348 坐标的公式为： <math display="block" id="M83"><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub><mo>=</mo><mo stretchy="false">(</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>+</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo stretchy="false">)</mo><mo>/</mo><mn mathvariant="normal">2</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207120&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207116&type= 26.41599846 4.57200003 <math display="block" id="M84"><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub><mo>=</mo><mo stretchy="false">(</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>+</mo><msub><mrow><mi>y</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo stretchy="false">)</mo><mo>/</mo><mn mathvariant="normal">2</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207128&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207124&type= 25.90799904 4.82599974 2.2　自适应步长调节原则 2 RRT⁃Connect算法中步长是给定值，这种给定步长在探索无障碍空间时会限制随机树的扩展速度 ［ 18 18 ］ 。本文在DRRT⁃Connect算法中加入了自适应调节步长的函数，具体为：随机树以初始步长 <math id="M85"><mi>ε</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207135&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207131&type= 1.43933344 2.79399991 探索未知空间，当扩展至新节点时，如在该节点处没有发生碰撞且未与另一棵树相连，则在下一次的扩展中，随机树将在原始步长上增加一个步长 <math id="M86"><mi>ε</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207143&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207139&type= 1.43933344 2.79399991 的宽度继续扩展，以此类推；当扩展中发生碰撞时，则将步长调节为原始步长 <math id="M87"><mi>ε</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207151&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207147&type= 1.43933344 2.79399991 ，然后按步长 <math id="M88"><mi>ε</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207159&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207155&type= 1.43933344 2.79399991 进行扩展。 2.3　避障原则 2 由于环境的复杂性，在DRRT⁃Connect执行过程中会遇到障碍物的情况，所遇障碍物主要包括第三节点生成时遇障与随机树扩展时遇障两种情况。 对于第一种情况，在DRRT⁃Connect采样第三节点之前，首先进行障碍检测，当第三节点位于障碍物中时，为了提高路径生成效率，则直接选择舍弃第三节点，采用RRT⁃Connect进行扩展。对于第二种遇障情况，即DRRT⁃Connect在扩展中遇到障碍物，当此时只有一棵树处于障碍物时，则采用交换扩展顺序的方式开始扩展另一棵树，若在扩展中两棵树同时遇障，此时则调用随机点生成函数，使其在构型空间中随机采样，然后选择较短树以该点为方向向前扩展一步，如脱离障碍物，然后以目标点为方向继续扩展，若该点与目标点仍存在障碍物，则继续调用该函数进行扩展随机树，直至摆脱障碍物。 2.4　算法实现 2 基于以上三点原则，DRRT⁃Connect算法构建过程如 图2 图2 所示，首先，算法分别以 <math id="M89"><mi>n</mi><mmultiscripts><mrow/><mprescripts/><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi></mrow><mrow/></mmultiscripts></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207167&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207163&type= 5.33399963 3.72533321 、 <math id="M90"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">l</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207175&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207171&type= 5.41866684 3.72533321 和 <math id="M91"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">c</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">r</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207183&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207179&type= 6.94266701 3.72533321 为扩展树的起点，然后 <math id="M92"><mi>n</mi><mmultiscripts><mrow/><mprescripts/><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi></mrow><mrow/></mmultiscripts></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207190&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207187&type= 5.33399963 3.72533321 、 <math id="M93"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">l</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207197&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207193&type= 5.41866684 3.72533321 皆以中间点 <math id="M94"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">c</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">r</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207202&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207199&type= 6.94266701 3.72533321 为目标点，同时并行向目标点扩展，而此时中间点 <math id="M95"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">c</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">r</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207209&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207205&type= 6.94266701 3.72533321 则分别以 <math id="M96"><mi>n</mi><mmultiscripts><mrow/><mprescripts/><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi></mrow><mrow/></mmultiscripts></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207213&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207210&type= 5.33399963 3.72533321 、 <math id="M97"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">l</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207222&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207217&type= 5.41866684 3.72533321 为目标点开始扩展，四棵树同时并行生长。在扩展中，当第一步扩展没有遇到障碍物，则在下一步将扩展步长增加 <math id="M98"><mi>ε</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207228&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207225&type= 1.43933344 2.79399991 ，当在扩展中遇到障碍物时，则将步长恢复至初始状态。 M_EXT函数为改进的随机树扩展函数，该函数主要目的是通过改进原始EXT扩展函数将原先步长固定的扩展方式改为步长随扩展状态的不同进行自适应调节，即当此时扩展状态为Advanced时，在下一次扩展中，算法将在原始步长基础上增加一个 <math id="M99"><mi>ε</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207247&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207242&type= 1.43933344 2.79399991 步长，以此类推。如果下次扩展中返回状态仍为Advanced，则在上次步长基础上继续增加 <math id="M100"><mi>ε</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207252&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207250&type= 1.43933344 2.79399991 ，若下次扩展中返回状态为Trapped，说明在扩展中随机树遇到障碍物，则首先将步长还原为原始步长，扩展至再次检测到障碍物。M_EXT函数如下： 2 M_EXT( <math id="M101"><mi>T</mi><mo>,</mo><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> ) 1 Step← <math id="M102"><mi>ε</mi></math> ; 2 <math id="M103"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">t</mi></mrow></msub></math> <math id="M104"><mo>←</mo></math> NearestNode( <math id="M105"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> , T ); 3 if NewNode( <math id="M106"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> , <math id="M107"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">t</mi></mrow></msub></math> , <math id="M108"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> ) 4 then 5 T . V <math id="M109"><mo>←</mo></math> <math id="M110"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> ; 6 T . E <math id="M111"><mo>←</mo></math> ( <math id="M112"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> , <math id="M113"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">t</mi></mrow></msub></math> ); 7 if dist( <math id="M114"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> , <math id="M115"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">t</mi></mrow></msub></math> ) < Step 8 then 9 <math id="M116"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi></mrow></msub></math> <math id="M117"><mo>←</mo></math> <math id="M118"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub></math> ; 10 Return Reached; 11 else 12 Step←Step + <math id="M119"><mi>ε</mi></math> ; 13 Return Advanced; 14 Step← <math id="M120"><mi>ε</mi></math> ; 15 Return Trapped; BU_DRRT⁃Con函数为DRRT⁃Connect算法建树过程，算法首先调用M_EXT函数，分别将从起始点 <math id="M121"><mi>n</mi><mmultiscripts><mrow/><mprescripts/><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi></mrow><mrow/></mmultiscripts></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207265&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207261&type= 5.33399963 3.72533321 开始的随机树 <math id="M122"><msub><mrow><mi>T</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207271&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207267&type= 3.72533321 3.72533321 和从目标点 <math id="M123"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">l</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207277&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207274&type= 5.41866684 3.72533321 开始的随机树 <math id="M124"><msub><mrow><mi>T</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207283&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207280&type= 3.72533321 3.72533321 向外扩展一步，生成 <math id="M125"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207291&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207287&type= 6.34999990 3.72533321 和 <math id="M126"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207297&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207294&type= 6.34999990 3.72533321 。此后，通过引入目标偏置策略，使算法以 <math id="M127"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">c</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">r</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207304&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207301&type= 6.94266701 3.72533321 为起点，分别以 <math id="M128"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207312&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207308&type= 6.34999990 3.72533321 和 <math id="M129"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207318&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207315&type= 6.34999990 3.72533321 为目标点，向外扩展随机树 <math id="M130"><msub><mrow><mi>T</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207324&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207321&type= 3.72533321 3.72533321 和 <math id="M131"><msub><mrow><mi>T</mi></mrow><mrow><mn mathvariant="normal">4</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207330&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207327&type= 3.72533321 3.72533321 ，并调用CON函数，将两树快速扩展，直至在扩展过程中遇到障碍后停止，通过SWAP函数交换扩展顺序，使得 <math id="M132"><msub><mrow><mi>T</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207335&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207333&type= 3.72533321 3.72533321 、 <math id="M133"><msub><mrow><mi>T</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207341&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207338&type= 3.72533321 3.72533321 以 <math id="M134"><msub><mrow><mi>T</mi></mrow><mrow><mn mathvariant="normal">3</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207347&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207345&type= 3.72533321 3.72533321 、 <math id="M135"><msub><mrow><mi>T</mi></mrow><mrow><mn mathvariant="normal">4</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207351&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=4207349&type= 3.72533321 3.72533321 停止点为目标点进行扩展。如此循环，直至四棵树连接，生成一条完整路径。 2 BU_DRRT⁃Con( <math id="M136"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi></mrow></msub><mo>,</mo><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">c</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">r</mi></mrow></msub><mo>,</mo><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">l</mi></mrow></msub></math> ) 1 <math id="M137"><msub><mrow><mi>T</mi></mrow><mrow><mn>1</mn></mrow></msub></math> .init( n init ); <math id="M138"><msub><mrow><mi>T</mi></mrow><mrow><mn>2</mn></mrow></msub><mo>.</mo><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi><mo stretchy="false">(</mo><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">l</mi></mrow></msub><mo stretchy="false">)</mo></math> ; <math id="M139"><msub><mrow><mi>T</mi></mrow><mrow><mn>3</mn></mrow></msub><mo>.</mo><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi><mo stretchy="false">(</mo><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">c</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">r</mi></mrow></msub><mo stretchy="false">)</mo></math> ; <math id="M140"><msub><mrow><mi>T</mi></mrow><mrow><mn>4</mn></mrow></msub><mo>.</mo><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">t</mi><mo stretchy="false">(</mo><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">c</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">r</mi></mrow></msub><mo stretchy="false">)</mo></math> ; 2 for k = 1 to K do 3 <math id="M141"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">d</mi></mrow></msub><mo>←</mo><mi mathvariant="normal">S</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">p</mi><mi mathvariant="normal">l</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">N</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">d</mi><mi mathvariant="normal">e</mi><mo stretchy="false">(</mo><mo stretchy="false">)</mo></math> ; 4 if ( M_EXT( <math id="M142"><msub><mrow><mi>T</mi></mrow><mrow><mn>1</mn></mrow></msub></math> , <math id="M143"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">c</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">r</mi></mrow></msub></math> )≠Trapped, M_EXT( <math id="M144"><msub><mrow><mi>T</mi></mrow><mrow><mn>2</mn></mrow></msub></math> , <math id="M145"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">c</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">r</mi></mrow></msub></math> )≠Trapped) 5 then 6 if ( CON( <math id="M146"><msub><mrow><mi>T</mi></mrow><mrow><mn>3</mn></mrow></msub></math> , <math id="M147"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi><mn>1</mn></mrow></msub></math> )=Reached, CON( <math id="M148"><msub><mrow><mi>T</mi></mrow><mrow><mn>4</mn></mrow></msub></math> , <math id="M149"><msub><mrow><mi>n</mi></mrow><mrow><mi mathvariant="normal">n</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">w</mi><mn>1</mn></mrow></msub></math> )=Reached) 7 then 8 Return PATH( <math id="M150"><msub><mrow><mi>T</mi></mrow><mrow><mn>1</mn></mrow></msub></math> <math id="M151"><mo>,</mo><msub><mrow><mi>T</mi></mrow><mrow><mn>2</mn></mrow></msub></math> ， <math id="M152"><msub><mrow><mi>T</mi></mrow><mrow><mn>3</mn></mrow></msub></math> <math id="M153"><mo>,</mo><msub><mrow><mi>T</mi></mrow><mrow><mn>4</mn></mrow></msub></math> ); 9 SWAP( <math id="M154"><msub><mrow><mi>T</mi></mrow><mrow><mn>1</mn></mrow></msub></math> , <math id="M155"><msub><mrow><mi>T</mi></mrow><mrow><mn>3</mn></mrow></msub></math> ) <math id="M156"><mo>,</mo></math> SWAP( <math id="M157"><msub><mrow><mi>T</mi></mrow><mrow><mn>2</mn></mrow></msub></math> , <math id="M158"><msub><mrow><mi>T</mi></mrow><mrow><mn>4</mn></mrow></msub></math> )； 10 Return Failed； 3　 仿真实验与分析 1 3.1　仿真实验 2 为了对DRRT⁃Connect算法进行性能比较，本实验对 图3 图3 所示的地图进行仿真，地图中空白区域代表无障碍物区，红色区域代表障碍物区。 仿真所使用的硬件平台为Intel Core（TM）i5-4200M 2.5GHz CPU，RAM 6GB，算法通过Python编程实现。 实验中，分别以RRT、RRT* ［ 19 19 ］ 和RRT⁃Connect作为改进算法的比较对象，通过比较证明DRRT⁃Connect性能的优越性。 图4 图4 为DRRT⁃Connect算法随机树探索过程，从 图4 图4 中可以看出，改进后的算法分别以起始点、目标点以及中间第三节点为起点同时生成四棵随机树，从而加快对未知区域的探索，以加速最终路径生成。 图5 图5 中（a）、（b）、（c）、（d）为DRRT⁃Connect算法与RRT、RRT*、RRT⁃Connect三种算法的仿真结果图。从 图5 图5 中可以看出，相比于其他三种算法，DRRT⁃Connect算法所生成的路径具有平滑性好且具有一定的方向性。该结果充分展示出通过四棵树生成的路径在应用到实际场景中时，具有较好的表现。 在实验中，分别对每种算法进行50次仿真验证， 图6 图6 所示为4种算法寻路时间对比图。从 图6 图6 中可以看出，DRRT⁃Connect算法在50次的仿真实验中耗时远远低于RRT与RRT*算法，相较于RRT⁃Connect算法而言，由于坐标精度原因，无法明显观测出速度的提升，DRRT⁃Connect算法平均路径生成时间将在下文分析。 图7 图7 所示为4种算法在生成路径中所需迭代次数示意图，由于自适应步长的加入，DRRT⁃Connect算法在迭代次数上相较于RRT⁃Connect算法也有较为明显优势。 表1 表1 所示为在50次实验中4种算法的迭代次数、路径长度与生成路径所花费时间的平均值，从数据中可以更为直观的看出，相比于RRT与RRT*算法，DRRT⁃Connect算法在迭代次数与路径生成时间上都有非常明显的优势；相比于RRT⁃Connect算法，DRRT⁃Connect算法在路径规划时间上只需要RRT⁃Connect算法的50%，在迭代次数上相比RRT⁃Connect算法降低了32.3%，大大提高了路径的生成效率。 算法 迭代次数 路径长度 生成时间/s RRT 3 591 912.34 246.42 RRT* 3 604 807.28 425.05 RRT⁃Connect 316 857.07 2.92 DRRT⁃Connect 214 851.72 1.46 3.2　性能分析 2 在上述实验结果中，改进后的DRRT⁃Connect算法在迭代次数与路径生成时间上的性能均明显优于改进算法，而在路径长度上相较于RRT*算法不具有明显优势。 迭代次数降低的原因归功于算法中引入的目标偏置策略，该策略使原始RRT算法中使用的随机采样扩展变为具有目标性的扩展方式，将原来每一步扩展都需要首先进行随机采样的方式变为以随机树停止位置作为目标点，直接使算法朝向目标点扩展一步，只有当向目标点扩展时遇到障碍物，才调用随机采样函数，从而使随机树摆脱障碍物，保证算法的概率完备性。 路径生成速度大幅提高的原因主要是DRRT⁃Connect算法将RRT⁃Connect算法的两棵随机树扩展方式变为四棵树通过扩展，大幅加速算法生成路径的效率；在无障碍空间中，算法调用自适应步长调节函数，该函数与目标偏置策略相结合，可以使随机树更高效的向目标点进行探索，达到提高路径生成效率的期望。 DRRT⁃Connect算法在生成路径长度上并未获得提升，主要原因在于DRRT⁃Connect算法以RRT⁃Connect算法为基础，并未考虑路径的最优性，而是以提升路径生成效率为目标。通过自适应步长调节函数，可以在一定程度上提高路径的平滑性，但并不能很大幅度降低路径长度。 4　 结 语 1 本文通过对RRT⁃Connect算法进行改进，提出了一种可以实现同时生成四棵树并且扩展中步长可自适应调节的DRRT⁃Connect算法，对改进后算法的原理及实现过程进行了详细分析，通过仿真实验结果，验证了改进后的算法相对于其他算法在路径生成效率及路径平滑性上都具有明显提升。 本文所提算法目前主要用于障碍物较少的简单环境下实现快速路径规划，下一步将尝试针对复杂环境下以及第三节点难以选取的情况进行改进，从而提高算法的泛化性，以便更好地满足工程实践的需要。 参考文献 李学鋆 . 基于UTMD的汽车自动驾驶的路径规划寻优算法 ［J］. 汽车安全与节能学报 ， 2018 ， 9 （ 4 ）： 449 - 455 . 基于UTMD的汽车自动驾驶的路径规划寻优算法 LI X Y . Path planning optimization-algorithm for a self-driving vehicle based on UTMD principle ［J］. Journal of Automotive Safety and Energy ， 2018 ， 9 （ 4 ）： 449 - 455 . DOI： 10.39 69/j.issn.1674-8484（Ch http://dx.doi.org/10.3969/j.issn.1674-8484（Ch ）. Path planning optimization-algorithm for a self-driving vehicle based on UTMD principle 周光明 ， 贾梦雷 ， 陈宗海 . 移动机器人未知环境自主探测的一种高效算法 ［J］. 上海交通大学学报 ， 2005 ， 39 （ 6 ）： 936 - 940 . DOI： 10.16183/j.cnki.jsjtu.2005.06. 022 http://dx.doi.org/10.16183/j.cnki.jsjtu.2005.06.022 . 移动机器人未知环境自主探测的一种高效算法 ZHOU G M ， JIA M L ， CHEN Z H . An efficient algorithm for mobile robot’s autonomous exploration in unknown environments ［J］. Journal of Shanghai Jiaotong University ， 2005 ， 39 （ 6 ）： 936 - 940 . DOI： 10.16183/j.cnki.jsjtu.2005.06. 022（Ch http://dx.doi.org/10.16183/j.cnki.jsjtu.2005.06.022（Ch ）. An efficient algorithm for mobile robot’s autonomous exploration in unknown environments CORTES L J ， JAILLET L ， SIMEON T . Molecular disassembly with RRT⁃like algorithms ［C］// IEEE International Conference on Robotics and Automation . Washington D C， IEEE ， 2007 ： 3301 – 3306 . DOI： 10. 1109/ROBOT.2007.363982 http://dx.doi.org/10.1109/ROBOT.2007.363982 . Molecular disassembly with RRT⁃like algorithms LIU Y ， BADLER N I . Real-time reach planning for animated characters using hardware acceleration ［C］// International Conference on Computer Animation and Social Agents . Washington D C ： IEEE ， 2003 ： 86 – 93 . DOI： 10.1109/CASA.2003.1199308 http://dx.doi.org/10.1109/CASA.2003.1199308 . Real-time reach planning for animated characters using hardware acceleration 张捍东 ， 陈阳 ， 吴玉秀 . 未知环境下移动机器人实时路径规划 ［J］. 计算机工程与应用 ， 2018 ， 54 （ 19 ）： 140 - 146 . DOI： 10.3778/j. issn.1002-8331.1706-0225 http://dx.doi.org/10.3778/j.issn.1002-8331.1706-0225 . 未知环境下移动机器人实时路径规划 ZHANG H D ， CHEN Y ， WU Y X . Real time path planning for mobile robot in unknown environment ［J］. Computer Engineering and Applications ， 2018 ， 54 （ 19 ）： 140 - 146 . DOI： 10. 3778/j. issn.1002-8331.1706-0225（Ch http://dx.doi.org/10.3778/j.issn.1002-8331.1706-0225（Ch ）. Real time path planning for mobile robot in unknown environment SINGH Y ， SHARMA S ， SUTTON R ， et al . Towards use of Dijkstra algorithm for optimal navigation of an unmanned surface vehicle in a real-time marine environment with results from artificial potential field ［J］// The International Journal on Marine Navigation and Safety of Sea Transportation ， 2018 ， 12 （ 1 ）： 125 - 131 . DOI： 10.12716/1001.12.01.14 http://dx.doi.org/10.12716/1001.12.01.14 . Towards use of Dijkstra algorithm for optimal navigation of an unmanned surface vehicle in a real-time marine environment with results from artificial potential field ANH V L ， VEERAJAGADHESWAR P ， VINU S ， et al . Modified a star algorithm for efficient coverage path planning in tetris inspired self-reconfigurable robot with integrated laser sensor ［J］. Sensors ， 2018 ， 18 （ 8 ）： 1 - 27 . DOI： 10.3390/s18082585 http://dx.doi.org/10.3390/s18082585 . Modified a star algorithm for efficient coverage path planning in tetris inspired self-reconfigurable robot with integrated laser sensor CAO Q ， HUANG Y ， ZHOU J . An evolutionary artificial potential field algorithm for dynamic path planning of mobile robot ［C］// IEEE/RSJ International Conference on Intelligent Robots and Systems . Washington D C ： IEEE ， 2007 ： 3331 - 3336 . DOI： 10.1109/IR-OS.2006.282508 http://dx.doi.org/10.1109/IR-OS.2006.282508 . An evolutionary artificial potential field algorithm for dynamic path planning of mobile robot 周凌云 ， 丁立新 ， 邹桢苹 . 多启发式信息蚁群优化算法求解取样送检路径规划问题 ［J］. 武汉大学学报（理学版） ， 2017 ， 63 （ 5 ）： 439 - 447 . DOI： 10.14188/j.1671-8836.2017.05.009 http://dx.doi.org/10.14188/j.1671-8836.2017.05.009 . 多启发式信息蚁群优化算法求解取样送检路径规划问题 ZHOU L Y ， DING L X ， ZOU Z P . Ant colony optimization with multi-heuristic information for sampling inspection path planning problem ［J］. Journal of Wuhan University （Natural Science Edition） ， 2017 ， 63 （ 5 ）： 439 - 447 . DOI： 10.14188/j.1671-8836.2017.05.009（Ch http://dx.doi.org/10.14188/j.1671-8836.2017.05.009（Ch ）. Ant colony optimization with multi-heuristic information for sampling inspection path planning problem WANG X ， ZHANG G ， ZHAO J ， et al . A modified membrane-inspired algorithm based on particle swarm optimization for mobile robot path planning ［J］. International Journal of Computers Communications and Control ， 2015 ， 10 ： 732 - 745 . DOI： 10.15837/ijccc. 2015.5.2030 http://dx.doi.org/10.15837/ijccc.2015.5.2030 . A modified membrane-inspired algorithm based on particle swarm optimization for mobile robot path planning MOHAMED E ， ALAA T ， ABOUL E H . Bezier curve based path planning in a dynamic field using modified genetic algorithm ［J］. Journal of Computational Science ， 2018 （ 25 ）： 339 - 350 . DOI： 10.1016/j.jocs.2017.08.004 http://dx.doi.org/10.1016/j.jocs.2017.08.004 . Bezier curve based path planning in a dynamic field using modified genetic algorithm KURDI M M ， DADYKIN A K ， ELZEIN I ， et al . Proposed system of artificial neural network for positioning and navigation of UAV-UGV ［C］// 2018 Electric Electronics ， Computer Science， Biomedical Engineerings&#x 1001 b 3 ; Meeting （EB BT）. Washington D C： IEEE ， 2018 ： 1 - 6 . DOI： 10.1109/EBBT.2018. 8391459 http://dx.doi.org/10.1109/EBBT.2018.8391459 Proposed system of artificial neural network for positioning and navigation of UAV-UGV LAVALLE S M ， KUFFNER J J . Randomized kinodynamic planning ［C］// Proceedings 1999 IEEE International Conference on Robotics and Automation . Washington D C ： IEEE ， 2001 ： 378 - 400 . DOI： 10.11 09/ROBOT.1999.770022 http://dx.doi.org/10.1109/ROBOT.1999.770022 . Randomized kinodynamic planning MATTHEW J ， ALEJANDRO P . Optimal bidirectional rapidly-exploring random trees ［J］. Robotics and Autonomous Systems ， 2015 （ 68 ）： 1 - 11 . DOI： 10.1016/j. robot. 201 5.02.007 http://dx.doi.org/10.1016/j.robot.2015.02.007 . Optimal bidirectional rapidly-exploring random trees ISLAM F ， NASIR J ， MALIK U ， et al . RRT*-Smart： Rapid convergence implementation of RRT* towards optimal solution ［C］// IEEE International Conference on Mechatronics and Automation . Washington D C ： IEEE ， 2012 ： 1651 - 1656 . DOI： 10.1109/ ICMA.2012. 6284384 http://dx.doi.org/10.1109/ICMA.2012.6284384 . RRT*-Smart： Rapid convergence implementation of RRT* towards optimal solution KUFFNER J J ， LAVALLE S M . RRT-connect： An efficient approach to single-query path planning ［C］// Proceedings 2000 IEEE International Conference on Robotics and Automation . Washington D C ： IEEE ， 2000 ： 995 - 1001 . DOI： 10.1109/ROBOT. 2000.844730 http://dx.doi.org/10.1109/ROBOT.2000.844730 . RRT-connect： An efficient approach to single-query path planning OLZHASl A ， HUSEYIN A V . A novel RRT*-Based algorithm for motion planning in dynamic environments ［C］// IEEE International Conference on Mechatronics and Automation . Washington D C ： IEEE ， 2017 ： 1416 - 1421 . DOI： 10.1109/ICM A.2017.8016024 http://dx.doi.org/10.1109/ICMA.2017.8016024 . A novel RRT*-Based algorithm for motion planning in dynamic environments AN B ， KIM J ， PARK F C . An adaptive stepsize RRT planning algorithm for open-chain robots ［J］. IEEE Robotics and Automation Letters ， 2018 ， 3 （ 1 ）： 312 - 319 . DOI： 10.11 09/lra.2017.2745542 http://dx.doi.org/10.1109/lra.2017.2745542 . An adaptive stepsize RRT planning algorithm for open-chain robots KARAMAN S ， FRAZZOLI E . Sampling based algorithms for optimal motion planning ［J］. The International Journal of Robotics Research ， 2011 ， 30 （ 7 ）： 846 - 894 . DOI： 10.11 77/0278364911406761 http://dx.doi.org/10.1177/0278364911406761 . Sampling based algorithms for optimal motion planning 10.14188/j.1671-8836.2019.03.008 TP 242.6 A 1671-8836（2019）03-0283-07 WANG Kun,HUANG Bo,ZENG Guohui, et al .Faster Path Planning Based on Improved RRT⁃Connect Algorithm [J]. J Wuhan Univ （ Nat Sci Ed ）,2019,65(3):283-289. DOI: 10.14188/j.1671-8836.2019.03.008 http://dx.doi.org/10.14188/j.1671-8836.2019.03.008 (Ch). WANG Kun,HUANG Bo,ZENG Guohui,et al.Faster Path Planning Based on Improved RRT⁃Connect Algorithm [J].J Wuhan Univ（Nat Sci Ed）,2019,65(3):283-289. DOI:10.14188/j.1671-8836.2019.03.008(Ch). 王坤,黄勃,曾国辉, 等.基于改进RRT⁃Connect的快速路径规划算法[J].武汉大学学报（理学版）,2019,65(3):283-289. DOI:10.14188/j.1671-8836.2019.03.008 王坤,黄勃,曾国辉, 等.基于改进RRT⁃Connect的快速路径规划算法[J].武汉大学学报（理学版）,2019,65(3):283-289. DOI:10.14188/j.1671-8836.2019.03.008 国家自然科学基金 （61603242）；上海工程技术大学机械电子工程学科建设项目（2018xk⁃A⁃03）；江西省经济犯罪侦查与防控技术协同创新中心开放课题（JXJZXTCX⁃030） 2018-07-18 2019-06-24 2020-11-28 武汉大学学报（理学版） 3 65