面向水下未知空间探测的改进RRT路径搜索算法

张季然 陈德山 李廷文 吕洁印 汪 洋*

(武汉理工大学国家水运安全工程技术研究中心1) 武汉 430063) (武汉理工大学交通与物流工程学院2) 武汉 430063) (长江水上交通监测与应急处置中心3) 武汉 430014) (深圳中集智能科技有限公司4) 深圳 518057)

0 引 言

使用水下机器人探测水下未知空间,是辅助工程人员获取水下未知空间特征的一种重要手段.在事前不掌握全局空间信息的情形下,水下机器人需要通过一定策略的巡航,以便了解水下空间的几何形态.常用的水下机器人包括ROV(remote operated vehicle)和AUV(autonomous underwater vehicle),均可携带声呐、光学摄像机、超短基线定位系统等设备用于水下探测和导航[1],操作人员能够实时地控制水下机器人的运动.

相对于陆地上的路径搜索,水下环境具有更高的空间维度,加大了水下探测复杂度.水下环境中摄像头能见度低,探测范围极其有限,无法使用深度摄像头或激光雷达等光学传感器.受水中杂质与波流影响,水下三维声呐的成像会产生许多额外噪点,传统的GPS定位系统在水中无法使用[2].

针对水下路径规划领域的研究,张汝波等[3]针对水下复杂环境中可能存在多个不同威胁等级的障碍物,提出了一种改进蚁群算法合理规划水下机器人最优航行路径.朱大奇等[4]提出了一种基于生物启发的集成自组织映射算法,用于自主水下机器人(AUV)系统在三维水下避障环境下的任务分配和路径规划.Vidal等[5]提出一种新的自主水下机器人三维探测和覆盖方法,并能够引导机器人,在一次探索中获得对占用数据和光学数据的完全发现和覆盖.Shen等[6]提出了一种利用曲率重构的水下机器人在线规划未知环境覆盖路径的算法.Hernández等[7]提出了一种在未知环境下AUV在线规划无碰撞路径的框架,该框架允许水下机器人逐步构建周围环境地图,并考虑机器人的运动约束来规划可行的三维路径.Li等[8]提出一种水下自主航行器最优路径规划方法进行海底地形匹配导航,以避开这些区域.

目前的研究大多数是在水下空间结构的先验信息较充分的条件下进行,从二维路径搜索逐渐发展至三维路径搜索,而在水下空间未知的水域中进行路径探索仍然涉及较少.为此,文中提出一种面向水下未知空间探测的混合RRT搜索算法.在缺乏环境先验信息的水下空间中,通过水下机器人自身携带的三维声呐系统获得当前位置的环境信息,在搜索过程中按照局部搜索与全局搜索两个尺度进行计算,提升水下未知空间中路径搜索和规划的效率,并通过真实环境实验中测试了提出算法的性能.

1 模型感知及问题描述

1.1 声呐数据模型

水下未知空间的路径搜索依赖于水下机器人的自主探索和绘制[9].已知水下机器人的初始位置状态与姿态信息,对整个水下未知空间进行路径搜索,寻找出未知空间内部起始点与目标点之间满足约束且无碰撞的可行路径,并且尽可能高效完成作业任务.在水下机器人进行路径搜索时必须考虑到水下复杂的空间环境:①水下场景通常具有很大的规模尺寸;②水下可能存在狭窄空间,机器人的可穿越空间由于这些空间从而受到限制;③机器人容易出现在局部环境中反复进行路径搜索的问题,导致路径搜索耗时长,效率低[10].

对采集环境信息的声呐进行简化,定义dmax为三维声呐传感器能探测到的最大有效范围,扫测范围为声呐正前方-45°～45°,声呐模型示意图见图1.

图1 声呐模型示意图

满足以下条件的目标可被实验声呐探测到.

(1)

式中:(x0,y0,z0)假设为水下机器人坐标;(x,y,z)为目标坐标;R为探测范围,有0≤R≤dmax;θ为水平扫测半径,φ为垂直扫测半径,有-π/4≤θ≤π/4,0≤φ≤π/4.

1.2 问题描述

将水下三维未知空间内的搜索区域设置为Lx×Ly×Lz的长方体,对目标区域进行栅格化,将其均分为M×N×K的三维立方网格,工作空间为M.工作空间分别由自由空间Mfree、已占用空间Mocc和未知空间Mun构成.水下机器人经过的栅格区域为已占用空间Mocc,未经过的栅格区域为未知空间Mun,可通行区域为自由空间Mfree.在任务区域随机设置Ni个静态子目标点Gi,要求水下机器人在水下未知空间寻找到所有子目标点,并规划出起始点与最终目标点之间的最优路径.

