CN105589464B - 一种基于速度障碍法的uuv动态避障方法 - Google Patents

Abstract

一种基于速度障碍法的UUV动态避障方法，涉及一种UUV动态避障方法。解决了现有UUV的路径规划方法在动态环境中存在避开移动障碍物的准确性差的问题。本发明将障碍物的运动不确定性转化为位置不确定性；根据障碍物的运动不确定性，获得最小安全角α₁和最大安全角α₂；获得多障碍物对UUV造成的综合速度危险度，根据UUV动力学约束规律，确定UUV运动的速度空间，根据障碍物的位置不确定性和障碍物的最大作用范围，获得UUV与所有障碍物的最小碰撞时间；利用UUV的综合速度危险度和UUV与所有障碍物的最小碰撞时间，获得UUV的优化目标路径函数；利用速度障碍法寻找目标函数的最小值点，作为UUV运动的下一个航路点，实现对UUV运动路径的规划。本发明适用于UUV动态避障。

Description

一种基于速度障碍法的UUV动态避障方法

技术领域

本发明涉及一种UUV动态避障方法。

背景技术

随着人类对海洋资源需求的不断增加,人类的活动空间逐渐由近岸、浅海扩展到了更深的海域，UUV技术得到了越来越多的关注,尤其在各国的军事领域中扮演着越来越重要的角色。

UUV(Unmanned Underwater Vehicle无人水下航行器)的航路规划是高效、安全的完成作业任务的重要保障。而UUV在航行中有可能会遇到动态障碍物，这就要求UUV能够对移动障碍物做出迅速有效的避障反应。国内外众多学者对动态避障问题进行了深入的研究，提出了许多有效的规划方法，比如人工势场法(APF)、向量场直方图法、动态窗法(DWA)和行为法等，它们对局部环境具有较强的适应能力，仅依赖有限传感器信息在线避碰，具备很高的效率。然而对动态障碍物而言，潜在碰撞区才是真实的危险区，非当前障碍物区域，存在避开移动障碍物的准确性差的问题。

发明内容

本发明为了解决现有UUV的路径规划方法在动态环境中，存在避开移动障碍物的准确性差的问题。提出了一种基于速度障碍法的UUV动态避障方法。

本发明所述的一种基于速度障碍法的UUV动态避障方法，该方法的具体步骤为：

步骤一、将障碍物的运动不确定性转化为位置不确定性；根据障碍物的运动不确定性，获得最小安全角α₁和最大安全角α₂；

步骤二、根据步骤一获得的最小安全角α₁和最大安全角α₂，获得第i个障碍物对于UUV以速度v_r航行时的危险度VR_i(v_r)为：

其中，γ是UUV与障碍物的碰撞角；i＝1,2,...,n，n为障碍物的个数；

n个障碍物对UUV的航行速度v_r产生多个不同的速度危险度，n个障碍物对UUV造成的综合速度危险度为：

步骤三、根据UUV动力学约束规律，确定UUV运动的速度空间Ω_r；

步骤四、根据障碍物的位置不确定性和障碍物的最大作用范围，建立UUV到达障碍物边缘的最短时间函数；获得UUV与所有障碍物的最小碰撞时间t_col(v_r)；其中，X_ro为UUV与障碍物之间的距离和为障碍物O_i半径的上限估计值；

步骤五、利用UUV的综合速度危险度VR(v_r)和UUV与所有障碍物的最小碰撞时间t_col(v_r)，获得UUV的优化目标路径函数；

步骤六、利用速度障碍法寻找目标函数的最小值点，作为UUV运动的下一个航路点，实现对UUV运动路径的规划。

本发明将障碍物的运动不确定性转化为位置不确定性，既降低障碍物运动不确定性带来的碰撞影响，也避免了动障碍物直接膨化带来的避碰保守问题，UUV提高避碰安全性。

