行人导航系统航向角约束算法研究
2018-11-21宋春雷陈家斌凌艺菲
余 佩，宋春雷，陈家斌，凌艺菲
(北京理工大学自动化学院，北京 100081)
0 引言
行人导航系统，又称为单兵导航系统，是导航定位领域里的一个重要分支，近年来得到越来越多的研究，被广泛应用于救援抢险、军事作战等各个领域[1]。目前，全球定位系统(Global Positioning System,GPS)室外导航技术的研究已经非常成熟且应用十分广泛。然而由于GPS导航技术依赖于接收的卫星信号，在室内或被建筑物等遮挡的复杂环境中，卫星信号易受到干扰或遮蔽，定位导航信号将丢失或出现错误，不能很好地发挥其导航定位功能[2]。基于低成本惯性测量模块(Interial Measurement Unit, IMU)的自包含传感器行人导航系统不需要依靠外部设备，可以独立自主地实现导航定位功能，同时具有体积小、质量小、便于携带等特点，因此有着很好地发展前景和研究价值[3]。
基于自包含传感器的行人导航系统，利用可穿戴式惯性传感器测量行人运动过程中的加速度、角速率信息，通过导航算法对所测量的数据进行处理和解算，实现了对行人的定位和导航。然而系统内部存在惯性器件，其误差会随时间累积，是造成导航系统误差的主要原因。为了抑制导航误差，一般加入零速修正技术作为辅助。加入零速修正的行人导航系统定位精度虽然会得到很大的提高，然而由于航向角不可观测，无法估计出航向误差，因此零速修正对于航向角误差修正起不到很好的作用，航向角依旧会逐渐发散[4]。此种航向角漂移误差在行人行走路线存在多次转弯时会更加明显。
针对行人导航系统存在的航向角漂移误差问题，本文在捷联惯导解算和零速修正的基础上，加入磁力计作为辅助手段，补偿航向角误差；同时采用一种地图线路匹配的算法对航向角误差进行修正。在行人行走过程中，弯道部分采用磁力计作为参考航向角，对捷联解算出的航向角进行误差修正，直道部分采用地图线路匹配算法辅助修正航向角误差。最终得到的导航轨迹图，航向角漂移得到抑制，位置误差大幅减少，导航精度明显提高。
1 基础原理
1.1 坐标系定义
1.1.1 载体坐标系
载体坐标系(b系)与载体固连，原点O位于载体的重心处，三轴分别表示为xb、yb和zb。文中，行人导航系统的载体坐标系定义为前-右-下，即xb轴沿载体的纵轴指向前进的方向，yb轴沿载体横轴指向自身右侧，zb轴沿竖直方向指向底部并垂直于xb轴和yb轴构成的平面，构成右手正交坐标系。实验设备选用的足绑式IMU的输出为载体坐标系下的惯性数据。
1.1.2 导航坐标系
导航坐标系(n系)也叫作当地地理坐标系，是根据实际导航需要而选取的基准坐标系。本文中将北-东-地(N-E-D)正交坐标系选取为导航坐标系(xn-yn-zn)。坐标系原点位于惯性平台原点在大地水平面上的投影，北轴指向真北方向，地轴沿着当地铅锤方向指向地球内部，东轴水平向东且与北地两轴构成右手正交坐标系。文中，导航解算结果选取导航坐标系为参考坐标系。
1.2 零速修正
1.2.1 零速区间检测
根据行人运动学模型，行人行走的一个步态周期主要分为触地期、站立期、离地期和摆动期4个阶段[5]。当行人处于站立期时，速度本该为零，此阶段也叫作零速区间，误差的存在使得行人零速区间里的实际速度并不为零，因此，可以在零速区间将速度和角速度置零进行误差补偿[6]。
零速修正的前提和关键是零速区间的检测。由于行人行走的步态呈现周期性变化，因此，可以按规律对陀螺仪输出的角速度和加速度计输出的加速度设置阈值来识别零速区间。
理想情况下，零速区间内行人角速度和水平方向加速度为零，而z轴方向上加速度应为g。实际上，受到干扰作用后，虽然输出与理论值有一定的差别，但是输出值仍然会保持在一定的阈值范围内波动[7]。选取零速区间内特征最明显的加速度模值，z轴加速度、角速度模值和y轴角速度，对它们设置阈值。
设在k时刻加速度计在三轴上的输出分别为axk、ayk和azk，陀螺仪三轴上的输出分别为ωxk、ωyk和ωzk，计算角速度和加速度模值为：
设置阈值判断条件：
只有同时满足C1、C2、C3、C4为1时，该时刻才可被判断为零速区间[8]。
其中，阈值的设定与陀螺仪和加速度计的零偏噪声的方差特性有关，因此，可以在初始静止时测得的数据平均值上加入一定的扰动来获得。这里阈值的设定需要足够精确，过大会导致误判，而过小会导致漏检。
针对本文的行人导航系统，经过干扰测试后设置的阈值为thamin=8.5，thamax=11，thazmin=-11，thazmax=-8.5，thω=0.87和thωy=0.5。设定阈值处理后检测出的零速区间如图1所示，绿线表示最终得到的零速区间。
1.2.2 EKF零速修正技术
在检测出的零速区间中，使用扩展卡尔曼滤波对导航的速度和角速度误差进行估计和补偿。系统状态变量为
Q=diag(0.012,0.012,0.012,0,0,0,0.012,0.012,0.012,0.032,0.032,0.032)
R=diag(0.012,0.012,0.012,0.032,0.032,0.032)
零速修正后，行人的速度和角速度误差得到补偿，捷联解算出的位置误差也能得到修正。行人足绑IMU沿直线行走采集数据，经过零速修正后得到的速度如图2所示。
经过修正后，速度误差得到了很好的抑制，航向角误差也得到了一定的修正，但是航向误差仍然较大。得到的航向角如图3所示。为了解决航向误差修正的问题，提出了磁力计辅助修正算法和地图线路匹配算法。
2 航向角误差修正集成算法
2.1 磁力计辅助航向修正
可以计算出航向角：
其中，ψ0为磁偏角，可以通过查表得到。
在零速区间内，以磁力计测得的航向角作为参考理论航向角，捷联解算航向角与磁力计测得航向角的差值作为扩展卡尔曼滤波的一个观测量，记为δψ，得到系统的观测方程为：
对应的观测矩阵为
R=diag(0.032,0.012,0.012,0.012,0.032,0.032,0.032)
使用磁力计辅助补偿航向角误差，可以使得系统的航向角测量精度进一步提高，然而当磁干扰较大时，磁力计受影响较大，会使得航向角误差得不到很好的补偿。
2.2 地图线路匹配约束航向
考虑到行人行走经常受到各种环境的约束，如行人行走的道路多以直线为主，许多建筑物都呈矩形等。在此情况下，当行人直线行走时,可以通过建筑方向、道路方向等基础信息，为行人导航提供参考航向角，约束行人航向角的漂移误差。
假设行人沿一个闭合矩形行走，可以根据简易地图获取道路的方向来作为参考航向，矩形的四方向正东、正南、正西和正北分别代表着航向角90°、±180°、-90°和0°，分别记为ψB(i)，其中，i=1,2,3,4。
如图4所示，行人每行走一步可以通过公式计算出一个航向ψs。
地图线路匹配算法的基本步骤为：
1)判断当前时刻是否为零速区间结束后的第一个采样点，若是，表示此采样点处行人开始下一步行走，将ψs作为实际航向角，否则将捷联解算出的航向角作为实际航向角。
2)比较实际航向角与ψB(i)，如果|ψ-ψB(i)|
3)以实际航向与选择的ψB做差作为扩展卡尔曼滤波器的一个观测量，得到航向角的误差估计量，对航向角误差进行补偿。
地图线路匹配的初始航向角可以由磁力计测出的航向角确定。由于其在直线行走时对航向角约束明显，因此，仅在行人直线行走时采用地图线路匹配的算法辅助修正航向角。
3 实验及结果
3.1 实验设备
设计实验验证算法的可行性，实验设备选取荷兰Xsens公司研发的MTi-G-710 GNSS/INS，此设备重21g，是一个集全球导航卫星系统(Global Navigation Satellite System, GNSS)信号接收器和微型惯性测量单元(Miniature Inertial Measurement Unit, MIMU)于一体的微惯性测量系统,内部还包含了磁力计、气压计等辅助传感器,具有体积小、质量小、便于拆卸等特点。其主要性能指标如表1所示。
表1 MTi-G-710性能指标
IMU安装于鞋子后跟，设备通过USB与电脑相连，采样频率设置为100Hz，传感器输出信息经过USB接口传输到电脑，通过MT-Manager进行采集存储。
3.2 实验设计及结果
实验地点选取在北京理工大学东操场，行人佩戴装配沿着操场逆时针行走一圈，全长总计400m。在相同的环境下，分别使用捷联解算加零速修正算法、磁力计辅助捷联解算零速修正算法和地图线路匹配/磁力计辅助的捷联解算零速修正算法进行导航实验。得到的结果如图5所示。
以起始点作为原点，行人逆时针行走一圈之后回到起始点位置。从图5中可以看出，仅对零速区间的速度和角速度误差补偿后得到的导航轨迹图误差较大，轨迹终点在(1.713，56.59)，位置误差为56.93m，虽然轨迹图整体形状规则，但是航向角漂移明显，严重偏离真实轨迹。加入磁力计辅助后，轨迹终点在(-3.03，-2.956)，位置误差为4.23m，航向角误差得到补偿，航向漂移明显减弱，导航精度显著提高。最后加入地图线路匹配算法，轨迹终点在(-0.85，-0.86)，位置误差为1.2m，导航精度得到进一步提高。
4 结论
由于航向误差不可观测，捷联惯导解算加零速修正技术对航向角漂移误差起不到很好的抑制作用，航向角误差依旧呈发散状态。在零速区间内对速度和角速度误差进行补偿的基础上，加入磁力计测量值解算出的航向角作为辅助，辅助修正航向角漂移误差。考虑到磁力计容易受到外界磁干扰的影响，引入地图线路匹配算法。根据道路方向具有规律性的特点，由简易地图分析确定出的行人行走线路方向作为参考航向修正航向角漂移误差。最终的导航系统中，弯道部分采用磁力计辅助航向角修正，直道部分采用地图线路匹配算法辅助航向角修正。并将算法应用到实际实验中，实验结果表明，磁力计辅助航向修正使得航向角漂移得到明显的抑制，而磁力计加地图线路匹配算法辅助航向修正使得航向误差进一步减小，导航精度进一步提高，最终的导航位置误差仅为1.2m，验证了算法的可行性，取得了很好的导航效果。
