AUV避障路径动态快速规划算法研究

苏祥宇,何 燕

(青岛科技大学机电学院,山东 青岛 266061)

0 引言

近年来,自主式水下机器人(autonomous underwater vehicle,AUV)作为水下特种工作的智能装备,因其在海洋地质考察、海底石油勘探、海底打捞与海底管道修复等领域的应用而引起了广泛关注[1-4]。为克服复杂环境中不确定障碍物对AUV航行产生的威胁,AUV需要实时进行避障路径规划确保航行安全。

常见的避障路径规划方法有人工势场法[5-6]、模糊逻辑法[7-8]和神经网络法[9-11]等。但人工势场法多用于静态环境;模糊逻辑法输入量增多时,推理规则和模糊控制规则表会急剧膨胀,导致避障算法运算量剧增;人工神经网络法需要大量样本数据进行训练,且容易陷入局部最优。

为降低避障算法运算量,提高AUV动态避障实时性,本文提出一种基于最小安全会遇距离的避障路径动态快速规划算法。

1 避障路径动态快速规划

船舶避障过程通常可以分为障碍物探测、碰撞风险评估和避障路径规划3个阶段。本文假定已通过声呐等传感器完成对障碍物的探测,重点研究碰撞风险评估和避障路径规划问题。碰撞风险评估包括碰撞危险判断与避障时机预估。避障路径规划是根据会遇态势,实时规划避障路径。

1.1 碰撞风险评估

1.1.1 碰撞要素

确定船舶是否存在碰撞危险的最根本的因素是两船会遇时的最小会遇距离(dCPA)和两船到达最小会遇距离时的时间(tCPA)。dCPA、tCPA定义如图1所示,本船位于A点,船速为VA,目标船位于B点,船速为VB,目标船对于本船的相对速度为VAB,过B点作速度VAB的平行线BC,则BC即为目标船与本船的相对运动航迹线,过A点作AC⊥BC于C点,则有:

图1 dCPA、tCPA定义

dCPA=|AC|

(1)

tCPA=|BC|/|VB-VA|

(2)

1.1.2 碰撞危险判断

AUV搭载有声呐和惯性导航设备等传感器,借助AUV装载的传感器可以获取AUV和目标船的位置和速度信息,当AUV检测到目标船时,计算AUV与目标船的会遇参数。若检测到多个具有碰撞风险的目标船时,对dCPA最小的目标船进行优先避让。

最近会遇点如图2所示,用PA(xA,yA,zA)表示AUV的位置,PB(xB,yB,zB)表示目标船的位置,用VA(uA,vA,wA)和VB(uB,vB,wB)分别表示AUV和目标船在大地坐标系中的速度,tCPA和dCPA计算式为:

图2 最近会遇点

(3)

(4)

如果|VB-VA|→0,则tCPA→+∞,表示两船不可能发生碰撞;tCPA<0,表示两船会遇结束;因此当发生碰撞危险时,tCPA和dCPA应满足

tCPA>0&dCPA≤dSPA

dSPA为设定的最小安全会遇距离。

1.2 避障安全区域设置

船域是AUV周围必须维护的安全区域。本文设计的船域由2个以AUV为中心的球体组成,由外到内分别为常规避障区和紧急避障区,紧急避障区边界是以最小安全会遇距离为半径的球体,常规避障区边界是以2.5倍最小安全会遇距离为半径的球体。

当目标船进入AUV的常规避障区时,避障路径动态快速规划算法实时计算tCPA和dCPA,并进行碰撞风险评估。如果存在碰撞危险,则AUV将实时规划避障路径并启动避障;如果没有碰撞危险,AUV将继续向目标航路点航行。

1.3 避障路径规划

当AUV与目标船有碰撞危险时,避障路径动态快速规划算法首先利用AUV、目标船及相对运动航迹线构建辅助平面,然后基于最小安全会遇距离在辅助平面中计算避障航路点,以避障航路点为下一航路点生成避障航路,AUV将按照新规划的避障航路航行,从而避免与目标船发生碰撞。

