单兵自主导航技术发展综述

， ，

(国防科技大学 智能科学学院，长沙410073)

0 引言

行人导航是指对行人进行导航定位的技术，而单兵导航通常是指面向单兵作战的这类行人导航技术，要求在未知环境下具有无需提前铺设基础设施，仅依靠自身传感器进行自主导航定位的能力。当前常用的单兵导航系统一般以卫星导航为核心，辅以地图匹配的形式，在室外信号良好的开阔环境下可以实现高精度的导航定位。但卫星导航在室内、地下、城市巷道等信号不良或缺失的环境下难以完成导航任务，且信号易被干扰，是一种非自主导航方式，限制了单兵导航的应用范围。

室内定位方法有很多种，裴凌对当前主流室内定位技术的原理和方法进行了详细分析[1]，其中很多室内定位技术可以应用在行人导航上。根据对导航基础设施的依赖程度不同，可把室内定位技术分为三类：一是需要铺设专用的导航设施，如无线电、声学、光学等定位设施，常见的有室内伪卫星、射频(Radio Frequency Identification，RFID)、超宽带(Ultra Wide Band，UWB)、蓝牙和ZigBee定位等；二是利用环境现有设备作为导航辅助设施，如WLAN指纹、GSM定位、地理信息辅助定位等；三是无任何基础设施，完全依靠行人自身传感器进行的导航，代表方式为基于微惯性测量单元(Micro Inertial Measure Unit, MIMU)的惯性导航，并可辅助以视觉、地磁导航等。在消费级的应用领域如仓库、商超、机场、展览馆等，具备提前铺设基础设施的条件，行人导航可直接采用第一或第二类导航方式。而对于消防救援、抢险救灾、反恐作战等往往面临的是未知环境，不能提前铺设基础设施或者现场设施已被损毁，这就对行人导航的自主性提出了更高的要求，也是当前行人导航面临的最大挑战，目前还未形成能完全满足各种应用需求的成熟解决方案。本文讨论的单兵导航针对的是第三种场景，即未知环境下依靠单兵自身携带的传感器进行自主导航的技术。

近年来，微惯性技术的快速发展为单兵自主导航的实现创造了条件。惯性导航是一种完全不依赖任何外部信息的自主导航技术，微惯性测量单元MIMU在成本、尺寸、质量、功耗等方面能够很好地满足单兵导航系统的应用需求。但MIMU精度较低，直接用惯性导航算法得到的单兵导航结果会迅速发散，不能完成导航任务。因此，在基于MIMU微惯性导航的基础上，寻求各种主动或被动的方法来抑制误差发散提高导航精度，成为当前解决单兵自主导航问题的主要思路。

本文首先阐述单兵自主导航技术的发展过程，然后重点讨论基于MIMU的微惯性单兵导航技术的原理和方法，随后针对微惯性单兵导航存在的问题，分析几种主流的辅助导航方法和单兵协同导航方法，最后探讨单兵自主导航技术的未来发展趋势。

1 单兵自主导航技术发展简述

单兵定位的传统方法是采用地图、罗盘和计步的方式来实现，直到20世纪90年代GPS出现之后，美军率先把GPS导航应用到陆军的单兵定位装备中，基本解决了单兵的室外高精度定位问题。但在室内、丛林等信号缺失环境下的单兵导航依然需要依赖传统方法。20世纪90年代中期，美军为陆地勇士 (Land Warrior)单兵装备研制了导航系统，首次引入了包含加速度计和磁强计的步行航位推算模块与GPS组合构成单兵导航系统，在卫星信号不良时可切换为航位推算模式[2]。2006年，美军在新一代单兵装备未来勇士 (Future Force Warrior)中升级了器件和步行航位推算(Pedestrian Dead Reckoning，PDR)算法，可以适应更复杂的战场状况[3]。PDR算法需要对人体进行运动学建模，根据步频、腿长等参数来计算步长，并结合计步信息及航向信息推算得到位置。其误差不随时间发散，仅与累计的步行路程相关，但不同单兵个体的模型参数差异较大，精度提高有限，有一定应用局限性。

PDR算法中MIMU原则上可以安装于胸、腰、足等不同部位来进行建模和计步。随着MIMU精度的进一步提高，单兵导航系统出现另外一种方案，把MIMU安装于足部，利用脚落地时的零速特点作为导航滤波算法中的虚拟观测，通过周期性的速度约束，达到比PDR算法更好的精度和适应性，这就是单兵的零速修正(Zero Velocity Update，ZUPT)算法。

20世纪90年代末，Elwell等[4]提出了利用行人步态特点设计ZUPT算法的思路。2005年，美国InterSense公司的Foxlin[5]首次设计了足部安装的单兵惯导系统，用ZUPT算法取得了较好的实验结果，并于2009年开发了微导航单兵定位模块NavShoe,测试精度达到了1%里程，优于PDR水平。同时期，Woodman[6-7]、Ojeda[8]、Godha[9]、Borenstein[10]等也对基于MIMU的行人导航方法进行了深入研究。主流的单兵自主导航研究大多基于足部安装的MIMU，采用惯性导航(Inertial Navigation System,INS)、扩展Kalman滤波(Extend Kalman Filter,EKF)和ZUPT的算法架构，简称INS-EKF-ZUPT算法。理论研究主要集中在两个方面：一是ZUPT算法中的步态检测方法；二是针对ZUPT算法中航向角不可观的问题，寻求不同的方法修正航向角漂移误差。