水下机器人的初始位置设为Xinit,终点位置设为Xgoal.将水下的路径搜索问题定义为在水下未知空间中,在起点至终点之间寻找一条可通行路径σ:[0,T]→M,有σ(0)=Xinit,σ(T)=Xgoal,所有的子目标点σ(t)∈Gi(t),∀t∈[0,T].水下机器人从起始点Xinit出发,通过三维声呐传感器以Xinit为圆心,R为半径探测水下机器人周围的未知空间Mun.最优路径规划问题被定义为对给定成本函数c的可行轨迹σ*的最小化搜索,通过子目标点Gi(t)将Xinit至Xgoal联系起来.

(2)

为解决上述问题,按照全局路径和局部路径的最优试探开展同步计算.在局部路径计算层面基于周边dmax范围探测数据,结合前沿点信息进行小尺度路径搜索寻找子目标点σ(t)∈Gi(t);而在全局路径计算层面进行粗粒度的路径分支决策,并将已选分支的边缘信号反馈给局部路径的计算,实现水下未知空间的路径探索.

2 RRT路径搜索算法

2.1 RRT算法基本原理

快速扩展随机树(rapidly exploring random tree)算法在陌生高维环境中能快速的导向空白区域,快速的找出一条可通行的路径.RRT算法的路径搜索过程类似于树枝的生长、扩散过程[11].在未知空间内进行探索时,以状态空间中的一个初始点作为根节点,通过随机采样增加叶节点的方式,生成一个快速扩展随机树.

在水下机器人进行环境探测时,以起始点Xstart作为根节点,通过在工作空间M中随机采样得到随机点Xrand,然后从当前的随机树中寻找与Xrand点最近的叶节点Xnearest点,若Xnearest点和Xrand点之间无碰撞,则将Xrand增加为新的叶Xnew,如此循环,将搜索导向未知空白区域,直到叶节点中包含目标点Xgoal或者在目标点所在的区域内,则完成搜索,生成随机搜索树.RRT算法的扩展过程见图2.

图2 RRT算法的扩展过程

2.2 RRT*算法

RRT*算法是在基本RRT算法上进行改进的,与基本RRT算法相比,RRT*算法在RRT基础上,改进了原有的节点选择函数,新增了父节点优化的过程[12].该算法是渐近优化的,随着迭代次数的增加,使得路径规划的结果能在一定程度上收敛至当前随机树下的最优解[13].RRT*算法和重布线过程见图3～4.

图3 RRT*算法示意图

图4 重布线过程

3 水下实时路径探索算法设计

为提高路线搜索效率,水下机器人需要满足以下要求:①提高水下机器人对整体环境的了解程度;②减少水下机器人对区域重复搜索;③减少水下机器人反向搜索.综合以上三方面因素,提出了一种混合RRT搜索算法,具体思路为:将探索路径规划问题重新划分为两个分叉阶段,即负责在当前机器人姿态周围的空间内进行高效探索的局部搜索阶段,利用RRT*算法对局部探测区域进行路径规划;当机器人局部探索完成之后,将机器人重新定位回全局空间内部的全局搜索阶段,利用RRT算法进行全局搜索并作为局部搜索阶段的补充,将全局探测前沿点信息传递给局部搜索阶段,更好的对局部区域进行路径规划,全局搜索阶段中记录了机器人当前时刻在水下的位置信息以及已探索空间的区域信息,避免机器人陷入反复进行局部探索的处境中.在探测过程中,将局部探测水下空间感知信息按时间顺序逐步添加至全局水下空间感知信息中.

3.1 混合RRT搜索算法

混合RRT搜索算法是基于RRT路径规划算法所改进的搜索算法,基本RRT算法对于未知区域有着强烈的倾向.在混合RRT搜索算法中,通过三维声呐获取水下空间感知信息,获取环境边界区域Mb.在全局路径计算层面,利用RRT算法进行粗粒度的路径分支决策,并将已选分支的边缘信号反馈给局部路径的计算,再通过RRT*算法进行微局部路径搜索并更新水下空间感知信息,实现水下空间路径搜索.混合RRT搜索算法的框架图见图5.

