基于航向角的WSN/INS组合导航系统的控制方法

摘要

一种基于航向角的WSN/INS组合导航系统及方法，属于多传感器数据融合领域。该组合导航系统包括参考节点部分和未知节点部分，参考节点部分包括参考节点无线网络接收模块、超声测距模块和时间同步模块；未知节点部分包括未知节点无线网络接收模块、INS导航模块和数据处理模块。利用扩展卡尔曼滤波得到系统中INS测量的位置误差和速度误差最优预估值，将INS本身测出的导航信息与最优预估值作差，得出最优的导航信息。本方法通过运用WSN/INS组合导航的方法，克服了在地下巷道、狭长隧道等GPS信号长期失锁的环境下INS导航的误差随时间漂移的问题。能满足地面城市交通、狭长隧道、小型智能机器人等中低精度的定位和定向的要求。

主权项

一种基于航向角的WSN/INS组合导航系统的控制方法，该组合导航系统包括参考节点部分和未知节点部分，参考节点部分包括参考节点无线网络接收模块、超声测距模块和时间同步模块，其中，参考节点部分中的参考节点无线网络接收模块负责完成远程命令的收发，在接到远程命令之后首先控制时间同步模块完成测距的时间同步，接着打开超声测距模块测量相应的距离信息，最后，将超声测距模块测量得到的距离信息通过参考节点无线网络接收模块返回给未知节点；未知节点部分包括未知节点无线网络接收模块、INS导航模块和中央数据处理模块，其中，未知节点部分中的未知节点无线网络接收模块负责完成未知节点与参考节点之间的距离信息的采集，并将采集得到的数据传送给中央数据处理模块；INS导航模块完成的是陀螺加表信息的采集，并将采集得到的数据传送给中央数据处理单元；中央数据处理单元负责将未知节点无线网络接收模块和INS导航模块采集到的数据进行数据融合，从而得到导航信息；其特征在于，所述控制方法包括下列步骤：（1）将导航过程分为培训过程和自适应补偿过程两部分，将有WSN信号的导航过程称为培训过程，而只有INS信号的导航过程称之为自适应补偿过程；（2）在培训过程中，在本地相对坐标系中将INS和WSN进行集成，通过扩展卡尔曼滤波对得到的同步导航数据进行数据融合；（3）构建扩展卡尔曼滤波器的系统方程，该系统方程以INS每一时刻两个方向的位置误差、速度误差、加速度计误差和航向角作为状态变量，滤波器的系统方程如式(1)所示，其中为采样周期，为系统噪声，为载体的航向角，为系统的采样周期计数；   (1)（4）通过INS测量每个时刻未知节点在x和y方向的位置，将得到的位置信息与参考节点的位置信息通过距离公式计算出INS测量得到的未知节点和第个参考节点之间的距离如式(2)所示：(2)其中，为INS测量得到的未知节点的坐标，为第i个参考节点的坐标，为参考节点的数目；通过超声测距模块测量出未知节点和第个参考节点之间的距离，将的平方与的平方作差，差值定义为，在此基础上，以作为滤波器的观测量，与此同时，将INS测量的每个时刻的两个方向的速度与WSN中包含的测速传感器测量得到的速度作差，将差值也作为滤波器的观测量，同时，将INS测量得到的航向角也作为观测量，通过上述的观测量构建观测方程，滤波器观测方程如式(3)所示： (3)其中，为参考节点在相对坐标系中的位置，，，，为观测方程噪声矩阵，为系统的采样周期计数；为载体的航向角；（5）滤波器进行数据滤波，在滤波的过程中，将KF得到的这一时刻最优的误差估计与时间输入到人工智能算法中，通过BP神经网络建INS预估的相对导航信息随时间偏差的模型；（6）若未知节点离开搭建有WSN的区域进入自适应补偿过程，在这一过程，组合导航系统获取不到WSN测量的相对导航信息，只能依靠MINS系统完成这一部分的自主导航，INS利用在培训过程训练的误差模型对测量的绝对导航信息进行误差补偿，得到最优的导航信息。