国外很多研究团队在单兵导航领域开展了大量深入细致的研究。瑞典皇家理工大学(KTH)的Skog和Nilsson等自2010年起在行人导航领域进行了系统性研究[11-17]，深入研究了步行导航算法，对多种步态检测算法和ZUPT算法进行定量分析，设计实现了步行导航模块OpenShoe,固定在鞋上进行导航，短距离测试精度优于1%里程。同时开展了双脚MIMU导航、多人协同导航、MIMU传感器阵列等单兵导航优化方法的研究。加拿大卡尔加里大学(University of Calgary)的导航定位实验室(PLANS)长期从事行人导航方面的研究[18-21]。从2010年开始，研究了地磁信息辅助方法，采用多传感器、多MIMU融合方式改善精度，研究了地图、视觉辅助及场景约束下的多源融合单兵导航算法。美国密歇根大学的Borenstein于2009年提出了利用建筑物方向对单兵航向进行约束的方法[10]。英国诺丁汉大学的Moore教授团队深入研究了单兵的建筑物航向约束方法，在单兵室内定位、无卫星环境下的导航技术等方面都开展了深入的研究[22-28]。西班牙自动化和机器人研究中心的Jiménez等也在单兵EKF算法、ZUPT算法、单兵航向约束算法等方面进行了深入研究[29-34]。德国航空航天研究中心的Robertson团队提出了一种新的仅用MIMU数据的即时定位与地图构建(Simultaneous Localization and Mapping, SLAM)算法，命名为Foot-SLAM算法，能够粗略生成室内地图并实现人员的实时室内定位[35-37]。

在国内，北京自动化控制设备研究所、国防科技大学、清华大学、北京理工大学、南京航空航天大学、西北工业大学、东南大学等多个研究团队近年来也在单兵自主导航领域开展了系统性的研究工作，取得了诸多研究成果[38-45]。

2 基于MIMU的单兵惯性导航方法

基于MIMU的惯性导航是完全自主的导航方法。虽然纯惯性导航位置误差发散很快，但利用惯性器件可以测量单兵运动的特点，把运动规律作为虚拟观测设计导航滤波算法，同时结合惯性器件的误差估计和补偿方法，可以有效抑制导航误差发散，一定条件下可以独立完成单兵导航任务。

2.1 PDR算法

PDR算法的基本思路是通过加速度信息或计步测量装置对行走时的步数和步长进行估计，结合磁敏感器或陀螺获得的航向信息逐步推算出行人的位置、行走距离和方向信息，如图1所示。算法可以分为4个阶段：步态探测、步长估计、航向估计和位置推算。

图1 PDR算法过程示意图Fig.1 The procedure of PDR algorithm

步态检测是通过MIMU识别人在行走过程中的步伐状况，主要包括步频检测、步伐判断。步态检测主要完成对步行期间内的步态周期进行划分，输出一个完整的步态周期的起始和结束时间，用于后续的步长估计和航向估计。步态检测一般通过陀螺和加速度计输出数据进行判别，也可以采用支持向量机( Support Vector Machine，SVM)方法对动作进行识别。步态检测的准确性直接影响到PDR算法的精度。

步长估计是指估算单兵每一步的长度。步长指同一只脚相邻两次触地所迈过的距离，也就是一个步态周期走过的长度。步长估计一般通过步长建模完成。常用的步长模型有伪常数步长模型、线性步长模型、非线性步长模型和基于神经网络的步长模型等多种。一般步长模型都需要事先标定，模型参数随使用者的身体特征、步行习惯而存在差异。

航向估计是估算行人的实时航向。步行推算中的航向估计至关重要，航向角的准确性直接关系到最终导航轨迹与实际运动轨迹的差异。航向角的初始对准一般基于磁罗盘完成，步行过程中，航向角依靠陀螺仪和磁罗盘共同维持。

位置推算是基于步长和航向递推行人的位置坐标。步行推算主要依靠每一步估算出的步长和航向角递推更新完成，导航精度受步长估计和航向估计的精度影响。定位误差随着步行的距离即步数的增加而逐渐累积。

PDR算法实现简单，安装应用方便，早期只需要磁罗盘和加速度计即可完成，后来随着MIMU技术的发展，一般直接用集成的MIMU模块实现，可安装于人体腰部、背部、脚上等不同位置。但其步长模型与人体的身体特征和运动习惯等相关性较大，精度有限。

2.2 零速修正算法

1)基本原理

人在步行过程中，脚部会往复出现离地、摆动、触地、静止4个阶段，且呈周期性变化，如图2所示。固定在鞋上的MIMU器件能够敏感到脚部的运动状态。当步态检测模块检测到脚部处于落地、静止状态时，此时固定在脚部的MIMU单元的速度理论上应该为零，因此该阶段也被称作零速阶段。把速度为零作为导航滤波算法的虚拟观测，可以建立Kalman滤波方程，即为ZUPT算法。

图2 步行周期性变化规律图Fig.2 The cyclical action of pedestrian step

根据惯性导航的误差方程建立INS-EKF-ZUPT单兵导航算法，EKF滤波方程可设计不同的滤波状态。以常用的15状态滤波器为例，设该模型的系统状态为导航系统误差δx，如式(1)所示。

δx=[δφδνδrΔbηb]T

(1)

式中，δφ=[φNφEφD]为姿态角误差，δν=[δvNδvEδvD]为速度误差，δr=[rNrErD]为位置误差，Δb=[ΔbxΔbyΔbz]为加速度计零偏，ηb=[ηbxηbyηbz]为陀螺零偏。

Kalman滤波状态方程和量测方程分别为：

(2)

z=Hx+v

(3)

系统的观测量z如下

z=[δνk]=νk-[0,0,0]

(4)

式中，δνk为第k时刻的速度误差，νk为第k时刻的速度。

则观测矩阵为

H=[03×3I3×303×303×303×3]

(5)

这就是ZUPT算法的基本原理，而其中零速状态的检测是影响算法精度的主要因素之一。

2)零速检测

