用于WSN/INS组合导航的多级非线性滤波器的控制方法

摘要

一种用于WSN/INS组合导航的多级非线性滤波器的控制方法，属于多传感器数据融合领域。本发明是在WSN/INS组合导航模型中使用两组滤波器。一组滤波器（滤波器1）对WSN测量的载体在两个方向（东向和北向）的位置和速度做预估，用预估的速度计算这一时刻载体运动的航向角并输入到下一滤波器中。另一组滤波器（滤波器2）则对每个时刻INS测量的载体在两个方向的位置误差、速度误差和航向角进行预估。INS测量位置、速度信息与预估出的位置误差、速度误差作差，最终得到最优的导航信息。该方法通过两组滤波器的设计，将传统INS航向角的误差由累积误差转变为随机误差，使滤波精度得到明显的提高。

说明书

技术领域

本发明涉及一种用于WSN/INS组合导航的多级非线性滤波器的控制方法，属于多传感器数据融合领域。

背景技术

全球定位系统（Global positioning systems，GPS）和惯性导航系统（Inertial navigation system, INS）均为目前应用最广泛的导航系统之一。其中GPS能够提供精确地，具有持续稳定导航精度的导航信息，但是在室内、高层建筑密集的市区、矿井、隧道等环境下，GPS信号失锁，不能进行定位。INS虽然具有全自主、运动信息全面、短时、高精度的优点，虽然可以实现自主导航，但误差随时间积累，长航时运行条件下将导致导航精度严重下降。因此，INS对GPS导航信息的补偿只能是短期补偿，而目前最为常用的GPS/INS组合导航系统的导航精度依赖于GPS的导航精度，在GPS长时间失锁的情况下，组合导航系统无法提供高精度的导航信息。

近年来，无线传感器网络（Wireless Sensors Network, WSN）以其低成本、低功耗和低系统复杂度的特点在短距离定位领域表现出很大的潜力。WSN为在无GPS信号地区，即所谓的“盲区”时，如室内、高层建筑密集的市区、矿井、隧道等环境下未知节点定位提供了可能。但由于WSN采用的通信技术通常为短距离无线通信技术（如ZigBee、WIFI等），因此若想完成长距离的目标跟踪定位，需要大量的网络节点共同完成，这增加了WSN的网络负担。除此之外，WSN只能提供位置和速度信息，不能提供全面的运动信息。

为了在GPS长失锁环境下得到长时间的稳定的导航信息，许多学者提出将WSN定位技术引入到低成本的INS系统中，构建WSN/INS组合导航系统，如东南大学Y. Xu,虽然这一组合方式很好的解决了地下密闭环境下的长距离目标跟踪和导航设备成本高的问题，但是由于现有的低成本INS技术，使得INS系统测量得到的信息（如航向角，加速度信息）准确性大大降低，再加上INS系统本身的误差累积现象，使得低成本INS技术难以提供稳定的导航信息。

发明内容

为了解决低成本INS出现的累积误差的情况，本发明提出了一种用于WSN/INS组合导航的多级非线性滤波器的控制方法，通过预估WSN测量的每一时刻的速度信息构建载体的航向角信息，将INS测量的航向角的累积误差转为随机误差，使滤波精度得到明显的提高。

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

一种用于WSN/INS组合导航的多级非线性滤波器的控制方法，包括下列步骤：

（1）组合方法将导航过程分为培训过程和自适应补偿过程两部分，将有WSN信号的导航过程称为培训过程，只有INS信号的导航过程称之为自适应补偿过程；

（2）在培训过程中，在本地相对坐标系中通过构建两个滤波器将INS和WSN进行数据融合； 

（3）构建扩展卡尔曼滤波器1的系统方程，该系统方程以WSN每一时刻两个方向的位置                                                、速度作为系统状态变量，滤波器的系统方程如式(1)所示，其中为采样周期，为系统噪声，

          (1)

扩展卡尔曼滤波器1的观测方程以每个时刻未知节点与参考节点之间的距离为观测量，于此同时，将载体速度传感器测量的载体的速度也作为观测量构建滤波器的观测方程，滤波器的观测方程如式(2)所示，

          (2)

其中，为RN节点在相对坐标系中的位置，为观测噪声；

（4）构建扩展卡尔曼滤波器2的系统方程，该系统方程以INS每一时刻两个方向，即东向和北向的位置误差、载体的前向和横向速度误差、前向和横向加速度计误差和航向角作为状态变量，滤波器的系统方程如式(3)所示，其中为采样周期，为系统噪声，

    (3)

通过INS测量每个时刻未知节点在x和y方向，即东向和北向的位置，将得到的位置信息与参考节点的位置信息通过距离公式计算出INS测量得到的未知节点和第i个RN节点之间的距离，通过WSN中包含的超声测距模块测量出未知节点和第i个RN节点之间的距离，将的平方与的平方作差，差值定义为，在此基础上，以作为滤波器的观测量；与此同时，将INS测量的载体在两个方向的速度与滤波器1输出的最优速度预估作差，将差值也作为滤波器的观测量，同时，将滤波器1中得到的航向角也作为观测量，通过上述的观测量构建观测方程，

滤波器观测方程如式(4)所示：

  (4)

其中，为参考节点在相对坐标系中的位置，，，；

