[发明专利]一种应用联邦H∞滤波器实现组合导航精确定位的方法及系统

权利要求书

1.一种应用联邦H_∞滤波器实现组合导航精确定位的方法，其特征是，包括步骤如下：

(1)将WSN网络中的参考节点分簇：在WSN中，将自身位置已知的节点定义为参考节点RN，将自身位置未知的节点定义为未知节点BN，将在一定区域内的RN节点归为一簇，每簇中随机选择一个节点做为本簇的簇首节点，负责采集本簇中所有RN节点采集到的数据，并负责将这些信息与其他簇进行数据交换；

(2)判断BN节点在哪一个簇的覆盖范围内，根据获得的能够覆盖的簇的个数选择定位方式：INS采集到的信息为每个时刻通过自身陀螺加表测量的数据经过解算得到的当前未知节点BN的位置和速度，WSN采集到的信息为每个时刻BN节点与RN节点之间的距离信息，BN节点通过WSN获取RN节点传来的信号，根据接收到的RN节点的信号判断BN节点在哪一个簇的覆盖范围内，根据获得的能够覆盖的簇的个数选择定位方式：若能够覆盖BN节点的簇仅有一个，则进行单簇定位，若能够覆盖BN节点的簇大于一个，即BN节点处于簇重叠的位置，则进行多簇融合定位；

(3)单簇定位或多簇融合定位：

单簇定位：INS测量RN节点与BN节点之间的距离，将距离信息输入局部H_∞滤波器，对INS测量的BN的位置误差和速度误差进行预估，将INS采集到的信息与预估值作差，得出结果修正的导航信息；

多簇融合定位：a.在覆盖BN节点的每个簇中，INS测量该簇内RN节点与BN节点之间的距离，将距离信息输入该簇的局部滤波器处理，对每簇中INS测量的BN的位置误差和速度误差进行预估；b.在局部滤波器得到的误差估计的基础上，采用分布式H_∞滤波器，对局部滤波器得到的数据进行数据融合，得到系统中INS测量的位置误差和速度误差最优预估值；c.最后将INS本身测出的导航信息与最优预估值作差，得出结果修正的导航信息。

2.根据权利要求1所述的一种应用联邦H_∞滤波器实现组合导航精确定位的方法，其特征是，步骤(3)的步骤a中对每簇中INS测量的BN的位置误差和速度误差进行预估的方法为：

①以每个时刻未知节点在本地相对坐标系的x和y两个方向上的位置误差(e_x，e_y)和速度误差(e_vx，e_vy)作为状态向量构建局部滤波器的系统方程如式(I)所示：

式(I)中，X_c，k为系统的状态变量，T为系统的采样周期，(ω_x，k，ω_y，k)和(ω_vx，k，ω_vy，k)为系统方程每个时刻的位置误差噪声和速度误差噪声，为随机值，ω_c，k为系统的噪声向量，A_c为系统矩阵；

②采用INS测出当前时刻BN节点的位置和速度，采用WSN测出当前时刻BN节点的位置和速度，将INS测出当前时刻BN节点的位置减去WSN测出当前时刻BN节点的位置，得到位置误差；将INS测出当前时刻BN节点的速度减去WSN测出当前时刻BN节点的速度，得到速度误差，将计算出的位置误差和速度误差作为观测向量输出，如下式(II)所示：

式(II)中，y_c，k为观测向量，X_c，k为系统的状态变量，(Δr_x，k，Δr_y，k)和(Δv_x，k，Δv_y，k)为观测方程每个时刻的位置误差和速度误差，(υ_x，k，υ_y，k)和(υ_vx，k，υ_vy，k)为观测方程每个时刻的位置误差噪声和速度误差噪声，为随机值，υ_c，k为系统的噪声向量，C_c为观测矩阵；

③用上述得到的式(I)和式(II)，加入每个簇需要估计的量与状态向量的线性关系，即z_c，k＝L_cx_c，k，得到每个簇的系统状态方程，如式(III)所示，

xc,k+1=Acxc,k+Bcwc,kyc,k=Ccxc,k+Dcξc,kzc,k=Lcxc,kk=0,1,2,...---(III)]]>

其中，x_c，k∈Rⁿ为第c个簇的系统状态，A_c为每个局部滤波器的系统矩阵，B_c为每个局部滤波器的系统噪声矩阵，这里取单位阵，C_c为每个局部滤波器的观测矩阵，D_c为每个局部滤波器的观测噪声矩阵，这里取单位阵，w_c，k和ξ_c，k分别为每个簇的系统噪声和观测噪声，为随机值；

④根据每个簇的系统状态和观测方程，应用式(III)中的x_c，k+1和y_c，k，定义下式：

x^c,k+1=Acx^c,k+Kc(yc,k-Ccx^c,k)z^c,k=Lcx^c,k---(IV)]]>

其中和分别为x_c，k和z_c，k的观测值，K_c为卡尔曼增益，定义x~c,k=xc,k-x^c,,]]>z~c,k=zc,k-z^c,k,]]>得到式(V)：

x~c,k+1=A~cx~c,k+B~cζi,kz~c,k=zc,k-z^c,k=Lcx~c,k---(V)]]>

其中，A~c=Ac-KcCc,]]>B~c=B1-KcDc,]]>

通过式(V)得到每簇中INS测量的BN的位置误差和速度误差预估向量。