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

摘要

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

主权项

一种基于信息滤波的协同导航方法，其特征在于，包括以下步骤：步骤一、建立坐标系地理坐标系：OX_tY_tZ_t地理坐标系表示载体所处的东北天坐标系，其原点O选在载体重心处，X_t指向东，Y_t指向北，Z_t沿垂线方向指向天；载体坐标系：OX_bY_bZ_b载体坐标系固连在本身，其坐标原点O位于载体的重心处，X_b沿横轴指向右，Y_b沿纵轴指向前，Z_b垂直于OX_bY_b，并沿载体的竖直轴指向上；步骤二、建立水面艇编队协同导航的运动模型；记k时刻整个系统状态为即横坐标x_k、纵坐标y_k和航向角速度v在坐标系内沿纵轴指向前，它在地理系的投影和航向角有关，在东向轴的投影为在北向轴的投影为则跟随艇运动方程可表示为：$\left\{\begin{array}{c}{x}_{k+1}=x_k+\Delta t·v_k·{\mathrm{cos\phi }}_k\\ y_{k+1}=y_k+\Delta t·v_k·{\mathrm{sin\phi }}_k\\ {\phi }_{k+1}={\phi }_k+\omega ·\Delta t\end{array}\right.---\left(1\right)$式中v_k，分别为k时刻跟随艇的速度、航向角，v_k+1，均受高斯白噪声干扰；Δt为采样周期，ω为航向角变化角速率；运动方程简写为：x_k+1＝f(x_k，u_k，w_k)＝x_k+Г(u_k+w_k)      (2)式中表示跟随艇在k时刻的状态；u_k＝(v_k φ_k)^T；Г(u_k+w_k)为非线性项，w_k为高斯白噪声，且噪声的方差矩阵为：式中，对角线上的元素分别为v_k和的方差；步骤三、水面艇协同导航观测方程的建立；根据跟随艇从时刻t_k到时刻t_k+1运动的距离矢量，将t_k时刻跟随艇与领航艇的几何位置平移到t_k+1时刻，t_k时刻、t_k+1时刻以领航艇为圆心、两艇间距为半径的两个圆有交点；通过几何关系，可得到时刻t_k和时刻t_k+1两条艇间距离的表达式：$R_k^2={(x_{k+1}^S-d_{x_{k,k+1}}-x_k^M)}^2+{(y_{k+1}^S-d_{y_{k,k+1}}-y_k^M)}^2$    (4)$R_{k+1}^2={(x_{k+1}^S-x_{k+1}^M)}^2+{(y_{k+1}^S-y_{k+1}^M)}^2$式中，右上角标S和M分别表示跟随艇和领航艇，代表跟随艇由t_k时刻到t_k+1时刻沿着x轴经过的距离，代表跟随艇由t_k时刻到t_k+1时刻沿着y轴经过的距离；列出量测方程如下：$\begin{array}{c}{Z}_{k+1}=h(x_{k+1}^S,y_{k+1}^S,x_k^M,y_k^M,x_{k+1}^M,y_{k+1}^M,d_{x_{k,k+1}},d_{y_{k,k+1}})+w_z\\ =\left[\begin{array}{c}{R}_k^2\\ R_{k+1}^2\end{array}\right]+w_z\end{array}---\left(5\right)$式中，Z_k+1为k+1时刻的观测量，w_z为观测噪声；步骤四、水面艇协同导航系统模型的线性化；线性化以后的状态转移矩阵为：${\Phi }_{k+1,k}=\frac{\partial f}{\partial X_k^T}{|}_{X_k={\hat{X}}_k}\approx I+\Delta t·\frac{\partial f}{\partial {X_k}^T}=I+\Delta t·\left[\begin{array}{ccc}1& 0& -\Delta t·v_k{\mathrm{sin\phi }}_k\\ 0& 1& \Delta t·v_k{\mathrm{cos\phi }}_k\\ 0& 0& 1\end{array}\right],$式中，I为单位阵；线性化系统噪声激励矩阵为：$G_k=\frac{\partial f}{\partial {u_k}^T}=\left[\begin{array}{cc}\Delta t·{\mathrm{cos\phi }}_k& 0\\ \Delta t·{\mathrm{sin\phi }}_k& 0\\ 0& \Delta t\end{array}\right];$线性化系统量测矩阵可表示为：$H_{k+1}=\frac{\partial Z_{k+1}}{\partial {X_{k+1}}^T}=\left[\begin{array}{ccc}2*\left(x_{k+1}^S-d_{x_{k,k+1}}-x_k^M\right)& 2*\left(y_{k+1}^S-d_{y_{k,k+1}}-y_k^M\right)& 0\\ 2*\left(x_{k+1}^S-x_{k+1}^M\right)& 2*\left(y_{k+1}^S-y_{k+1}^M\right)& 0\end{array}\right]---\left(6\right)$步骤五、信息滤波更新过程$I_k=I_{k/k-1}+H_k^T{R}_k^{-1}{H}_k---\left(7b\right)$$K_k=I_k^{-1}{H}_k^T{R}_k^{-1}---\left(7c\right)$${\hat{X}}_{k/k-1}={\Phi }_{k,k-1}{\hat{X}}_{k-1}---\left(7d\right)$${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})---\left(7e\right)$上式中，I_k/k‑1为状态更新之后的信息矩阵，I_k是量测更新之后的信息矩阵，K_k为滤波增益，为状态更新之后的状态，是量测更新之后的状态。