零速检测是步态检测中的一个核心环节。步态检测主要包括步频检测、步态判断和零速检测。当MIMU被固定在鞋上时，内置的加速度计和陀螺仪能够敏感脚部的变化状况，实时检测抬起、迈步、落地、静止等周期性的动态变化。而加速度和角速度是直接反映脚部动态性的量值，大多数步态检测算法借助了它们的统计学特征，利用其周期性变化规律完成。基于MIMU的步态检测算法大致可以分为三类：基于加速度信息的步态检测、基于角速度信息的步态检测和融合了加速度和角速度信息的步态检测算法[46]。这些算法都是基于数据的模值、方差和周期性变化特征，通过设定阈值，判断当前时刻处于零速时刻(落地、静止)或抬起迈步时刻，而阈值一般要根据使用者的体态特征和步行习惯事先设定。测试表明，在参数设计得当的条件下，同时融合陀螺和加速度计输出的角速度和加速度信息的检测模型精度一般要优于单独使用角速度或加速度的检测模型，可靠性更高。

为了提高零速检测精度，除了用MIMU直接检测外，也可以通过附加其他传感器来提高测量精度，如在鞋底增加压力传感器，通过测量鞋底与地面的压力变化来更加精确地判断脚步落地时刻。Ozkan Bebek[47]在鞋上安装了一组地面反作用力感应器，通过压力数据分析行人的步行状态，进而更加精准地找到了ZUPT时刻，使得校正过程更加及时和准确。田晓春等也通过在鞋底安装薄膜压力传感器的方法提高了零速检测精度[38]。相比MIMU检测，压力传感器检测精度更高，但需要在鞋底额外安装压力传感器，增加了系统复杂性和应用难度。

通过零速检测，可以在惯性导航解算过程中，在每一步的脚落地时刻施加ZUPT算法，对速度误差、位置误差和水平姿态角误差进行修正，提高导航定位精度。但是，ZUPT算法中航向角是不可观的，意味着算法并不能对航向角漂移进行修正。因此，由于陀螺漂移误差，单兵航向角误差逐渐积累，成为单兵导航的主要误差源。

2.3 运动约束算法

ZUPT算法是单兵导航中最基本的一种运动约束算法，实际上，为了进一步提高精度，单兵的一些其他规律性运动也可以利用来设计为运动约束算法。

类似于零速检测，在设计运动约束算法之前，首先要准确检测和识别单兵的运动模式，总结运动规律。单兵的运动模式，除了步频、步长等步态动作外，还有诸如走、跑、转弯等行进动作，上下坡、上下楼梯等与环境相关的动作，立、坐、趴和躺等一系列与身体姿态相关的动作等。单兵运动模式识别，是指从MIMU采集到的数据中，处理分析得到当前单兵的动作状态。基于单兵不同运动模式下的人体运动规律，根据识别出的步态、动作及姿势等信息研究设计运动学约束条件，利用有效的非完整约束作为虚拟观测，通过EKF组合导航算法来提高导航精度，并能对导航误差和器件误差进行在线估计和修正。常用的运动约束有航向约束和高度约束方法。

1)航向约束

如前所述，零速修正算法并不能对航向角漂移进行修正，考虑到大多数建筑物的走廊是直线的和直角弯的，在室内步行时，对于较长距离的直行和转弯路线，一般对应的是楼道直线和楼道拐角。通过算法中的运动判断模块识别每一步的航向角，基于连续步伐的航向角变化序列判断单兵的行走状态如直行和转弯等，如图3所示。当判断状态为直线行走时，可认为此段路线理想航迹角均值不变；当判断状态为转弯(90°或180°)时，可认为航迹角理想变化量应为90°或180°，把输出航迹角与理想航迹角的偏差作为观测量用于Kalman滤波中，起到基于运动特征进行航向约束的作用[10,40]。

图3 直行/转弯示意图Fig.3 The steps of going straight and turning

2)高度约束

惯性导航的高度通道是发散的，如果在室内需要提供楼层信息，常用的方法是利用气压高度计输出作为高度约束。但在一些特殊情况如消防救火、士兵作战等条件下，现场的大火或爆炸可能瞬间改变气压，影响气压与高度的关系。此时可以利用上下楼梯的运动规律进行高度和楼层估计。

每一级台阶的高度在建筑规范里大多为定值，而每一步所踏台阶的数量，决定着当前这一步高度更新的大小。因此，动作的准确识别和误判的及时排除，决定着高度方向上的位置估计精度。固定在鞋上的MIMU能够有效地感知高度变化引起的加速度和角速度变化，通过动作判断一步的台阶数量来估算高度变化量。为了减小误判，可用一系列高度变化量建立高度序列，建立隐藏马尔科夫模型(Hidden Markov Model, HMM)生成概率最大的动作序列，排除动作误判后被用于高度误差修正算法中。谷阳[48]、陈昶昊[40]等基于此思路设计高度约束算法，测试得到较好的楼层判定效果。

2.4 多MIMU信息融合算法

针对MIMU普遍精度不高的问题，可以采用2个或多个MIMU共同导航的方式来提高导航的精度和可靠性。即在人体安装多个MIMU，每个MIMU都进行导航解算，然后对多个传感器信息或导航结果进行信息融合和优化，得到更优的导航解。

多MIMU数据融合技术研究可以追踪到1991年，McMillan为解决水下探测器的长时间稳定导航问题，建立了双IMU的Kalman滤波模型，进行双导航系统数据的融合[49]，随后被广泛地应用到其他各种设备的导航系统上。2003年，Brand提出将双MIMU模型应用于行人导航上，利用双MIMU间的空间距离约束实现了导航误差的抑制[50]。加拿大卡尔加里大学导航定位实验室的Bancroft[51]，瑞典皇家理工大学的Nilsson和Skog[52-53]等也分别针对行人导航的背景，对多MIMU数据融合进行了更为细致的研究，并通过实验论证了多MIMU在行人导航系统中的可行性和有效性。研究成果表明，双MIMU数据融合可以降低MIMU噪声，提高导航定位结果的精度和稳定性。国内的孟祥宾[41]、陈昶昊[40]、时伟[54]等对双脚不等式约束算法也进行了分析和研究。钱伟行[55]还通过多个MIMU在人体下肢分布式安装，设计了基于人体下肢运动学机理构建的虚拟足部惯性测量组件来实现单兵导航定位的方法。

