登陆0 引言
随着导航技术的发展，卫星导航技术已成为成熟的、全方位的现代导航系统，但随着应用领域的扩大，卫星导航系统的缺点也逐渐显露出来，特别是在室内、山洞和隧道内，由于卫星导航信号质量较差，进而无法获取导航数据，已成为卫星导航技术的致命缺点。目前，基于MEMS的室内行人航迹推算作为一种新兴的室内个人导航定位方法受到了广泛关注[1]。而随着惯导技术的发展，以ADI公司为代表的MEMS惯性测量单元打破传统惯导器件的体积大、价格昂贵的格局，从而使惯导应用走向个人[2-3]。
个人导航系统主要用于跟踪定位徒步行走人员的实时位置[4-5]。目前，个人导航研究取得了一系列成果。文献[6]中提到的将MEMS器件安装在被测试人员的腰部，通过行走频率和加速度计方差估计出步长，从而得到行走速度和距离，此方案无法适应不同的步行者和不同的行走方式而局限性很大。文献[7]利用无线电信号识别路标点，与地图进行匹配，从而大大提高了定位进度，但是这要借助外部设备才能进行定位。文献[4]、[5]将MEMS惯性测量单元安装在鞋上，利用磁力计对方位修正，在一定程度上提高了导航精度。文献[8]利用GPS/DR组合导航进行行人导航，并提出DR参数估计方法提高了航迹推算参数的精度。此外，还有其他的三角定位法、GPS/INS组合、UWB/MEMS微机电系统传感器整合的PDR算法等[9-10]。
另一方面，器件的性能和价格很大程度上影响着行人惯性导航的计算精度和应用范围。为此，本文在MEMS惯性测量单元器件上开发出一套行人鞋式导航系统，用于验证算法的有效性，为进一步提高导航精度奠定基础。
本试验所采用的硬件以STM32F103芯片为控制核心，通过定时采样MPU6050的加速度、角速度以及HMC5883的磁力信息，经过姿态解算和导航解算，经串口把导航信息输出，再用MATLAB对串口信息进行处理，最终得到导航信息的图形化显示，系统框图如图1所示。
2 算法结构
该系统算法分为三个子算法，算法总体结构设计如图2所示。
采用地理坐标系作为导航系，以东、北、天 3个方向作为导航系的x、y、z轴正方向，MPU6050的x、y、z 3轴与HMC5883的3轴、载体x、y、z 3轴均呈平行关系。描述载体运动的直观方法为欧拉角法，本文采用习惯定义，载体的俯仰角、横滚角和偏航角分别用θ、γ和表示。下面详细说明各算法的设计。
2.1 算法基础
2.1.1 姿态更新算法
姿态更新算法采用四元数法，该算法只需求解4个未知量的线性微分方程组[11]，算法简单，易于操作，较为实用，克服了方向余弦法未知量多、计算量大的缺点，同时避免了欧拉角算法易出现方程退化的问题。
由以上关系可将实时计算的四元数法与直观的欧拉角法联系起来，经扩展卡尔曼滤波，即可得到准确的运载体姿态。
2.1.2 9DOF全姿态算法
9DOF全姿态算法融合了三轴磁力计、三轴加速度计和三轴陀螺仪的数据，以四元数为状态量，加速度计和磁力计原始输出为观测量，通过扩展卡尔曼滤波算法，得到全姿态欧拉角。
四元数微分方程（即卡尔曼滤波的状态方程）的连续形式为：
2.1.3 初始化
在东北天坐标系下完成磁场强度的初始化，公式为：
试验前，采用椭球拟合的方法对惯导器件进行初始标定。然后，根据以上模型构造出扩展卡尔曼滤波器，通过对惯导器件和磁力计噪声的数学模型研究，用试验的方法调整其噪声参数，最终得到准确的姿态输出。根据试验，为兼顾其他算法和姿态实时更新要求，姿态更新频率最终确定为66 Hz。
2.2 捷联式惯导算法
在捷联惯导中，陀螺和加速度计的输出经计算机按一定的算法处理后，获得离散时间点上的姿态、航向、速度和位置等导航信息。
2.2.1 速度算法
捷联惯导的基础——比力方程为：
2.2.2 位置算法
采用普通的速度积分法，公式如下：
经试验，仅采用以上的速度和位置算法会因器件的测量和噪声带来的误差，导致速度误差急剧增大，而使位置快速发散，导航失败。针对这种情况，需设计卡尔曼滤波器，对速度和位置进行修正，即零速检测和零速修正算法[12-13]。
2.3 零速检测算法
首先对人的行走模态进行分析。由于所采用的器件精度较低，不能感受重力、地球的自转角速度，也就不会产生牵连角速度，因而载体Z轴正方向指向导航系正方向静止放置时，存在正的1 g的比力。将测试板装配在测试人员的一只脚上，通过收集传感器的输出数据，绘制出图3步行的加速度特征曲线和图4步行的角速度特征曲线。
由图分析得，可将人的行走分为两个过程，抬脚和放脚为运动过程，触地稳定后到第二次抬脚前为静止过程。两个过程的加速度和角速度都有明显的不同，因此可通过设置适当的算法将这两个过程区分开来，本试验采用以下3个判断条件：
(1)导航系的合加速度收敛
根据对行走模态的加速度特征分析，在收脚期间，合加速度是减小的，通过对加速度连续多次采样，判断合加速度的收敛情况。本试验采用连续7次采样，满足5次的后一次值减去前一次值为负即为收敛。
(2)载体系的合加速度小于门限值fbm
由图3可看出，静止期间的合加速度值处于较低水平，因此可通过设置门限值来区分静止和运动。
2.4 零速校正算法
基于零速检测，用卡尔曼滤波算法对速度误差和位置误差进行估计[14]，实验中还融入对方位的估计，从而实现对速度和位置在各方位上的有效补偿。下面说明本算法的实现。状态量为：
由于所选的观测量只有在判断为静止即脚的速度为零时才能被观测，所以，在运动期间，滤波器只做时间更新，在静止期间做完整更新。
3 试验结果
为验证算法的有效性，在一块室内矩形区域(如图7所示)进行了步行试验，各次试验结果如图5～图7所示。
其中，图5是未加任何校正的导航结果实测图，可看到未经修正的纯惯性导航的位置误差发散很快，这主要是由水平姿态误差和速度误差的积累引起的。图6是加速度和位置校正但未加入方位校正的导航实测图，可看到方位误差严重，解算的方位已偏离了真实路径。图7是加入速度、位置和方位校正的综合算法实测图，可看到导航结果得到明显改善。
4 结论
本文的个人鞋式导航算法在充分分析个人行走特点的基础上，经过零速检测和零速校正对普通导航算法的补偿，实现了惯性器件的室内步行导航，证明了此算法的有效性，为进一步提高精度和改进算法奠定了基础。
参考文献
[1] 贺锋涛，赵胜利，周广平，等.基于模糊逻辑的室内导航步长估计方法研究[J].电子技术应用，2016，42(11)：59-61.
[2] JIMENEZ 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].The Workshop on Positioning Navigation & Communication.IEEE，2010：135-143.
[3] FISCHER C，GELLERSEN H.Location and navigation support for emergency responders：A survey[J].IEEE Pervasive Computing，2010，9(1)：38-47.
[4] 崔潇，秦永元，周琪，等.鞋式个人导航系统算法和试验研究[J].测控技术，2013，32(3)：138-142.
[5] 张金亮，秦永元，梅春波.基于MEMS惯性技术的鞋式个人导航系统[J].中国惯性技术学报，2011，19(3)：253-256.
[6] SHIN S H，CHAN G P，CHOI S.New map-matching algorithm using virtual track for pedestrian dead reckoning[J].ETRI Journal， 2010，32(6)：891-900.
[7] RENAUDIN V，YALAK O，TOME P，et al.Indoor navigation of emergency agents[J].European Journal of Navigation，2007，5(3)：36-45.
[8] 田增山，朝磊，邢培基，等.行人导航系统中航迹推算参数估计方法的研究[J].电子技术应用，2009，35(12)：84-87.
[9] 宋敏，申闫春.室内定位航位推测算法的研究与实现[J].计算机工程，2013，39(7)：293-297.
[10] PITTET S，RENAUDIN V，MERMINOD B，et al.UWB and MEMS based indoor navigation[J].Journal of Navigation，2008，61(3)：369-384.
[11] 樊炳辉，张凯丽，王传江，等.基于四元数的前臂假肢手部自平衡的设计[J].电子技术应用，2016，42(5)：78-81.
[12] TIAN X，CHEN J，HAN Y，et al.A novel zero velocity interval detection algorithm for self-contained pedestrian navigation system with inertial sensors:[J].Sensors，2016，16(10)：1578.
[13] SKOG I，NILSSON J O，HANDEL P.Evaluation of zero-velocity detectors for foot-mounted inertial navigation systems[C].Indoor Positioning and Indoor Navigation(IPIN)，2010 International Conference on.IEEE，2010：1-6.
[14] 奔粤阳，孙枫，高伟，等.惯导系统的零速校正技术研究[J].系统仿真学报，2008，20(17)：4639-4642.
作者信息:
钱文高1，陈静杰1，马红岩2
（1.中国民航大学 电子信息与自动化学院，天津300300；2.中国民航大学 基础实验中心，天津300300）
推荐阅读：易民生活网
