[发明专利]一种提高水面艇编队导航精度的信息滤波方法

摘要

本发明公开了一种提高水面艇导航精度的信息滤波方法，涉及一种协同导航技术。实现了低精度水面艇利用其它艇高精度导航信息提高自身定位精度。其方法是：建立相应坐标系；在所建坐标系的基础上列写水面艇协同导航的运动模型和观测模型；将非线性模型进行线性化；利用信息滤波进行时间更新和观测更新。本发明适用于水面艇的实时导航定位。

主权项

1.一种基于信息滤波的协同导航方法，其特征在于，包括以下步骤：步骤一、建立坐标系地理坐标系(下标为t)：OX_tY_tZ_t地理坐标系表示载体所处的东北天坐标系，其原点O选在载体重心处，X_t指向东，Y_t指向北，Z_t沿垂线方向指向天(东北天)。载体坐标系(下标为b)：OX_bY_bZ_b载体坐标系固连在本身，其坐标原点O位于载体的重心处，X_b沿横轴指向右，Y_b沿纵轴指向前，Z_b垂直于OX_bY_b，并沿载体的竖直轴指向上。步骤二、建立水面艇编队协同导航的运动模型。记k时刻整个系统状态为X_k＝[x_k  y_k  ψ_k]^T，即横坐标x_k、纵坐标y_k和航向角ψ_k。速度v在坐标系内沿纵轴指向前，它在地理系的投影和航向角有关，在东向轴的投影为v·sinψ，在北向轴的投影为v·cosψ，则跟随艇运动方程可表示为：xk+1=xk+Δt·vk·cosψkyk+1=yk+Δt·vk·sinψkψk+1=ψk+ω·Δt---(1)]]>式中v_k，ψ_k分别为k时刻跟随艇的速度、航向角，v_k+1，ψ_k均受高斯白噪声干扰。Δt为采样周期，ω为航向角变化角速率。运动方程简写为：x_k+1＝f(x_k，u_k，w_k)＝x_k+Γ(u_k+w_k)           (2)式中x_k＝[x_k  y_k  ψ_k]^T，表示跟随艇在k时刻的状态；u_k＝[v_k  ψ_k]^T；Γ(u_k+w_k)为非线性项，w_k为高斯白噪声，且噪声的方差矩阵为：Qk=E(wkwkT)=σvk200σφk2---(3)]]>式中，对角线上的元素分别为v_k和ψ_k的方差。步骤三、水面艇协同导航观测方程的建立。根据跟随艇从时刻t_k到时刻t_k+1运动的距离矢量，将t_k时刻跟随艇与领航艇的几何位置平移到t_k+1时刻，t_k时刻、t_k+1时刻以领航艇为圆心、两艇间距为半径的两个圆有交点。通过几何关系，可得到时刻t_k和时刻t_k+1两条艇间距离的表达式：Rk2=(xk+1S-dxk,k+1-xkM)2+(yk+1S-dyk,k+1-ykM)2]]>        (4)Rk+12=(xk+1S-xk+1M)2+(yk+1S-yk+1M)2]]>式中，右上角标S和M分别表示跟随艇和领航艇，代表跟随艇由t_k时刻到t_k+1时刻沿着x轴经过的距离，代表跟随艇由t_k时刻到t_k+1时刻沿着y轴经过的距离。列出量测方程如下：Zk+1=h(xk+1S,yk+1S,xkM,ykM,xk+1M,yk+1M,dxk,k+1,dyk,k+1)+wz]]>=Rk2Rk+12+wz---(5)]]>式中，Z_k+1为k+1时刻的观测量，w_z为观测噪声。步骤四、水面艇协同导航系统模型的线性化。线性化以后的状态转移矩阵为：Φk+1,k=∂f∂XkT|Xk=X^k≈I+Δt·∂f∂XkT=I+Δt·10-Δt·Vksinψk01Δt·Vkcosψk001,]]>式中，I为单位阵。线性化系统噪声激励矩阵为：Gk=∂f∂ukT=Δt·cosψk0Δt·sinψk00Δt.]]>线性化系统量测矩阵可表示为：Hk+1=∂Zk+1∂Xk+1T=2*(xk+1S-dxk,k+1-xkM)2*(yk+1S-dyk,k+1-ykM)02*(xk+1S-xk+1M)2*(yk+1S-yk+1M)0---(6)]]>步骤五、信息滤波更新过程Ik/k-1=Qk-1-1-Qk-1-1Φk,k-1(Ik-1+Φk,k-1TQk-1-1Φk,k-1)-1Φk,k-1TQk-1-1---(7a)]]>Ik=Ik/k-1+HkTRk-1Hk---(7b)]]>Kk=Ik-1HkTRk-1---(7c)]]>X^k/k-1=Φk,k-1X^k-1---(7d)]]>X^k=X^k/k-1+Kk(Zk-HkX^k/k-1)---(7e)]]>上式中，I_k/k-1为状态更新之后的信息矩阵，I_k是量测更新之后的信息矩阵，K_k为滤波增益，为状态更新之后的状态，是量测更新之后的状态。