一种基于航向角准确修正的惯性导航室内定位算法

邓 平，朱飞翔，赵荣鑫

（西南交通大学信息编码与传输重点实验室，成都 610097）

当今城市建筑物内部结构错综复杂，火灾现场危险性极大，对消防员的人身安全构成了极大的威胁。救灾时如果不能准确确定消防员的位置所在，在消防员自身遇险时就难以实施救援。GPS/北斗等全球卫星定位导航系统虽然在室外定位精度比较高，但是在室内环境一般缺乏有效信号覆盖，很难进行准确、有效的定位[1]。基于WiFi、UWB、ZigBee、RFID 等设备的室内定位技术一般都需要提前在建筑物内部署信号源等基础设施，难以在没有基础设施的一般建筑物内实施室内实时精准定位[2-4]。惯性导航定位技术是一种可以全天候、全时段、实时工作，且工作时不依赖事先部署基础设施，不易受到外界干扰的一种自主式定位导航技术。近年来随着微电机（MEMS）技术的发展，基于MEMS 的惯性测量单元（IMU）的体积越来越小，测量精度越来越高，性能愈来愈强，使得基于IMU 的室内定位技术的研究及应用受到了人们的广泛关注[5,6]。

基于IMU 进行三维室内定位，需要实时采集行人行走过程中的3 轴加速度、3 轴角速度、高度数据，进而计算出步频、步长以及航向角，用于行人航位推算（PDR）定位。因此步频检测、步长估计和航向估计的精确度决定了PDR 定位的性能优劣。目前步频检测主要采用的方法有峰值检测法[7]，零点交叉法[8]等，但研究中发现这类方法在上下楼梯时会导致合加速度波形即使去掉重力分量，仍会关于X 轴不对称，造成使用峰值阈值的步频检测出现错漏判，产生一定的步频检测误差。为此，本文在峰值检测法的基础上提出一种峰峰值探测法，即在三窗式滑动窗口中先进行无差别的波峰波谷识别，再进行针对性的横向时间阈值和纵向峰峰值阈值判决来完成步频检测。

在航向角的估计中由于陀螺仪自身不可避免会出现数据漂移现象，航向角势必会随着时间累计而产生不可逆的误差[9,10]。为了降低仅靠陀螺仪数据进行航向判断的误差，文献[11]采用陀螺仪、磁力计等数据，利用互补滤波结合PI 调节控制来校正陀螺仪零偏，但磁力计受室内环境中的钢结构、金属等干扰严重，其测量值会出现严重的偏差。文献[12]采用粒子滤波和地图匹配技术得到最佳航向估计，但由于室内环境复杂多变，地图匹配工作量大不易实现。文献[13]提出了一种启发式偏移消除算法(Heuristic Drift Elimination，HDE)，利用建筑物90 °主方向作为约束条件对角速度进行反馈修正，取得了较好效果。但是随着现代建筑设计风格的多样化，室内建筑主方向不仅仅是横平竖直，诸如环形、弧形等多种不规则转弯方向使得室内行人行走路径转向逐渐增多，这类场景下HDE 算法的航向角估计精度已不能满足室内精确定位导航的需求。为此，本文在HDE 算法的基础上，细化设定了新的建筑物主方向角度，设计了不同方向行走路径判别规则，对沿着建筑主方向直行、非主方向直行以及转弯时，分别用不同的反馈系数直接去准确修正航向角，有效提高了航向角的估计精度。

1 步频探测与步长估计

基于捷联式惯性导航器件的室内航迹推算，需要在一定的坐标系下开展研究。其中定义运载体坐标系是惯性导航器件自身定义的坐标系（b 系），向右为X轴，向上为Y 轴，垂直模块向外为Z 轴；选择大地地理坐标系作为导航坐标系（n 系），向东为X 轴，向北为Y 轴，垂直向天空为Z 轴。本文算法利用IMU 采集的加速度、角速度、高度、四元素等数据信息进行PDR 推算实时确定消防员的位置。

