一种基于双向搜索RRT*的AUV回坞路径规划方法

一种基于双向搜索rrt*的auv回坞路径规划方法
技术领域
1.本发明属于航海技术领域，涉及自主水下机器人(autonomous underwater vehicle，auv)的一种路径规划方法。具体为将回坞所受到的角度约束加入到扩展树生成的过程中，然后引入双向扩展的思想加速路径生成，在使用rrt*方法生成可行路径之后，加入smart原则对路径进行优化，确保算法所生成路径可以使auv顺利进入回收笼。


背景技术：

2.近年来，自主驾驶汽车、无人机、工业叉车等移动机器人逐渐在大众视野中频繁出现，针对这些移动机器人的操作问题，不可避免要谈论到的一项技术就是路径规划问题，路径规划算法的目的是找到一条从初始位置到目标位置的无碰撞可行路径。auv在环境监测、水下侦察、深海采样等领域可以发挥重要作用。但是由于目前电池技术还没有突破性进展，导致auv自身携带能源非常有限，所以在需要长时间水下工作时，需要将auv进行对接回收。
3.然而使用母船对齐回收会使auv作业范围受限，且回收成本较大，为了节约成本并提高作业效率，auv自主回坞路径规划控制成为了海洋工程领域重要的研究问题之一。在本发明中，水下航行器的路径规划目的在于规划出一条能避开障碍物的从起点到回收笼的路径，并尽可能地使其满足路径最短、能量消耗最少等附加准则。
4.在众多路径规划的算法中，基于随机采样的路径规划方法因其无需考虑复杂约束的优势被广泛应用，其中比较有代表性的有两种方法：概率路线图法和快速拓展随机树法。这两种算法十分类似，都是通过在已知的地图上随机采样来建立无向图，通过遍历搜索的方法寻找出一条相对最优的路径。不同点在于，前者是在搜索可行路径之前通过采样在地图上构建出完整的无向图，再进行图中优化路径的搜索；而后者则是从特定的某个点作为起始点，在搜索的同时进行采样和构建地图。经典的rrt*算法对路径的规划是一个渐进最优的过程，通过迭代次数的增加可以将路径代价不断优化，但是与此同时，收敛速率会随着迭代次数增加而大幅变慢。


技术实现要素：