1)双脚MIMU距离约束

当2个或多个MIMU安装于人体不同位置时，任何一个MIMU都可以进行导航解算，得到导航结果，每个导航结果代表的是安装位置的导航信息。虽然人体不是刚体，不同位置的MIMU之间的距离可能是相对变化的，但人体的高度和四肢伸展范围是有限的，即不同位置的MIMU之间的距离是有上限的。在多个MIMU导航滤波模型中，加入距离上限约束，可以优化导航结果。其中最常用的是双脚分别安装MIMU进行双脚距离约束，如图4所示。

图4 双脚MIM距离约束关系示意图Fig.4 The range constraint of two MIMUs

双脚MIMU距离约束算法是将每个MIMU子系统独立进行惯性导航解算和零速检测，再经过ZUPT和距离最大值约束分别对两MIMU系统间的位置关系、速度和角速率进行观测，针对双MIMU系统设计带不等式约束的EKF算法，对导航解算的结果进行优化。实际上，除了双脚距离约束外，安装于身体其他部位如肩部、腰部等的MIMU也可以用于距离约束。

2) MIMU阵列

另外一种多MIMU优化方法是MIMU阵列，即多个MIMU按阵列方式固联排列虚拟为一个MIMU。多个MIMU误差相对独立，可以平滑器件漂移和噪声，提高数据采样率和系统可靠性，甚至多个MIMU中的磁场传感器可以建立一个磁场指纹，从而提高系统性能。瑞典皇家理工大学的Nilsson和Skog[16,56]，国防科技大学的马明等[57]对此作了分析研究。

3 单兵常用辅助导航方法

为了进一步提高精度和环境适应性，单兵导航可以借助现场一些实时可用的导航信息源，如磁航向、建筑物方向、视觉信息等来辅助导航。地磁和视觉信息可通过单兵随身携带的磁传感器和视觉传感器获取，建筑物方向信息可在现场作为参数提前配置。虽然利用了外部环境信息，但并不需要提前铺设基础设施，也不需要在导航过程中接收外部信号，故符合导航自主性要求。同时，作为一种可选的辅助导航手段，在导航过程中仅在条件具备时使用，条件不具备时在算法中可自动失效，不影响导航自主性。

3.1 地磁航向辅助方法

由于MIMU精度较低不能实现自寻北，一般单兵导航直接用地磁传感器完成寻北和定向，磁航向也可在导航解算中作为航向外部观测量抑制航向角漂移。由于磁强计存在零偏误差、标度因数误差和非正交误差等，在使用磁强计计算磁航向角之前，需要对磁强计误差进行校准。对于足部安装的单兵导航系统，动态环境下磁传感器的输出稳定性较差，一般仅在脚部的零速区间内采用磁航向角辅助，以减小动态误差的影响。地磁信息最主要的问题在于环境的磁场干扰，特别是有些室内环境磁场干扰严重，此时可结合惯性传感器信息对磁航向角的置信度进行评估，当判断磁航向角干扰较大时，减小磁航向角的约束权值。只要得到可信的磁航向，就可以作为导航滤波算法的外部航向观测量，起到修正航向漂移的效果。

另外地磁匹配的方法也被用于行人导航，但需要预先测量环境磁场建立磁场分布图，不适用于未知环境下的自主导航。

3.2 楼向信息航向辅助方法

单兵的活动范围一般较小，在一个新的未知环境下很难有足够详细的数字地图，特别是室内地图，故采用地图匹配导航往往是不现实的。但一些简单的环境信息如建筑物形状、方向等信息是现场可立即观测到的，可被作为导航的先验信息来辅助导航。2009年，美国密歇根大学的Borenstein提出了一种利用建筑物方向约束的启发式误差抑制(Heuristic Drift Reduction, HDR)方法[10]。2011年，英国诺丁汉大学的Abdulrahim基于类似的思路，提出了一种基于EKF框架的建筑物楼向修正方法[22]，西班牙的Jiménez基于EKF算法框架对楼向约束方法进行了进一步的分析和优化，可在室内应用中显著提高导航定位精度[31]。李杨寰[45]、陈昶昊[40]，张新喜[58]等把楼向约束与运动约束相结合，对航向约束算法做了进一步优化，在单兵室内导航试验中得到了较好的测试效果。

楼向修正的基本原理是利用已知的建筑物方向信息作为航向的外部观测量，把楼向信息与运动约束的航向修正算法相结合。考虑到大部分建筑物为规则形状，走廊是直的，转弯是直角，行人一般会沿大楼走廊方向行进和转弯，这就是一种天然的场景约束条件。当判断行人当前处于直线行走或转弯时，默认其行走的方向与楼向平行或垂直，此时用楼向信息作为导航滤波算法的航向观测量，可以修正航向角漂移。楼向按照所处象限划分，可被划分为东南西北4个方向区间，两两互为90°角关系。也可划分为更细的区间如8个方向，两两互为45°角。当前所处的楼向可以作为航向角场景约束加入滤波算法，修正惯导解算的航向角误差。

初始楼向信息的获取是楼向修正算法的前提。简单的方法是作为先验知识在导航开始前作为初始参数人工输入。另一种方法是通过图像处理算法，在大尺度的数字地图上仅对建筑物轮廓特征进行特征提取，计算得到建筑物的楼向信息。

楼向信息辅助是一种典型的地理几何信息辅助导航方法，在室内应用可明显改善单兵自主导航性能。对于室外场景，则需灵活利用环境特征信息，如道路方向、隧道方向、区域几何形状等场景信息来辅助导航，而在人可以任意行走的开阔地带如操场、草坪等场景则不适用。

3.3 视觉信息辅助导航方法

