，Ω r (11)
[0053] 其中，Jd ( Vr )为UUV的目标路径函数;Xr为UUV的位置坐标，Xg为目标点位置坐标，为 了使UUV趋向目标点航行，UUV相对目标点的速度：
[0055]避碰速度的由安全性和趋近目标两个因素决定;减少碰撞风险提高安全性，同时 快速趋近目标位置，所以，避碰决策的优化目标函数定义为：
[0057]避碰决策的优化目标函数由优化目标函数由碰撞危险度、目标速度偏差和碰撞时 间三部分组成;其中ωρ，ων，〇^均是权重系数；ωρ+ων=1;当VR(v r) = l时，UUV以速度^航 行必定与障碍物发生碰撞，则不可取。
[0058]由于UUV可达速度空间ΩΓ有限，有时会出现ΩΓ处于速度障碍之内，在Qr*UUV找 不到逃离碰撞区的安全速度，即J( Vr) = %，，说明可达速度空间内的任意速度都会 导致碰撞;为了避免碰撞，UUV要减速到最小速度V,, = FT?，并以最大转向能力调整航向，Θ1· 取边界航向土和'^中与目标航向偏差最小的航向。实现UUV运动的避碰。利用速度障碍法寻 找目标函数值最小点，作为下一航路点，逐步实现UUV航路规划。
[0059] 具体实施例：
[0060] UUV从起点（0，0)航行到终点（450，450)，起始航向45°，设计了3个与UUV交叉相遇 的动障碍物(^(^(^，假设动障碍物设为矩形^^障碍物位置和运动信息未知采用前视声纳作 为避碰感知设备。利用公式（1)得到动障碍物的速度危险度，利用公式(3)得到动障碍物的 可达速度空间，利用公式（13)得到优化目标函数中的系数。然后，利用速度障碍法寻找目标 函数值最小点，作为下一航路点，逐步实现UUV航路规划。仿真轨迹如图4所示。
【主权项】
1. 一种基于速度障碍法的UUV动态避障方法，其特征在于，该方法的具体步骤为： 步骤一、将障碍物的运动不确定性转化为位置不确定性;根据障碍物的运动不确定性， 获得最小安全角Qi和最大安全角口 2; 步骤二、根据步骤一获得的最小安全角Oi和最大安全角02,获得第i个障碍物对于UUVW 速度Vr航行时的危险度VRi (Vr)为：(1) 其中，丫是UUV与障碍物的碰撞角；i = 1，2，...，n，n为障碍物的个数； 多障碍物0 =似,,.巧....，〇,，！对UUV的航行速度Vr产生多个不同的速度危险度，n个障碍 物对UUV造成的综合速度危险度VR( Vr )为：步骤=、根据UUV动力学约束规律，确定UUV运动的速度空间Q r; 步骤四、根据障碍物的位置不确定性和障碍物的最大作用范围，建立UUV到达障碍物 W X。,.巧,.）边缘的最短时间函数;获得UUV与所有障碍物的最小碰撞时间心。1 ( Vr );其中，Xnr 为UUV与障碍物之间的距离和巧,为障碍物化半径的上限估计值； 步骤五、利用UUV的综合速度危险度VR( Vr)和UUV与所有障碍物的最小碰撞时间tcoi (Vr)，获得UUV的优化目标路径函数； 步骤六、利用速度障碍法寻找目标函数的最小值点，作为UUV运动的下一个航路点，实 现对UUV运动路径的规划。2. 根据权利要求1所述的一种基于速度障碍法的UUV动态避障方法，其特征在于，步骤 一中所述的根据障碍物的运动不确定性，获得最小安全角Qi和最大安全角〇2的方法为： 根据障碍物的运动不确定性，获得障碍物化半径的下限估计值运,。:运,。=3。+也+和障碍物 Oi半径的上限估计值巧.,：巧。二馬+人',+ +心<,+化,* . Sp是障碍半径的估计偏差，A R。是障碍物运动不确定性的估计上限，Rsafe是安全距离，Rd 是障碍物半径，Rr是UUV半径； 根据障碍物半径估计下限gw，计算最小安全角，根据障碍物半径的估 计上限.我f得最大安全角。3. 根据权利要求1或2所述的一种基于速度障碍法的UUV动态避障方法，其特征在于，步 骤=所述的确定UUV运动的速度空间Qr通过： 获得，其中， 其中，Atf是UVV速度的预测时间，令Atf = 4 AT; A T为决策周期，A Wmax是周期A T内的 航向角速度变化最大值，是UUV运动的航向角，电是UUV运动的航向角的下限，是是UUV运 动的航向角的上限，A Vmax是周期A T内的线速度最大变化量，巧是最大前向速度，是 UUV最小前向速度，Vx是UUV的水平速度，Vy是UUV的垂直速度，电是前进速度的下限，巧是前 进速度的上限。4.根据权利要求1或2所述的一种基于速度障碍法的UUV动态避障方法，其特征在于，步 骤四、建立UUV到达障碍物缉(义"，思)边缘的最短时间函数;获得UUV与所有障碍物的最小 碰撞时间心。1 ( Vr )的具体过程为： 在障碍物膨化之后，障碍物的作用范围最大，W1：。为半径的障碍物巧J形成的 速度障碍为VOi;当UUV的运动速度Vr EVOi时，碰撞时间T表示W相对速度Vr。到达障碍 ", (A'.,,，巧。倘缘的最短时间T，T满足公式： 仰(8) 其中，a(WA',"，巧,,)戍巧0,化。，巧J的边缘，当式（8)有多个解时，取最小时间解作为 Vra的碰撞时间， UUV与障碍物坪(疋。，巧。谈生碰撞的条件为:乂(vjn马体W焉>0:; 其中，A(Vrn) = {tVr。I t>0}为从原点沿UUV与障碍物的相对速度方向Vrn的射线； 当巧-||义Sin(处< 0时，V,. g ro,，UUVW与障碍物的相对速度为Vrn航行时，UUV与障 碍物巧(义馬。)不发生碰撞； 当思-||本.SinW^ S 0时，VrEVOi ,UUVW与障碍物的相对速度为Vrn航行时，UUV与障 碍物9 (X,。:，是。)发生碰撞的最近碰撞距离为： (9) W相对速度Vr。航行时，UUV与障碍物巧(而，馬)发生碰撞的时间为：当环境中分布有多个障碍物时，UUVW速度Vr航行时，与所有障碍物的最小碰撞时间为 tcol ( Vr )，即：tcol ( Vr ) =min (tcol ( Vro , Oi ))。5.根据权利要求4所述的一种基于速度障碍法的UUV动态避障方法，其特征在于，步骤 五中利用UUV的综合速度危险度VR( Vr )和UUV与所有障碍物的最小碰撞时间twl ( Vr )，确定 UUV的优化目标路径函数的过程为： 在速度空间上寻找UUV相对目标点的避碰最优速度/: v* = arg min(jd(vr)) ,Xr一拍，VrE Qr (11) 其中，Jd(Vr)为UUV的目标路径函数;Xr为UUV的位置坐标，Xg为目标点位置坐标，为了使 UUV趋向目标点航行，UUV相对目标点的速度：(12) 避碰速度的由安全性和趋近目标两个因素决定;减少碰撞风险提高安全性，同时快速 趋近目标位置，所W，避碰决策的优化目标函数为：避碰决策的优化目标函数由优化目标函数由碰撞危险度、目标速度偏差和碰撞时间= 部分组成;其中Wp, "V，Wt均是权重系数；《p+? V=I。
【专利摘要】一种基于速度障碍法的UUV动态避障方法，涉及一种UUV动态避障方法。解决了现有UUV的路径规划方法在动态环境中存在避开移动障碍物的准确性差的问题。本发明将障碍物的运动不确定性转化为位置不确定性；根据障碍物的运动不确定性，获得最小安全角α1和最大安全角α2；获得多障碍物对UUV造成的综合速度危险度，根据UUV动力学约束规律，确定UUV运动的速度空间，根据障碍物的位置不确定性和障碍物的最大作用范围，获得UUV与所有障碍物的最小碰撞时间；利用UUV的综合速度危险度和UUV与所有障碍物的最小碰撞时间，获得UUV的优化目标路径函数；利用速度障碍法寻找目标函数的最小值点，作为UUV运动的下一个航路点，实现对UUV运动路径的规划。本发明适用于UUV动态避障。
【IPC分类】G05D1/06
【公开号】CN105589464
【申请号】CN201610182817
【发明人】张伟, 王秀芳, 严浙平, 陈涛, 李娟 , 滕延斌
【申请人】哈尔滨工程大学
【公开日】2016年5月18日
【申请日】2016年3月28日