（5）在滤波器进行数据滤波的过程中，将扩展卡尔曼滤波器2得到的这一时刻最优的误差估计与时间加入到BP神经网络算法中，通过人工智能算法构建INS预估的相对导航信息随时间偏差的模型；

（6）若未知节点离开搭建有WSN的区域进入自适应补偿阶段，在这一阶段，组合导航系统获取不到WSN测量的相对导航信息，只能依靠MINS系统完成这一部分的自主导航，INS利用在培训过程训练的误差模型对测量的绝对导航信息进行误差补偿，得到最优的导航信息。

本发明的有益效果如下：

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

2、提出了WSN/INS组合导航的多级非线性滤波模型，在局部滤波器中，使用以每一时刻WSN测量的BN节点在大地相对坐标系中x（东向）和y（北向）方向的位置和速度为系统状态变量，以WSN中包含的超声测距模块测量出BN节点和第i个RN节点之间的距离和未知节点载体上的速度传感器测量的速度（载体实际速度）为观测量，对WSN测量的载体在两个方向（东向和北向）的位置和速度做预估，用预估的速度计算这一时刻载体运动的航向角。这一航向角用以替代INS本身的航向角，局部滤波器预估出的航向角误差为随机误差，而INS自身预估的航向角为累积误差，因此本发明的优点在于将航向角的累积误差转化为随机误差，减少了INS测量误差的时漂现象。

附图说明

图1为用于WSN/INS组合导航的多级非线性滤波器的系统示意图。

图2为用于WSN/INS组合导航的多级非线性滤波器的控制方法示意图。

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

图4为实施例的参考节点、未知节点分布。

具体实施方式

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

如图1所示，一种用于WSN/INS组合导航的多级非线性滤波器的系统，包括参考（RN）节点部分和未知（BN）节点部分，参考节点部分由参考节点无线网络接收模块、超声测距模块和时间同步模块组成；未知节点部分由未知节点无线网络接收模块、INS导航模块、中央数据处理模块组成。 

如图2所示，在WSN/INS组合导航模型中使用两组滤波器。一组滤波器（EKF1）以WSN每一时刻两个方向的位置、速度作为系统状态变量，滤波器的系统方程如式(1)所示，其中为采样周期，为系统噪声。

           (1)

EKF1的观测方程以每个时刻未知节点与参考节点之间的距离为观测量，于此同时，将载体速度传感器测量的载体的速度也作为观测量构建滤波器的观测方程，滤波器的观测方程如式(2)所示，

            (2)

其中，为RN节点在相对坐标系中的位置，为观测噪声。

另一组滤波器（EKF 2），该系统方程以INS每一时刻两个方向，即东向和北向的位置误差、载体的前向和横向速度误差、前向和横向加速度计误差和航向角作为状态变量，滤波器的系统方程如式(3)所示，其中为采样周期，为系统噪声，

   (3)

通过INS测量每个时刻未知节点在x和y方向，即东向和北向的位置，将得到的位置信息与参考节点的位置信息通过距离公式计算出INS测量得到的未知节点和第i个RN节点之间的距离，通过WSN中包含的超声测距模块测量出未知节点和第i个RN节点之间的距离，将的平方与的平方作差，差值定义为，在此基础上，以作为滤波器的观测量；与此同时，将INS测量的载体的在两个方向的速度与滤波器1输出的最优速度预估作差，将差值也作为滤波器的观测量，同时，将滤波器1中得到的航向角也作为观测量，通过上述的观测量构建观测方程，

滤波器观测方程如式(4)所示：

  (4)

其中，为参考节点在相对坐标系中的位置，，，；

在滤波器进行数据滤波的过程中，将扩展卡尔曼滤波器2得到的这一时刻最优的误差估计与时间加入到BP神经网络算法中，通过人工智能算法构建INS预估的相对导航信息随时间偏差的模型，若未知节点离开搭建有WSN的区域进入自适应补偿阶段，在这一阶段，组合导航系统获取不到WSN测量的相对导航信息，只能依靠MINS系统完成这一部分的自主导航，INS利用在培训过程训练的误差模型对测量的绝对导航信息进行误差补偿，得到最优的导航信息。

WSN/INS组合导航的多级非线性滤波器的控制方法流程如图3所示，图4给出了实施例的参考节点、未知节点分布的环境及按图3流程得到的BN节点沿着图示的轨迹运动。方法的具体步骤如下： 

(1) 通过WSN模块中附带的载体速度计测量得到的载体的在某一时刻的速度为24.4194m/s，在这一时刻BN节点周围的RN节点坐标分别为（120，48.6764），（125，50.4963），（130，52.3161），（125，40.4963）（m）。上述信息通过扩展卡尔曼滤波器1得到数据为（125.9841，46.3180，22.8992，8.73615），通过预估得到的最优的两个方向的速度信息，得到这一时刻载体的航向角信息为北偏东69.1179度。

(2) INS采集加表信息，在采集到加表信息之后进行解算，获得某一时刻通过INS测量得到的BN的位置信息为（113.3593，55.6551）（m），载体的前向和横向速度信息为（21.9607，10.8944）（m/s）。将上述信息与(1)中测量的信息同时输入到扩展卡尔曼滤波器2中，得到这一时刻的INS的位置误差预估为（-12.7502，9.6947）（m）。将这一时刻内INS本身测出的导航信息减去主滤波器的最优误差估计，最后得到修正后的结果（126.1095，45.9604）（m）。