随着摄像设备的微小型化，单兵随身装备摄像头在很多条件下是必要的且是容易实现的。而视觉信息在记录影像的同时，也可以同时用作导航辅助信息。摄像设备可以实时获取环境影像信息并进行特征提取，基于影像信息设计视觉导航算法对惯性导航进行姿态或位置的辅助观测，以修正惯导误差。

20世纪60年代，以图像跟踪技术为代表的视觉导航技术就被应用在了军事领域上。20世纪70年代末，MIT的Marr教授创立了视觉计算理论，从视觉传感器获取的图像中提取有效信息，包括载体的导航信息。如今视觉导航技术已经被广泛地应用于移动机器人、无人机、交通运输、农业生产和工业生产等多领域中。

20世纪90年代就出现了将视觉辅助导航技术应用于行人导航背景。Aoki等通过处理接收到的图片和GPS定位信息，能够将行人的绝对坐标在室内平面图直接显示出来[59]。近年来，视觉陀螺仪的概念也被提了出来，基于视觉传感器获得的图像信息，监测连续图片上的消影点、消影线变化状况，由此设计算法估计相机的位姿角。该算法早先被用于机器人、无人机系统，随后也被改进应用在行人导航上，并取得了较好的成效[60]。Laura等在研究了连续图像定位技术的基础上，通过视觉陀螺仪算法有效地估计当前航向变化，有效地抑制了步行导航系统的误差发散[61]。

当前视觉导航技术发展迅速，针对单兵导航的应用特点，可用的视觉辅助方法主要包括：惯性/立体视觉测姿与定向方法，视觉里程计/惯性组合导航方法、基于图像特征的场景识别方法及图像匹配定位方法、基于视觉SLAM的图节点递推与闭环优化方法等。随着视觉传感器的小型化、精密化，基于视觉辅助的单兵导航定位系统可在不显著增加导航系统成本的前提下，进一步提升导航定位精度。基于视觉辅助的导航技术正成为国内外研究热点，其在单兵导航上的应用还需要进一步深入研究。

4 单兵协同导航方法

协同导航需要发送或接收单兵之间的相对观测信息，严格意义上不属于自主导航。但从单兵分队整体来看，每个单兵随身携带相对测距设备，内部相互测距并交互信息，而不需要与外界信息交换，也不需要外界其他辅助设施的支持，故整体上符合自主性要求，在单兵分队导航应用中可以作为一种补充的自主导航手段。

4.1 发展概述

协同导航(Cooperative Navigation, CN)是指多运动体在各自的导航信息基础上，通过平台间相互观测所构成的约束关系,综合其他平台的导航信息进行数据融合，实现更优的导航效果。协同导航技术在地面机器人领域取得了良好的导航效果，目前已拓展到水下潜航器、无人机、卫星、导弹等领域。

单兵通常以分队形式作战，单兵作战系统的通信功能使得单兵分队成为一个通信网络，为单兵协同导航提供了网络基础。通过增加单兵间相对观测(距离、相对方位等)，实现单兵导航信息共享，是提高单兵作战系统导航精度的一种理想方式。

2005年，美国InterSense公司的Foxlin在实现ZUPT方法的同时，也提出了利用团队成员之间的测距信息进行协同导航的思想[5]。之后，一些先进的应急人员定位系统也采用了协同导航方案，美国国土安全部先后资助研发了针对消防员或者警察室内定位的先进定位系统A3DL、针对单兵的定位系统M3DL以及先进消防员定位系统GLANSER。A3DL系统包含足部惯导、GPS接收机和测距模块，其主要特点是利用小组成员之间、不同小组之间、成员与基站之间的距离测量和通信实现室内精确定位。通过替换部分传感器(如采用军用GPS接收机、军用电台等)，可以将A3DL系统改造为针对单兵的定位系统M3DL。GLANSER系统提出了利用消防员之间以及消防员和地面基站(消防车等)之间的距离测量进行协同导航的方案。2011年，针对士兵和应急响应人员室内定位问题，瑞典多家机构联合提出了多传感器融合的方案，指出协同定位是提高定位精度和适用性的关键，图5所示为城市作战中利用协同定位的场景[62]。2013年，Nilsson提出了一种分层式多人协同导航算法结构，底层进行ZUPT辅助的惯导解算；上层利用行人间的距离数据，仅对行人的位置和航向状态进行估计。实验结果表明，协同导航可以有效抑制微惯性件导航误差的发散，改善导航精度[14]。钱伟行将行人与移动机器人进行协同导航也得到了较好的效果[63]。

图5 单兵协同导航系统示意图[62]Fig.5 The concept of cooperative positioning for soldiers

值得注意的是，A3DL、GLANSER等行人协同导航方案均采用了UWB测距技术。激光雷达、相机等方位测量手段和激光、超声等距离测量手段要求两者之间通视(没有遮挡物)，在室内环境中可应用场景受限较多。Wi-Fi、超高频等传统射频测距技术和UWB新兴测距技术可以在非通视条件下进行。与传统射频测距技术相比，UWB测距技术具有测量精确、抗干扰能力强、抗多径误差能力强、功耗较低等优势，因此成为目前行人协同导航方案中的主流相对观测方式。同时，在非通视条件下，UWB测距噪声呈现明显的非高斯特征，是协同导航数据融合算法需要重点解决的难点问题。

4.2 单兵协同导航算法

单兵协同导航系统是一个网络，单兵及其他载体平台都是网络中的节点。单兵协同导航是在单节点导航的基础上，利用网络节点之间的数据传输能力，通过在节点之间增加相对观测(距离、方位等)来实现的，如图6所示。单节点导航信息是以MIMU模块为核心的多源信息融合的结果。分队成员间导航信息的交互是分队协同导航的关键。

(a)多节点协同导航 (b)单节点导航信息图6 协同导航系统信息融合示意图Fig.6 The information fusion of cooperative navigation system

