基于步进式运动模型的单兵协同导航算法设计

穆 华，潘献飞，吴美平，胡小平

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

0 引言

单兵等作战人员通常在未知环境中执行任务，用于定位的外界信息(地图、卫星等)可用性受限，单兵如何自主定位是近几年研究的热点和难点问题。基于足部微惯性测量单元(Micro Inertial Measurement Unit，MIMU)和单兵间测距的协同导航技术是一种解决卫星信号受限环境下单兵自主导航难题的有效途径。

通过在人体上固联MIMU进行惯性导航解算是一种新的单兵导航技术。该技术具有隐蔽性、抗干扰性和自主性等优势，具有应用于单兵自主导航系统的潜力。1999年，Elwell首先提出了零速修正(Zero Velocity Update，ZUPT)算法[1]。美国InterSense公司的Foxlin 于2005年首次设计了足部安装MIMU的单兵惯导系统[2]，并于2009年开发了单兵定位模块NavShoe，测试精度达到1%里程。之后主流的单兵自主导航研究大多基于足部安装的MIMU，针对ZUPT算法中航向角不可观的问题，寻求不同的方法修正航向角漂移误差。一种方法是引入磁罗盘[2]，但是磁罗盘容易受到环境磁场的干扰；另外一种方法是引入位置参考，如全球定位系统(Global Positioning System，GPS)[3]、建筑物平面图[4]、事先布设的电子标签(Radio Frequen-cy Identification，RFID)[5]以及单兵之间的距离测量[2，6-7]等。在上述手段中，引入单兵间距离测量即在多个单兵间进行协同导航(Cooperative Navigation，CN)。这种手段可以在多人之间实现导航资源共享，并且不需要事先布设设施，也不依赖环境特征，自主性更好[8]。

2005年 Foxlin提出了单兵协同导航的思想[2]。之后，一些先进的应急人员定位系统也采用了协同导航方案，如美国国土安全部资助研发的先进消防员定位系统GLANSER[9]，以及瑞典相关机构提出的单兵协同导航系统[6]等。值得注意的是，上述单兵协同导航方案均采用了新型超宽带(Ultra-Wideband，UWB)测距技术。这是由于与传统射频测距技术相比，UWB测距技术具有测量精确、抗干扰能力强、抗多径误差能力强、功耗较低等优势；同时与激光、超声等距离测量手段相比，UWB测距技术具有较强的穿透能力。

协同导航系统是一种分布式系统，协同导航算法的设计难点之一在于如何建立合适的协同导航系统模型，尽量降低系统的通信需求。复杂的通信需求会制约系统的可实现性和可靠性。文献[10]仅以位置作为状态变量，建立协同导航系统的运动方程，将位置的估计结果反馈到各单兵进行惯性导航系统误差修正；实验数据处理结果验证了算法的有效性。文献[7]在足部MIMU模块采用步进式惯导解算，以位置和航向角作为状态变量，建立协同导航系统的运动方程，无需反馈修正；在模拟UWB测距数据的条件下对该算法进行了验证。

为了促进单兵协同导航系统的实际应用，本文设计了一种基于步进式运动模型的协同导航算法。与文献[7]不同，本文足部模块仍然采用一般的ZUPT辅助的惯导解算，而无需采用步进式惯导解算。本文和文献[10]采用不同的协同导航系统模型，分析了同一组协同导航试验数据，试验结果可以互相印证各自算法的正确性。文献[10]仅给出了一种参数设置下的导航结果，而本文分析了不同条件下的导航结果，更全面地展示了协同导航在提高定位精度方面的表现。

1 单兵协同导航系统组成

单兵协同导航系统是一个包含若干个单兵节点的网络，每个单兵节点的基本导航设备包含足部MIMU模块、进行节点间距离测量的UWB模块、进行数据收集及计算的处理板，以及在单兵间进行数据传输的无线数传模块。

足部MIMU模块的传感器包括三轴陀螺、三轴加表、磁力仪和气压高度计，进行基于ZUPT的惯导解算，具有独立的导航定位能力。

本文选用的UWB模块采用双向飞行时间法进行高精度距离测量，模块A向模块B发送测距请求信号，模块B接收并处理后发送应答信号。可见，测距结果由测距发起者得到。对于无线电波，天线越高对传播越有利。因此，UWB模块安装在头部或肩部。

单兵之间采用900MHz频段的无线数传模块。这个频段无线电波的穿透能力可以轻松地在室内/城市环境下实现数百米远距离传输，数据速率可达10kbit/s甚至更高。

处理板收集足部惯导解算信息和UWB测距信息，并与无线数传模块之间进行数据传输。足部惯导解算信息通过蓝牙发送给处理板，UWB信息通过有线连接发送给处理板，处理板和无线数传模块之间通过Wi-Fi互连。

本文研究的单兵协同导航系统最小组成如图1所示。除了上述基本导航设备外，单兵的其他导航设备如卫星接收机、视觉里程计等都可以纳入协同导航系统中。

图1 单兵协同导航系统硬件配置示意图Fig.1 Hardware configuration diagram of the cooperative navigation system for soldiers