通过UUV的综合速度危险度和可达速度空间中的最大前向速度，确定UUV的优化目标路径函数，使得UUV避碰决策速度快，能在动障碍环境下获得较好的避碰效果，对未知动态环境有良好的适应性。

附图说明

图1为本发明中UUV环境建模示意图；

图2为本发明中UUV速度障碍的避碰过程示意图；

图3为具体实施方式一所述的UUV与障碍物的碰撞角示意图；

图4为本发明中UUV动态避碰仿真图。

具体实施方式

具体实施方式一、结合图1、图2和图3说明本实施方式，本实施方式所述的一种基于速度障碍法的UUV动态避障方法，该方法的具体步骤为：

步骤一：将障碍物的运动不确定性转化为位置不确定性，获得障碍物O_i半径的下限估计值R _ro和障碍物O_i半径的上限估计值

根据障碍物的运动不确定性，获得障碍物O_i半径的下限估计值R _ro：R _ro＝R_o+R_r+δ_p

障碍物O_i半径的上限估计值

δ_p是障碍半径的估计偏差，ΔR_o是障碍物运动不确定性的估计上限，R_safe是安全距离，R_o是障碍物半径，R_r是UUV半径；

根据障碍物半径估计下限R _ro，计算最小安全角根据障碍物半径的估计上限得最大安全角γ为UUV与障碍物的碰撞角，当α₁<γ<α₂时，由于障碍物的运动不确定性，UUV保持当前航速和航向与障碍物发生碰撞是概率事件，γ越靠近α₁，UUV与障碍物发生碰撞的概率越大，γ越靠近α₂，UUV与障碍物发生碰撞概率越小；第i个障碍物对于UUV以速度v_r航行时的危险度VR_i(v_r)：

由于碰撞角γ：当障碍物的速度v_o确定，对于UUV的速度变化空间中任意一个速度v_r，求出UUV与障碍物的相对运动速度v_ro和UUV与障碍物的距离X_ro获得相应的速度危险度，即UUV以速度v_r航行会带来的安全风险；对于多障碍物会对UUV的航行速度v_r产生多个不同的速度危险度，对UUV造成的综合速度危险度为：

步骤二：在速度空间上，考虑UUV的运动学约束，UUV的可达速度空间为：

为了减小计算的复杂度，采用决策周期ΔT内UUV线速度最大幅值变化和航向变化来近似表示其运动学约束；其中，Δt_f是可达速度的预测时间，ΔT为决策周期，Δw_max是周期ΔT内的航向角速度变化最大值，θ_r是UUV运动的航向角，θ_r 是UUV运动的航向角的下限，是UUV运动的航向角的上限，Δv_max是周期ΔT内的线速度最大变化量，是最大前向速度，是最小前向速度，在没有辅推和不允许主推差动的情况下，为保证UUV调整航向时的舵效，对的值进行阈值设定；v_x是UUV的水平速度，v_y是UUV的垂直速度，V_r 是前进速度的下限，是前进速度的上限，对于可达速度预测时间，令Δt_f＝4ΔT；

步骤三：根据障碍物的位置不确定性和障碍物的最大作用范围，计算UUV与障碍物的碰撞时间；

碰撞时间是当UUV与障碍物相对速度保持不变的情况下，UUV与障碍物发生碰撞的最短时间，是评价碰撞风险的一个通用量度，并且碰撞时间也反应了UUV自身安全时间限制；

在障碍物膨化之后，障碍物的最大作用范围，以为半径的障碍物形成的速度障碍为VO_i，当v_r∈VO_i时，碰撞时间τ表示以相对速度v_ro到达障碍边缘的最短时间，τ满足下式：

其中，表示的边缘，当式(8)有多个解时，取最小时间解作为v_ro的碰撞时间，UUV与障碍物发生碰撞的条件为：

其中，λ(v_ro)＝{tv_ro|t>0}为从原点沿UUV与障碍物的相对速度方向v_ro的射线；