单兵协同导航问题的特点和难点在于参与协同导航的单兵的状态是相关的。现有的协同导航算法种类各式各样，其核心算法主要有迭代贝叶斯估计方法(Kalman滤波、信息滤波、粒子滤波等)和优化方法两类。按照数据融合结构，则可以分为集中式、分层式、分散式算法三类。集中式是将数据直接通过中央处理单元进行处理，简单直观但是数据量和运算量巨大，适应能力和可靠性差。分层式是在每一个层级进行处理，最终由中央处理单元进行全局估计，中央处理单元的存在使得系统的可扩展性受到限制并且鲁棒性降低。分散式是每个系统中的成员都可以进行信息处理，成员间进行信息交互，是一种不依赖中心节点的结构，具有很好的灵活性和适应性，单个成员故障不会对系统产生影响，但对系统通信和协调能力有较高要求。

Kalman滤波以及各种改进算法在求解协同导航问题中比较常见。采用Kalman滤波要实现最优估计，就要保持对所有运动体的状态协方差矩阵的更新；为减小计算量或通信量而对协方差矩阵近似就不会得到一致估计。对于非高斯噪声，如UWB测量误差，可对测量误差进行简单的偏差校正后，再采用Kalman滤波来求解。信息滤波与Kalman滤波在数学上是等价的，在计算上的不同特点使得信息滤波具有分布式特征，因此在分散式协同导航算法设计上得到了应用。在信息滤波中，虽然观测更新可以利用每个运动体的局部信息完成，但是具有物理意义的均值和方差的计算仍然要用到所有运动体的信息。考虑到Kalman滤波、信息滤波在处理非高斯噪声上的局限性，可寻求非高斯滤波来求解协同导航问题。如将UWB测量噪声建模为柯西分布，采用基于确定性采样的高斯滤波方法融合UWB测量数据[62]。

5 单兵自主导航技术的发展趋势

经过多年发展，虽然取得了很大的研究进展，但单兵自主导航技术还未形成成熟的解决方案，未知环境下的单兵自主导航问题并未完全解决。当前基于MIMU的单兵导航技术在一定环境条件下可以满足导航需求，但其导航性能依然受到外部环境条件和单兵运动条件的影响，在复杂条件下的导航普适性和可靠性还有待加强。单兵自主导航技术未来的发展主要集中在以下两个方向。

一是从根本上大幅度提高微惯性器件和微惯性导航的水平，使得仅用惯性导航就能实现单兵的高精度导航，不再需要其他辅助信息，这是最直接和最根本的解决方法。2010年，美国国防高级研究计划局(DARPA)启动了Micro-PNT项目，目标是实现体积在10mm3以内，质量不超过2g，功率控制在1W以内的微型导航定位终端的制造。项目主要面对24h以内的惯性导航需求，用于单兵作战系统、小型无人机和小型无人潜航器等小型作战平台在GPS受限条件下的导航。其中单兵自主导航是其主要目标需求之一。如果微导航系统达到Micro-PNT的性能指标，则只需要MIMU即可满足单兵导航的性能要求。

二是在仅用惯性导航不能满足要求时，能更充分灵活地利用多种导航信息，即在单兵自主导航技术的基础上，拓展研究单兵全源导航方法。2011年，同样是美国国防高级研究计划局首次提出一种全新的导航信息融合概念-全源导航(All Source Positioning and Navigation, ASPN)。全源导航是针对卫星受限条件下的导航定位需求提出的，其思路是充分利用导航过程中可能存在的多种导航信息源，设计一套支持导航单元实时配置和即插即用功能的算法架构。如前所述，单兵导航针对未知环境，故只能选用自主导航技术，不能依赖不确定的外部信息。但实际上，单兵面临的环境复杂多样，每种环境下都可能已经存在一些非自主但有效的导航信息源，如果单兵导航系统能充分利用这些信息则可以大大提高导航性能。其难点在于不同环境下信息源的不确定性，可能存在不同的信号形式、不同的更新率、不同的定位解算方法，这对信息融合算法提出了很高的自适应要求，而这正是全源导航技术需要解决的问题。相比普通行人导航，单兵导航面临更复杂的动作模式和行进模式，更复杂多变的外部环境，更多临时的意外状态。单兵全源导航技术能在自主导航的基础上，自适应地利用现场已有的多种导航信息源，是未来进一步提高单兵导航精度、适应性和可靠性的有效途径。

[1] 裴凌, 刘东辉, 钱久超. 室内定位技术与应用综述[J]. 导航定位与授时, 2017, 4(3): 1-10.

[2] Marth R B, Levi R, Durboraw I N, et al. The integrated navigation capability for the Force XXI Land Warrior[C]// IEEE Position Location and Navigation Symposium. IEEE, 1998:193-200.

[3] Dorneich M C, Ververs P M, Whitlow S D, et al. Evaluation of a tactile navigation cueing system and real-time assessment of cognitive state[C]// Proceedings of the Human Factors and Ergonomics Society Annual Meeting, 2006: 2600-2604.

[4] Elwell J. Inertial navigation for the urban warrior[C]// Proceedings of the SPIE, 1999: 196-204.

[5] Foxlin E. Pedestrian tracking with shoe-mounted inertial sensors[J]. IEEE Computer Graphics and Applications, 2005, 25(6): 38-46.

[6] Woodman O, Harle R. Pedestrian localisation for indoor environments[C]// Proceedings of the 10thInternational Conference on Ubiquitous Computing, 2008: 114-123.

[7] Woodman O. Pedestrian localisation for indoor environments[D]. University of Cambridge, 2010.

[8] Ojeda L, Borenstein J. Non-GPS navigation for security personnel and first responders[J]. Journal of Navigation, 2007, 60(3): 391.

[9] Godha S, Lachapelle G. Foot mounted inertial system for pedestrian navigation[J]. Measurement Science and Technology, 2008, 19(7): 075202.