3.2 局部搜索算法

局部搜索算法是将传感器可探测环境作为当前窗口,在该窗口内进行路径规划,在其中搜寻最优子目标点Xtemp,探测起始位置到子目标点的之间的可行区域,规划出当前窗口下的局部路径,并将所选取的最优子目标点更新为下一窗口的起始点[14].三种不同情形子目标点的选取策略见图6.

图6 三种不同情形子目标点的选取策略

局部搜索算法的探测流程如下:初始化起始点Xinit,水下机器人搭载的三维声呐传感器的扫描半径为r,扫描区域为0°～90°,见图6a).局部搜索算法生成扩展树的节点在RRT中通过树的分支(边)相互连接,树上的每一个点都是一个顶点,储存在顶点集合V中.RRT中树的一个分支为一条边,每条边根据两个连通点的空间坐标存储在边集合E中,顶点集合与边集合共同组成了图像G=(V,E).在每一次迭代中随机在初始点Xinit周围取一个点作为采样点,记为Xrand,通过NearestVertex公式寻找到树中距离当前采样点Xrand最近的节点,记为Xnearest,NearestVertex公式定义为

NearestVertex(x)=argmin‖x-v‖

(3)

式中:x为环境内自由空间内的任意一点,v∈V.以Xnearest节点为中心,η为生长半径Xrand方向进行生长,在半径另一端生成的节点记为Xnew.检验Xnew节点是否位于未知区域内或者Xnew与以Xnearest之间的线段上的任意一点是否位于未知区域内.若上述任一条件成立,则认为Xnew节点为局部探测的边界点.如果Xnew或Xnew与Xnearest之间的点都在已知区域内,则此次迭代尚未找到未知空间边界点,把Xnew作为一个顶点加入到顶点集合中,将他们之间的连接线作为树枝加入到边集合中,通过不断迭代去对未知空间进行路径搜索.考虑局部搜索算法是在微局部区域进行路径搜索,单个区域最大迭代次数上限设置为200次.

3.3 全局搜索算法

全局搜索算法在整个路径搜索期间(直到水下空间感知信息被完全探索完毕)树的起始点不会重置并一直保持增长.全局搜索算法是局部搜索算法的补充,局部搜索算法只能在机器人当前扫描的区域进行探测,在远离机器人的位置探测性能不佳,可能会错失探索水下空间感知信息上的小角落,此时通过全局搜索算法可确保在远离机器人当前位置的未知区域可以被探测.

RRT算法扩展方式较为平均,按照随机采样的方式进行新节点的采样和生成.在反馈子目标点给局部搜索算法的过程中,为了减少局部搜索过程中子目标点选取的随机性,采用启发式搜索策略,使得子目标点的选取具有一定趋势偏向于优良选取结果,从而提高路径搜索效率.对于机器人扫测范围内任意子目标点x,有估计函数:

f(x)=p(x)+q(x)

(4)

式中:f(x)为机器人起始点到子目标点距离的估计函数;p(x)为状态空间中从起始点到扫测边界点距离的实际代价值;q(x)为状态空间中从起始点到子目标点距离的最佳代价估计值.在选择子目标点的过程中,选取多个子目标点作为候选点,并分别计算候选子目标点Xtemp到起始点的代价估计值.由于在机器人扫测范围内,p(x)即声呐扫测半径r,因此比较不同的候选点的估计函数f(xtemp),即比较q(xtemp).

q(xtemp)=w1·D(xstart,xtemp)-w2·D(hstart,htemp)

(5)

式中:D(xstart,xtemp)为起始点与子目标点间的欧式距离;D(hstart,htemp)为起始点与子目标点在Z轴方向上的距离;w1与w2分别为两个距离的权重系数,为保证机器人行驶路线在水平方向上的平缓性,w1取值0.7,w2取值0.3.则估计函数为.

(6)

当D(xstart,xtemp)>r时,将此子目标点Xtemp从候选点中排除.

混合RRT搜索算法通过局部搜索与全局搜索相结合,其伪代码见表1.

表1 混合RRT搜索算法伪代码

4 计算仿真与实物场景测试

4.1 仿真分析

