路永乐，惠嘉威，杨 杰，罗 毅，修蔚然
（重庆邮电大学 智能传感技术与微系统重庆市高校工程研究中心，重庆 400065）
行人导航系统可以在陌生环境中为行人提供导航定位信息[1]。在室外环境中，主要依靠卫星定位系统实现行人导航系统，但是在高楼、森林等复杂环境中，卫星信号会严重衰退，无法满足导航需求[2]。而惯性导航系统（Inertial Navigation System,INS）具有隐蔽性高、抗干扰能力强等优势，因此行人惯性导航系统成为了主要研究方向[3]。
然而惯性测量单元（Inertial Measurement Unit,IMU）存在误差，且导航误差会随时间迅速累积，导致INS 长航时定位精度无法满足需求[4]。研究人员采用了多种补偿方式解决此问题，常见的包括：使用磁力计修正航向误差[5]，采用蓝牙、WIFI、超带宽等定位技术辅助IMU 定位[4]。但磁力计易受周围环境影响而产生大量磁干扰，蓝牙等定位技术无法满足特殊环境需求。针对足绑式IMU 行人导航系统，通常采用零速修正(Zero Velocity Update,ZUPT)算法，将静止时刻的速度作为误差观测量修正其他信息[6]，但仅能在行人静止区间内修正误差。
针对以上问题，研究人员利用行人运动时足部特征进行误差修正：（1）利用多IMU 进行约束。文献[7]在行人足部安装IMU 阵列并根据各IMU 数据误差情况进行选择，剔除其中漂移误差较大的数据，并使用剩余IMU 数据计算行人导航信息；
文献[8]根据脚尖与脚跟相对距离为常量建立约束方程，利用两个IMU信息相互校正提高精度。（2）基于距离进行约束。文献[9]提出双脚间位置始终处于一个半径为双足最大距离的球体空间内，由此构建不等式约束算法；
文献[10]利用行人运动过程中步长及抬脚高度作为约束条件，构建双足椭球约束模型修正导航误差。（3）多传感器信息融合，利用测距传感器实时测量步长。文献[11]在行人足部安装超声波模块测量双足间相对距离辅助双足IMU 定位；
文献[12]将毫米波雷达安装在行人足部，实时提供行人双足相对位置信息。
上述方法通过引入多个IMU 或其它传感器修正惯性导航系统误差，可以有效提高系统精度，但增加了行人导航系统的复杂性和硬件成本，限制了其推广应用。基于以上问题，本文在固定距离约束的基础上，提出基于自适应步长约束的行人导航系统误差修正算法，通过零速检测划分运动区间，在静止与运动区间内分别进行零速修正与步长约束修正，并根据IMU 信息自适应行人步长，提高算法稳定性。该算法利用行人脚跟处的IMU 实现惯性定位，在不增加系统复杂性的基础上，提高行人导航系统定位精度。
1.1 行人足部运动特征
如图1所示，行人行走过程中足部特征呈周期性变化。其中每个步态周期又分为运动区间和静止区间[13]。一个完整的运动区间以脚跟离地为开始，依次经历脚尖离地、摆动、脚跟触地，最后以脚尖触地为结束。然后进入静止区间，足部与地面保持静止，直到下一次脚跟离地。
图1 足部运动特征Fig.1 Foot movement characteristics
行人的一个步态周期中，单侧脚轨迹存在步长约束条件。如图2所示，设行人跨步长为l，则单侧脚的空间位置一定处于半径l为的球体内。若将连续两次触地点间距离差值记为Δd，则一定满足Δd≤l。
图2 足部轨迹模型Fig.2 Foot trajectory model
1.2 步长约束模型
足绑式惯性元件的行人导航系统，通过IMU 采集行人加速度与角速度信息，利用捷联惯导算法解算得到位姿信息[14]。捷联惯导微分方程如下所示：
设k-1时刻和k时刻导航系统的状态向量分别为：
刚才还整齐列着的队伍一下子散乱开来,大家涌进仓库。张连长望着知青们奔向仓库的背影,束手无策地自语:“这老爷子,真添乱!”
式(4)表示任意时刻单侧脚的运动轨迹必须满足步长约束条件，通过利用带约束条件的最小二乘法，可以将位置信息表示为：
利用拉格朗日乘数法求解式(6)，引入拉格朗日乘子λ，构建拉格朗日函数：
对式(7)进行求解：
最终得到行人当前时刻位置信息为：
2.1 总体框架
图3 总体框架Fig.3 Overall framework
2.2 零速修正
零速修正算法将静止时的速度为零作为误差观测量，利用卡尔曼滤波修正导航误差。常用的零速检测有加速度方差法、加速度阈值法、角速度能量法等[16]。本文采用基于广义似然比检测法（Generalized Likelihood Ratio Test,GLRT）进行零速检测，该方法同时利用到加速度计信息和陀螺仪信息，检测准确度更高[17]。
如图4所示，GLRT 检测结果为高电平时表示为零速，检测结果为低电平时表示为非零速。由于零速检测存在误判，如图4 虚线区域所示，有部分零速点出现在非零速区域，导致导航解算结果错误，最终引起导航误差。因此必须消除误判情况，提高零速检测准确度。
图4 GLRT 零速检测结果Fig.4 Result of GLRT zero-speed detection
通过行人步态特征可知，行人每行进一步，足部状态都会分为静止阶段和运动阶段，而每一种状态都必须持续一段时间，才能转变为另一种状态。因此设计基于时间阈值检测的零速检测算法：
(1)利用零速检测算法对行人足部状态分组，将运动阶段和静止阶段分别定义为Cmove,m和Cstatic,s，其中m表示该运动阶段内包含的数据数量，s表示该静止阶段内包含的数据数量。
(2)将各阶段的数据数量与时间阈值ΔT进行比较。若各阶段的数据数量均大于时间阈值，则表示零速区间检测正确；
反之，若某一阶段的数据数量小于时间阈值，则表明该阶段检测错误。
(3)对行人每一步的足部运动状态结果都进行时间阈值检测，对误检区间进行修正。
2.3 自适应步长估计
根据1.2 节分析，步长约束模型首先需要得到行人的步长数据，而运动中的行人步长为一个动态变量。影响行人步长的因素有很多，例如行人的身高、腿长、体重、路面情况、运动状态等。根据分析可知行人在运动过程中，迈步长度与垂直方向加速度信息之间具有很大的关联性[18]。根据z轴加速度与迈步长度的关系建立自适应步长估计模型：
式中，Sn为第n步迈步长度；
分别为第n次步态周期中z 轴加速度的最大值与最小值；
τ 为步长系数，该系数针对不同的行人及其运动状态具有特定的值。
由1.2 节分析可知，当前时刻的位置信息可以由式(10)得到。在此理论之上，根据IMU 数据识别步态，并根据自适应步长估计模型计算当前步长。将计算出的步长Sn带入式(10)，得到带有自适应步长的位置信息：
式中，Δt为IMU 采样周期。
本文选择在重庆邮电大学香樟园五号楼进行实验验证。设定起点坐标为(0,0)，实验人员沿矩形路线行走，通过RTS-820S 全站仪标定，矩形的长为50 m，宽为10 m。行走路线的起点与终点坐标重合，以终点定位误差作为衡量算法效果的依据。实验过程中将WT901BC 姿态仪固定在实验人员右脚脚跟。
实验(1)：实验人员按实验路线以1 m 的固定步长行走两圈，行进距离共计240 m，实验结果如图5所示，轨迹误差结果如表1所示。图5(a)为未步长约束的运动轨迹，其终点位置坐标为(0.87 m，3.25 m)，闭环误差为1.40%D；
图5(b)为使用步长约束的运动轨迹，其终点位置坐标为(-1.05 m，-1.02 m)，闭环误差为0.61%D，且图5(b)更符合实际运动轨迹，导航系统精度得到提高。
图5 运动轨迹Fig.5 Motion trajectory
表1 实验方案误差比较Tab.1 Error comparison of test schemes
实验(2)：选取三名身高不同的实验人员在一条长为24 m 的走廊上进行三次行走实验，通过比较三次实验估计距离值与实际距离值之间的误差关系，选取各实验人员τ 的最优值并验证自适应步长估计模型的准确性，实验结果如表2所示。表中跨步长指一侧脚足跟着地到该侧脚足跟再次着地所进行的距离，即本文中单侧脚单次运动行进的距离。对比三次实验结果可知，本文使用的自适应步长估计模型估计的平均步长误差为1.94%，能够较为准确地估计步长。
表2 自适应步长模型误差Tab.2 Error of adaptive step length estimation model
实验(3)：三名实验人员按实验路线行走两圈，单次实验行进距离为240 m。每次实验包括零速修正、零速修正结合固定步长约束、零速修正结合自适应步长约束三种情况，实验结果如图6-8所示，轨迹误差结果如表3所示。对比图6-8 中各行走轨迹以及表3数据可知，不使用步长约束的轨迹误差较大，起终点间距离误差平均值为2.50 m，闭环误差平均值为1.04%D；
使用固定步长约束的轨迹，起终点间距离误差平均值为2.90 m，闭环误差平均值为1.20%D，且实验轨迹会出现不符合实际运动路线的情况；
而使用自适应步长约束算法修正后的起终点间距离误差平均值下降至0.18 m，闭环误差平均值下降至0.07%D，且实验轨迹更符合实际运动路线。因此本文提出的算法可以有效提高导航系统定位精度。
图6 实验一运动轨迹Fig.6 The trajectory of experiment one
表3 自适应步长约束实验结果Tab.3 Experimental results of adaptive step size constraint
图7 实验二运动轨迹Fig.7 The trajectory of experiment two
图8 实验三运动轨迹Fig.8 The trajectory of experiment three
本文针对基于足绑式惯性传感器的行人导航系统误差随时间累积而导致定位精度下降的问题，提出基于自适应步长约束的导航误差修正算法。算法在行人足部静止时进行零速修正，在运动时通过自适应步长约束修正误差，最终提高行人导航系统的精度。针对零速修正、零速修正结合固定步长约束以及零速修正结合自适应步长约束进行了三组对比实验，数据显示本文提出的基于自适应步长约束的行人导航系统误差修正算法相比于只使用零速修正情况，终点与起点间距离误差的平均值由2.50 m 下降至0.18 m，导航闭环误差平均值由1.04%D下降至0.07%D；
相比于零速修正结合固定步长约束算法修正，终点与起点间距离误差的平均值由2.90 m 下降至0.18 m，导航闭环误差平均值由1.20%D下降至0.07%D。实验结果表明，本文提出的方案能够有效提升导航精度，具有一定的工程应用价值。