[10] Borenstein J, Ojeda L, Kwanmuang S. Heuristic reduction of gyro drift for personnel tracking systems[J]. Journal of Navigation, 2009, 62(1): 41-58.

[11] Skog I, Handel P, Nilsson J O, et al. Zero-velocity detection—An algorithm evaluation[J]. IEEE Transactions on Biomedical Engineering, 2010, 57(11): 2657-2666.

[12] Colomar D S, Nilsson J O, Händel P. Smoothing for ZUPT-aided INSs[C]// 2012 International Conference on Indoor Positioning and Indoor Navigation (IPIN). IEEE, 2012: 1-5.

[13] Nilsson J O, Skog I, Händel P, et al. Foot-mounted INS for everybody-an open-source embedded implementation[C]// 2012 IEEE/ION Position Location and Navigation Symposium (PLANS). IEEE, 2012: 140-145.

[14] Nilsson J O, Zachariah D, Skog I, et al. Cooperative localization by dual foot-mounted inertial sensors and inter-agent ranging[J]. EURASIP Journal on Advances in Signal Processing, 2013, 2013(1): 164.

[15] Nilsson J O, Skog I, Händel P. Aligning the forces—Eliminating the misalignments in IMU arrays[J]. IEEE Transactions on Instrumentation and Measurement, 2014, 63(10): 2498-2500.

[16] Skog I, Nilsson J O, Händel P, et al. Inertial sensor arrays, maximum likelihood, and cramér-rao bound[J]. IEEE Transactions on Signal Processing, 2016, 64(16): 4218-4227.

[17] Pasku V, De Angelis A, Moschitta A, et al. A magnetic ranging-aided dead-reckoning positioning system for pedestrian applications[J]. IEEE Transactions on Instrumentation and Measurement, 2017, 66(5): 953-963.

[18] Du S. Integration of precise point positioning and low cost MEMS IMU[D]. University of Calgary, 2010.

[19] Afzal M H. Use of Earth's magnetic field for pedestrian navigation[M]. Lap Lambert Academic Publishing, 2011.

[20] Saeedi S. Context-aware personal navigation services using multi-level sensor fusion algorithms[D]. University of Calgary, 2013.

[21] Dhital A. Reliability improvement of sensors used in personal navigation devices[D]. University of Calgary, 2015.

[22] Abdulrahim K, Hide C, Moore T, et al. Using constraints for shoe mounted indoor pedestrian navigation[J]. Journal of Navigation, 2011, 65(1): 15-28.

[23] Abdulrahim K, Hide C, Moore T, et al. Increased error observability of an inertial pedestrian navigation system by rotating IMU[J]. Journal of Engineering and Technological Sciences, 2014, 46(2): 211-225.

[24] Jing H, Bonenberg L K, Pinchin J, et al. Detection of UWB ranging measurement quality for collaborative indoor positioning[J]. Journal of Location Based Services, 2015, 9(4): 296-319.

[25] Kealy A, Retscher G, Toth C, et al. Collaborative navigation as a solution for PNT applications in GNSS challenged environments-report on field trials of a joint FIG/IAG working group[J]. Journal of Applied Geodesy, 2015, 9(4): 244-263.

[26] Basiri A, Amirian P, Winstanley A, et al. Seamless pedestrian positioning and navigation using landmarks[J]. Journal of Navigation, 2016, 69(1): 24-40.

[27] Jing H, Pinchin J, Hill C, et al. An adaptive weighting based on modified DOP for collaborative indoor positioning[J]. Journal of Navigation, 2016, 69(2): 225-245.

[28] Peltola P, Hill C, Moore T. Particle filter for context sensitive indoor pedestrian navigation[C]// 2016 International Conference on Localization and GNSS (ICL-GNSS). IEEE, 2016: 1-6.

[29] Jimenez A R, Seco F, Prieto C, et al. A comparison of pedestrian dead-reckoning algorithms using a low-cost MEMS IMU[C]// IEEE International Symposium on Intelligent Signal Processing. IEEE, 2009: 37-42.

[30] Jiménez A R, Seco F, Prieto J C, et al. Indoor pedestrian navigation using an INS/EKF framework for yaw drift reduction and a foot-mounted IMU[C]// 2010 7thWorkshop on Positioning Navigation and Communication (WPNC). IEEE, 2010: 135-143.

[31] Jiménez A R, Seco F, Zampella F, et al. Improved Heuristic Drift Elimination (iHDE) for pedestrian navigation in complex buildings[C]// 2011 International Conference on Indoor Positioning and Indoor Navigation (IPIN). IEEE, 2011: 1-8.

[32] Jiménez A R, Seco F, Zampella F, et al. Improved heuristic drift elimination with magnetically-aided dominant directions (MiHDE) for pedestrian navigation in complex buildings[J]. Journal of Location Based Services, 2012, 6(3): 186-210.

[33] Jimenez A R, Zampella F, Seco F. Improving inertial pedestrian dead-reckoning by detecting unmodified switched-on lamps in buildings[J]. Sensors (Basel), 2014, 14(1): 731-769.

[34] Jiménez A, Seco F. Comparing Decawave and Bespoon UWB location systems: Indoor/outdoor performance analysis[C]// 2016 International Conference on Indoor Positioning and Indoor Navigation (IPIN). IEEE, 2016: 1-8.

[35] Robertson P, Angermann M, Khider M. Improving simultaneous localization and mapping for pedestrian navigation and automatic mapping of buildings by using online human-based feature labeling[C]// 2010 IEEE/ION Position Location and Navigation Symposium (PLANS). IEEE, 2010: 365-374.

[36] Robertson P, Puyol M G, Angermann M. Collaborative pedestrian mapping of buildings using inertial sensors and FootSLAM[C]// ION GNSS DLR, 2011:1366-1377.

