CN102494685B - 用于wsn/ins高精度实时组合导航的h无穷信息融合方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明涉及一种用于WSN/INS高精度实时组合导航的H∞信息融合方法，属于多传感器数据融合领域。

背景技术

对未知节点精确位置信息的需求推动了精确定位技术的发展，这种趋势在未来很长一段时间里仍将保持不变。近年来，无线传感器网络(WirelessSensors Network，WSN)以其低成本、低功耗和低系统复杂度的特点在短距离定位领域表现出很大的潜力。WSN为在无GPS信号地区，即所谓的“盲区”时，如室内、高层建筑密集的市区、矿井、隧道等环境下未知节点定位提供了可能，但由于WSN采用的通信技术通常为短距离无线通信技术(如ZigBee、WIFI等)，因此若想完成长距离的目标跟踪定位，需要大量的网络节点共同完成，这给WSN的实际网络拓扑实现带来了一定困难，同时也与WSN低成本、低功耗的特点不相符

微惯性导航系统(MEMS inertial navigation system，MINS)具有全自主、运动信息全面、短时、高精度的优点，虽然可以实现自主导航，但误差随时间积累，长航时运行条件下将导致导航精度严重下降。为了克服在无GPS导航信号的密闭环境下单独使用MINS长航时运行时导航精度严重下降的问题，本发明提出将WSN和INS进行组合的高精度实时组合导航系统。利用基于WSN的无线定位技术测量未知节点的相对导航信息，将相对导航信息与MINS上得到的绝对导航信息通过滤波器进行数据融合，得到理想的导航信息，充分利用了未知节点的环境信息，实现了相对导航信息与绝对导航信息的融合，得到一种比上述任何单一导航方法精度更高的导航信息。

在处理组合导航数据融合的问题时，最常用的方法是使用滤波器。最著名的方法是卡尔曼滤波器(Kalman Filte，KF)，然而由于KF是面向线性系统设计的，因此仅对线性定常系统有很好的滤波效果。为了能够实现非线性系统的滤波问题，许多学者通过雅克比矩阵将非线性的系统转化为线性系统进行滤波，从而产生了扩展卡尔曼滤波器(Extended Kalman Filter，EKF)。EKF对非线性系统有很好的滤波效果，是目前实际生产中应用较为广泛的一种滤波方法。但是由于KF、EKF在滤波过程中假设系统的状态噪声和观测噪声均为标准白噪声，所以并不完全符合实际应用中的情况。另外由于设备的硬件问题，导致每一时刻数据的采集周期有小幅不确定的情形，数据获取并不能按理想情况进行，而现有的KF、EKF并没有考虑这种情况。

发明内容

为了解决上述问题，本发明提供了一种用于WSN/INS高精度实时组合导航的H∞信息融合方法，将INS(惯性导航系统)、WSN(无线传感器网络)采集到的未知节点的信息进行集成，在相对坐标系中采用H∞滤波方法。该方法以每个时刻未知节点的在两个方向上的位置误差和速度误差作为状态向量，以每一时刻根据两种系统采集得到的数据计算出的位置误差和速度误差作为观测向量输出，由状态向量和观测向量输出重新构建一个渐近稳定的系统，即H∞滤波器。通过H∞滤波器来预估每一时刻最优的状态向量。这种方法克服传统滤波方法(如KF、EKF)对系统的噪声有特定要求的问题，更适合实际应用中的多传感器数据融合。与此同时，在构建H∞滤波器的系统方程中加入了对数据采集周期的不确定度描述，更加逼近实际中采用周期由于硬件原因导致每一时刻的采集周期有小幅不确定的情形。这种滤波器得到的状态预估比传统的滤波方法精度更高，滤波器的鲁棒性更强。

本发明为解决其技术问题采用如下技术方案：

1、在有WSN信号的培训区域内，利用采用超声波与ZigBee/WIFI等短距离通信技术相组合的无线定位技术测量的相对导航信息，与MINS上得到的绝对导航信息通过扩展卡尔曼滤波器/H∞滤波器进行数据融合，得到的导航信息，比上述任何单一导航方法得到的信息精度更高。

2、MINS利用在培训区域训练的误差模型对测量的绝对导航信息进行误差补偿，得到最优的导航信息，避免了因WSN导航无法工作导致导航误差快速下降、无法保证导航精度的问题。

3、数据融合采用的滤波器的系统(如式1所示)以每个时刻未知节点的在两个方向上的位置误差(e_x，e_y)和速度(e_vx，e_vy)误差作为状态向量。将同一时刻INS和WSN测量得到的位置和速度信息的差值作为系统的观测量。