当时，UUV以v_ro航行不会与障碍物发生碰撞；

而当时，v_r∈VO_i，UUV以与障碍物的相对速度为v_ro航行时，与障碍物发生碰撞的最近碰撞距离RA为：

以相对速度v_ro航行时，UUV与障碍物发生碰撞的时间为：

当环境中分布有多个障碍物时，UUV以v_r航行时，与所有障碍物的最小碰撞时间为t_col(v_r)，即：t_col(v_r)＝min(t_col(v_ro,O_i))，i＝1,2,...,n；

步骤五优化目标函数；

在速度空间上寻找UUV相对目标点的避碰最优速度v^*：

v^*＝arg min(J_d(v_r))，X_r→X_G，v_r∈Ω_r (11)

其中，J_d(v_r)为UUV的目标路径函数；X_r为UUV的位置坐标，X_G为目标点位置坐标，为了使UUV趋向目标点航行，UUV相对目标点的速度：

避碰速度的由安全性和趋近目标两个因素决定；减少碰撞风险提高安全性，同时快速趋近目标位置，所以，避碰决策的优化目标函数定义为：

避碰决策的优化目标函数由优化目标函数由碰撞危险度、目标速度偏差和碰撞时间三部分组成；其中ω_p,ω_v,ω_t均是权重系数；ω_p+ω_v＝1；当VR(v_r)＝1时，UUV以速度v_r航行必定与障碍物发生碰撞，则v_r不可取。

由于UUV可达速度空间Ω_r有限，有时会出现Ω_r处于速度障碍之内，在Ω_r中UUV找不到逃离碰撞区的安全速度，即J(v_r)＝∞,说明可达速度空间内的任意速度都会导致碰撞；为了避免碰撞，UUV要减速到最小速度并以最大转向能力调整航向，θ_r取边界航向θ_r 和中与目标航向偏差最小的航向。实现UUV运动的避碰。利用速度障碍法寻找目标函数值最小点，作为下一航路点，逐步实现UUV航路规划。

具体实施例：

UUV从起点(0，0)航行到终点(450，450)，起始航向45°，设计了3个与UUV交叉相遇的动障碍物O₁,O₂,O₃，假设动障碍物设为矩形。障碍物位置和运动信息未知采用前视声纳作为避碰感知设备。利用公式(1)得到动障碍物的速度危险度，利用公式(3)得到动障碍物的可达速度空间，利用公式(13)得到优化目标函数中的系数。然后，利用速度障碍法寻找目标函数值最小点，作为下一航路点，逐步实现UUV航路规划。仿真轨迹如图4所示。

Claims (5)

1.一种基于速度障碍法的UUV动态避障方法，其特征在于，该方法的具体步骤为：

步骤一、将障碍物的运动不确定性转化为位置不确定性；根据障碍物的运动不确定性，获得最小安全角α₁和最大安全角α₂；

步骤二、根据步骤一获得的最小安全角α₁和最大安全角α₂获得第i个障碍物对于UUV以速度v_r航行时的危险度VR_i(v_r)为：

其中，γ是UUV与障碍物的碰撞角；i＝1,2,...,n，n为障碍物的个数；

多障碍物对UUV的航行速度v_r产生多个不同的速度危险度，n个障碍物对UUV造成的综合速度危险度VR(v_r)为：

步骤三、根据UUV动力学约束规律，确定UUV运动的速度空间Ω_r；

步骤四、根据障碍物的位置不确定性和障碍物的最大作用范围，建立UUV到达障碍物边缘的最短时间函数；获得UUV与所有障碍物的最小碰撞时间t_col(v_r)；其中，X_ro为UUV与障碍物之间的距离和为障碍物O_i半径的上限估计值；

步骤五、利用UUV的综合速度危险度VR(v_r)和UUV与所有障碍物的最小碰撞时间t_col(v_r)，获得UUV的优化目标路径函数；