[37] Puyol M G, Bobkov D, Robertson P, et al. Pedestrian simultaneous localization and mapping in multistory buildings using inertial sensors[J]. IEEE Transactions on Intelligent Transportation Systems, 2014, 15(4): 1714-1727.

[38] 田晓春, 陈家斌, 尚剑宇, 等. 基于自包含传感器的单兵导航系统设计[J]. 导航定位与授时, 2017, 4(5): 54-59.

[39] 张新喜, 张嵘, 郭美凤, 等. 基于足绑式 INS 的行人导航三轴磁强计在线校准[J]. 清华大学学报 (自然科学版), 2016, 56(2): 211-217.

[40] 陈昶昊. 单兵自主导航算法优化及误差修正方法研究[D]. 长沙: 国防科技大学, 2016.

[41] 孟祥宾. 基于运动/场景约束的单兵导航算法研究[D]. 长沙: 国防科技大学, 2015.

[42] 徐元, 陈熙源, 王宜敏, 等. 一种采用足部航姿参考和肩部航向参考的室内个人导航方法[J]. 中国惯性技术学报, 2016, 24(3): 325-329.

[43] Zhang X, Zeng Q, Meng Q, et al. Implementation of smartphone seamless positioning system based on mobile navigation electronic map[C]// 2016 4thInternational Conference on Ubiquitous Positioning, Indoor Navigation and Location Based Services (UPINLBS). IEEE, 2016: 89-93.

[44] Xu Y, Chen X, Cheng J, et al. Improving tightly-coupled model for indoor pedestrian navigation using foot-mounted IMU and UWB measurements[C]// 2016 IEEE International Instrumentation and Measurement Technology Conference Proceedings (I2MTC). IEEE, 2016: 1-5.

[45] Li Y-H, Song Q, Ma M, et al. Near Real Time Heading Drift Correction for indoor pedestrian tracking based on sequence detection[C]// 2016 International Conference on Indoor Positioning and Indoor Navigation (IPIN). IEEE, 2016: 1-4.

[46] Chen C, Chen Z, Pan X, et al. Assessment of zero-velocity detectors for pedestrian navigation system using MIMU[C]// 2016 IEEE Chinese Guidance, Navigation and Control Conference (CGNCC). IEEE, 2016: 128-132.

[47] Bebek Ö, Suster M A, Rajgopal S, et al. Personal navigation via high-resolution gait-corrected inertial measurement units[J]. IEEE Transactions on Instrumentation and Measurement, 2010, 59(11): 3018-3027.

[48] Gu Y, Ma M, Li Y-H, et al. Accurate height estimation based on apriori knowledge of buildings[C]// 2013 International Conference on Indoor Positioning and Indoor Navigation (IPIN). IEEE, 2013: 1-7.

[49] Mcmillan J C, Ramotaur R. Development environment for DIINS (Dual Inertial Integrated Navigation System)[R]. Defence Research Establishment Ottawa(ONTARIO), 1991.

[50] Brand T J, Phillips R E. Foot-to-foot range measurement as an aid to personal navigation[C]// Proceedings of the 59thAnnual Meeting of the Institute of Navigation and CIGTF 22ndGuidance Test Symposium. 2001: 113-121.

[51] Bancroft J B, Lachapelle G, Cannon M E, et al. Twin IMU-HSGPS integration for pedestrian navigation[C]// ION GNSS, 2008: 1377-1387.

[52] Skog I, Nilsson J O, Zachariah D, et al. Fusing the information from two navigation systems using an upper bound on their maximum spatial separation[C]// 2012 International Conference on Indoor Positioning and Indoor Navigation (IPIN). IEEE, 2012: 1-5.

[53] Prateek G V, Girisha R, Hari K, et al. Data fusion of dual foot-mounted INS to reduce the systematic heading drift[C]// 2013 4thInternational Conference on Intelligent Systems Modelling & Simulation (ISMS). IEEE, 2013: 208-213.

[54] 时伟, 王阳. 基于不等式约束卡尔曼滤波的双 MIMU 导航位置校正方法[J]. 中国惯性技术学报, 2017, 25(1): 11-16.

[55] 钱伟行, 曾庆化, 万骏炜, 等. 基于人体下肢运动学机理的行人导航方法[J]. 中国惯性技术学报, 2015, 23(1):24-28.

[56] Nilsson J O, Skog I. Inertial sensor arrays—A literature review[C]// 2016 European Navigation Conference (ENC). IEEE, 2016: 1-10.

[57] Ma M, Song Q, Li Y H, et al. A heading error estimation approach based on improved quasi-static magnetic field detection[C]//2016 International Conference on Indoor Positioning and Indoor Navigation (IPIN). IEEE, 2016: 1-8.

[58] 张新喜, 张嵘, 郭美凤, 等. 足绑式行人导航偏航角误差自观测算法[J]. 中国惯性技术学报, 2015, 23(4): 457-466.

[59] Aoki H, Schiele B, Pentland A. Realtime personal positioning system for a wearable computer[C]// The 3rdInternational Symposium on Wearable Computers, Digest of Papers. IEEE, 1999: 37-43.

[60] Xu C, Zhang L, Cheng L, et al. Pose estimation from line correspondences: A complete analysis and a series of solutions[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2017, 39(6): 1209-1222.

[61] Laura R, Heidi K, Ruizhi C, et al. Heading change detection for indoor navigation with a Smartphone camera[C]// 2011 International Conference on Indoor Positioning and Indoor Navigation(IPIN). IEEE, 2011: 1-7.

[62] Rantakokko J, Rydell J, Strömbäck P, et al. Accurate and reliable soldier and first responder indoor positioning: multisensor systems and cooperative localization[J]. IEEE Wireless Communications, 2011, 18(2): 10-18.

[63] 钱伟行, 彭晨, 田恩刚, 等. 基于导航信息双向融合的行人/移动机器人协同导航方法[J]. 中国惯性技术学报, 2014, 22(1): 74-78.