采用距离平方和距离平方变化率的WSN/AHRS紧组合方法

摘要

本发明公开了一种采用距离平方和距离平方变化率的WSN/AHRS紧组合方法，包括：选取导航区域内任意一个参考节点的位置作为坐标原点，构建相对坐标系；在所述相对坐标系中将AHRS即姿态和方位参照系统、WSN即无线传感器网络进行集成，通过扩展卡尔曼滤波器对得到的同步导航数据在导航计算机中进行数据融合；构建扩展卡尔曼滤波器的状态方程和观测方程；将AHRS采集到的当前时刻的未知节点的位置和速度与滤波器输出的AHRS误差做差，最终得到当前时刻的未知节点的位置和速度的最优估计。本发明有益效果：由于该紧组合方法中将距离平方变化率作为数据滤波器的观测向量，因此无需新添设备就可以完成对目标节点速度误差的预估，进而完成对目标节点的速度预估。

主权项

一种采用距离平方和距离平方变化率的WSN/AHRS紧组合方法，其特征是，包括以下步骤：(1)选取导航区域内任意一个参考节点的位置作为坐标原点，并分别选取x向和y向构建相对坐标系；(2)在所述相对坐标系中将AHRS即姿态和方位参照系统、WSN即无线传感器网络进行集成，通过扩展卡尔曼滤波器对得到的同步导航数据在导航计算机中进行数据融合；(3)以AHRS每一时刻在x和y两个方向的位置误差和速度误差作为状态变量，构建扩展卡尔曼滤波器的状态方程；(4)分别计算k时刻AHRS测量得到的未知节点与第i个参考节点之间的距离，以及k时刻AHRS测量得到的未知节点与第i个参考节点的距离平方的变化率；(5)以每一时刻AHRS和WSN各自测量的未知节点与参考节点之间的距离平方之差和距离平方变化率之差作为观测量，构建卡尔曼滤波器的观测方程；(6)将AHRS采集到的当前时刻的未知节点的位置和速度与滤波器输出的AHRS误差做差，最终得到当前时刻的未知节点的位置和速度的最优估计；所述步骤(5)中卡尔曼滤波器的观测方程具体为：$\left[\begin{array}{c}{\mathrm{\delta d}}_1^2\\ {\mathrm{\delta d}}_2^2\\ .\\ .\\ .\\ {\mathrm{\delta d}}_i^2\\ \delta {\stackrel{·}{d}}_1^2\\ \delta {\stackrel{·}{d}}_2^2\\ .\\ .\\ .\\ \delta {\stackrel{·}{d}}_i^2\end{array}\right]=\left[\begin{array}{c}2(x_k^I-x_1){\mathrm{\delta x}}_k+2(y_k^I-y_1){\mathrm{\delta y}}_k-({\mathrm{\delta x}}_k^2+{\mathrm{\delta y}}_k^2)\\ 2(x_k^I-x_2){\mathrm{\delta x}}_k+2(y_k^I-y_2){\mathrm{\delta y}}_k-({\mathrm{\delta x}}_k^2+{\mathrm{\delta y}}_k^2)\\ .\\ .\\ .\\ 2(x_k^I-x_i){\mathrm{\delta x}}_k+2(y_k^I-y_i){\mathrm{\delta y}}_k-({\mathrm{\delta x}}_k^2+{\mathrm{\delta y}}_k^2)\\ 2{\mathrm{vx}}_k^I{\mathrm{\delta x}}_k+2(x_k^I-x_1){\mathrm{\delta vx}}_k-2{\mathrm{\delta x}}_k{\mathrm{\delta vx}}_k+2{\mathrm{vy}}_k^I{\mathrm{\delta y}}_k+2(y_k^I-y_1){\mathrm{\delta vy}}_k-2{\mathrm{\delta y}}_k{\mathrm{\delta vy}}_k\\ 2{\mathrm{vx}}_k^I{\mathrm{\delta x}}_k+2(x_k^I-x_2){\mathrm{\delta vx}}_k-2{\mathrm{\delta x}}_k{\mathrm{\delta vx}}_k+2{\mathrm{vy}}_k^I{\mathrm{\delta y}}_k+2(y_k^I-y_2){\mathrm{\delta vy}}_k-2{\mathrm{\delta y}}_k{\mathrm{\delta vy}}_k\\ .\\ .\\ .\\ 2{\mathrm{vx}}_k^I{\mathrm{\delta x}}_k+2(x_k^I-x_i){\mathrm{\delta vx}}_k-2{\mathrm{\delta x}}_k{\mathrm{\delta vx}}_k+2{\mathrm{vy}}_k^I{\mathrm{\delta y}}_k+2(y_k^I-y_i){\mathrm{\delta vy}}_k-2{\mathrm{\delta y}}_k{\mathrm{\delta vy}}_k\end{array}\right]+v$其中，v为系统的观测噪声，为k时刻AHRS解算的未知节点的x向和y向的位置，(x_i,y_i)为第i个参考节点的x向和y向的位置；(δx_k,δy_k)和(δvx_k,δvy_k)分别为AHRS测量的未知节点在k时刻的x向和y向的位置误差和速度误差；(δx_k+1,δy_k+1)和(δvx_k+1,δvy_k+1)分别为AHRS测量的未知节点在k+1时刻的x向和y向的位置误差和速度误差和加速度误差；为k时刻AHRS测量得到未知节点的x和y向的速度；为k时刻AHRS测量得到未知节点与第i个参考节点之间的距离平方的误差；为k时刻AHRS测量得到的未知节点到第i个参考节点的距离变化率平方的误差。