一种应用视锥法的UUV水下避碰方法、存储介质和产品与流程

一种应用视锥法的uuv水下避碰方法、存储介质和产品
技术领域
1.本发明涉及无人潜器控制技术领域，更具体的说是涉及一种应用视锥法的uuv水下避碰方法、存储介质和产品。


背景技术：

2.水下无人潜器(uuv)在水下航行遇到动态、静态障碍物时，需要自主避让障碍物，完成指定任务。
3.水下无人潜器(uuv)水下空间避碰方法，需要对基于传感器实时探测到的的非预期障碍物、运动目标进行安全会遇评估，并采取应急避碰机动策略，从而形成和提高自主航行能力。
4.因此，如何提高水下无人潜器水下避碰智能化水平是本领域技术人员亟需解决的问题。


技术实现要素：

5.有鉴于此，本发明提供了一种应用视锥法的uuv水下避碰方法、存储介质和产品，涉及一种uuv水下航行时的障碍物危险判别及自动避碰方法，针对水下三维空间的避碰环境避让动态与静态障碍物生成避碰路径，提升uuv在水下环境中的智能化水平。
6.为了实现上述目的，本发明采用如下技术方案：
7.一种应用视锥法的uuv水下避碰方法，包括以下步骤：
8.步骤1：采集障碍物空间位置和速度，计算障碍物的危险程度，筛选出满足设定碰撞危险条件的避碰障碍物；
9.根据障碍物空间位置、速度和uvv的位置、速度评估避碰危险程度，判断起避时机；如果存在多个障碍物，则根据障碍物空间位置和速度计算障碍物的危险程度，筛选出满足设定碰撞危险条件的障碍物；
10.步骤2：根据避碰障碍物的空间位置采用视锥法构建每个避碰障碍物的视锥模型，生成避碰射线集；
11.将每个避碰障碍物看做一个球体，以uuv的位置为圆锥顶点做避碰障碍物的球面的切线，形成一个圆锥面获得的圆锥为视锥模型，构成的圆锥的顶角γa表示为：
[0012][0013]
其中，r0为避碰障碍物膨化半径；d0为uuv距避碰障碍物的距离，根据uuv的位置和避碰障碍物的空间位置计算；
[0014]
将圆锥的顶角扩大，获得避让圆锥顶角，避让圆锥顶角γe表示为：
[0015]
γe＝γa+α0[0016]
其中，α0为设置的角度余量，为固定常数；避让圆锥顶角γe为一组射线集合，作为避碰射线集；
[0017]
将顶角扩大两倍，获得一组射线集合，生成避碰射线集。
[0018]
步骤3：根据避碰障碍物的速度筛选出动态障碍物，如果障碍物速度为0则为静态障碍物，否则为动态障碍物；并根据动态障碍物的速度对视锥模型进行相对运动补偿，获得修正避碰射线集；
[0019]
将视锥模型沿动态障碍物运动方向旋转补偿角γ
ca
，则将视锥模型中的任意一条向量radi:(xi,yi,zi)均沿动态障碍物运动方向旋转补偿角γ
ca
，xi、yi、zi分别表示向量的三维坐标值，则旋转后的向量表示为：
[0020][0021]
其中,v
ox
,v
oy
,v
oz
为动态障碍物的运动速度在三维坐标中的投影，t为待求解补偿参数；
[0022]
在空间坐标系中，由radi与之间的空间角度求解方程得：
[0023][0024]
其中cosγ
ca
的数值通过求解相对速度获得，令r＝x
i2
+y
i2
+z
i2
，v＝v
x2
+v
y2
+v
z2
，x＝x
·vx
+x
·
vy+x
·
vz再对上式取平方，转化为：
[0025][0026]
对待求解参数t，得到一元二次方程式：
[0027]
(x
2-cos2γ
ca
rv)
·
t2+2rx(1-cos2γ
ca
)
·
t+r2(1-cos2γ
ca
)＝0
[0028]
再根据动态障碍物的速度方向在两个t值中取真值，得到旋转补偿后的向量
[0029]
步骤4：对所有避碰障碍物对应的避碰射线集或修正避碰射线集进行融合，获得航线集；
[0030]
对所有动态障碍物对应的修正避碰射线集，以及其他避碰障碍物的避碰射线集取并集，并剔除重复射线，建立航线集u；
[0031]
若干障碍物对应的修正避碰射线集之间存在重叠、相交或互斥关系；对重叠的两组修正避碰射线集取并集，只考虑视锥模型的最外层射线；对互斥的两组修正避碰射线集取并集，并采用代价函数选取最优射线；对相交的两组修正避碰射线集取并集，并剔除交集部分射线；
[0032]
表示为：
[0033][0034]
航线集u表示为：
[0035]
u＝(radi|θ
ij
＜αj,j＝1,2,
…
n)
[0036]
其中，θ
ij
表示第j个视锥模型的圆锥面上的第i条射线与中心线的空间夹角，即视锥模型的锥面上的第i条射线与第j个障碍物-uuv连线(第i个视锥模型的中心线)的空间夹角；αj为第j个视锥模型的顶角，n为视锥模型的总个数；；radi表示视锥模型中的向量；
[0037]
步骤5：采用代价函数从航向集中选取指令航向；
[0038]
构建代价函数计算航线集中每条射线的代价，表示为：
[0039][0040][0041][0042]
其中，ψv与θv分别表示uuv的偏航角和俯仰角；表示航线集中射线φ的偏航角；表示航线集中射线φ的俯仰角；为航线集中射线φ的代价观测矩阵；表示uuv的偏航角和航线集中射线φ的偏航角的差值；表示uuv的俯仰角度和航线集中射线φ的俯仰角的差值；
[0043]
最小化代价函数，获得指令航向，表示为：
[0044][0045][0046][0047]
其中，φ
ca
为航线集中使代价函数最小的射线；ψ
dca
为φ
ca
的偏航角；θ
dca
为φ
ca
的俯仰角。
[0048]
经由上述的技术方案可知，与现有技术相比，本发明公开提供了一种应用视锥法的uuv水下避碰方法、存储介质和产品，涉及水下无人潜器三维空间避碰方法，针对三维空间内障碍物的位置、速度因素，建立视锥法避碰航向射线集，对动态目标进行动态补偿，修正射线集，并对多目标进行视锥融合，最终从融合后的视锥射线集中，选取最容易实现的射线作为uuv的避碰指令航向，提升水下无人航行器自动化程度。本发明提升了水下无人潜器的自主避碰能力，方法简单，容易实现，实时性好，具有良好的实际应用价值，为水下无人潜器三维空间自主航行提供一种实用性强的避碰方法。
附图说明
[0049]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
[0050]
图1附图为本发明提供的应用视锥法的uuv水下避碰方法主流程图；
[0051]
图2附图为本发明提供的三维视锥法原理示意图；
[0052]
图3附图为本发明提供的相对运动补偿角度示意图；
[0053]
图4附图为本发明提供的视锥相对运动补偿示意图；
[0054]
图5附图为本发明提供的避碰仿真结果示意图。
具体实施方式
[0055]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0056]
本发明实施例公开了一种水下无人潜器三维空间避碰方法，如图1所示，包括：1、根据障碍物空间位置、速度和本船的位置、速度评估避碰危险程度，判断起避时机；2、根据障碍物的位置尺度，建立视锥模型，生成避碰射线集；3、根据障碍物是否是动态目标，对动态目标进行视锥补偿，修正射线集；4、对多目标进行视锥融合，剔除不满足要求的射线，得到融合后的射线集；5、对融合后的视锥射线集中，选取最容易实现的射线作为uuv的避碰指令航向。
[0057]
以下结合附图对本发明实施方式进一步地详细描述：
[0058]
1.威胁目标筛选
[0059]
根据障碍物的碰撞危险对探测到的多个障碍物进行筛选，过滤掉低威胁目标，在uuv航行过程中根据障碍物的位置和速度信息实时计算危险程度，满足设定碰撞危险条件后开始启动避让算法。考虑避碰危险的几个影响因素——最近水平会遇距离dcpa、最近深度会遇距离hcpa、最短会遇时间tcpa。由三维空间的运动特性，设定碰撞危险条件为：
[0060]
{(dcpa＜d)∩(hcpa＜h)∩(0＜tcpa＜t)}
[0061]
其中d、h、t为设定的距离、深度、会遇时间阈值。当障碍物超过设定阈值时，将其放入威胁列表中。
[0062]
2.根据障碍物的位置设置视锥模型
[0063]
视锥法的核心是基于对障碍物保持恒定的避障角度，在视锥法的具体实现上，根据障碍物的空间位置和大小，将其看做一个球体，以uuv的位置为中心做球面的切线，形成一个圆锥面，圆锥的顶角γa大小为：
[0064][0065]
其中r0为障碍物膨化半径，d0为uuv距障碍物距离，并设避让圆锥顶角γe设置为：
[0066]
γe＝γa+α0[0067]
其中α0为设置的角度余量，为固定常数。再将顶角扩大为两倍，得到一组射线集合，为了安全避开障碍物，uuv应沿着射线集合中的某一条作为指令航向。视锥示意如图2所示。视锥模型包含的所有切线构成射线集合，即避碰射线集。
[0068]
3.对动态目标的视锥进行动态补偿
[0069]
对动态目标的避碰，结合uuv的速度与障碍物的速度，对视锥进行相对运动补偿，引入补偿角γ
ca
，γ
ca
的大小应能反映障碍物在垂直uuv当前运动速度方向上的运动速度，γ
ca
的计算方法如图3所示。
[0070]
具体计算公式如下：
[0071]
[0072]
其中，v
ρ
表示uuv的速度矢量，v
ca
表示矢量相加速度，|v
ρ
|表示uuv的速度大小，|v
ca
|为矢量叠加的速度大小。
[0073]
将视锥沿障碍物运动方向旋转γ
ca
，对空间中的视锥坐标具体计算如下：
[0074]
对视锥中的任意一条向量radi:(xi,yi,zi),即扩大后的视锥模型的顶角到球面的切线，将其沿障碍物运动方向旋转补偿γ
ca
后得到则有：
[0075][0076]
令r＝x
i2
+y
i2
+z
i2
，v＝v
x2
+v
y2
+v
z2
，x＝x
·vx
+x
·
vy+x
·
vz再对上式取平方，将上式转化为
[0077][0078]
对待求解参数t，得到一元二次方程式：
[0079]
(x
2-cos2γ
ca
rv)
·
t2+2rx(1-cos2γ
ca
)
·
t+r2(1-cos2γ
ca
)＝0
[0080]
再根据速度方向在两个t值中取真值，得到旋转补偿后的视锥向量如图4所示，图中箭头所指向的视锥为补偿后的视锥。
[0081]
4.多目标视锥融合
[0082]
当多个目标的危险度同时达到阈值时，根据视锥法会产生多个视锥，它们可能是重叠、相交、互斥的。建立航线集u，从u中选取最优航线引导uuv前进。对于重叠的视锥，u取两视锥的并集，只考虑视锥的最外层射线。对于互斥的视锥，u取视锥的并集，在并集中根据代价函数选取最优的航线，对于相交的视锥，需要剔除两视锥的交集部分，即：
[0083][0084]
在算法的具体实现上，引入变量θ
ij
，表示第i条射线与第j个障碍物-uuv连线的空间夹角，将u表示为：
[0085]
u＝(radi|θ
ij
＜αj,j＝1,2,
…
n)
[0086]
其中αj为第j个视锥的顶角，n为视锥的总个数。
[0087]
5.射线集内最优航向的挑选：
[0088]
引入变量φ并构建代价函数衡选择射线集合中指令航向的代价：
[0089][0090][0091][0092]
ψv与θv表示uuv的偏航角和俯仰角度。
[0093]
最小代价函数值对应的射线即为指令航向方向，即：
[0094][0095][0096][0097]
避碰结果如图5所示，图5(a，b，c，d)中为相同障碍物分布空间中规划出的同一条航路，四张图分别对四个观察角度进行演示。由图可得通过视锥法可以有效选择避让的时机，并衡量水平避让或垂直避让的方式实现难易程度，在适合的方向上避让障碍物。
[0098]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
[0099]
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