2 单兵协同导航算法设计

单兵协同导航算法设计的核心在于单兵运动建模，运动模型决定了是否需要对足部惯导解算进行反馈修正，同时运动模型的维数直接影响协同导航系统的通信量和计算量。本文建立了单兵的步进式运动模型，仅以单兵位置和航向角为状态变量，且无需对惯导解算进行反馈修正。

2.1 协同导航系统模型

1)建模思路

足部MIMU模块采用ZUPT辅助的惯性导航算法。在单兵行走的每一步，存在一个脚部静止的零速阶段。通过零速检测算法检测零速阶段，并把零速作为虚拟观测，可以修正足部惯导解算的速度误差和水平姿态角误差，但是不能有效修正航向角误差。航向角误差累积会影响定位精度。

以一段连续ZUPT结束时刻为一步的开始时刻，此时，足部MIMU的水平姿态角误差很小。再加上初始速度为0，所以一步的定位误差主要由起始时刻的航向误差引起。对于每一步的惯导解算，可以利用一个中间导航坐标系，将初始航向误差从导航结果中分离出来，得到误差不累积的位移增量和航向增量信息。

以单兵位置和航向为状态变量，以单兵每步的位移增量和航向增量为系统输入，建立单兵的步进式运动模型。所有单兵的步进式运动模型组合成协同导航系统的状态方程，结合单兵间距离观测方程，建立协同导航系统模型。

2)一步惯性导航解算

取北东地坐标系为导航坐标系，并且定义姿态矩阵

(1)

其中，Mi[·](i=1,2,3)为初等转换矩阵；γ、θ、ψ分别为滚动角、俯仰角和方位角。

考虑一步行走过程，设一步起始时刻MIMU的姿态角为(γ0,θ0,ψ0)，一步包含L次惯导解算，第l(l=1,2,…,L)次姿态解算的结果为

(2)

(3)

由于一步起始时刻的速度为0，第l次速度解算的结果为

(4)

第l次位移增量的结果为

(5)

由于

(6)

并且M3[ψ0]gn=gn，因此在式(3)～式(5)两端左乘M3[ψ0]，得

(7)

(8)

(9)

式(7)～式(9)相当于在一个中间导航坐标系n′中的惯性导航计算过程，这个坐标系与导航坐标系n之间的关系为

(10)

在中间导航坐标系n′中获取的位移增量和航向增量与初始航向ψ0无关。由式(9)，一步结束时(l=L)在中间导航坐标系n′和导航坐标系n中位移增量的关系为

(11)

3)步进式运动模型

利用n′系中一步位移增量dp′s，可得n系下的步进式运动模型

(12)

其中，变量xs表示第s步结束时MIMU在n系下的位置；变量χs表示第s步结束时MIMU的航向角；ws表示第s步的位移增量误差和航向增量误差，其方差根据第s步惯性导航解算结果计算。

Xs=f(Xs-1,us)+ws

(13)

4)协同导航系统状态模型

设N个单兵进行协同导航,协同导航系统的状态方程由N个单兵的步进式运动模型组成

(14)

其中，上标i表示单兵编号，下标si表示单兵i的第si步(i=1,2,…,N)。注意，除了齐步行进，相同时刻不同单兵的步数通常是不一样的。

协同导航系统的观测方程为

(15)

式(14)和式(15)组成协同导航系统模型。距离观测使得各单兵导航状态不再独立，而是成为了相关变量。

2.2 步进式协同导航算法

建立协同导航系统模型之后，采用Kalman滤波进行协同导航计算，信息流程如图2所示。

图2 步进式协同导航算法信息流程图Fig.2 Information flow chart of the cooperative navigation algorithm based on stepwise localization

考虑将某个单兵作为协同中心，涉及的通信如下：

1)足部模块将每步增量及方差阵发送给处理板，再通过无线数传模块发送到协同中心的处理板，发送频率为每步1次，发送数据为一个4维向量和一个4×4的对称矩阵；

2)测距数据由协同中心的UWB模块收集，并传输到协同中心的处理板。

2.3 协同导航算法比较

比较本文、文献[7]以及文献[10](算法信息流程见图3)三种算法。从算法结构上看，文献[10]的算法是带反馈修正的闭环结构，本文和文献[7]则是开环结构。

图3 反馈式协同导航算法信息流程图Fig.3 Information flow chart of the cooperative navigation algorithm with feedback

本文算法与文献[7]中的算法相比，通信量是一样的，区别在于本文算法中足部模块并不进行步进式惯导解算，协同导航中心步进式运动模型的输入(每步在n′坐标系的位移增量和航向增量)是根据足部惯导解算结果转换得到的。

可见，本文步进式协同导航算法的优点是无需改变足部惯导算法，且无需进行反馈修正，这对系统模块化设计及系统工程实现具有重要意义。

3 单兵协同导航试验分析

3.1 试验条件

