基于距离信息的行人协同导航研究

陈 泽，吴美平，陈昶昊

（国防科学技术大学机电工程与自动化学院，长沙410073)

基于距离信息的行人协同导航研究

陈 泽，吴美平，陈昶昊

（国防科学技术大学机电工程与自动化学院，长沙410073)

针对行人在室内导航中GPS信息无法获取以及纯惯导解算结果发散严重的问题，提出了通过协同导航的方式提高行人室内导航精度的方法。利用行人间的相对距离约束关系，抑制纯惯导解算结果的发散。详细推导了行人协同导航的模型，采用信息滤波的方法解算导航结果，并在理论推导中发现信息滤波更加适合行人协同导航的工程实践。设计了一种使用微型惯性测量器和超宽带测距设备的行人协同导航系统，进行实际效果验证。通过数据采集与处理，将行人导航轨迹图输出，分析协同导航以及单独导航的误差，验证协同导航的有效性。实验结果表明，协同导航与单独导航相比，对导航轨迹和导航误差有更好的修正效果。

协同导航；行人导航；距离约束；信息滤波

0 引言

协同导航（Cooperative Navigation，CN)是各导航平台在运动过程中通过相互之间的观测，利用平台之间的约束关系，提高系统导航精度的一种导航方式。协同导航在时空覆盖域、时间效率、性能鲁棒性、故障容错与系统重构等方面具有独特优势，并且配置灵活，可以根据平台能力合理分配工作量，而且具有更强的目标分辨力［1］。在协同导航中，装备低精度导航设备的移动平台可以利用其他高精度平台的导航信息提高导自身导航精度。同时，只需要部分平台装备有界定位误差的导航系统，系统中每个平台都可以具有误差有界的定位能力，解决惯性导航系统的误差发散问题［2］。

行人导航（Pedestrian Navigation，PN)是一项应用前景十分广阔的技术。行人主要通过携载GPS和微型惯性测量器（Micro Inertial Measurement Unit，MIMU)设备完成导航和实时定位［3］。但是随着城市的建设，越来越高大的建筑物阻碍了GPS信号的接收，使得GPS信息不可用。同时，由于一般MIMU测量精度的局限性，其测量结果会随着测量时间增加而发散，导航精度因此不能满足要求［4］。

本文主要研究利用距离信息进行行人协同导航的方法。在详细推导行人协同导航模型和算法的基础上，通过具体的实验分析利用协同导航的方式对行人导航结果的影响，对这种方法的特点做出总结并得到相应的结论，进一步提高行人导航的精度。

1 行人协同导航模型

行人运动模型用于描述在导航坐标系下行人的位移、速度等运动学参数之间的关系。本文重点研究多行人协同系统的导航性能，在单个行人运动模型的基础上，建立多行人协同导航模型。

建立单个行人Kalman滤波模型［1］，记k（k＝0，1，2，…)时刻第i个行人的状态矢量为：

其中，ri（k）为k时刻第i个行人的位置信息；vi（k）为k时刻第i个行人的速度。则单个行人非线性运动状态空间形式为：

其中，fi是第i个行人k＋1时刻的状态矢量与k时刻的状态矢量、输入矢量之间的投影函数；Ui（k)为k时刻第i个行人的控制输入矢量，在此选取k时刻第i个行人的加速度作为输入量；Wi（k)为系统噪声，方差为Qi（k)。

建立单个行人位置信息观测方程，将GPS对第i个行人的位置输出作为其状态观测，得到第i个行人位置信息观测方程为：

其中，Zi（k)为k时刻第i个行人的观测矢量；hi为第i个行人k时刻的状态矢量和观测矢量之间的投影函数；ζi（k)为k时刻第i个行人的观测噪声，方差为Ri（k)。

建立行人相对位置信息观测方程，设k时刻第i个行人对第j个行人的距离观测记为Zij（k)，则对应的行人间位置信息观测方程为：

其中，ζij（k)为k时刻第i个行人和第j个行人之间测距的观测噪声，方差为Rij（k)；hij为第i个行人和第j个行人k时刻的状态矢量和观测矢量之间的投影函数。

多行人协同导航的状态矢量由全部行人的状态矢量组成，设k时刻的系统状态矢量为X（k)。由N个行人组成的系统状态矢量、输入矢量和系统噪声为：

则系统的状态方程为：

其中，系统噪声ζ（k)的方差为Q（k)。

对式（6)中的每一个非线性方程进行线性化，得到线性化的系统状态空间方程为：

当把单个行人和行人间的位置信息观测方程写为多行人协同导航系统的观测方程时，只需适当扩展观测矩阵。对由N个行人组成的多行人协同导航系统，k时刻第i个行人位置信息线性化观测方程为：