辅助平面内避障航路点计算如图3所示,用Pi(xi,yi,zi)表示图3中A,B,…,F各点坐标,为实现安全避障AUV航行速度由VA(uA,vA,wA)调整至VAS(uAS,vAS,wAS),则AUV与目标船的相对速度由VAB(uAB,vAB,wAB)变化至VASB(uASB,vASB,wASB),选择避障机动后的会遇点作为AUV的避障航路点,具体计算步骤如下:

图3 辅助平面内避障航路点计算

AUV与目标船的相对速度VASB的具体表达式为

VASB=VB-VAS

(5)

(6)

(7)

PD=PB+(VB-VA)·tCPA

(8)

|AF|=dSPA/cosθ3

(9)

θ1=arccos(dCPA/|AB|)

(10)

θ2=arccos(dSPA/|AB|)

(11)

θ3=θ1-θ2

(12)

(13)

AUV在避障过程中航速保持不变,则有

(14)

式(13)和式(14)联立可得VAS,则AUV采取避障机动后的会遇时间tSPA为

(15)

避障航路点的坐标为

W=PA+VAS·tSPA

(16)

通过调整dSPA可实现对避障航路点W的调整,从而生成不同的避障航路。

避障路径规划流程如图4所示。

图4 动态避障流程

2 AUV制导与控制系统设计

2.1 AUV运动模型

AUV运动学模型为

(17)

x、y、z为AUV在大地坐标系中的位置;θ为纵倾角;ψ为航向角;u为纵荡速度;v为横荡速度;w为垂荡速度;q为纵摇角速度;r为艏摇角速度。

AUV的动力学模型为

(18)

m(·)为系统惯性系数;d(·)为流体动力阻尼力;τu、τq和τr分别为螺旋桨和方向舵在纵荡、纵摇和艏摇时产生的控制输入。

2.2 AUV空间机动制导器设计

为了获得空间制导算法,对二维路径制导算法进行了扩展。通过2次坐标旋转,将惯性坐标系转换为平行于轨迹轴的坐标系。新坐标系给出了AUV与规划路径的位置偏差。

AUV空间机动制导如图5所示,绕惯性坐标系z轴旋转α角度完成第1次旋转,绕第1次旋转后的坐标系y轴旋转β角度完成第2次旋转,α和β计算式为:

图5 AUV空间机动制导

(19)

(20)

Wk(xk,yk,zk)为上一航路点坐标;Wk+1(xk+1,yk+1,zk+1)为下一航路点坐标。

定义旋转后坐标系中位置偏差向量为e=[ex,ey,ez]T,其中,ex为纵向位置偏差,ey为横向位置偏差,ez为垂直位置偏差,Wi(xi,yi,wi)为航路点坐标,偏差向量计算式为

e=Ry(β)Rz(α)(PA-Wi)T

(21)

其中,Rz(α)和Ry(β)为旋转矩阵,即:

(22)

(23)

前视路径制导算法通过引入虚拟目标,实时计算AUV与虚拟目标的位置偏差,并根据偏差量计算AUV指令位置以保证AUV沿期望轨迹航行。在该算法中,根据式(21)获得偏差向量e,并应用其分量ey和ez将AUV引导至虚拟目标。

基于前视路径制导算法可得三维空间下指令纵倾θd和指令航向ψd计算式为:

θd=β+arctan(ez/Δz)

(24)

ψd=α+arctan(-ey/Δy)

(25)

Δy和Δz分别为新坐标系下的前视距离。通过比例增益kθ和kψ获得期望角速度为:

qd=-kθ(θ-θd)

(26)

rd=-kψ(ψ-ψd)

(27)

2.3 AUV运动控制器设计

以某型AUV为研究对象设计控制器,分别控制水平面和垂直面运动。定义纵荡线速度误差z1、垂直面纵摇角速度误差z2和水平面航向角速度误差z3为

(28)

