用于WSN/INS高精度实时组合导航的H无穷信息融合方法

摘要

本发明公布了一种用于WSN/INS高精度实时组合导航的H无穷信息融合方法，该方法以每个时刻未知节点的在两个方向上的位置误差和速度误差作为状态向量，以每一时刻根据两种系统采集得到的数据计算出的位置误差和速度误差作为观测向量输出，由状态向量和观测向量输出重新构建一个渐近稳定的系统，即H无穷滤波器。通过H∞滤波器来预估每一时刻最优的状态向量。与此同时，在构建H∞滤波器的系统方程中加入了对数据采集周期的不确定度描述，更加逼近实际中采用周期由于硬件原因导致每一时刻的采集周期有小幅不确定的情形。这种滤波器得到的状态预估比传统的滤波方法精度更高，滤波器的鲁棒性更强。

主权项

一种用于WSN/INS高精度实时组合导航的H无穷信息融合方法，其特征在于包括下列步骤：(1)以每个时刻未知节点的在两个方向上的位置误差(e_x,e_y)和速度误差(e_vx,e_vy)作为状态向量，系统方程如式(1)所示：$\left[\begin{array}{c}{e}_{x,k+1}\\ e_{y,k+1}\\ e_{\mathrm{vx},k+1}\\ e_{\mathrm{vy},k+1}\end{array}\right]=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{e}_{x,k}\\ e_{y,k}\\ e_{\mathrm{vx},k}\\ e_{\mathrm{vy},k}\end{array}\right]+\left[\begin{array}{c}{\omega }_{x,k}\\ {\omega }_{\mathrm{vx},k}\\ {\omega }_{y,k}\\ {\omega }_{\mathrm{vy},k}\end{array}\right]---\left(1\right)$(2)对采样周期T的随机不确定度描述如下所示：$\left[\begin{array}{c}{e}_{x,k+1}\\ e_{y,k+1}\\ e_{\mathrm{vx},k+1}\\ e_{\mathrm{vy},k+1}\end{array}\right]=\{\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]+\left[\begin{array}{cccc}0& 0& \mathrm{\delta t}& 0\\ 0& 0& 0& \mathrm{\delta t}\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]{\beta }_k\}\left[\begin{array}{c}{e}_{x,k}\\ e_{y,k}\\ e_{\mathrm{vx},k}\\ e_{\mathrm{vy},k}\end{array}\right]+\left[\begin{array}{c}{\omega }_{x,k}\\ {\omega }_{\mathrm{vx},k}\\ {\omega }_{y,k}\\ {\omega }_{\mathrm{vy},k}\end{array}\right]---\left(2a\right)$即X_k+1＝(A+E·β_k)X_k+Bω_k  (2b)(3)采用每个时刻通过的位置误差和速度误差作为观测量：通过将同一时刻INS和WSN测量得到的位置和速度信息做差得到：$\left[\begin{array}{c}{\mathrm{\Delta r}}_{x,k}\\ {\mathrm{\Delta r}}_{x,k}\\ {\mathrm{\Delta v}}_{x,k}\\ {\mathrm{\Delta v}}_{y,k}\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{e}_{x,k}\\ e_{y,k}\\ e_{\mathrm{vx},k}\\ e_{\mathrm{vy},k}\end{array}\right]+\left[\begin{array}{c}{\upsilon }_{x,k}\\ {\upsilon }_{\mathrm{vx},k}\\ {\upsilon }_{y,k}\\ {\upsilon }_{\mathrm{vy},k}\end{array}\right]---\left(3a\right)$即Y_k＝CX_k+Dυ_k  (3b)综上所述，系统方程如式4所示，其中Z_k＝LX_k为需要估计的量与状态向量的线性关系；(4)根据系统方程每个时刻的X和Y的输出，来对系统方程中需要预估的值进行预估：$\left\{\begin{array}{c}{\hat{X}}_{k+1}=A_f{\hat{X}}_k+B_f{Y}_k\\ {\hat{Z}}_k=L_f{\hat{X}}_k,k=\mathrm{0,1},2,...\end{array}\right.---\left(5\right)$(5)首先由式(4)和式(5)得到式(6)其中$\tilde{A}=A_f,$$\tilde{B}=B_1-B_f{D}_1;$(6)如果线性矩阵不等式组(7)存在可行解P＝P′>0，系统(6)是渐进稳定的，并且存在H无穷性能指标γ，γ的大小决定了滤波器的误差大小,γ越大约束越小,鲁棒性越差$\left[\begin{array}{cccccccc}-P& 0& A^{\prime }P-C^{\prime }{Q}^{\prime }& 0& A^{\prime }P-C^{\prime }{Q}^{\prime }& L^{\prime }& E^{\prime }P& 0\\ 0& {-\lambda }^{2}I& {B_1}^{\prime }P-D_1^{\prime }{Q}^{\prime }& 0& 0& 0& 0& 0\\ \mathrm{PA}-\mathrm{QC}& {\mathrm{PB}}_1-{\mathrm{QD}}_1& -P& 0& 0& 0& 0& 0\\ 0& 0& 0& -P& 0& 0& 0& 0\\ \mathrm{PA}-\mathrm{QC}& 0& 0& 0& -P& 0& 0& 0\\ L& 0& 0& 0& 0& -I& 0& 0\\ \mathrm{PE}& 0& 0& 0& 0& 0& -P& 0\\ 0& 0& 0& 0& 0& 0& 0& -P\end{array}\right]<0---\left(7\right).$