一种提高水面艇编队导航精度的信息滤波方法技术

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

【技术实现步骤摘要】

【技术保护点】
一种基于信息滤波的协同导航方法，其特征在于，包括以下步骤：步骤一、建立坐标系地理坐标系(下标为t)：OXtYtZt地理坐标系表示载体所处的东北天坐标系，其原点O选在载体重心处，Xt指向东，Yt指向北，Zt沿垂线方向指向天(东北天)。载体坐标系(下标为b)：OXbYbZb载体坐标系固连在本身，其坐标原点O位于载体的重心处，Xb沿横轴指向右，Yb沿纵轴指向前，Zb垂直于OXbYb，并沿载体的竖直轴指向上。步骤二、建立水面艇编队协同导航的运动模型。记k时刻整个系统状态为Xk＝[xk??yk??ψk]T，即横坐标xk、纵坐标yk和航向角ψk。速度v在坐标系内沿纵轴指向前，它在地理系的投影和航向角有关，在东向轴的投影为v·sinψ，在北向轴的投影为v·cosψ，则跟随艇运动方程可表示为：xk+1=xk+Δt·vk·cosψkyk+1=yk+Δt·vk·sinψkψk+1=ψk+ω·Δt---(1)式中vk，ψk分别为k时刻跟随艇的速度、航向角，vk+1，ψk均受高斯白噪声干扰。Δt为采样周期，ω为航向角变化角速率。运动方程简写为：xk+1＝f(xk，uk，wk)＝xk+Γ(uk+wk)???????????(2)式中xk＝[xk??yk??ψk]T，表示跟随艇在k时刻的状态；uk＝[vk??ψk]T；Γ(uk+wk)为非线性项，wk为高斯白噪声，且噪声的方差矩阵为：Qk=E(wkwkT)=σvk200σφk2---(3)式中，对角线上的元素分别为vk和ψk的方差。步骤三、水面艇协同导航观测方程的建立。根据跟随艇从时刻tk到时刻tk+1运动的距离矢量，将tk时刻跟随艇与领航艇的几何位置平移到tk+1时刻，tk时刻、tk+1时刻以领航艇为圆心、两艇间距为半径的两个圆有交点。通过几何关系，可得到时刻tk和时刻tk+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分别表示跟随艇和领航艇，代表跟随艇由tk时刻到tk+1时刻 沿着x轴经过的距离，代表跟随艇由tk时刻到tk+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)式中，Zk+1为k+1时刻的观测量，wz为观测噪声。步骤四、水面艇协同导航系统模型的线性化。线性化以后的状态转移矩阵为：Φ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)上式中，Ik/k?1为状态更新之后的信息矩阵，Ik是量测更新之后的信息矩阵，Kk为滤波增益，为状态更新之后的状态，是量测更...

【技术特征摘要】

【专利技术属性】
技术研发人员：徐博，刘杨，陈春，池姗姗，金辰，王文佳，田学林，郭宇，肖永平，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：