当消防员处于行走或者奔跑时会产生3 个方向的加速度值。由于本文设计的单人惯导定位装置穿戴在腰间，消防员移动时由于其运动方向的不确定性，导致定位装置不可能完全水平或者竖直，故没有一个方向轴能够保持不动，因此我们采用合加速度，即三轴加速度之和来检测加速度的周期性变化：

式中，α t为t时刻的三轴合加速度幅值，分别为t时刻加速度计的三轴加速度值。步频探测时合加速度的峰值检测至关重要，若采用单个固定时间的滑动窗口提取时域峰值，对不同行进速度的适应性较差，而且对单个滑动窗口的数据进行滤波时，一方面加速度幅值下降会给步长估计带来误差，另一方面波形失真会导致峰值提取丢失。采用峰值探测法[7]时，通常采用固定门限阈值来判决波峰波谷，则可能会因为上下楼梯等复杂行走姿态，导致合加速度波形不是关于X 轴对称而出现错漏判现象。

针对上述问题，本文将采集的合加速度数据输入一个双端队列，并将队列分解成三个窗口，选取中间窗口进行峰值提取，而前后两个窗口仅参与数据滤波、峰值保存过程，目的是防止滑动窗口中遭截断的左右边界处波峰波谷数据因滤波而产生失真，导致当前峰值低于所设的峰值门限阈值进而造成峰值漏检。

图1 单窗和三窗峰值检测Fig.1 Single window and three window peak detection

如图1 所示，分别是单个固定时间的滑动窗口和三窗式滑动窗口在绿色虚线框中对同一个采样周期内的数据进行时域峰值提取，从图1 中红色虚线框中可以看出，单个固定时间的滑动窗口在边界处即采样点20 附近，因滤波造成加速度波谷幅值减小进而造成波谷值提取丢失，而对应的三窗式滑动窗口由于前后两窗口参与了滤波，在采样点210 附近未发生波谷提取丢失现象。

对中间窗口进行时域峰值提取，采用的是本文提出的一种峰峰值探测法，即首先经过低通滤波滤除抖动，然后进行波峰波谷探测无差别粗检，再进行纵横经验阈值针对性细检。无差别粗检是指进行峰值探测时无需设置经验阈值进行判别，探明为波峰或者波谷即存入相应的数据队列中暂存；针对性细检是指利用横向的时间经验阈值和纵向的幅值经验阈值进行判决，其中纵向的阈值我们设置为波峰、波谷队列中对应的波峰值和波谷值差值的绝对值。测试结果表明（采样频率200 Hz），正常行走时，横向时间上，一步大概在50-60 个采样点之间，典型值为55；纵向幅值上，一步的峰峰值应大于2.3。如图2 所示，是使用普通峰值检测法和本文峰峰值探测法，分别对楼梯行走过程中的合加速度波形进行有效峰值探测，其中向上箭头为有效波峰值，向下箭头为有效波谷值。从图2 红色虚线矩形框中对比可看出当加速度波形不是关于X 轴对称时，峰值检测法中会产生明显错漏判，而峰峰值探测法则不会产生。

图2 峰值检测与峰峰值探测Fig.2 Peak and peak-to-peak detection

本文采用Harvey Weinderg 等人提出非线性步长模型[14]来实时估计每一步步长，经验公式如下：

式中，k为跟身高、步频等参数有关的常数，可通过训练来获得；Amax、Amin分别是波峰、波谷队列中存储的每一步的加速度的最大值和最小值。

综上，本文设计的步频探测与步长估计算法描述如下：

1、给定一个固定长度为N的队列Q；

2、先填充N/3 冗余数据进队首，为后续滤波做准备；

3、队列N个数据填满后做低通滤波、去重力等数据处理；

4、进行峰峰值探测，获取N/3－2N/3 区间探测的步频和步长等数据；

5、队首N/3 个数据出队列，等待队尾进N/3 个数据后，重复第4 步操作。

2 航向角准确估计