ud为期望纵荡线速度;qd为期望纵摇角速度;rd为期望艏摇角速度。

由式(28)可得

(29)

式(18)与式(29)联立可得AUV速度跟踪误差动力学为

(30)

为使速度跟踪误差收敛,设计AUV空间机动控制律为

(31)

其中,设计参数ku>0、kq>0和kr>0。

3 仿真分析

为了更好地说明避障路径动态快速规划算法的有效性,基于MATLAB/Simulink仿真平台构建仿真环境。AUV运动模型参数为:m11=25 kg,m22=17.5 kg,m33=30 kg,m55=22.5 kg·m2,m66=15 kg·m2,d11=30 kg·s-1,d22=30 kg·s-1,d33=30 kg·s-1,d55=20 kg·m2·s-1,d66=20 kg·m2·s-1,ρgGML=5。AUV运动控制器参数设置为:ku=1,kq=1,kr=1,kθ=1,kψ=1。设置AUV最小安全会遇距离为20 m,仿真过程中设置交叉、对遇和追越3种会遇态势,对AUV复杂会遇场景下的避障路径动态规划和AUV避障效果分别进行仿真。

复杂会遇局面下AUV与目标船的运动参数如表1所示,不同仿真时间点各船的位置关系如图6～图15所示,AUV在设置的交叉、对遇和追越3种会遇态势下均能实现安全避障。

表1 复杂会遇局面仿真参数

图6 AUV与目标船B交叉会遇

图7 AUV对目标船B执行避障操作

图8 AUV对目标船B完成避障操作并返航

图9 AUV与目标船C对遇

图10 AUV对目标船C执行避障操作

图11 AUV对目标船C完成避障操作并返航

图12 AUV即将追越目标船D

图13 AUV对目标船D执行避障操作

图14 AUV对目标船D完成避障操作并返航

图15 AUV到达目标航路点

复杂会遇局面下避碰过程中dCPA变化曲线如图16所示,AUV在T=38.6 s时开始对交叉会遇的目标船B执行避障操作,避障机动后dCPA由14.1 m逐渐增大,在T=63.6 s时到达避障航路点,dCPA达到设定的最小安全会遇距离;在T=88.6 s时开始对对遇的目标船C执行避障操作,避障机动后dCPA由3.2 m逐渐增大,在T=105.3 s时到达避障航路点,dCPA达到设定的最小安全会遇距离;在T=129.4 s时开始对要追越的目标船D执行避障操作,避障机动后dCPA由4.4 m逐渐增大,在T=157.1 s时到达避障航路点,dCPA达到设定的最小安全会遇距离。仿真结果显示,AUV遇到障碍物时通过调整dCPA至设定的最小安全会遇距离,从而保证以较短的避障航路完成规避动作。

图16 复杂会遇局面下避碰过程中dCPA变化曲线

从图6～图16可以看出,当目标船进入AUV常规避障区时,AUV对碰撞风险进行评估,结果显示AUV与目标船均存在不同类型的碰撞风险,避障路径动态快速规划算法根据预先设定的dSPA给出了相应的避障航路点,仿真结果显示即使在对遇、追越和交叉3种会遇同时存在的情况下算法依然能快速进行避障路径计算,确保了复杂会遇态势下的航行安全。

4 结束语

为提高复杂会遇态势下AUV避障路径动态规划的实时性,本文提出一种基于最小安全会遇距离的避障路径动态快速规划算法。根据AUV和目标船的会遇态势,利用会遇距离实时进行碰撞风险评估;设置避障安全区域,预估避障时机;构建辅助平面求解避障航路点,通过执行避障路径以较小角度和较短路径实现对目标船的有效避障。为验证该算法的有效性,设计了AUV空间机动制导器和运动控制器,并基于MATLAB/Simulink仿真平台对不同会遇态势下AUV的避障能力进行仿真测试。仿真结果表明,本文算法具有较高的实时性,能够有效应对AUV所面临的各种复杂会遇局面。