[发明专利]采用距离平方和距离平方变化率的WSN/AHRS紧组合方法

说明书

技术领域

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

背景技术

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

在现有的定位方式中，全球卫星导航系统(Global Navigation Satellite System,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)中扩展卡尔曼滤波器的状态方程具体为：