本文采用的JY901B 惯导模块能够直接输出四元数实时信息并更新，故可先对运载体坐标下的三轴矢量角速度用四元数表示的姿态变换矩阵进行坐标系变换，变换成导航坐标系下对应的角速度，再进行积分即可得到原始的航向角ψ。运载体坐标系下的三轴矢量角速度可以表示为导航坐标系下的三轴矢量角速度表示为载体坐标系到导航坐标系下的姿态旋转矩阵为：

其中，q0、q1、q2、q3表示四元数。然后可按式(4)把转换为：

此时只需要对分量进行积分即可以得到角度增量，加上前一个时刻的航向角ψt1，可得到当前时刻的航向角ψt2：

式中t1～t2的时间间隔为一个采样周期。为单轴陀螺在t时刻的角速度数值。

2.1 HDE 航向角估计算法

启发式偏移消除（Heuristic Drift Elimination，HDE）算法[13]框图如图3 所示。

图3 HDE 算法框图Fig.3 Framework of HDE algorithm

图中，ψ为航向角、Ii为反馈变量、ic为反馈系数、sgn(Ei)为符号函数，定义为：

MOD(ψi-1, Δ )是用来计算前一时刻航向角相对于当前主方向的偏移角，定义为：

其中，INT(k) 为取整函数，结果为不大于k值的最大整数。所以MOD函数的结果在区间[0,m]内。

2.2 改进的航向角准确修正算法

HDE 算法将航向角的主方向设置为4 个或8 个，角度间隔Δ=90 °或45 °，此时HDE 算法会将当前航向角朝着假设的建筑主方向拉拢，即通过减小当前航向与主方向之间的差值进行修正。但是在室内救灾时，消防员除沿着建筑通常的主方向行走以外，还可能存在非建筑主方向的直行以及连续大、小角度转弯的情况[15]，这些情况可能导致HDE 算法对航向角的过度修正甚至修正失效[16]。为此本文首先细化设定16 个主方向来描述现代建筑内楼道转弯可能出现的各种情况，主方向间的角度间隔Δ=22.5 °；然后通过区分直线行走和转弯的不同行走状态，以及在直线行走的前提下，区分沿着建筑主方向行走和非建筑主方向行走两种不同状态[17]。区分了行走状态后，对于沿着非建筑主方向和转弯的行走路径，减小HDE 算法的反馈系数ic，以减缓航向角方向修正回当前的伪主方向，从而降低系统对用来修正航向角的反馈变量Ii的敏感性，达到航向角准确修正目的。改进的航向角准确修正算法（IHDE）原理如图4 所示。

图4 改进的航向角准确修正算法Fig.4 Improved algorithm for accurate correction of heading

1）主方向的设定

如图5 所示，为本文设定的16 个主方向，主方向的角度间隔Δ=22.5 °，由于未使用磁力计，设置初始航向角ψ0=0 °。将主导方向细分为16 个方向是为了应对长时间沿着非主方向或者持续小角度转弯行走的情况，以减缓HDE 算法对角度的过度修正，增强算法适应各种不同室内道路环境的能力。

图5 16 个主方向Fig.5 16 main directions

2）直行的判决

两年来，酒店共计接待顾客15万余人次。同时，酒店的高品质服务工作得到当地政府的充分认可，两年来共接待国家、省、市领导30余次。各种国际化会议和民间友好活动选择在这里举办，越来越多的外国宾客也纷至沓来。通过优化服务工作，提高管理水平和服务质量，让酒店有了更高、更快的发展。凭借和谐团结的队伍，完美高效的服务，酒店将延续这份业绩，在今后实现新突破。

由于IMU 模块采用腰部佩戴的方式，航向角曲线有着明显的特征。图6 是算法测试时行走的两段转弯和三段直行的航向角曲线图，其中红色点为直行每一步的航向角，绿色点为转弯每一步的航向角。

图6 直行和转弯时的航向角Fig.6 Heading for turning and going straight