$\left\{\begin{array}{c}{X}_{k+1}=(A+E\·{\mathrm{\β}}_k)X_k+B_1{\mathrm{\ζ}}_k\\ Y_k=C{X}_k+D_1{\mathrm{\ζ}}_k\\ Z_k=L{X}_k\end{array}\right.---\left(1\right)$

依靠上述系统构建一个渐近稳定的系统(如式2所示)，根据系统每个时刻的状态向量和观测量输出，对系统中需要预估的值进行预估。

$\left\{\begin{array}{cc}{\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},K\end{array}\right.---\left(2\right)$

4、具体的实现方法是，首先设 由式(1)和式(2)得到式(3)。

其中

4、如果线性矩阵不等式组(4)存在可行解P＝P′＞0，Q，则系统(3)是渐进稳定的，并且存在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& -{\mathrm{\&lambda;}}^{2}I& {B_1}^{\&prime;}P-D_1^{\&prime;}{Q}^{\&prime;}& 0& 0& 0& 0& 0\\ \mathrm{PA}-\mathrm{QC}& P{B}_1-Q{D}_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(4\right)$

本发明能满足地面城市交通、狭长隧道、小型智能机器人等中低精度的定位和定向的要求。

附图说明

图1为WSN/MINS高精度实时组合导航系统。

图2为组合导航系统利用H_∞滤波组合导航方法示意图。

图3为自适应补偿区域组合导航模型。

具体实施方式

下面结合附图对本发明创造做进一步详细说明

如图1所示，本发明用于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}{\mathrm{\&omega;}}_{x,k}\\ {\mathrm{\&omega;}}_{\mathrm{vx},k}\\ {\mathrm{\&omega;}}_{y,k}\\ {\mathrm{\&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]{\mathrm{\&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}{\mathrm{\&omega;}}_{x,k}\\ {\mathrm{\&omega;}}_{\mathrm{vx},k}\\ {\mathrm{\&omega;}}_{y,k}\\ {\mathrm{\&omega;}}_{\mathrm{vy},k}\end{array}\right]---\left(2a\right)$

即X_k+1＝(A+E·β_k)X_k+B_ω  (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}{\mathrm{\&upsi;}}_{x,k}\\ {\mathrm{\&upsi;}}_{\mathrm{vx},k}\\ {\mathrm{\&upsi;}}_{y,k}\\ {\mathrm{\&upsi;}}_{\mathrm{vy},k}\end{array}\right]---\left(3a\right)$

即Y_k＝CX_k+Dυ_k  (3b)

综上所述，系统方程如式4所示，其中Z_k＝LX_k为需要估计的量与状态向量的线性关系；

$\left\{\begin{array}{c}{X}_{k+1}=(A+E\&CenterDot;{\mathrm{\&beta;}}_k)X_k+B_1{\mathrm{\&zeta;}}_k\\ Y_k=C{X}_k+D_1{\mathrm{\&zeta;}}_k\\ Z_k=L{X}_k\end{array}\right.---\left(4\right)$

(4)根据系统方程每个时刻的X和Y的输出，来对系统方程中需要预估的值进行预估：

$\left\{\begin{array}{cc}{\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},K\end{array}\right.---\left(5\right)$

(5)首先 由式(4)和式(5)得到式(6)

其中

(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& -{\mathrm{\&lambda;}}^{2}I& {B_1}^{\&prime;}P-D_1^{\&prime;}{Q}^{\&prime;}& 0& 0& 0& 0& 0\\ \mathrm{PA}-\mathrm{QC}& P{B}_1-Q{D}_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).$

如图2为有WSN信号的培训区域组合导航模型，在这一培训区域，WSN导航系统与MINS导航系统同时工作，将通过WSN获得的导航信息与微惯性导航系统上得到的导航信息通过H∞滤波器进行数据融合；在采用滤波器进行数据融合，并得出最优导航信息的同时，将滤波器做出误差信息和时间信息通过人工智能算法培训MINS的误差模型，寻找二者之间的关系。

若未知节点离开搭建有WSN的区域，则组合导航系统进入自适应补偿区域，如图3所示。在这一区域，组合导航系统获取不到WSN测量的相对导航信息，只能依靠MINS系统完成这一部分的自主导航。MINS利用在培训区域训练的误差模型对测量的绝对导航信息进行误差补偿，得到最优的导航信息，避免了因MINS/WSN导航无法工作导致导航误差快速下降、无法保证导航精度的问题，具有一定的可行性和前瞻性。

Claims (1)

1.一种用于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}{\mathrm{\&omega;}}_{x,k}\\ {\mathrm{\&omega;}}_{\mathrm{vx},k}\\ {\mathrm{\&omega;}}_{y,k}\\ {\mathrm{\&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]{\mathrm{\&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}{\mathrm{\&omega;}}_{x,k}\\ {\mathrm{\&omega;}}_{\mathrm{vx},k}\\ {\mathrm{\&omega;}}_{y,k}\\ {\mathrm{\&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}{\mathrm{\&upsi;}}_{x,k}\\ {\mathrm{\&upsi;}}_{\mathrm{vx},k}\\ {\mathrm{\&upsi;}}_{y,k}\\ {\mathrm{\&upsi;}}_{\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& {-\mathrm{\&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).$