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

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

【技术实现步骤摘要】
一种应用视锥法的UUV水下避碰方法、存储介质和产品


[0001]本专利技术涉及无人潜器控制
，更具体的说是涉及一种应用视锥法的UUV水下避碰方法、存储介质和产品。

技术介绍

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

技术实现思路

[0005]有鉴于此，本专利技术提供了一种应用视锥法的UUV水下避碰方法、存储介质和产品，涉及一种UUV水下航行时的障碍物危险判别及自动避碰方法，针对水下三维空间的避碰环境避让动态与静态障碍物生成避碰路径，提升UUV在水下环境中的智能化水平。
[0006]为了实现上述目的，本专利技术采用如下技术方案：
[0007]一种应用视锥法的UUV水下避碰方法，包括以下步骤：
[0008]步骤1：采集障碍物空间位置和速度，计算障碍物的危险程度，筛选出满足设定碰撞危险条件的避碰障碍物；
[0009]根据障碍物空间位置、速度和UVV的位置、速度评估避碰危险程度，判断起避时机；如果存在多个障碍物，则根据障碍物空间位置和速度计算障碍物的危险程度，筛选出满足设定碰撞危险条件的障碍物；
[0010]步骤2：根据避碰障碍物的空间位置采用视锥法构建每个避碰障碍物的视锥模型，生成避碰射线集；
[0011]将每个避碰障碍物看做一个球体，以UUV的位置为圆锥顶点做避碰障碍物的球面的切线，形成一个圆锥面获得的圆锥为视锥模型，构成的圆锥的顶角γ
a
表示为：
[0012][0013]其中，R0为避碰障碍物膨化半径；d0为UUV距避碰障碍物的距离，根据UUV的位置和避碰障碍物的空间位置计算；
[0014]将圆锥的顶角扩大，获得避让圆锥顶角，避让圆锥顶角γ
e
表示为：
[0015]γ
e
＝γ
a
+α0[0016]其中，α0为设置的角度余量，为固定常数；避让圆锥顶角γ
e
为一组射线集合，作为避碰射线集；
[0017]将顶角扩大两倍，获得一组射线集合，生成避碰射线集。
[0018]步骤3：根据避碰障碍物的速度筛选出动态障碍物，如果障碍物速度为0则为静态障碍物，否则为动态障碍物；并根据动态障碍物的速度对视锥模型进行相对运动补偿，获得修正避碰射线集；
[0019]将视锥模型沿动态障碍物运动方向旋转补偿角γ
ca
，则将视锥模型中的任意一条向量rad
i
:(x
i
,y
i
,z
i
)均沿动态障碍物运动方向旋转补偿角γ
ca
，x
i
、y
i
、z
i
分别表示向量的三维坐标值，则旋转后的向量表示为：
[0020][0021]其中,v
ox
,v
oy
,v
oz
为动态障碍物的运动速度在三维坐标中的投影，t为待求解补偿参数；
[0022]在空间坐标系中，由rad
i
与之间的空间角度求解方程得：
[0023][0024]其中cosγ
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
再对上式取平方，转化为：
[0025][0026]对待求解参数t，得到一元二次方程式：
[0027](X2‑
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＝(rad
i
|θ
ij
＜α
j
,j＝1,2,
…
n)
[0036]其中，θ
ij
表示第j个视锥模型的圆锥面上的第i条射线与中心线的空间夹角，即视锥模型的锥面上的第i条射线与第j个障碍物
‑
UUV连线(第i个视锥模型的中心线)的空间夹角；α
j
为第j个视锥模型的顶角，n为视锥模型的总个数；；rad
i
表示视锥模型中的向量；
[0037]步骤5：采用代价函数从航向集中选取指令航向；
[0038]构建代价函数计算航线集中每条射线的代价，表示为：
[0039][0040][0041][0042]其中，ψ
V
与θ
V
分别表示UUV的偏航角和俯仰角；表示航线集中射线φ的偏航角；表示航线集中射线φ的俯仰角；为航线集中射线φ的代价观测矩阵；表示UUV的偏航角和航线集中射线φ的偏航角的差值；表示UUV的俯仰角度和航线集中射线φ的俯仰角的差值；
[0043]最小化代价函数，获得指令航向，表示为：
[0044][0045][0046][0047]其中，φ
ca
为航线集中使代价函数最小的射线；ψ
dca
为φ
ca
的偏航角；θ
dca
为φ
ca
的俯仰角。
[0048]经由上述的技术方案可知，与现有技术相比，本专利技术公开提供了一种应用视锥法的UUV水下避碰方法、存储介质和产品，涉及水下无人潜器三维空间避碰方法，针对三维空间内障碍物的位置、速度因素，建立视锥法避碰航向射线集，对动态目标进行动态本文档来自技高网...

【技术保护点】

【技术特征摘要】
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...

【专利技术属性】
技术研发人员：于双宁，李冀永，王琳琳，钟荣兴，郭亦平，韩俊庆，韦一，王益民，
申请(专利权)人：中国船舶重工集团公司第七零七研究所九江分部，
类型：发明
国别省市：