步骤六、利用速度障碍法寻找目标函数的最小值点，作为UUV运动的下一个航路点，实现对UUV运动路径的规划。

2.根据权利要求1所述的一种基于速度障碍法的UUV动态避障方法，其特征在于，步骤一中所述的根据障碍物的运动不确定性，获得最小安全角α₁和最大安全角α₂的方法为：

根据障碍物的运动不确定性，获得障碍物O_i半径的下限估计值R _ro：R _ro＝R_o+R_r+δ_p；

障碍物O_i半径的上限估计值δ_p是障碍半径的估计偏差，△R_o是障碍物运动不确定性的估计上限，R_safe是安全距离，R_o是障碍物半径，R_r是UUV半径；

根据障碍物半径估计下限R _ro，计算最小安全角根据障碍物半径的估计上限得最大安全角

3.根据权利要求1或2所述的一种基于速度障碍法的UUV动态避障方法，其特征在于，步骤三所述的确定UUV运动的速度空间Ω_r通过：

获得，其中，

其中，△t_f是UUV速度的预测时间，令△t_f＝4△T；△T为决策周期，△w_max是周期△T内的航向角速度变化最大值，θ_r是UUV运动的航向角，θ_r 是UUV运动的航向角的下限，是UUV运动的航向角的上限，△v_max是周期△T内的线速度最大变化量，是最大前向速度，是UUV最小前向速度，v_x是UUV的水平速度，v_y是UUV的垂直速度，V_r 是前进速度的下限，是前进速度的上限。

4.根据权利要求1或2所述的一种基于速度障碍法的UUV动态避障方法，其特征在于，步骤四、建立UUV到达障碍物边缘的最短时间函数；获得UUV与所有障碍物的最小碰撞时间t_col(v_r)的具体过程为：

在障碍物膨化之后，障碍物的作用范围最大，以为半径的障碍物形成的速度障碍为VO_i；当UUV的运动速度v_r∈VO_i时，碰撞时间τ表示以相对速度v_ro到达障碍边缘的最短时间τ，τ满足公式：

其中，表示的边缘，当式(8)有多个解时，取最小时间解作为v_ro的碰撞时间，

UUV与障碍物发生碰撞的条件为：

其中，λ(v_ro)＝{tv_ro|t>0}为从原点沿UUV与障碍物的相对速度方向v_ro的射线；

当时，UUV以与障碍物的相对速度为v_ro航行时，UUV与障碍物不发生碰撞；

当时，v_r∈VO_i，UUV以与障碍物的相对速度为v_ro航行时，UUV与障碍物发生碰撞的最近碰撞距离为：

以相对速度v_ro航行时，UUV与障碍物发生碰撞的时间为：

当环境中分布有多个障碍物时，UUV以速度v_r航行时，与所有障碍物的最小碰撞时间为t_col(v_r)，即：t_col(v_r)＝min(t_col(v_ro,O_i))。

5.根据权利要求4所述的一种基于速度障碍法的UUV动态避障方法，其特征在于，步骤五中利用UUV的综合速度危险度VR(v_r)和UUV与所有障碍物的最小碰撞时间t_col(v_r)，确定UUV的优化目标路径函数的过程为：

在速度空间上寻找UUV相对目标点的避碰最优速度v^*：

v^*＝arg min(J_d(v_r)),X_r→X_G,v_r∈Ω_r (11)

其中，J_d(v_r)为UUV的目标路径函数；X_r为UUV的位置坐标，X_G为目标点位置坐标，为了使UUV趋向目标点航行，UUV相对目标点的速度：

避碰速度由安全性和趋近目标两个因素决定；减少碰撞风险提高安全性，同时快速趋近目标位置，所以，避碰决策的优化目标函数为：

避碰决策的优化目标函数由优化目标函数由碰撞危险度、目标速度偏差和碰撞时间三部分组成；其中ω_p,ω_v,ω_t均是权重系数；ω_p+ω_v＝1。