对行走中的航向角进行时域特征提取并分类。假定连续3 步的航向角数值分别为ψi-2、ψi1-、ψi，如上图6 中所示，可分为4 类：第1 类为T1 直行类，可采用3 步航向数值分别进行差分相乘后，得到判决条件C1= (ψi-ψi-1) × (ψi-1-ψi-2)，当C1< 0 时，则判决当前在直行；第2 类为T2 直行变转弯类，航向角ψ i仍满足条件C1< 0，但是此时已经处于转弯状态，显然发生了误判。此时可对相邻的航向角数值进行差分求绝对值后，得到判决条件C2= |ψi-ψi-1|，当同时满足C1< 0 和C2<θ时，则可以防止误判发生；第3 类为T3 转弯类，可用连续的3 步航向数值两两差分取绝对值后 ， 得判决条件C3= (|ψ i-1-ψi-2|＜β and|ψ i-ψi-1|＜β)，当C3为true 成立时，可判决当前在走直线，反之则在转弯；第4 类为T4 转弯变直行类，当航向角ψ i满足C1< 0 时，即可判断当前在直行。

综上，是否直行可按式(8)进行判断。

当S1为1 的时候判定为直线行走，可以进入下一项主方向判决；当S1为0 时为转弯，需修正反馈系数ic。θ、β为相邻两航向角度差值的经验门限阈值，一般取15 °、10 °。

3）主方向的判决

对于直行前提下判断是否沿着建筑主方向行走，先选择4 个主方向，其角度间隔Δ=90 °。原因是一方面22.5 °的倍数中虽包含了4 个主方向，但还有很多非主方向；另一方面正常行走过程中航向值波动在正负10 °左右，Δ=22.5 °的主方向判决条件比较苛刻，难以实现。

故先对连续的3 步航向角数值求出其均值后除以360 °取余数得到的一个角度值为μ，再运用式(7)令n=μ、m= 90°把μ映射到第一象限中去，最后选择与象限轴夹角最小的角度σ=min(μ, 90° -μ)，如图7所示。主方向判决公式如式9 所示。

图7 主方向的偏移角度Fig.7 Offset angle of the dominant direction

3 PDR 航迹推算算法

将上述改进的航向角修正算法解算出来的航向角ψ i，与探测的步长d以及高度h相结合，便可以在已知上一时刻的坐标位置基础上，按式(10)PDR 推算出当前的位置坐标：

式中，Ek、Nk、Hk分别为相对位置坐标系中的X、Y、Z 轴距离分量，其中（E0、N0、H0）是三维坐标系中的初始点。

推算定位时本文首先对采集的三轴合加速度进行去重力、滤波，角速度进行坐标系转换并积分，高度数据进行卡尔曼滤波，然后对三轴合加速度进行峰峰值探测获取步频、步长、高度等信息，对角速度坐标转换、积分得到的角度进行航向角修正，最后由式(10)计算出消防员的当前相对位置坐标。算法架构如图8所示。

图8 算法架构Fig.8 Algorithm architecture

4 实验结果及分析

本文采用 JY901B惯导模块以及STM32F103C8T6 单片机设计了定位数据采集装置，装置系于腰间中间部位，如图9 所示。定位装置采集的数据通过串口传输到PC，由Python 软件进行PDR等后续处理。传感器的数据采集速率为200 Hz，波特率为460800 Hz，工作电压为3.3 V。实验场地为西南交通大学9 号教学楼。

图9 数据采集Fig.9 Data acquisition

1）峰峰值探测实验

实验目标在教学楼A 区的一楼到三楼的楼梯中行走，先上楼再下楼，回到起点，目的是验证本文提出的峰峰值探测法的有效性。如图10 所示，是分别采用峰值检测法和峰峰值探测法后推算出的行走轨迹对比。从图中看，使用峰值检测法时，由于多处发生错漏判，导致轨迹偏离真实轨迹较多，而使用峰峰值探测法时，轨迹明显优于前者且接近真实轨迹。在定位误差上，使用峰值检测法的首尾均方根（RMSE）定位误差为2.37 m，平均水平定位误差为2.33 m，而峰峰值探测法的对应误差分别为0.69 m 和0.68 m。实验结果表明当合加速度波形不是关于X 轴对称时，峰峰值探测法有着更好的探测精度。