其中，

类似地，对由N个行人组成的协同导航系统，k时刻第i个行人对第j个行人位置信息线性化观测方程为：

其中，

通过以上推导得到线性化系统模型为：

2 协同导航信息滤波算法

k时刻状态估计值为Xe（k)，状态预测值为Xp（k)，状态协方差矩阵估计值为Pe（k)，状态协方差矩阵预测值为Pp（k)，则：

信息滤波是用信息状态矢量y和信息矩阵Y来代替Kalman滤波的X和P进行处理，即：

信息状态和信息矩阵的递推方程集可直接由Kalman滤波方程获得。在数学上信息滤波与常规的Kalman滤波结果是一致的。

将式（13)代入到式（14)中，得：

其中，Yp（k)为预测信息矩阵；yp（k)为预测信息向量。

信息滤波更新过程为：

其中，Ye（k)为k时刻的信息矩阵估计值；ye（k)为信息向量估计值，E（k)为观测更新贡献的信息更新矩阵，e（k)为观测更新贡献的信息更新矢量，即：

对于k时刻行人i的观测，将式（8)代入式（17)，信息更新矩阵和更新向量为：

对于k时刻行人i、j间的相对观测，将式（10)代入式（17)，观测更新的信息更新矩阵为：

由式（18)～式（21)可知，联合分布参数的更新仅改变测量的行人和被测行人之间的信息参数，具有局部性，更适合运用在行人协同导航的工程实践中。

3 实验验证

3．1 硬件介绍

实验中所用的UWB测距模块是美国Time Do⁃main公司的PulsON 440测距模块。模块采用超宽带脉冲射频信号机制进行双向飞行时间测距和通信，在高度多径和高度反射的环境下，点对点精度可以达到2cm，室内穿墙误差小于0.5m。使用的MIMU为荷兰Xsens公司的MTi集成传感器。它内部包含有三轴加速度计和三轴陀螺以及三轴磁强计，静态测试得到的陀螺3个轴零偏稳定性参数分别为30.7（°)／h，26.8（°)／h，40.5（°)／h。如图1所示，所用的这2个模块具有功耗低、体积小、精度高、可靠性强等优势。

图1 P440测距模块和MIMUFig．1 P440 ranging module and MIMU

3．2 行人协同导航实验

实验由3人完成，3人均装备MIMU和P440测距模块，3人行走路径均为矩形框，如图2所示。通过差分GPS的方法得到3个人的行走轨迹，以差分GPS数据作为实际值参考。在实验中，只有第一个人的GPS信息可用，即模拟其余2个人在室内无法得到GPS信号的情况。

图2 实验路线Fig．2 Experiment route

如图3所示，实验中将MIMU固定在实验者左脚上，将P440测距通信模块固定在实验者左肩上，引出USB线连接电脑采集数据，MIMU和P440测距模块输出频率均为100Hz。实验者需要先进行3min左右的静止准备，完成MIMU初始对准。

图3 设备安装示意图Fig．3 The installation instruction of equipment

3．3 实测导航轨迹结果

通过对单独导航得到的行人运动轨迹和协同导航得到的行人运动轨迹进行比较，分析协同导航的方法对整体导航结果的改善。

导航解算结果如图4所示，图中左半部分是3个行人单独导航的结果，图中右半部分是3个行人协同导航的结果。其中，第一个行人由于GPS信号可以接收到，在GPS信息的修正下，其单独导航解算结果以及协同导航解算结果基本与真实轨迹拟合。第二个人和第三个人的GPS信号不可接收到，在只能利用自身MIMU的情况下，导航结果出现发散的情况；而进行协同导航后，导航结果得到了改善，第二个人和第三个人的运动轨迹基本和真实轨迹拟合。

如图5所示，通过与GPS测得的数据进行比较，计算得到3个行人单独导航和协同导航的误差。其中，CN代表协同导航，AN代表单独导航。在导航刚开始时，由于惯导误差不是特别大，所以协同导航与单独导航的误差基本一致。但是随着时间推移，在没有GPS信息下的单独导航，导航结果发散较大，而协同导航此时的误差并没出现较大发散。因此协同导航能够有效地减少行人室内导航的漂移误差，进一步提高在没有卫星条件下行人导航的精度。

图4 单独导航与协同导航轨迹Fig．4 Trajectories of alone navigation and cooperative navigation

图5 单独导航与协同导航误差Fig．5 Errors of alone navigation and cooperative navigation

4 结论