设计了混合RRT搜索算法与RRT算法的对比仿真实验,水下未知空间环境见图7.任务空间大小为600 m×600 m×300 m,起始点坐标为(50,50,150),搜索目标点坐标为(500,500,150),随机树的扩展步长为5 m,声呐探测最大半径设为80 m.起点为图7左侧圆圈,搜索目标点为图9右侧圆圈.水下障碍物模型用五个圆柱体进行代替,将水下机器人与搜索目标点视为质点,为了防止实际环境下水下机器人与障碍物进行碰撞,将水下机器人与障碍物的最短距离设为4 m.

图7 水下未知空间

混合RRT搜索算法搜索过程见图8.

图8 混合RRT搜索算法搜索图

由图8可知:细线为全局搜索算法生成的扩展树,黑色粗线为局部路径搜索算法生成的扩展树,中间线段为水下机器人实际行走的路线,见图9.

图9 水下机器人航行路线

混合RRT搜索算法搜索过程的俯视图与水下机器人航行路线的俯视图分别见图10～11.

图10 混合RRT搜索算法搜索过程俯视图

图11 水下机器人航行路线俯视图

图12为RRT算法规划过程的三维视角与二维俯视图视角,采用相同的起点与终点坐标,灰色线段为最终路径.

图12 RRT算法路线图

图13为RRT*算法规划过程的三维视角与二维俯视图视角,黑色线段为搜索过程,灰色线段为最终路径.

图13 RRT*算法路线图

将RRT算法、RRT*算法与混合RRT搜索算法的路径长度、寻路时间、迭代次数以及有效顶点个数等数据进行30次实验对比,见图14.由于RRT*算法为渐进最优,所以将其迭代次数统一设置为与RRT算法迭代次数平均值相近次数500次.

图14 各参数对比图

表2为30次仿真实验中各算法路径长度、寻路时间、迭代次数与有效顶点个数的平均值.由各对比图与数据表可知:在各项指标对比上,混合RRT搜索算法皆有良好的表现.与RRT算法相比,路径长度降低了16.4%,寻路时间缩短了68.5%,迭代次数降低了84.1%,顶点个数减少了90.8%;与RRT*算法相比,路径长度降低了11.4%,寻路时间缩短了19.9%,迭代次数降低了84.1%,顶点个数减少了72.4%.由此可见:混合RRT搜索算法在路径探索上具有明显优势,生成路径最短且符合航行器运动学模型,可应用于水下未知空间的航行器路径探索.

表2 仿真结果对比图

4.2 实验验证

实验所用数据由美国某公司研制的BV5000型号[15]三维扫描声呐系统采集获取.该系统扫描区间可分为垂直面以及水平面,其中在系统垂直面为单独扫描扇形区域,系统发射256条波束,每条波束之间的角度间隔为0.18°.在完成依次垂直面扫描后,通过云台水平面旋转,即可对三维空间进行扫描并构建,BV5000声呐系统的水平面探测方向可达360°,因此其探测范围为一定空间范围内的柱状区域.BV5000波束频率为1.35 MHz,探测空间范围最大为30 m,点云图精度最小可达1.2 cm.由于声波在水下长距离传播的特性,该系统可在浑浊水域作业.水下机器人无水下环境的先验信息,通过搭载的BV5000声呐不断获取水下环境信息,实验水池大小为25 m×25 m×5 m,所有障碍物皆为静态障碍物,包括水下管道、模拟舱室与翻扣沉船.

设置水下机器人的起始位置(-1,2.5,0),设立搜索目标点位置(13.98,2.24,0.94),根据搭载的BV5000声呐完成水下未知空间探测实验.使用混合RRT搜索算法的局部搜索探测路径图与路径搜索图见图15,灰色线段为水下机器人探测路线.

图15 局部路径搜索图

全局空间的路径搜索见图16,水下机器人在实验水池内完成了从起点到搜索目标的之间的空间探测与路径搜索,实验结果证明,混合RRT搜索算法在水下机器人的路径规划上具有可行性,并能根据实际环境信息规划出无碰撞路径,使水下机器人能够快速高效的到达搜索目标点.

图16 全局路线搜索图与全局路线图

5 结 束 语

文中提出一种基于水下未知空间探测的混合RRT搜索算法,利用RRT算法快速性的特点及其趋于未知空间延展的特性对水下未知空间进行路径搜索.将全局路径规划细分为一步步的局部路径规划,利用局部搜索阶段与全局搜索阶段相结合,将所提混合RRT搜索算法应用到水下未知空间.最后通过仿真实验与真实环境实验验证了算法的有效性,后续研究将在实际水域内进行验证与优化.