3名试验人员，分别标记为100、101和102，在校园马路上开展协同导航试验。3人行走在一条马路的两侧，100走外侧，101和102走内侧，且101在102的前面。100和102行走时长约1400s，行走1圈回到各自的起点；101行走时长约1000s，中途退出协同导航。每人足部安装MIMU模块，肩部安装UWB模块和卫星接收机。MIMU模块中惯性测量单元的主要性能指标如表1所示。试验人员101的差分GPS轨迹见图4，图中红点是GPS输出的定位结果，蓝线是没有GPS输出的地方拟合的路线。

表1 MIMU的性能指标

图4 试验人员101的差分GPS轨迹Fig.4 Pedestrian 101 trajectories given by differential GPS

3.2 定位精度分析

对试验数据进行事后处理，对比分析不同条件下独立导航和协同导航的定位精度。

1)足部惯导模块定位结果

足部惯导模块进行ZUPT辅助的惯导解算，可以独立给出单兵轨迹。惯导轨迹精度受两方面的影响，一是初始对准的航向精度，二是ZUPT参数(如零速检测阈值[11]和零速观测噪声方差)。采用三轴磁力仪确定初始对准的航向，其精度取决于磁场环境以及磁力仪的磁干扰补偿精度。ZUPT参数对轨迹形状影响较大。

参数设置一：

在足部定位算法中，零速检测算法和器件噪声参数设置是相同的。在进行事后处理时，通过调整ZUPT参数和初始航向，可以使解算轨迹更接近参考轨迹。采用表2所示ZUPT参数，得到轨迹如图5所示。

表2 ZUPT参数

图5 3人足部惯导模块解算轨迹(参数设置一)Fig.5 The trajectories of three pedestrians given by the foot-mounted MIMU(Parameter setting 1)

可见，100和102的惯导解算定位精度较高，101的航向在行走早期阶段出现了较大偏差，但是轨迹形状并不失真。

参数设置二：

对于在线实现，无法保证初始对准的航向精度及采用最优的ZUPT参数，此时，足部惯导模块定位精度会降低。当3人都采用100的ZUPT参数设置，并且100采用磁力仪确定的初始航向(与参数设置一中的航向相差5°)，得到轨迹如图6所示。此时，101的定位结果没有显著变化，而102的定位精度显著降低。

图6 3人足部惯导模块解算轨迹(参数设置二)Fig.6 The trajectories of three pedestrians given by the foot-mounted MIMU(Parameter setting 2)

2)无GPS条件下的协同导航结果

分别采用参数设置一和参数设置二，仅利用足部惯导模块和UWB测距模块，采用本文算法进行协同导航，定位轨迹分别如图7和图8所示。

图7 3人协同导航轨迹(参数设置一，无GPS)Fig.7 Cooperative navigation trajectories of three pedestrians (Parameter setting 1 without GPS)

图8 3人协同导航轨迹(参数设置二，无GPS)Fig.8 Cooperative navigation trajectories of three pedestrians (Parameter setting 2 without GPS)

对于参数设置一，协同后，101的航向偏差得到了有效修正，3人的定位精度都有提高。

对于参数设置二，在前期3人协同阶段，3人协同定位的精度都有提升；而在后期仅有100和102协同的阶段，102的自定位误差影响了协同导航的效果，在102定位误差降低的同时，100的定位误差反而有所增加。

3)有GPS条件下的协同导航结果

对于参数设置二，将100在3个时刻的GPS观测数据(见表3)加入协同导航，定位轨迹如图9所示。可见，单个试验人员少量的GPS位置修正可以有效提高每个人员的定位精度。

表3 参与协同导航的GPS观测数据

图9 3人协同导航轨迹(参数设置二，带GPS)Fig.9 Cooperative navigation trajectories of three pedestrians (Parameter setting 2 with GPS)

4)不同试验条件下的定位精度

为了定量评估不同试验条件下的定位精度，以差分GPS输出的定位结果为参考值，按照式(16)计算不同试验条件下的均方根误差(Root Mean Square Error,RMSE)

(16)

表4 不同试验条件的定位精度

4 结论

本文利用足部惯性导航解算每步步长和航向增量误差不累积的特点，建立了单兵的步进式运动模型，由此设计的协同导航算法无需反馈修正，具有模块化和低通信量的优势，易于工程实现。对一次3人协同导航的试验数据进行了详细分析，结果表明：

1)在每个成员初始对准误差小、ZUPT参数优化的情况下，仅利用测距的协同导航可以使每个成员都获得高精度的定位结果；

2)当初始对准和ZUPT无法确保最优时，仅利用测距的协同导航可以提高整体的平均定位精度，但不一定能提高每个成员的定位精度，增加单个成员少量的卫星观测则可以显著提高每个成员的定位精度。

单兵协同导航技术未来的发展主要集中在两个方向：一是提高足部惯导模块的导航性能，这一方面可以通过提高器件精度来实现，另一方面可以通过引入学习算法实现零速检测参数的在线优化；二是充分利用实际环境中可能存在的多种导航信息源，将UMB基站、卫星接收机、视觉/惯性组合导航系统、随机信号导航系统等纳入协同导航系统中，以提高协同导航系统的可靠性和可用性。