本文研究了利用协同导航的方式对行人导航进行优化的方法。针对在GPS信号接收不理想以及MIMU解算结果发散的情况，通过对多个行人之间的导航信息进行融合，达到整体导航效果改善的目标。利用行人之间较为准确的距离测量信息，对单个行人的MIMU解算结果的发散进行抑制。通过实验验证与单个行人独立导航轨迹进行对比，结果表明行人协同导航方法具有更高的导航精度和环境适应能力，进一步提高了行人导航算法的可靠性和实用价值。

［1］穆华.多运动平台协同导航的分散式算法研究［D］.国防科学技术大学，2010. MU Hua.Decentralized algorithms of cooperative navi⁃gation for mobile platforms［D］.National University of De⁃fense Technology，2010.

［2］张金亮，秦永元，梅春波.基于MEMS惯性技术的鞋式个人导航系统［J］.中国惯性技术学报，2011，19（3)：253⁃256. ZHANG Jin⁃liang，QIN Yong⁃yuan，MEI Chun⁃bo.Shoe⁃mounted personal navigation system based on MEMS inertial technology［J］.Journal of Chinese Inertial Tech⁃nology，2011，19（3)：253⁃256.

［3］练军想，唐康华，潘献飞，等.GNSS与惯性及多传感器组合导航系统原理（第二版)［M］.北京：国防工业出版社，2015. LIAN Jun⁃xiang，TANG Kang⁃hua，PAN Xian⁃fei，et al. Principles of GNSS inertial and multisensor integrated nav⁃igation systems（2nd)［M］.Beijing：National Defense Industry Press，2015.

［4］Nilsson J O，Zachariah D，Skog I，et al.Cooperative lo⁃calization by dual foot⁃mounted inertial sensors and inter⁃agent ranging［J］.Journal on Advances in Signal Process⁃ing，2013，164（1)：1⁃17.

［5］王文佳，孙睿智，高伟，等.基于通信延迟误差补偿的协同导航算法［J］.火力与指挥控制，2014，39（12)：27⁃30. WANG Wen⁃jia，SUN Rui⁃zhi，GAO Wei，et al.Coopera⁃tive navigation based on error compensation of communica⁃tion delays［J］.Fire Control＆Command Control，2014，39（12)：27⁃30.

［6］钱伟行，彭晨，田恩刚，等.基于导航信息双向融合的行人／移动机器人协同导航方法［J］.中国惯性技术学报，2014，22（1)：74⁃78. QIAN Wei⁃xing，PENG Chen，TIAN En⁃gang，et al.Pe⁃destrian／mobile robot cooperative navigation method based on navigation information bidirectional fusion［J］.Journal of Chinese Inertial Technology，2014，22（1)：74⁃78.

［7］Frankz K，Robertsonz P，Angermann M.Reliable real⁃time recognition of motion related human activities using MEMS inertial sensors［C］.Proceedings of the ION GNSS，Potland，2010：2919⁃2932.

［8］万骏炜，曾庆化，陈磊江，等.行人惯性导航系统平台设计与实现［J］.计算机应用与软件，2015，32（2)：45⁃47＋170. WAN Jun⁃wei，ZENG Qing⁃hua，CHEN Lei⁃jiang，et al. Design and implementation of pedestrian inertial navigation system［J］.Computer Applications and Software，2015，32（2)：45⁃47＋170.

The Research of Cooperative Pedestrian Navigation Based on Distance Information

CHEN Ze，WU Mei⁃ping，CHEN Chang⁃hao
（College of Mechatronic Engineering and Automation，National University of Defense Technology，Changsha 410073)

In order to solve the problem of GPS information unavailable and the serious divergence of inertial naviga⁃tion in the indoor pedestrian navigation，the method is proposed to improve the accuracy of the indoor pedestrian navigation by cooperative navigation.By utilizing the relative distance constraint between pedestrians，the divergence of the inertial navigation result is restrained.This paper deduces the model of cooperative pedestrian navigation in detail.The navigation result is worked out by the method of information filter which is more suitable for cooperative pedestrian navigation in the engineering practice by deducing the theory.The paper designs a cooperative pedestrian navigation system，using a micro inertial measurement unit and ultra wideband measurement equipment，collecting and processing the data，analysing out⁃puts，finding errors，verifying the effectiveness of the algorithm.The experimental results show that the cooperative naviga⁃tion has the advantage of fixing errors，compared to the alone navigation.

cooperative navigation;pedestrian navigation;distance constraint;information filter

U666.1

A

1674⁃5558（2017)01⁃01263

10.3969／j.issn.1674⁃5558.2017.01.002

陈泽，男，硕士，研究方向为惯性导航。

2016⁃04⁃01

国家自然科学基金（编号：61203200)