航向约束的行人导航算法研究
郑佳瑶+苏中+李擎
摘 要： 針对MEMS惯性器件随时间累积零漂误差大及航向误差可观测性差，导致行人导航航向发散的问题，提出一种基于主方向的航向修正算法。基于行人沿直线行走航向角保持在常值范围内的事实，当航向发生变化时，将行人当前的航向与主航向的偏差作为观测值，利用 EKF进行误差估计从而修正航向，最后将导航模块固定在鞋面上进行约200 m的行走实验。实验结果表明，该方法能有效地抑制航向发散，定位误差约为总行走路程的1%。
关键词： 行人导航； 航向发散； 航向修正； EKF
中图分类号： TN961?34； TP391.9 文献标识码： A 文章编号： 1004?373X（2017）24?0001?04
Abstract： The MEMS inertial device has big zero?drift error with time accumulation and has poor observability for its heading error， which may cause the heading divergency of the pedestrian navigation. Therefore， a heading correction algorithm based on the main direction is proposed. Based on the fact that the heading angle while the pedestrian travels along the straight line maintains within the constant range， taking the deviation between the pedestrian′s current heading and master heading as the observation value when the heading changes， the extended Kalman filter （EKF） is used to estimate the error， so as to correct the heading. The navigation module is fixed on the upper surface of the shoes for about 200 m walking experiment. The experimental results show that the method can effectively suppress the heading divergency， and its positioning error is about 1% of the total walking distance.
Keywords： pedestrian navigation； heading divergency； heading correction； extended Kalman filter
0 引 言
目前，GPS室外定位技术已相当成熟，但GPS在建筑物遮挡时信号容易丢失，易受环境的影响且民用精度不高，在室内定位存在明显的缺陷，因此，无法满足人们对室内终端定位的需求。而低成本的基于MEMS?IMU的个人导航[1]系统不需要任何外部设施，非常适合在室内以及GPS信号较弱的地方使用。然而该系统的惯性器件存在随时间累积的漂移误差，是造成行人导航航向发散的主要误差源。为了解决这一问题，国内外研究人员[2]借助一些附加信息提出了建筑物信息融合算法、磁航向辅助算法等方法，但这些附加信息增加了系统的复杂度，易受客观条件的干扰，影响了导航系统的应用效果。基于启发式方法利用建筑物主方向校正航向的思想，Ref提出了一种启发式漂移减少的HDR算法来修正垂直方向的陀螺仪角速率[3]；但该算法需要设计特殊的滤波器对陀螺仪角速率进行滤波，增加了系统的复杂度。之后Ref等学者在HDR的基础上提出一种启发式漂移消除算法（HDE）[4]，通过修正陀螺仪的输出并利用反馈变量校正航向；但陀螺仪积分计算角度的过程会产生一定的累积误差，仍存在航向发散的现象。
本文在启发式思想HDE的基础上，提出一种基于主方向的航向修正算法来抑制航向发散。基于行人在结构化的室内环境中行走，航迹近似为直线，航向变化保持在常值范围内的事实，当有误差引起航向发生变化时，将行人当前的航向与主航向的偏差作为观测值，利用EKF进行误差估计从而修正航向角。相对比HDE算法直接修正陀螺仪输出，积分计算角度并利用反馈变量修正航向的方法，本文提出的算法能有效地避免HDE积分计算角度的过程产生累积误差的问题。
1 系统模型
本文将基于MEMS?IMU的导航模块固定在鞋面上，根据人行走时脚接触地面的周期性零速特征触发零速修正技术（ZUPT）[5]，由于航向误差的可观测性差，ZUPT无法修正航向误差，引入了一种基于主方向的航向修正算法，实时修正导航系统的累积误差。导航系统的整体框图如图1所示，其中虚线框部分为基于主方向的航向修正算法的实现过程。首先采集IMU数据，根据IMU中陀螺仪和加速度计的输出进行步态检测，当检测到行人处于脚接触地面的零速时刻，触发ZUPT算法，从而修正导航系统的速度、位置和水平姿态误差。基于行人在结构化的室内环境沿直线行走，航向变化保持在常值范围内的事实，当有误差引起该航向发生变化时，触发本文所提的航向修正算法，利用行人当前的航向与主航向的偏差估计航向角误差，从而抑制航向发散，最后将ZUPT算法和所提出的航向修正方法集成的误差作为系统的观测值，利用EKF进行误差估计，进一步提高导航系统的定位精度。
2 算法实现
2.1 ZUPT算法
基于步态检测的零速修正技术已广泛应用在鞋式行人导航中，本文采用三条件（C1，C2，C3）判断方法[6]并通过中值滤波技术来实现步态检测。当检测到脚在地面完全静止或处于行走时的支撑阶段，如图2所示，利用该时刻的静止步态作为触发条件来开启零速修正ZUPT方法，并将零速度更新作为“伪测量”应用于扩展卡尔曼滤波器进行误差估计，从而修正IMU系统的长时间累积误差，提高定位精度。
ZUPT算法的实现是根据人在行走的过程中，脚与地面接触瞬间，速度为零的事实，实时修正由于惯性传感器的测量误差、噪声以及其他因素引起的速度误差。速度误差的方程如下：
然后将该速度误差作为观测值，利用EKF进行误差估计，从而修正导航系统的速度、位置和水平姿态误差。由于惯性器件随时间累积的零漂误差较大且航向误差可观测性差[7]，导致ZUPT无法修正航向角，是造成行人导航航向发散的主要误差源。本文在ZUPT修正导航系统误差的同时，引入了一种基于主方向的航向修正算法来抑制航向发散，进一步提高导航系统的精度。
2.2 基于主方向的航向修正算法
2.2.1 航向估计
本文通过采集IMU数据进行捷联解算和四元数更新的方法[8]来推算航向角，具体过程如下：
四元数是指由四个元构成的数，表达式如下：
利用四元数初始姿态角可表示如下：
式中，分别表示偏航角、俯仰角和横滚角，且在初始时刻的值均为0。
利用毕卡三阶算法更新四元数的方法如下：
式中：为时刻的四元数；为3个轴角度平方和；为单位矩阵；为对机体坐标系相对地理坐标系的旋转角速度的斜对称矩阵的积分。
四元数更新后，利用四元数表示姿态转换矩阵，两者的关系如下：
得到姿态转换矩阵后，推算航向角如下：
式中，是姿态矩阵的第行、第列元素。
通过以上方法，可根据陀螺仪的输出计算得到航向角。由于陀螺仪输出存在随时间累积的漂移误差，从而导致航向发散的问题，需采用相关的方法来消除航向漂移误差。
2.2.2 航向修正
基于启发式方法（HDE）中主方向的思想，本文设定行人在室内行走有8个主航向，如图3所示，假设行人初始航向为0，左边为负，右边为正。
通常，行人在结构化的室内环境中沿直线行走时，航向变化不大且始终保持在常值范围内；当行人转向时，航向会发生较大的变化，基于这个事实，可以通过航向的变化判断行人是否转向。为了判断准确，本文采用相邻3步的航向进行转向判断，公式如下：
当行人沿直线行走，有误差引起航向发生变化时，利用行人当前的航向和主航向的偏差来估计航向角误差，如下：
最后将该偏差作为系统的观测值，利用EKF进行误差估计从而修正航向。
本文建立的12维状态方程如下：
本文通过集成传统的ZUPT算法和基于主方向的航向修正算法消除系统累积误差的过程，引入了速度误差和航向误差两个观测值。当行人处于静止步态触发ZUPT修正方法时，速度误差可直接被观测到，其观测方程如下：
当行人沿直线行走，有误差引起航向发生变化，触发文中所提的航向修正算法时，航向误差的观测方程如下：
由于HDE算法只适用于行人在规则的室内沿直线行走时的航向修正，当行人转弯时，HDE算法将停止航向修正并带来新的误差。为了使本文所提出的算法适用于行人转弯时继续航向修正，本文定义了一个航向偏差标准差表达式[9?11]，即式（18）中的，如下：
3 实验与结果
为了验证所提算法的有效性，本文将实验室自研制的基于MEMS?IMU的导航模块固定在鞋面上，在室内进行行走实验。
实验环境为教学楼6楼的走廊，行走的路线为如图4所示的一个闭环矩形，起止位置均为A，通过仿真實验得到的航迹推算结果如图5所示。
从图5中可以看出，由于航向误差的不可观测性，使得ZUPT修正的轨迹与实际轨迹存在偏差，定位误差为总行走路程的2.1%；而在ZUPT的基础上引入本文所提的修正方法后，修正的轨迹与真实轨迹基本吻合，定位误差为总行走路程的0.7%。该方法与传统的ZUPT算法相比，有效地解决了行人导航航向发散的问题，提高了导航系统的定位精度。
4 结 语
针对MEMS惯性器件随时间累积的零漂误差大及航向误差可观测性差，ZUPT无法修正航向从而导致航向发散的问题，因此，在启发式思想HDE的基础上提出了一种基于主方向的航向修正算法。首先设定行人在室内行走有8个主航向，然后基于行人在结构化的室内环境中行走，航迹近似为直线，航向变化保持在常值范围的事实。当有误差引起航向发生变化时，将行人当前的航向与主航向的偏差作为观测值，利用 EKF进行误差估计从而修正航向角。最后通过仿真实验验证了本文所提算法能有效地抑制航向发散，证明所提算法的有效性和准确性。
为了更好地验证本文所提方法的有效性，今后将在更复杂的室内环境中进行行走实验，并在该方法的基础上融入地图或超宽带UWB等定位信息，进一步提高导航定位精度。
参考文献
[1] 张金亮，秦永元，梅春波.基于MEMS惯性技术的鞋式个人导航系统[J].中国惯性技术学报，2011，19（3）：253?256.
[2] 谢波，江一夫，严恭敏，等.个人导航融合建筑平面信息的粒子滤波方法[J].中国惯性技术学报，2013，21（1）：1?6.
[3] LI Y， SONG Q， MA M， et al. Near real time heading drift correction for indoor pedestrian tracking based on sequence detection [C]// Proceedings of 2016 International Conference on Indoor Positioning and Indoor Navigation. Alcala de Henares： IEEE， 2016： 1?4.
[4] 楼喜中，周乐宇，叶敏展，等.基于角度的HDE算法在室内行人航迹定位中的研究[J].传感技术学报，2015，15（4）：598?602.
[5] 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]// Proceedings of 2010 IEEE the 7th Workshop on Positioning Navigation and Communication. Dresden： IEEE， 2010： 135?143.
[6] 李超，苏中，朱嘉林.可穿戴式自主定位技术的零速触发算法研究[J].传感技术学报，2014，14（5）：627?632.
[7] ZHANG X， RONG Z， GUO M F， et al. Yaw error self?observation algorithm for pedestrian navigation via foot?mounted inertial navigation system [J]. Journal of Chinese inertial technology， 2015， 23（4）： 457?466.
[8] 何瑜.四轴飞行器控制系统设计及其姿态解算和控制算法研究[D].成都：电子科技大学，2015：65?77.
[9] JIM?NEZ A R， SECO F， ZAMPELLA F， et al. Improved heuristic drift elimination （iHDE） for pedestrian navigation in complex buildings [C]// Proceedings of 2011 International Conference on Indoor Positioning and Indoor Navigation. Guimaraes： IEEE， 2011： 1?8.
[10] D?EZ L E， BAHILLO A， BATAINEH S， et al. Enhancing improved heuristic drift elimination for step?and?heading based pedestrian dead?reckoning systems [C]// Proceedings of 2016 IEEE the 38th Annual International Conference of Engineering in Medicine and Biology Society. Orlando： IEEE， 2016： 4415?4418.
[11] JU H J， LEE M S， PARK C G， et al. Advanced heuristic drift elimination for indoor pedestrian navigation [C]// Proceedings of 2014 International Conference on Indoor Positioning and Indoor Navigation. Busan： IEEE， 2014： 729?732.技术文
摘 要： 針对MEMS惯性器件随时间累积零漂误差大及航向误差可观测性差，导致行人导航航向发散的问题，提出一种基于主方向的航向修正算法。基于行人沿直线行走航向角保持在常值范围内的事实，当航向发生变化时，将行人当前的航向与主航向的偏差作为观测值，利用 EKF进行误差估计从而修正航向，最后将导航模块固定在鞋面上进行约200 m的行走实验。实验结果表明，该方法能有效地抑制航向发散，定位误差约为总行走路程的1%。
关键词： 行人导航； 航向发散； 航向修正； EKF
中图分类号： TN961?34； TP391.9 文献标识码： A 文章编号： 1004?373X（2017）24?0001?04
Abstract： The MEMS inertial device has big zero?drift error with time accumulation and has poor observability for its heading error， which may cause the heading divergency of the pedestrian navigation. Therefore， a heading correction algorithm based on the main direction is proposed. Based on the fact that the heading angle while the pedestrian travels along the straight line maintains within the constant range， taking the deviation between the pedestrian′s current heading and master heading as the observation value when the heading changes， the extended Kalman filter （EKF） is used to estimate the error， so as to correct the heading. The navigation module is fixed on the upper surface of the shoes for about 200 m walking experiment. The experimental results show that the method can effectively suppress the heading divergency， and its positioning error is about 1% of the total walking distance.
Keywords： pedestrian navigation； heading divergency； heading correction； extended Kalman filter
0 引 言
目前，GPS室外定位技术已相当成熟，但GPS在建筑物遮挡时信号容易丢失，易受环境的影响且民用精度不高，在室内定位存在明显的缺陷，因此，无法满足人们对室内终端定位的需求。而低成本的基于MEMS?IMU的个人导航[1]系统不需要任何外部设施，非常适合在室内以及GPS信号较弱的地方使用。然而该系统的惯性器件存在随时间累积的漂移误差，是造成行人导航航向发散的主要误差源。为了解决这一问题，国内外研究人员[2]借助一些附加信息提出了建筑物信息融合算法、磁航向辅助算法等方法，但这些附加信息增加了系统的复杂度，易受客观条件的干扰，影响了导航系统的应用效果。基于启发式方法利用建筑物主方向校正航向的思想，Ref提出了一种启发式漂移减少的HDR算法来修正垂直方向的陀螺仪角速率[3]；但该算法需要设计特殊的滤波器对陀螺仪角速率进行滤波，增加了系统的复杂度。之后Ref等学者在HDR的基础上提出一种启发式漂移消除算法（HDE）[4]，通过修正陀螺仪的输出并利用反馈变量校正航向；但陀螺仪积分计算角度的过程会产生一定的累积误差，仍存在航向发散的现象。
本文在启发式思想HDE的基础上，提出一种基于主方向的航向修正算法来抑制航向发散。基于行人在结构化的室内环境中行走，航迹近似为直线，航向变化保持在常值范围内的事实，当有误差引起航向发生变化时，将行人当前的航向与主航向的偏差作为观测值，利用EKF进行误差估计从而修正航向角。相对比HDE算法直接修正陀螺仪输出，积分计算角度并利用反馈变量修正航向的方法，本文提出的算法能有效地避免HDE积分计算角度的过程产生累积误差的问题。
1 系统模型
本文将基于MEMS?IMU的导航模块固定在鞋面上，根据人行走时脚接触地面的周期性零速特征触发零速修正技术（ZUPT）[5]，由于航向误差的可观测性差，ZUPT无法修正航向误差，引入了一种基于主方向的航向修正算法，实时修正导航系统的累积误差。导航系统的整体框图如图1所示，其中虚线框部分为基于主方向的航向修正算法的实现过程。首先采集IMU数据，根据IMU中陀螺仪和加速度计的输出进行步态检测，当检测到行人处于脚接触地面的零速时刻，触发ZUPT算法，从而修正导航系统的速度、位置和水平姿态误差。基于行人在结构化的室内环境沿直线行走，航向变化保持在常值范围内的事实，当有误差引起该航向发生变化时，触发本文所提的航向修正算法，利用行人当前的航向与主航向的偏差估计航向角误差，从而抑制航向发散，最后将ZUPT算法和所提出的航向修正方法集成的误差作为系统的观测值，利用EKF进行误差估计，进一步提高导航系统的定位精度。
2 算法实现
2.1 ZUPT算法
基于步态检测的零速修正技术已广泛应用在鞋式行人导航中，本文采用三条件（C1，C2，C3）判断方法[6]并通过中值滤波技术来实现步态检测。当检测到脚在地面完全静止或处于行走时的支撑阶段，如图2所示，利用该时刻的静止步态作为触发条件来开启零速修正ZUPT方法，并将零速度更新作为“伪测量”应用于扩展卡尔曼滤波器进行误差估计，从而修正IMU系统的长时间累积误差，提高定位精度。
ZUPT算法的实现是根据人在行走的过程中，脚与地面接触瞬间，速度为零的事实，实时修正由于惯性传感器的测量误差、噪声以及其他因素引起的速度误差。速度误差的方程如下：
然后将该速度误差作为观测值，利用EKF进行误差估计，从而修正导航系统的速度、位置和水平姿态误差。由于惯性器件随时间累积的零漂误差较大且航向误差可观测性差[7]，导致ZUPT无法修正航向角，是造成行人导航航向发散的主要误差源。本文在ZUPT修正导航系统误差的同时，引入了一种基于主方向的航向修正算法来抑制航向发散，进一步提高导航系统的精度。
2.2 基于主方向的航向修正算法
2.2.1 航向估计
本文通过采集IMU数据进行捷联解算和四元数更新的方法[8]来推算航向角，具体过程如下：
四元数是指由四个元构成的数，表达式如下：
利用四元数初始姿态角可表示如下：
式中，分别表示偏航角、俯仰角和横滚角，且在初始时刻的值均为0。
利用毕卡三阶算法更新四元数的方法如下：
式中：为时刻的四元数；为3个轴角度平方和；为单位矩阵；为对机体坐标系相对地理坐标系的旋转角速度的斜对称矩阵的积分。
四元数更新后，利用四元数表示姿态转换矩阵，两者的关系如下：
得到姿态转换矩阵后，推算航向角如下：
式中，是姿态矩阵的第行、第列元素。
通过以上方法，可根据陀螺仪的输出计算得到航向角。由于陀螺仪输出存在随时间累积的漂移误差，从而导致航向发散的问题，需采用相关的方法来消除航向漂移误差。
2.2.2 航向修正
基于启发式方法（HDE）中主方向的思想，本文设定行人在室内行走有8个主航向，如图3所示，假设行人初始航向为0，左边为负，右边为正。
通常，行人在结构化的室内环境中沿直线行走时，航向变化不大且始终保持在常值范围内；当行人转向时，航向会发生较大的变化，基于这个事实，可以通过航向的变化判断行人是否转向。为了判断准确，本文采用相邻3步的航向进行转向判断，公式如下：
当行人沿直线行走，有误差引起航向发生变化时，利用行人当前的航向和主航向的偏差来估计航向角误差，如下：
最后将该偏差作为系统的观测值，利用EKF进行误差估计从而修正航向。
本文建立的12维状态方程如下：
本文通过集成传统的ZUPT算法和基于主方向的航向修正算法消除系统累积误差的过程，引入了速度误差和航向误差两个观测值。当行人处于静止步态触发ZUPT修正方法时，速度误差可直接被观测到，其观测方程如下：
当行人沿直线行走，有误差引起航向发生变化，触发文中所提的航向修正算法时，航向误差的观测方程如下：
由于HDE算法只适用于行人在规则的室内沿直线行走时的航向修正，当行人转弯时，HDE算法将停止航向修正并带来新的误差。为了使本文所提出的算法适用于行人转弯时继续航向修正，本文定义了一个航向偏差标准差表达式[9?11]，即式（18）中的，如下：
3 实验与结果
为了验证所提算法的有效性，本文将实验室自研制的基于MEMS?IMU的导航模块固定在鞋面上，在室内进行行走实验。
实验环境为教学楼6楼的走廊，行走的路线为如图4所示的一个闭环矩形，起止位置均为A，通过仿真實验得到的航迹推算结果如图5所示。
从图5中可以看出，由于航向误差的不可观测性，使得ZUPT修正的轨迹与实际轨迹存在偏差，定位误差为总行走路程的2.1%；而在ZUPT的基础上引入本文所提的修正方法后，修正的轨迹与真实轨迹基本吻合，定位误差为总行走路程的0.7%。该方法与传统的ZUPT算法相比，有效地解决了行人导航航向发散的问题，提高了导航系统的定位精度。
4 结 语
针对MEMS惯性器件随时间累积的零漂误差大及航向误差可观测性差，ZUPT无法修正航向从而导致航向发散的问题，因此，在启发式思想HDE的基础上提出了一种基于主方向的航向修正算法。首先设定行人在室内行走有8个主航向，然后基于行人在结构化的室内环境中行走，航迹近似为直线，航向变化保持在常值范围的事实。当有误差引起航向发生变化时，将行人当前的航向与主航向的偏差作为观测值，利用 EKF进行误差估计从而修正航向角。最后通过仿真实验验证了本文所提算法能有效地抑制航向发散，证明所提算法的有效性和准确性。
为了更好地验证本文所提方法的有效性，今后将在更复杂的室内环境中进行行走实验，并在该方法的基础上融入地图或超宽带UWB等定位信息，进一步提高导航定位精度。
参考文献
[1] 张金亮，秦永元，梅春波.基于MEMS惯性技术的鞋式个人导航系统[J].中国惯性技术学报，2011，19（3）：253?256.
[2] 谢波，江一夫，严恭敏，等.个人导航融合建筑平面信息的粒子滤波方法[J].中国惯性技术学报，2013，21（1）：1?6.
[3] LI Y， SONG Q， MA M， et al. Near real time heading drift correction for indoor pedestrian tracking based on sequence detection [C]// Proceedings of 2016 International Conference on Indoor Positioning and Indoor Navigation. Alcala de Henares： IEEE， 2016： 1?4.
[4] 楼喜中，周乐宇，叶敏展，等.基于角度的HDE算法在室内行人航迹定位中的研究[J].传感技术学报，2015，15（4）：598?602.
[5] 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]// Proceedings of 2010 IEEE the 7th Workshop on Positioning Navigation and Communication. Dresden： IEEE， 2010： 135?143.
[6] 李超，苏中，朱嘉林.可穿戴式自主定位技术的零速触发算法研究[J].传感技术学报，2014，14（5）：627?632.
[7] ZHANG X， RONG Z， GUO M F， et al. Yaw error self?observation algorithm for pedestrian navigation via foot?mounted inertial navigation system [J]. Journal of Chinese inertial technology， 2015， 23（4）： 457?466.
[8] 何瑜.四轴飞行器控制系统设计及其姿态解算和控制算法研究[D].成都：电子科技大学，2015：65?77.
[9] JIM?NEZ A R， SECO F， ZAMPELLA F， et al. Improved heuristic drift elimination （iHDE） for pedestrian navigation in complex buildings [C]// Proceedings of 2011 International Conference on Indoor Positioning and Indoor Navigation. Guimaraes： IEEE， 2011： 1?8.
[10] D?EZ L E， BAHILLO A， BATAINEH S， et al. Enhancing improved heuristic drift elimination for step?and?heading based pedestrian dead?reckoning systems [C]// Proceedings of 2016 IEEE the 38th Annual International Conference of Engineering in Medicine and Biology Society. Orlando： IEEE， 2016： 4415?4418.
[11] JU H J， LEE M S， PARK C G， et al. Advanced heuristic drift elimination for indoor pedestrian navigation [C]// Proceedings of 2014 International Conference on Indoor Positioning and Indoor Navigation. Busan： IEEE， 2014： 729?732.技术文