5.要解决的技术问题
6.为了避免现有技术的不足之处，本发明提供一种基于双向搜索rrt*的auv回坞路径规划方法。本发明将采用rrt*算法作为基础，设计改进后的路径规划方法并完成仿真验证。
7.技术方案
8.一种基于双向搜索rrt*的auv回坞路径规划方法，其特征在于步骤如下：
9.步骤1：以初始状态和目标状态分别作为两端随机树的起点，记作x
init
和x
goal
，由这两个根节点分别开始进行搜索树t.a和搜索树t.b的扩展过程，t.a与t.b两颗搜索树是同时进行搜索的；
10.步骤2：进行以下步骤的循环，在每一次循环中完成随机树的扩展；
11.1)在状态空间内随机采样，采样点记作x
rand
；
12.2)遍历随机树上的每一个节点，寻找到距离x
rand
最近的节点记作x
near
；
13.3)然后以x
rand
和x
near
两个节点构建状态方程的输入u，并将u作为步长沿这两个节点的连线方向进行扩展，得到新节点x
new
；
14.4)如果是从x
init
生成的随机树t.a，则直接进行下一步；如果是从x
goal
扩展的随机树t.b，则需要对x
new
进行判断；为了确保两端生成的随机树可以尽快对接，需要对从目标点出发的随机树节点进行限制，确保生成的随机节点x
new
与目标点之间连线角度在一定范围r之内，若不满足，则返回3)重新进行x
new
的选取；
15.5)在将新节点x
new
插入随机数序列之前，首先通过近邻搜索为其寻找最佳父节点并进行重新布线，然后将更新后的父节点记为x
p
，可以使路径代价更小；
16.6)在x
p
和x
new
之间生成一条边，然后判断新连接的边是否会与障碍物碰撞，如果有碰撞则对x
p
以及x
new
进行舍弃，之后的循环中采用距离次之的新节点作为x
near
，然后按照前两步的方法生成新的随机节点x
new
，继续进行判断直到得到可行点，将新节点加入到随机树中，两节点之间生成的边界也存入边集中；
17.步骤3：在不断选择出可行的新节点与不断更新边集之后，直到从两端出发的两个随机树出现相交，表明规划完成，返回随机树t得到可行路径；然后对可行路径进行可见性优化，将路径中互相可见的节点相连，选择出路径代价最小且与障碍物无碰撞的轨迹作为最终路径；
18.步骤4：进行偏置性采样操作，根据步骤3中采取的可见性优化过程，有大量节点将被抛弃，使用这些节点所提供的障碍物顶点信息，在障碍物周围以信标节点为中心产生一定数量的采样点，利用偏置性半径将障碍物顶点附近的路径进一步优化。
19.步骤4中的数量取为50。
20.一种计算机系统，其特征在于包括：一个或多个处理器，计算机可读存储介质，用于存储一个或多个程序，其中，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现上述的方法。
21.一种计算机可读存储介质，其特征在于存储有计算机可执行指令，所述指令在被执行时用于实现上述的方法。
22.有益效果
23.本发明提供的一种基于双向搜索rrt*的auv回坞路径规划方法，与现有的rrt*算法相比，有益效果如下：
24.1.搜索速率快，路径收敛时间短
25.本发明中，采用从路径起点与终点同时进行扩展随机树的方法，与传统rrt*算法的随机树生成过程相比，无用节点的产生数量更少，收敛速度更快，可以快速形成可行路径。
26.2.路径代价小，避障特性更优
27.本发明中提出的smart优化原则，可以在生成可行路径之后，利用路径节点之间互相可见的原理对轨迹进行可见性优化，大幅减少了路径代价；而后偏置性采样将路径在障碍物顶点附近进行了进一步优化。
附图说明
28.附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
29.图1新节点扩展示意图；
30.图2近邻搜索以及重新选择父节点示意图；
31.图3可见性优化示意图；
32.图4双向搜索的方向约束示意图；
33.图5仿真环境示意图；
34.图6-1a单向搜索扩展随机树结果图；
35.图6-1b单向搜索可行路径结果图；
36.图6-2a无约束双向搜索扩展随机树结果图；
37.图6-2b无约束双向搜索可行路径结果图；
38.图6-3a带终点角度约束双向搜索扩展随机树结果图；
39.图6-3b带终点角度约束双向搜索可行路径结果图；
40.图6-3c可见性优化后路径结果图；
41.图6-3d偏置性采样后路径结果图。
具体实施方式
42.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图和实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
43.针对水下航行器回收过程的路径规划，以快速扩展随机树算法为基础工具算法进行展开研究。在rrt*算法生成路径的基础上，首先将回坞任务终点的角度约束加入随机树生成过程，然后引入双向搜索随机树的思想，快速生成可行路径；之后加入可见性优化和偏置性采样两部分优化方法，使得路径可行路径进一步优化，快速得到路径最优解，后文将这一改进称为smart原则。本发明提出的受约束的bi-rrt*-smart算法能在满足约束条件的情况下，快速规划出一条避开障碍物的路径，完成auv的回收路径规划任务，并且能以较快的速度收敛到最优解。
44.本发明包括以下步骤：
45.(1)假设环境在1km
×
1km的湖海区域内，有大大小小的岛屿或其他需要避开的礁石作为障碍物。在仿真环境中，湖海表现为像素500
×
500的矩形区域，在此区域内随机设置不规则形状的障碍物代表需要避开的岛屿或礁石，障碍物的设置示意图1所示。
46.黑色实心形状为障碍物区域，左上角为起始点的位置，右下角是目标点的位置，在仿真实验中，起始点和目标点的位置坐标分别设置为(10,10)和(490,490)，设置任务终点处角度约束为r∈[-7*pi/12,-6*pi/12]，终点距离约束为d＝200m，表明此角度约束仅对任务终点前直线距离200m处有效，后续将在仿真实验中分别对比有无终点约束情况以及有无smart优化情况。
[0047]
(2)设置扩展步长为u＝5，从起始点x
init
以及目标点x
goal
同时进行随机树的扩展，
经过以下步骤进行随机树的扩展更新：
[0048]
1)在状态空间内随机采样，采样点记作x
rand
；
[0049]
2)遍历随机树上每一个节点，寻找到距离x
rand
最近的节点记作x
near
；
[0050]
3)找到x
near
之后，由x
near
为出发点向x
rand
和x
near
两点之间连线方向做一个步长u的扩展，扩展之后得到新节点x
new
；
[0051]
4)在每次生成新的节点x
new
之后，通过近邻搜索以及重新布线步骤对其父节点进行优化重新分配。具体为在以x
new
为圆心，r为半径的区域内，以随机树节点之间连接的路径长度最短为目标，重新进行布线生成随机树，r的具体计算如下
[0052][0053]
其中，μ为基于环境的规划常数，n为搜索可行路径的迭代次数，经过以上对父节点的重新选择之后，可以使生成的路径代价更小。
[0054]
5)如果是从x
init
生成的随机树t.a，则直接进行下一步，如果是从x
goal
扩展的随机树t.b，则需要对x
new
进行判断。为了确保两端生成的随机树可以尽快对接，需要对从目标点出发的随机树节点进行限制，确保距离终点150m范围内生成的随机节点x
new
与目标点之间连线角度在一定范围r之内，若不满足，则返回第(2)步重新进行x
new
的选取；
[0055]
6)根据新生成的节点与其父节点之间连线会否与障碍物相交来判断局部路径可行性，如果不可行，则返回(2)处进行扩展点的重新选取，如果可行，则进行以下优化步骤。
[0056]
(3)通过从起始点x
init
以及目标点x
goal
同时进行随机节点的更新，双向随机树将迅速扩展并连接，得到可行路径之后，对随机树t.a与随机树t.b合成的t进行smart原则优化。
[0057]
1)进行可见性优化。具体为在得到可行路径之后，路径中各个节点是相互可见的，每次迭代从目标点向初始点的方向移动，检查每个节点与父节点连接的线段，如果此线段不会与障碍物相撞，则表明找到了更短的路径，整个过程不断重复直到最优；
[0058]
2)偏置性采样优化。路径规划的最优解考虑为以障碍物顶点作为拐点，根据上一步中采取的可见性优化过程，有大量节点将被抛弃，使用这些节点所提供的障碍物顶点信息，在障碍物周围以信标节点为中心产生一定数量(n＝50)的采样点，选取合适的偏置性半径rb，将障碍物顶点附近的路径进一步优化，算法迭代过程中将不断比较新路径与上一次路径成本，不断选取更接近障碍物顶点的信标节点进行优化，直到完成迭代。
[0059]
(4)经过一系列采样点的更新以及路径优化之后，可以快速得到连接终点与起始点的曲线路径，此路径满足避障特性，并且符合auv进入回收笼所需的角度要求，路径在距离终点一定范围内转向回收笼角度正前方，而且经过smart原则优化之后，路径代价将再次优化降低，将此路径返回至auv，即可通过控制系统完成回坞任务。
[0060]
根据附图6-1a、图6-1b与图6-2a、图6-2b的对比可知，本发明提出的双向搜索rrt*-smart路径规划算法与典型rrt*算法相比可以显著缩短搜索时间，通过扩展更少的采样点，在满足规避障碍物的情况下短时间收敛得到auv回坞路径，避免了太多无用节点的生成，极大提高了搜索效率。
[0061]
表1仿真结果对照表
[0062][0063]
附图6-3与6-2的对比分别为有无终点约束的仿真结果，通过图6-3可以看出，本发明提出的双向搜索rrt*-smart路径规划算法可以迅速将随机树收敛得到可行路径。不论是否增加终点约束条件，仿真结果效率都比较高，因此在附加终点角度约束的条件下进行最优路径的搜索，可以有效满足本发明针对的auv与回收笼自主对接的任务需求。
[0064]
从图6-3b、图6-3c与图6-3d三个图的对比中可以看出，针对双向rrt*-smart算法生成的可行路径经过可见性优化之后，路径的分段大大减少，且更为平滑，缩短了路径长度且有利于auv进行跟踪控制；而偏置性采样可以使得经过可见性优化之后的路径的拐点基本分布在障碍物顶点处，进一步将可行路径进行了优化，缩小了路径代价。由仿真结果可以对比得知本方法提出的约束条件下的双向rrt*-smart路径规划方法可以迅速生成auv回坞的最优可行路径。
[0065]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明公开的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。