技术特征：
1.一种应用视锥法的uuv水下避碰方法，其特征在于，包括以下步骤：步骤1：采集障碍物空间位置和速度，计算障碍物的危险程度，筛选出满足设定碰撞危险条件的避碰障碍物；步骤2：根据避碰障碍物的空间位置采用视锥法构建每个避碰障碍物的视锥模型，生成避碰射线集；步骤3：根据避碰障碍物的速度筛选出动态障碍物，并根据动态障碍物的速度对视锥模型进行相对运动补偿，获得修正避碰射线集；步骤4：对所有避碰障碍物对应的避碰射线集或修正避碰射线集进行融合，获得航线集；步骤5：采用代价函数从航向集中选取指令航向。2.根据权利要求1所述的一种应用视锥法的uuv水下避碰方法，其特征在于，步骤2的具体实现过程为：将每个避碰障碍物设定为一个球体，以uuv的位置为圆锥顶点做避碰障碍物的球面的切线，形成一个圆锥面获得的圆锥为视锥模型，构成的圆锥的顶角γ
a
表示为：其中，r0为避碰障碍物膨化半径；d0为uuv距避碰障碍物的距离；将圆锥的顶角扩大，获得避让圆锥顶角，避让圆锥顶角γ
e
表示为：γ
e
＝γ
a
+α0其中，α0为设置的角度余量；避让圆锥顶角γ
e
为一组射线集合，作为避碰射线集。3.根据权利要求1所述的一种应用视锥法的uuv水下避碰方法，其特征在于，步骤3的具体实现过程为：将视锥模型沿动态障碍物运动方向旋转补偿角γ
ca
，则将视锥模型中的任意一条向量rad
i
:(x
i
,y
i
,z
i
)均沿动态障碍物运动方向旋转补偿角γ
ca
，x
i
、y
i
、z
i
分别表示向量的三维坐标值，则旋转后的向量表示为：其中,v
ox
,v
oy
,v
oz
为动态障碍物的运动速度在三维坐标中的投影，t为待求解补偿参数；在空间坐标系中，由rad
i
与之间的空间角度求解旋转后的向量，表示为：之间的空间角度求解旋转后的向量，表示为：其中，cosγ
ca
表示uuv与动态障碍物的相对速度；v
ρ
表示uuv的速度矢量，v
ca
表示矢量相加速度，|v
ρ
|表示uuv的速度大小，|v
ca
|为矢量叠加的速度大小；令r＝x
i2
+y
i2
+z
i2
，v＝v
x2
+v
y2
+v
z2
，x＝x
·
v
x
+x
·
v
y
+x
·
v
z
再对上式取平方，转化为：
对待求解参数t，得到一元二次方程式：(x
2-cos2γ
ca
rv)
·
t2+2rx(1-cos2γ
ca
)
·
t+r2(1-cos2γ
ca
)＝0根据动态障碍物的速度方向在两个t值中取真值，得到旋转补偿后的向量获得修正避碰射线集。4.根据权利要求1所述的一种应用视锥法的uuv水下避碰方法，其特征在于，步骤4的具体实现过程为：对所有动态障碍物对应的修正避碰射线集，以及其他避碰障碍物的避碰射线集取并集，并剔除重复射线，建立航线集u，表示为：u＝(rad
i
|θ
ij
＜α
j
,j＝1,2,
…
n)其中，θ
ij
表示第j个视锥模型的圆锥面上的第i条射线与中心线的空间夹角；α
j
为第j个视锥模型的顶角；n为视锥模型的总个数；rad
i
表示视锥模型中的向量。5.根据权利要求1所述的一种应用视锥法的uuv水下避碰方法，其特征在于，步骤5的具体实现过程：步骤51：构建代价函数计算航线集中每条射线的代价，表示为：计算航线集中每条射线的代价，表示为：计算航线集中每条射线的代价，表示为：其中，ψv与θv分别表示uuv的偏航角和俯仰角；表示航线集中射线φ的偏航角；表示航线集中射线φ的俯仰角；为航线集中射线φ的代价观测矩阵；表示uuv的偏航角和航线集中射线φ的偏航角的差值；表示uuv的俯仰角度和航线集中射线φ的俯仰角的差值；步骤52：最小化代价函数，获得指令航向，表示为：步骤52：最小化代价函数，获得指令航向，表示为：步骤52：最小化代价函数，获得指令航向，表示为：其中，φ
ca
为航线集中使代价函数最小的射线；ψ
dca
为φ
ca
的偏航角；θ
dca
为φ
ca
的俯仰角。6.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时实现权利要求1-5任一所述方法的步骤。
7.一种计算机程序产品，包括计算机程序，其特征在于，该计算机程序被处理器执行时实现权利要求1-5任一所述方法的步骤。

技术总结
本发明公开了一种应用视锥法的UUV水下避碰方法、存储介质和产品，涉及无人潜器控制技术领域，是一种自主避碰方法，跟据障碍物空间位置、速度及本船的位置、速度生成避碰指令航向，包括：步骤1：采集障碍物空间位置和速度，计算障碍物的危险程度，筛选出满足设定碰撞危险条件的避碰障碍物；步骤2：根据避碰障碍物的空间位置采用视锥法构建每个避碰障碍物的视锥模型，生成避碰射线集；步骤3：根据避碰障碍物的速度筛选出动态障碍物，并根据动态障碍物的速度对视锥模型进行相对运动补偿，获得修正避碰射线集；步骤4：对所有避碰障碍物对应的避碰射线集或修正避碰射线集进行融合，获得航线集；步骤5：采用代价函数从航向集中选取指令航向。向。向。


技术研发人员：于双宁 李冀永 王琳琳 钟荣兴 郭亦平 韩俊庆 韦一 王益民
受保护的技术使用者：中国船舶重工集团公司第七0七研究所九江分部
技术研发日：2023.06.13
技术公布日：2023/9/6