图10 上下楼梯轨迹Fig.10 The trajectory of stairway up and down

2）弧形三角行走实验

弧形三角实验是沿着图9 中标注的一块草坪小道C 区行走，全程为133 m，路径中的两条直角边在主方向上，弧形斜边在非主方向上，目的是验证沿着非主方向长时间行走时本文提出的航向角准确修正算法的性能。本文对比了主方向间隔分别为90 °、22.5 °，采用HDE 算法航向角修正及本文航向角修正算法（IHDE）的性能，图11 分别是三种算法的航向角曲线和PDR 推算的行走轨迹。从图中可见，在弧形斜边行走时，本文算法的航向角和轨迹更加接近真实值。

图11 航向角曲线和行走轨迹Fig.11 Curve of heading and track

表1 所示为三种算法到终点时的航向角以及定位误差多次实验的统计结果，进一步验证了本文算法的有效性。本实验得到的定位精度接近文献[18]三角形实验的结果，但是本文行走轨迹为弧形三角且行走的距离约为前者的2.6 倍，实验条件更加苛刻，表明本文算法对不同的道路环境的鲁棒性。

表1 弧形三角实验精度分析Tab.1 Precision analysis of arc trigonometric experiment

3）绕圈实验

沿着9 号教学楼B 区上的起点开始行走10 圈回到起点，全程共行走约2340 m，一圈路线中有5 段是处于主方向，1 段非主方向。图12 分别是基于HDE算法及本文IHDE 算法得到的行走轨迹，结果表明本文算法能在较长时间、较大范围内实行对室内消防员的行走轨迹实现准确定位跟踪。实验测得首尾误差为2.33 m，水平定位误差为0.1%D。

图12 HDE 和IHDE 算法行走的轨迹Fig.12 The trajectory of the HDE and IHDE algorithms

相比于文献[19]中的非矩形实验，本文算法有着更高的定位精度和更长的续航时间。

4）多楼层行走实验

实验地点为西南交通大学9 号教学楼，范围为负一楼到四楼，从楼外进入，先在地下室绕墙柱环绕行走一圈，接着爬楼梯到地上每一层行走一圈，全程行走了大约1123 m。本实验目的是模拟消防员救灾的复杂室内场景，且在地下室绕柱行走以及爬4 层楼梯及途中转弯都沿着非主方向行走，可检验本文算法对复杂场景的适应性。图13 所示是测试得到的两种算法的行走轨迹，图14 是本文改进算法IHDE 的3d 行走轨迹。

图13 HDE 和IHDE 算法多楼层行走轨迹Fig.13 The multi-floor trajectory of the HDE and IHDE algorithm

实验结果表明，本文算法对于复杂实验环境具有更好的适应性，定位得到的行走轨迹更接近真实轨迹，对非主方向上的航向角漂移有着很好的抑制作用。在定位误差方面，本实验的首尾误差仅0.77 m，水平定位误差仅为0.069%D；而HDE 算法的首尾误差达到了7.51 m，水平定位误差为0.67%D，本文算法误差降低了一个数量级。

图14 多楼层行走3d 轨迹Fig.14 Multi-floor 3d tracks

4 结 论

针对消防员室内抢险救灾时对消防员自身的定位问题，实现在多楼层、上下楼梯、转弯时有多种转向角度等复杂行走场景下对消防员的准确定位跟踪，通过提出一种峰峰值步频探测法，提高了上下楼梯步频探测的精度；对航向角修正定义了更小的主方向间隔及更准确的航向角修正方法。实验测试结果表明，本文算法对于各种复杂室内环境具有良好的适应性，能够准确定位跟踪的有效续航时间长、定位精度高，能够较好地达到对消防员室内准确定位的目的。