考虑洋流影响的基于因子图的主从式AUV协同定位方法.pdf

(19)国家知识产权局(12)发明专利申请(10)申请公布号CN114577211A(43)申请公布日2022.06.03(21)申请号202210191144.4(22)申请日2022.02.25(71)申请人哈尔滨工程大学地址150001黑龙江省哈尔滨市南岗区南通大街145号哈尔滨工程大学科技处知识产权办公室(72)发明人奔粤阳孙炎李倩赵玉新周广涛吴磊蒋正华杨迪(51)Int.Cl.G01C21/20(2006.01)权利要求书5页说明书10页附图1页(54)发明名称考虑洋流影响的基于因子图的主从式AUV协同定位方法(57)摘要本发明提供考虑洋流影响的基于因子图的主从式AUV协同定位方法，考虑了海水洋流对AUV定位精度的影响，将东向洋流速度、北向洋流速度引入系统因子图中，参与从AUV的定位计算，提高待定位从AUV定位精度。本发明考虑洋流影响的基于因子图的主从式AUV协同定位方法与忽略洋流影响的基于因子图的主从式AUV协同定位方法，和考虑洋流影响的基于EKF的主从式AUV协同定位方法进行仿真对比。考虑洋流影响的基于因子图的主从式AUV协同定位方法与忽略洋流对AUV定位影响的方法相比，可以有效地提高AUV定位的精度。与考虑洋流影响的基于EKF的主从式AUV协同定位方法相比，由于保留泰勒展开二阶项而获得了更高的定位精度。CN114577211ACN114577211A权利要求书1/5页1.考虑洋流影响的基于因子图的主从式AUV协同定位方法，其特征在于，包括以下步骤：步骤1：建立考虑洋流影响的主从式AUV协同定位系统的状态方程；步骤2：建立考虑洋流影响的主从式AUV协同定位系统的量测方程；步骤3：建立考虑洋流影响的主从式AUV协同定位系统的因子图模型；步骤4：根据步骤3建立的系统因子图模型，计算待定位的从AUV时间更新后的东向位置、北向位置、东向洋流速度和北向洋流速度；步骤5：根据步骤3建立的系统因子图模型，计算待定位的从AUV量测更新后的东向位置、北向位置、东向洋流速度和北向洋流速度；步骤6：通过步骤4和步骤5，计算待定位的从AUV的东向位置、北向位置、东向洋流速度和北向洋流速度。2.根据权利要求1所述的考虑洋流影响的基于因子图的主从式AUV协同定位方法，其特征在于，所述步骤1建立的考虑洋流影响的主从式AUV协同定位系统状态方程为：xk＝xk‑1+vx，k‑1t+dxk‑1cosθk‑1‑dyk‑1sinθk‑1yk＝yk‑1+vy，k‑1t+dxk‑1sinθk‑1+dyk‑1cosθk‑1其中，xk‑1、yk‑1和xk、yk分别为k‑1时刻和k时刻待定位的从AUV在地理坐标系东向和北向的位置分量；vx,k‑1、vy,k‑1和vx,k、vy,k分别为k‑1时刻和k时刻洋流在地理坐标系东向和北向的速度分量；dxk‑1,dyk‑1,θk‑1分别为待定位的从AUV在载体坐标系右向位移增量、前向位移增量和航向角，分别由从AUV搭载的多普勒计程仪和罗经系统给出；和为洋流过程噪声；t是采样周期。3.根据权利要求1所述的考虑洋流影响的基于因子图的主从式AUV协同定位方法，其特征在于，所述步骤2建立考虑洋流影响的主从式AUV协同定位系统的量测方程具体为：其中，为k时刻主AUV的东向位置和北向位置，由主AUV搭载的高精度组合导航系统给出；δk是符合零均值高斯白噪声的距离量测噪声。4.根据权利要求1所述的考虑洋流影响的基于因子图的主从式AUV协同定位方法，其特征在于，所述步骤3因子图模型包含xk‑1、yk‑1、xk、yk、vx,k‑1、vy,k‑1、vx,k、vy,k、dk变量节点和Ak‑1,k、Bk‑1,k、Ck‑1,k、Dk‑1,k、Ek、函数节点，函数节点和的约束函数表示他们分别连接的xk‑1、yk‑1、vx,k‑1、vy,k‑1和dk变量节点的概率密度函数。5.根据权利要求4所述的考虑洋流影响的基于因子图的主从式AUV协同定位方法，其特征在于，所述步骤4计算待定位的从AUV时间更新后的东向位置、北向位置、东向洋流速度和北向洋流速度具体为：计算k时刻待定位的从AUV时间更新后的东向位置均值和北向位置均值2CN114577211A权利要求书2/5页其中：b1＝cos(μθ,k‑1)+μθ,k‑1sin(μθ,k‑1)，b2＝‑sin(μθ,k‑1)+μθ,k‑1cos(μθ,k‑1)，b3＝‑a1μθ,k‑1。b4＝sin(μθ,k‑1)‑μθ,k‑1cos(μθ,k‑1)，b5＝cos(μθ,k‑1)+μθ,k‑1sin(μθ,k‑1)，b6＝‑a2μθ,k‑1。其中：为k时刻东向位置均值和北向位置均值的方差，为k‑1时刻计算的从AUV的东向位置均值，为k‑1时刻计算的从AUV的北向位置均值，为k‑1时刻的东向位置均值和北向位置均值的方差，