一种基于速度障碍法的uuv动态避障方法
【技术领域】
[0001] 本发明涉及一种UUV动态避障方法。
【背景技术】
[0002] 随着人类对海洋资源需求的不断增加，人类的活动空间逐渐由近岸、浅海扩展到 了更深的海域，UUV技术得到了越来越多的关注，尤其在各国的军事领域中扮演着越来越重 要的角色。
[0003] UUV(Unmanned Underwater Vehicle无人水下航行器）的航路规划是高效、安全的 完成作业任务的重要保障。而UUV在航行中有可能会遇到动态障碍物，这就要求UUV能够对 移动障碍物做出迅速有效的避障反应。国内外众多学者对动态避障问题进行了深入的研 究，提出了许多有效的规划方法，比如人工势场法(APF)、向量场直方图法、动态窗法(DWA) 和行为法等，它们对局部环境具有较强的适应能力，仅依赖有限传感器信息在线避碰，具备 很高的效率。然而对动态障碍物而言，潜在碰撞区才是真实的危险区，非当前障碍物区域， 存在避开移动障碍物的准确性差的问题。
【发明内容】
[0004] 本发明为了解决现有UUV的路径规划方法在动态环境中，存在避开移动障碍物的 准确性差的问题。提出了一种基于速度障碍法的UUV动态避障方法。
[0005] 本发明所述的一种基于速度障碍法的UUV动态避障方法，该方法的具体步骤为：
[0006] 步骤一、将障碍物的运动不确定性转化为位置不确定性;根据障碍物的运动不确 定性，获得最小安全角αι和最大安全角α2;
[0007] 步骤二、根据步骤一获得的最小安全角αι和最大安全角α2,获得第i个障碍物对于 UUV以速度vr航行时的危险度VRi (vr)为：
[0009] 其中，γ是UUV与障碍物的碰撞角；i = 1，2，...，n，n为障碍物的个数；
[0010] η个障碍物0 =丨对UUV的航行速度vr产生多个不同的速度危险度，η个 障碍物对UUV造成的综合速度危险度为：
[0012] 步骤三、根据UUV动力学约束规律，确定UUV运动的速度空间Ω r;
[0013] 步骤四、根据障碍物的位置不确定性和障碍物的最大作用范围，建立UUV到达障碍 物边缘的最短时间函数;获得UUV与所有障碍物的最小碰撞时间其中， Xr。为UUV与障碍物之间的距离和尾0为障碍物Oi半径的上限估计值；
[0014]步骤五、利用UUV的综合速度危险度VR(vr)和UUV与所有障碍物的最小碰撞时间 w(Vr)，获得UUV的优化目标路径函数；
[0015]步骤六、利用速度障碍法寻找目标函数的最小值点，作为UUV运动的下一个航路 点，实现对UUV运动路径的规划。
[0016] 本发明将障碍物的运动不确定性转化为位置不确定性，既降低障碍物运动不确定 性带来的碰撞影响，也避免了动障碍物直接膨化带来的避碰保守问题，UUV提高避碰安全 性。
[0017] 通过UUV的综合速度危险度和可达速度空间中的最大前向速度，确定UUV的优化目 标路径函数，使得UUV避碰决策速度快，能在动障碍环境下获得较好的避碰效果，对未知动 态环境有良好的适应性。
【附图说明】
[0018] 图1为本发明中UUV环境建模示意图；
[0019]图2为本发明中UUV速度障碍的避碰过程示意图；
[0020]图3为【具体实施方式】一所述的UUV与障碍物的碰撞角示意图；
[0021 ] 图4为本发明中UUV动态避碰仿真图。
【具体实施方式】
【具体实施方式】 [0022] 一、结合图1、图2和图3说明本实施方式，本实施方式所述的一种基 于速度障碍法的UUV动态避障方法，该方法的具体步骤为：
[0023] 步骤一:将障碍物的运动不确定性转化为位置不确定性，获得障碍物⑴半径的下 限估计值&。和障碍物⑴半径的上限估计值^ :
[0024] 根据障碍物的运动不确定性，获得障碍物⑴半径的下限估计值&:& = Κ〇+Ι^+δρ
[0025] 障碍物〇i半径的上限估计
[0026] δρ是障碍半径的估计偏差，AR。是障碍物运动不确定性的估计上限，仏也是安全距 离，R。是障碍物半径，Rr是UUV半径；
[0027]根据障碍物半径估计下限&。，计算最小安全
，根据障碍物半径 的估计上限兄得最大安全角
γ为UUV与障碍物的碰撞角，当αΚ γ <α2时， 由于障碍物的运动不确定性，UUV保持当前航速和航向与障碍物发生碰撞是概率事件，γ越 靠近cn，UUV与障碍物发生碰撞的概率越大，γ越靠近α2，UUV与障碍物发生碰撞概率越小;第 i个障碍物对于UUV以速度vr航行时的危险度VRi (vr):
[0029]由于碰撞角γ:
，当障碍物的速度V。确定，对于UUV的速度变化空 间中任意一个速度vr，求出UUV与障碍物的相对运动速度vr。和UUV与障碍物的距离X r。获得相 应的速度危险度，即UUV以速度vr航行会带来的安全风险;对于多障碍物0=彳〇,.〇_.，〇"}， 会对UUV的航行速度v r产生多个不同的速度危险度，对UUV造成的综合速度危险度为：
[0031 ]步骤二:在速度空间上，考虑UUV的运动学约束，UUV的可达速度空间为：
[0037] 为了减小计算的复杂度，采用决策周期△ T内UUV线速度最大幅值变化和航向变化 来近似表示其运动学约束;其中，A tf是可达速度的预测时间，△ T为决策周期，△ wmax是周 期A T内的航向角速度变化最大值，01是1]1^运动的航向角，&是UUV运动的航向角的下限， 瓦是UUV运动的航向角的上限，△ vmax是周期△ T内的线速度最大变化量，是最大前向速 度，厂_是最小前向速度，在没有辅推和不允许主推差动的情况下，为保证UUV调整航向时 的舵效，对Kmm的值进行阈值设定;vx是UUV的水平速度，％是1]1^的垂直速度，1是前进速度 的下限，E是前进速度的上限，对于可达速度预测时间，令A tf = 4 Δ T;
[0038] 步骤三:根据障碍物的位置不确定性和障碍物的最大作用范围，计算UUV与障碍物 的碰撞时间；
[0039]碰撞时间是当UUV与障碍物相对速度保持不变的情况下，UUV与障碍物发生碰撞的 最短时间，是评价碰撞风险的一个通用量度，并且碰撞时间也反应了UUV自身安全时间限 制；
[0040]在障碍物膨化之后，障碍物的最大作用范围，以?ξ。为半径的障碍物形 成的速度障碍为VOi，当vr e VOi时，碰撞时间τ表示以相对速度vr。到达障碍Ο,(边缘 的最短时间，τ满足下式：
[0042] 其中，3(0,d，iy)表示的边缘，当式(8)有多个解时，取最小时间解 作为Vr。的碰撞时间，UUV与障碍物0,(Xra,iU发生碰撞的条件为：/Kvw)nO,(X ro，iU，0;
[0043] 其中，λ(νΜ) = {tVi·。! t>0}为从原点沿UUV与障碍物的相对速度方向vr。的射线；
[0044] 当巧-1足。||2 siti(r): < 0时，v叫，UUV以vr。航行不会与障碍物o,d:，見j发生 碰撞；
[0045] 而当巧-||X,」|2sin⑴>〇时，vreV0i，UUV以与障碍物的相对速度为v r。航行时，与 障碍物(AV,瓦)发生碰撞的最近碰撞距离RA为：
[0047]以相对速度Vr。航行时，UUV与障碍物发生碰撞的时间为：
[0049] 当环境中分布有多个障碍物时，UUV以vr航行时，与所有障碍物的最小碰撞时间为 tc〇i(Vr)，即：tc〇i(Vr) =min(tc〇i(vr〇,0i))，i = l，2，...，n;
[0050] 步骤五优化目标函数；
[0051 ]在速度空间上寻找UUV相对目标点的避碰最优速度
[0052] v* = arg min( Jd(Vr))，Xr-Xc