邹一凡, 李 佳, 王玮冰

(1.中国科学院 微电子研究所, 北京 100029; 2.中国科学院大学, 北京 100049)

0 引 言

相对于其他行人室内定位方法,基于惯性传感器的方法具有无需事先布置、实现成本低的优势,正在受到越来越多的重视[1,2]。其中,基于惯性导航系统(inertial navigation system,INS)的方法相较于基于步长—方向模型的方法,具有运算量少、实时性好、可以重建完整运动轨迹的优点[3]。同时,如何消除其定位的累积误差,是该领域的研究重点。本文在INS框架基础上,进行了新的定位系统方案设计。

1 INS原理

对于INS,其原理框图如图1所示。由9轴磁、角速度、重力(mageric,angular rate and gravity,MARG)传感器所采集到的数据,在误差补偿后用于对载体的姿态进行实时的估计,从而将加速度数据由传感器坐标系(S系)转换至地球坐标系(E系),经2次积分得到空间速度与位移。之后,利用零速更新(zero velocity update,ZUPT)算法对定位误差进行补偿,从而得到最终的定位结果[4]。

图1 INS框架

2 姿态估计算法

2.1 四元数基础运算

使用符号⊗表示四元数乘法,公式如下

(1)

(2)

(3)

式中 右上标*为四元数共轭。当Q=[q0q1q2q3]T时,Q*=[q0-q1-q2-q3]T。

2.2 扩展互补滤波

2020年,Madgwick S O H为了解决其先前提出的基于梯度下降的姿态估计算法[5]存在的问题,设计了新的算法,命名为扩展互补滤波(extended complementary filtering,ECF)[6]。本文即基于这一成果,进行了进一步的改进与应用。现将算法运算过程介绍如下：

(4)

(5)

(6)

式中Sak与Smk分别为加速度计与磁力计的测量值。括号内的运算分别为将指向下的向量[0 0 0 1]T与指向东的向量[0 0 1 0]T投影至传感器坐标系,且略去结果的第一个元素,以参与三维向量运算。

由此,即可实现对角速度的修正,通过调整参数以实现最佳性能。在实际应用场景中,加速度数据与磁力计数据的特性有所不同,因此将参数K分为2个参数Ka和Km,即将式(4)进一步重写为

(7)

当检测到较大的运动加速度或磁干扰时,即降低对应的参数值,从而改善姿态估计结果。

2.3 快速收敛算法

在算法开始工作后,需要一定的时间进行迭代以收敛至初始姿态,因此行人需静止一段时间才能开始运动,这与实际应用场景是不符的。同时,当进入ZUPT后,若姿态收敛较慢,可能将姿态误差延续至下一步,影响定位结果。为解决上述问题,本文提出一种姿态快速收敛(fast convergence,FC)算法。

首先计算

(8)

可见,向量X为加速度的测量向量A与重力的推测向量G经由叉乘得来,其方得到向量向与两向量垂直,模长为两向量夹角θ的正弦值。若能通过向量X实现这一夹角的旋转,即可直接收敛于真实姿态。首先将其长度转化为θ,得到向量

(9)

(10)

对于向量Sak×Smk的处理同理,这里不再赘述。该算法能够在2个采样周期内实现初始对准,能够在进入ZUPT后迅速修正姿态误差,且运算过程与ECF本身相契合。使用FC算法前后,初始对准时姿态估计所得欧拉角变化曲线对比如图2所示。其中,图2(a)中使用10倍于常值的K值；图2(b)中使用FC算法。值得一提的是,单次FC算法仅占用2个采样周期,其余时刻仍依照式(7)迭代。

图2 初始对准过程对比

2.4 算法整体流程

上述算法整体的流程如图3所示。在获得载体姿态后,即可对加速度数据进行积分,并通过ZUPT进行误差修正。这里不再进行赘述。

图3 姿态估计算法框图

3 实验验证与分析

3.1 导航模块设计

针对实验需求,面向实际应用场景,设计了一种惯性导航专用模块。通过对MCU进行编程,以125 Hz的采样频率,通过SPI接口从ICM—20948传感器中读取九轴数据,在运算更新姿态、位置信息后,通过BLE通信将位置解算结果传输至上位机。

实验过程中,模块被粘贴至实验对象鞋尖上方,使用笔记本电脑实时接收并展示运动轨迹。模块实物如图4所示。

图4 导航模块实物

3.2 实验内容

为了验证本文所提出系统方案的有效性,设计路径进行了轨迹还原实验,并与其他基于互补滤波(complementary filtering,CF)的方案进行结果对比,分别为基于梯度下降的算法(GDA)以及经典CF[7]。各姿态估计算法均通过实验取得最佳参数设置。

实验对象由6楼出发,下楼梯至4楼,经过一段走廊后,绕正方形轨迹2周,再沿原路返回。最终得到的平面轨迹如图5(a)所示。可见使用GDA或CF还原出的轨迹有相当大的误差,其原因在于无法利用磁力计数据进行有效的姿态校正。去掉这2条轨迹后,得到Y-Z平面的对比如图5(b)。

图5 各算法还原轨迹对比

可见,使用ECF算法能够较为精确地还原出组合路径下的运动轨迹。添加FC算法后,能够优化其在Z轴上的偏移。各实验条件下的返回原位置误差(return position error,RPE)如表1所示。

表1 实验精度分析

4 结 论

本文以MARG传感器为基础,针对室内定位的低成本应用需求,基于足部捷联惯性导航原理,设计了一种导航定位系统方案。针对姿态估计中的问题,设计了FC算法,应用于初始收敛以及零速时姿态估计的快速修正。通过实验证明,使用本文所设计的ECF+FC方案定位结果优于其他算法。系统平面定位误差小于0.5 %,空间定位误差小于1 %,能够适应较为复杂的应用场景,为将SINS应用于室内定位提供了有效的解决方案。

后续研究中,将考虑以下方面：1)与气压计融合以提高空间定位精度;2)利用先验信息,如地板为平面、楼层高度为固定值等,在上位机对最终定位结果进行修正。