CN104296741B - 采用距离平方和距离平方变化率的wsn/ahrs紧组合方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于复杂环境下组合定位技术领域，具体涉及一种采用距离平方和距离平方变化率的WSN/AHRS紧组合方法。

背景技术

近年来，随着计算机技术、信息技术、通讯技术、微电子技术的飞速发展，面向小区域的目标跟踪技术的研究与应用，逐渐成为目前该领域的研究热点。然而，在外界无线电信号微弱、电磁干扰强烈等一系列复杂室内环境中，对目标载体导航信息获取的准确性、实时性及鲁棒性有很大的影响。如何将室内环境下获取的有限信息进行有效的融合以满足小区域目标高导航精度的要求，消除外界环境的影响，具有重要的科学理论意义和实际应用价值。

在现有的定位方式中，全球卫星导航系统(Global Navigation SatelliteSystem,GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的位置信息，但是其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道等一些密闭的、环境复杂的场景，GNSS信号被严重遮挡，无法进行有效的工作。

近年来，WSN以其低成本、低功耗和低系统复杂度的特点在短距离局部定位领域表现出很大的潜力。伴随着全国范围内无线网络的普及和使用，学者们提出将基于WSN的目标跟踪应用于GNSS失效环境下的行人导航。这种方式虽然能够实现室内定位，但是由于室内环境复杂多变，WSN信号十分容易受到干扰而导致定位精度下降甚至失锁；除此之外，由于WSN采用的通信技术通常为短距离无线通信技术，因此若想完成大范围的室内目标跟踪定位，需要大量的网络节点共同完成，这必将引入网络组织结构优化设计、多节点多簇网络协同通信等一系列问题。因此现阶段基于WSN的目标跟踪在室内导航领域仍旧面临很多挑战。

为了克服上述两种导航方法需要参考节点并容易产生失锁的缺点，学者们提出将惯性导航系统(Inertial navigation system,INS)应用于小区域目标跟踪领域。特别是随着MEMS元件的产品化,微机械陀螺和加速度计开始尝试用于姿态测量，出现了姿态和方位参照系统(Attitude Heading Reference System,AHRS)，它由3只微机械陀螺、3只微机械加速度计和三轴地磁传感器组成，选用重力向量和地磁向量作为参考向量，为陀螺提供角度修正和零偏估计,实现动态环境下载体姿态控制。AHRS具有全自主、运动信息全面、短时、高精度的优点，虽然可以实现自主导航，但误差随时间积累，长航时运行条件下将导致导航精度严重下降。

发明内容

本发明的目的就是为了解决上述问题，提出了一种采用距离平方和距离平方变化率的WSN/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误差做差，最终得到当前时刻的未知节点的位置和速度的最优估计。

所述步骤(3)中扩展卡尔曼滤波器的状态方程具体为：

其中，(δx_k,δy_k)、(δvx_k,δvy_k)和(δax_k,δay_k)分别为AHRS测量的未知节点在k时刻的x向和y向的位置误差、速度误差和加速度误差，T为滤波器的采样周期，W为滤波器的状态噪声矩阵。

所述步骤(4)中计算k时刻AHRS测量得到的未知节点与第i个参考节点之间的距离的具体方法为：

其中，为k时刻AHRS解算的未知节点的x向和y向的位置，(x_i,y_i)为第i个参考节点的x向和y向的位置；

当前时刻未知节点与第i个参考节点之间的的理论真值为：

其中，为k时刻未知节点在x向和y向的理论位置；则k时刻AHRS测量得到的距离平方的误差为：

而则：

其中，为k时刻AHRS测量得到未知节点与第i个参考节点之间的距离平方的误差；(δx_k,δy_k)为AHRS测量的未知节点在k时刻的x向和y向的位置误差。

所述步骤(4)中计算k时刻AHRS测量得到的未知节点与第i个参考节点的距离平方的变化率的具体方法为：

其中，为k时刻AHRS解算的未知节点的x向和y向的速度；

而理论上的未知节点与第i个参考节点的距离平方的变化率为：

其中，为k时刻AHRS解算的未知节点的x和y向的速度；则k时刻AHRS测量得到的未知节点到第i个参考节点的距离变化率平方的误差为：

而

其中，为k时刻AHRS测量得到的x和y向的速度，则：

其中，为k时刻AHRS测量得到的未知节点到第i个参考节点的距离变化率平方的误差。

所述步骤(5)中卡尔曼滤波器的观测方程具体为：

其中，v为系统的观测噪声，为k时刻AHRS解算的未知节点的x向和y向的位置，(x_m,y_m)为第i个参考节点的x向和y向的位置。

本发明的有益效果是：

由于该方法在扩展卡尔曼滤波器进行数据融合时只需要获得AHRS的位置、速度信息和WSN测量得到的未知节点与已知节点之间的距离信息，而不需要获得WSN测量得到的未知节点的位置、速度信息，因此通过紧组合方法，能够有效的克服传统松组合方式必须至少获得3个以上未知节点的位置、速度信息才能完成数据融合的缺点，充分的利用了导航环境中的资源。

其次，由于在组合模型的推导过程中以AHRS的测量值与其误差的差值替代了参数的真实值，减少了传统紧组合方法由于泰勒展开后忽略二次项对定位精度造成的影响。

最后，由于该紧组合方法中将距离平方变化率作为数据滤波器的观测向量，因此无需新添设备就可以完成对目标节点速度误差的预估，进而完成对目标节点的速度预估，实现对目标节点速度的完全客观。

本导航方法的提出，有效的提高了小区域内目标跟踪的导航精度，可用于室内，地下矿井等密闭复杂环境下的长距离高精度目标定位跟踪。

附图说明

图1为用于采用距离平方和距离平方变化率的WSN/AHRS紧组合方法的系统示意图；

图2为用于采用距离平方和距离平方变化率的WSN/AHRS紧组合方法的控制方法示意图；

图3为本发明的方法流程图。

具体实施方式：

下面结合附图与实施例对本发明做进一步说明：

单一的导航方式容易受到目标周围导航环境的影响，不能提供持续稳定的高精度导航信息,为了克服单一导航技术存在的不同程度的缺陷，本发明提出一种采用距离平方和距离平方变化率的WSN/AHRS紧组合方法，以一个参考节点的位置作为坐标原点，并选取相对坐标系中的x向和y向。在此基础上，滤波器的系统方程以AHRS每一时刻在x和y两个方向的位置误差和速度误差作为状态变量，以每一时刻AHRS和WSN各自测量的未知节点与参考节点之间的距离平方之差和距离平方变化率之差作为观测量。

如图1所示，用于采用距离平方和距离平方变化率的WSN/AHRS紧组合方法的系统，包括参考节点部分和未知节点部分。参考节点部分由多个无线接收模块组成；未知节点部分由无线发送模块、无线路由模块、AHRS模块和控制模块组成。

如图2所示，在采用距离平方和距离平方变化率的WSN/AHRS紧组合方法使用一个滤波器进行数据融合。其中滤波器的系统方程以AHRS每一时刻在x和y两个方向的位置误差和速度误差作为状态变量，滤波器的状态方程为：

其中，(δx_k,δy_k)、(δvx_k,δvy_k)和(δax_k,δay_k)分别为INS测量的未知节点在k时刻的x向和y向的位置误差、速度误差和加速度误差，T为滤波器的采样周期，W为滤波器的状态噪声矩阵；

k时刻AHRS测量得到的未知节点与第i个参考节点之间的距离可通过式(2)得到：

其中，为k时刻AHRS解算的未知节点的x和y向的位置，(x_i,y_i)为第i个参考节点的x和y向的位置；当前时刻的理论真值应为：

其中，为k时刻未知节点在x和y向的理论位置；通过式(2)和式(3)可得：

而将其带入式(4)可得：

由于WSN独立工作下并不能对未知节点的速度进行直接测量，这会导致系统中的速度不可观测，进而影响组合导航系统的精度。为了解决这一问题，需要引入第三方的测速设备完成测速，这种方法虽然能够获取较高的测速精度，实现速度的可观测，但是增加了系统的实现成本，不利于组合导航系统的应用。为了克服这一问题，本专利提出通过采集距离平方变化率之差来完成对未知节点的AHRS速度解算误差的预估。k时刻AHRS测量得到的距离平方的变化率为：

其中，为k时刻AHRS解算的未知节点的x和y向的速度，而理论上的距离平方的变化率为：

其中，为k时刻AHRS解算的未知节点的x和y向的速度；则：

而其中，为k时刻AHRS测量得到的x和y向的速度，将其带入式(8)可得：

滤波器的观测方程为：

其中，v为系统的观测噪声；通过上述滤波器，可以对当前时刻AHRS的导航解算误差进行最优估计；将AHRS采集到的当前时刻的未知节点的位置和速度与滤波器输出的AHRS误差做差，最终得到当前时刻的未知节点的位置和速度的最优估计。

本方法的流程如图3示，方法的具体步骤如下：

(1)选取导航区域内任意一个参考节点的位置作为坐标原点，并分别选取大地坐标系中的东向和北向分别作为x向和y向构建相对坐标系；

(2)在所述相对坐标系中将AHRS即姿态和方位参照系统、WSN即无线传感器网络进行集成，通过扩展卡尔曼滤波器对得到的同步导航数据在导航计算机中进行数据融合；

(3)以AHRS每一时刻在x和y两个方向的位置误差和速度误差作为状态变量，构建扩展卡尔曼滤波器的状态方程；

(4)分别计算k时刻AHRS测量得到的未知节点与第i个参考节点之间的距离，以及k时刻AHRS测量得到的未知节点与第i个参考节点的距离平方的变化率；

(5)以每一时刻AHRS和WSN各自测量的未知节点与参考节点之间的距离平方之差和距离平方变化率之差作为观测量，构建卡尔曼滤波器的观测方程；

(6)将AHRS采集到的当前时刻的未知节点的位置和速度与滤波器输出的AHRS误差做差，最终得到当前时刻的未知节点的位置和速度的最优估计。

组合导航方法的数据融合通过扩展卡尔曼滤波器完成，首先无线接收模块获取参考节点的信号，在此基础上，计算未知节点与参考节点之间的距离的平方和距离平方的变化率，与此同时，AHRS通过采集传感器数据对导航信息进行解算，之后再完成未知节点与参考节点之间的距离的平方和距离平方的变化率的计算，将AHRS测量得到的距离平方和距离平方变化率与WSN测量得到的距离平方与距离平方变化率分别做差，差值作为扩展卡尔曼滤波器的观测向量。通过扩展卡尔曼滤波器对AHRS的导航信息解算误差进行预估，并对AHRS解算的导航信息进行误差补偿。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (4)

1.一种采用距离平方和距离平方变化率的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{\δd}}_1^2\\ {\mathrm{\δd}}_2^2\\ .\\ .\\ .\\ {\mathrm{\δd}}_i^2\\ \mathrm{\δ}{\stackrel{\·}{d}}_1^2\\ \mathrm{\δ}{\stackrel{\·}{d}}_2^2\\ .\\ .\\ .\\ \mathrm{\δ}{\stackrel{\·}{d}}_i^2\end{array}\right]=\left[\begin{array}{c}2(x_k^I-x_1){\mathrm{\δx}}_k+2(y_k^I-y_1){\mathrm{\δy}}_k-({\mathrm{\δx}}_k^2+{\mathrm{\δy}}_k^2)\\ 2(x_k^I-x_2){\mathrm{\δx}}_k+2(y_k^I-y_2){\mathrm{\δy}}_k-({\mathrm{\δx}}_k^2+{\mathrm{\δy}}_k^2)\\ .\\ .\\ .\\ 2(x_k^I-x_i){\mathrm{\δx}}_k+2(y_k^I-y_i){\mathrm{\δy}}_k-({\mathrm{\δx}}_k^2+{\mathrm{\δy}}_k^2)\\ 2{\mathrm{vx}}_k^I{\mathrm{\δx}}_k+2(x_k^I-x_1){\mathrm{\δvx}}_k-2{\mathrm{\δx}}_k{\mathrm{\δvx}}_k+2{\mathrm{vy}}_k^I{\mathrm{\δy}}_k+2(y_k^I-y_1){\mathrm{\δvy}}_k-2{\mathrm{\δy}}_k{\mathrm{\δvy}}_k\\ 2{\mathrm{vx}}_k^I{\mathrm{\δx}}_k+2(x_k^I-x_2){\mathrm{\δvx}}_k-2{\mathrm{\δx}}_k{\mathrm{\δvx}}_k+2{\mathrm{vy}}_k^I{\mathrm{\δy}}_k+2(y_k^I-y_2){\mathrm{\δvy}}_k-2{\mathrm{\δy}}_k{\mathrm{\δvy}}_k\\ .\\ .\\ .\\ 2{\mathrm{vx}}_k^I{\mathrm{\δx}}_k+2(x_k^I-x_i){\mathrm{\δvx}}_k-2{\mathrm{\δx}}_k{\mathrm{\δvx}}_k+2{\mathrm{vy}}_k^I{\mathrm{\δy}}_k+2(y_k^I-y_i){\mathrm{\δvy}}_k-2{\mathrm{\δy}}_k{\mathrm{\δ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个参考节点的距离变化率平方的误差。

2.如权利要求1所述的一种采用距离平方和距离平方变化率的WSN/AHRS紧组合方法，其特征是，所述步骤(3)中扩展卡尔曼滤波器的状态方程具体为：

$\left[\begin{array}{c}{\mathrm{\δx}}_{k+1}\\ {\mathrm{\δvx}}_{k+1}\\ {\mathrm{\δax}}_{k+1}\\ {\mathrm{\δy}}_{k+1}\\ {\mathrm{\δvy}}_{k+1}\\ {\mathrm{\δay}}_{k+1}\end{array}\right]=\left[\begin{array}{cccccc}1& T& \frac{T^2}{2}& 0& 0& 0\\ 0& 1& T& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& T& \frac{T^2}{2}\\ 0& 0& 0& 0& 1& T\\ 0& 0& 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\δx}}_k\\ {\mathrm{\δvx}}_k\\ {\mathrm{\δax}}_k\\ {\mathrm{\δy}}_k\\ {\mathrm{\δvy}}_k\\ {\mathrm{\δay}}_k\end{array}\right]+W$

其中，(δx_k,δy_k)、(δvx_k,δvy_k)和(δax_k,δay_k)分别为AHRS测量的未知节点在k时刻的x向和y向的位置误差、速度误差和加速度误差，T为滤波器的采样周期，W为滤波器的状态噪声矩阵；(δx_k+1,δy_k+1)、(δvx_k+1,δvy_k+1)和(δax_k+1,δay_k+1)分别为AHRS测量的未知节点在k+1时刻的x向和y向的位置误差、速度误差和加速度误差。

3.如权利要求1所述的一种采用距离平方和距离平方变化率的WSN/AHRS紧组合方法，其特征是，所述步骤(4)中计算k时刻AHRS测量得到的未知节点与第i个参考节点之间的距离的具体方法为：

$d_i^{INS}=\sqrt{{(x_k^I-x_i)}^2+{(y_k^I-y_i)}^2},i=1,2,...,m$

其中，为k时刻AHRS解算的未知节点的x向和y向的位置，(x_i,y_i)为第i个参考节点的x向和y向的位置；

k时刻未知节点与第i个参考节点之间的理论真值为：

$d_i^{\mathrm{Re}al}=\sqrt{{(x_k^{\mathrm{Re}al}-x_i)}^2+{(y_k^{\mathrm{Re}al}-y_i)}^2},i=1,2,...,m$

其中，为k时刻未知节点在x向和y向的理论位置；k时刻AHRS测量得到的距离平方的误差为：

$\begin{array}{c}{\mathrm{\δd}}_i^2={\left(d_i^{INS}\right)}^2-{\left(d_i^{\mathrm{Re}al}\right)}^2\\ ={(x_k^I-x_i)}^2+{(y_k^I-y_i)}^2-\[{(x_k^{\mathrm{Re}al}-x_i)}^2+{(y_k^{\mathrm{Re}al}-y_i)}^2\],i=1,2,\mathrm{...},m\end{array}$

而则：

${\mathrm{\δd}}_i^2=2(x_k^I-x_i){\mathrm{\δx}}_k+2(y_k^I-y_i){\mathrm{\δy}}_k-({\mathrm{\δx}}_k^2+{\mathrm{\δy}}_k^2),i=1,2,...,m$

其中，为k时刻AHRS测量得到未知节点与第i个参考节点之间的距离平方的误差；(δx_k,δy_k)为AHRS测量的未知节点在k时刻的x向和y向的位置误差。

4.如权利要求1所述的一种采用距离平方和距离平方变化率的WSN/AHRS紧组合方法，其特征是，所述步骤(4)中计算k时刻AHRS测量得到的未知节点与第i个参考节点的距离平方的变化率的具体方法为：

$\frac{d{\left(d_i^{INS}\right)}^2}{dt}=2(x_k^I-x_i){\mathrm{vx}}_k^I+2(y_k^I-y_i){\mathrm{vy}}_l^I,i=1,2,...,m$

其中，为k时刻AHRS测量得到未知节点的x和y向的速度；为k时刻AHRS测量得到的未知节点与第i个参考节点之间的距离；为k时刻AHRS解算的未知节点的x向和y向的位置；

而理论上的未知节点与第i个参考节点的距离平方的变化率为：

$\frac{d{\left(d_i^{\mathrm{Re}al}\right)}^2}{dt}=2(x_k^{\mathrm{Re}al}-x_i){\mathrm{vx}}_k^{\mathrm{Re}al}+2(y_k^{\mathrm{Re}al}-y_i){\mathrm{vy}}_k^{\mathrm{Re}al},i=1,2,...,m$

其中，为k时刻AHRS解算的未知节点的x和y向的速度；为k时刻未知节点在x向和y向的理论位置；为k时刻未知节点与第i个参考节点之间的理论真值；则k时刻AHRS测量得到的未知节点到第i个参考节点的距离变化率平方的误差为：

$\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{d}}_i^2=\frac{{\left(d_i^{INS}\right)}^2}{dt}-\frac{{\left(d_i^{\mathrm{Re}al}\right)}^2}{dt}\\ =\[2(x_k^I-x_i){\mathrm{vx}}_k^I+2(y_k^I-y_i){\mathrm{vy}}_k^I\]-\[2(x_k^{\mathrm{Re}al}-x_i){\mathrm{vx}}_k^{\mathrm{Re}al}+2(y_k^{\mathrm{Re}al}-y_i){\mathrm{vy}}_k^{\mathrm{Re}al}\]\end{array}$

而

其中，为k时刻AHRS测量得到的x和y向的速度，(δx_k,δy_k)和(δvx_k,δvy_k)分别为AHRS测量的未知节点在k时刻的x向和y向的位置误差和速度误差；则：

$\mathrm{\δ}{\stackrel{\·}{d}}_i^2=2{\mathrm{vx}}_k^I{\mathrm{\δx}}_k+2(x_k^I-x_i){\mathrm{\δvx}}_k-2{\mathrm{\δx}}_k{\mathrm{\δvx}}_k+2{\mathrm{vy}}_k^I{\mathrm{\δy}}_k+2(y_k^I-y_i){\mathrm{\δvy}}_k-2{\mathrm{\δy}}_k{\mathrm{\δvy}}_k,i=1,2,...,m$

其中，为k时刻AHRS测量得到的未知节点到第i个参考节点的距离变化率平方的误差。