基于车轮安装MEMS IMU的航迹推算算法研究

本文由来：前一篇文章“零速更新(ZUPT)辅助INS定位”，并通过开源方案ZUPT-aided-INS进行了算法验证。在验证过程中，意识到在进行多源传感器融合算法中，利用载体自身运动信息进行约束修正非常重要，因为这不需要额外增加传感器在现有系统上进行ZUPT/ZIHR，可以提高系统精度和鲁棒性，特别是在部分场景中多源传感器中部分传感器退化的情况下，在日常工作中也常称为“engineer trick”。
在阅读相关论文时，发现论文“基于车轮安装MEMS IMU的航迹推算算法研究”，发现文章写的很好，收获很大，很有参考意义，算是阅读记录一下吧。

文章参考来源论文：“基于车轮安装MEMS IMU的航迹推算算法研究”。

由于Wheel-IMU跟随车轮不断旋转，安装方式和imu坐标系定义不同，在算法中的处理方式也不同。基于旋转效应对捷联惯导误差的调制作用，充分挖掘Wheel-IMU的定位能力。基于陀螺间接测量得到的车轮速度，进一步探讨由车轮速度信息构建不同观测值用于滤波量测更新的算法，并基于载体自身运动信息约束，构建零速更新和零航向角速率观测值，设计一种性能最优、计算最简洁的约束算法。

一、Wheel-IMU的安装方案和安装角的标定方法

为了避免俯仰角出现奇异问题，将imu的旋转轴定义为X轴，指向车体右侧，且认定imu与载体之间的偏航角偏差固定为90°。imu与载体系坐标定义如下图：

1.旋转调制效应分析

姿态误差和速度误差受陀螺和加速度计误差影响，而周期性旋转可以抑制误差累计，以陀螺为例，假设车轮imu的X轴指向正北，且在短时间内车轮转速固定，则imu角速度误差在n系下的投影为：

假定在车轮旋转一圈的时间内误差恒定，对上式积分得到由于陀螺误差引起的姿态误差：

从上式可以看出，由于车轮旋转，imu非旋转轴的测量误差在n系下被调制成正弦波，经过一圈积分后，累计误差为零。同理，加速度计测量误差所引起的速度误差，在经过一圈的积分后，非旋转轴累计误差为零。
因此，在一圈旋转周期内造成的东向和地向的累计速度误差以及俯仰、航向误差为零，imu安装在车轮上在一定程度上抵消了imu的常值偏差。

2.车轮imu安装角标定

由于imu安装无法与车轮完全对齐，存在imu中心与车轮中心不重合的误差，以及imu轴系与车轮坐标系不平行的误差，分别称为杠臂误差和安装角误差。
由于imu随着车轮一直旋转，车轮坐标系和imu坐标系的y/z轴指向会周期性地变化，所以忽略imu与车轮坐标系之间的横滚角，只对航向角和俯仰安装角进行校准。
忽略地球自转，在载体沿直线形式或者车轮空转时，可以认为imu测量的角速度完全来自于车轮角速度的投影，则有：

仅考虑航向角和俯仰角，忽略无关项，有：

代入上试，得到：

求解，可以得到imu相对于车轮的安装角：

实际应用中，可以将轮架空，提前利用静止时段数据扣除陀螺零偏，然后是车轮空转，利用一段时间数据求平均值。对于不能架空车轮的机器，可以沿着既定轨迹走一段直线，并采集wheel-imu数据求平均结果。
另杠臂误差的校准，后续会增广到卡尔曼滤波的状态向量中进行实时估计。

二、基于滤波航迹推算算法

1.算法整体框架

a.初始化载体位置、速度和姿态（PVA）后，利用wheel-imu测量数据进行机械编排推算载体状态；
b.利用imu角速度和比力测量值探测载体状态，判断载体是否进入静止状态，若进入，则进行ZUPT和ZIHR量测更新；
c.当载体开始运动时，采用速度信息作为量测更新。速度约束观测值有三种形式：（1）利用imu角速度和车轮半径计算车轮前进速度，结合NHC在v系进行约束，即传统的ODO/INS组合导航思路；（2）将车轮前进速度投影到n系，积分得到imu在一段时间内的位移，以此作为观测值；（3）通过杠臂、车轮半径和imu水平姿态将imu速度投影到车轮上的触地点，利用“当载体不发生侧滑和颠簸等状况时，车轮上与地面接触点相对于n系速度为零”这一条件构建约束。
d.利用滤波后得到的imu误差估计值补偿imu原始数据，同时载体运动误差反馈给载体状态，得到滤波更新之后的运动信息。并将ekf输出的航向结果，结合车轮速度得到平面DR推算位置结果。
三种观测值构建的航迹推算系统框架如下：

2.系统误差状态方程（视觉里程计比例因子、imu的零偏误差和比例因子误差、杠臂误差）

考虑车辆行驶过程中，由于自重、温度等变化导致轮胎压强变化造成车轮实际半径与标称半径的偏差，将该误差建模为与车轮半径按比例变化，视为里程计比例因子误差sv，则：

由于杠臂误差直接会影响到imu的向心加速度和速度投影，与其它误差有较好的区分度，所以imu y、z轴的杠臂误差有较强的客观性。将上述误差增光到卡尔曼滤波系统状态中，可以得到基于车轮安装imu的航迹的推算算法滤波模型的状态向量如下：

将imu的零偏误差和比例因子误差建模为一阶高斯-马尔科夫过程，可得imu零偏和比例因子误差的微分方程：

将里程计比例因子和杠臂误差建模为随机游走，其微分方程为：

3.车轮速度信息约束算法

a.车轮中心速度约束

补偿imu安装角和陀螺误差后，可以更准确由陀螺x轴测量值和车轮半径计算车轮前进速度：
对于一般车辆，其在平面上正常行驶，假设不发生颠簸和侧滑，车体只有沿着前进方向的速度，侧向和垂向速度为零。这一基本假设，称为非完整性约束（NHC）。车辆满足该约束时，载体坐标系下的观测向量可以表示为：

车辆运动约束和非完整性约束均属于载体坐标系下的观测值，因此与imu的状态构建约束时需要获得b系和v系的转换关系。
在传统的车载里程计、惯导组合导航算法中，imu安装在车身，需要提前标定或在线标定imu安装角，并视为固定值。而在该方案中，imu安装在车轮上，imu相对于载体的横滚角程周期性变化，无法感受载体的水平姿态，即仅仅依靠车轮imu，无法判断车辆是上坡还是下坡。
因此，假设车辆行驶在水平面上，v系相对于n系只存在航向角误差，则有：

载体的航向角误差可认为与imu的航向角误差相等。
由imu速度经过杠臂投影得到的车轮速度为：

则，车轮中心速度约束方程误差方程可以表示为：

ps：状态向量的误差方程有固定思路，如下：
该推导方式下，误差等于预测减去观测值。当然也可以观测减去预测。

b.里程增量约束算法

思路：由陀螺角速度和车轮半径可以计算车轮在任意时刻的速度，经过积分之后可以得到位移增量。
n系下的里程速度观测：

由imu速度推算得到的车轮在n系下的速度为：

两式相减得到：

两边积分，并认为积分区间内误差恒定，则：

忽略地球自转，则：

综上，得到里程增量约束算法的观测方程：

c.车轮接触地点零约束算法

根据非完整性约束，轮式机器人正常行驶在路面上时，测向和垂向的速度为零。
假设某个瞬间车轮中心相对于地面前进的速度为v，此时由于车轮子在旋转，车轮上与地面接触点相对于车轮中心的速度也为v，方向与车辆前进的方向相反，则当载体不发生颠簸和侧滑时，该点相对于地面的速度始终为零。
不同于传统的里程计速度约束，该方案无需获得车轮速度，而是通过杠臂将车轮imu速度投影换算至车轮与地面接触点，这时该点速度v(p2n) = [0 0 0]^T。
车轮接触点零速投影示意图如下：

由imu速度计算得到的车轮触地点p的速度为：

l(p2b)表示p点在b系下的杠臂。随着车轮转动，p点在b系下的位置动态地变化，需要根据imu横滚角实时计算。假设车辆在水平面上运动，则：

p点速度观测值可以表示为：

因此，速度投影约束算法的误差方程可以表示为：

4.零速修正和静止航向锁定

在不外加传感器的情况下，依靠载体自身的运动信息引入约束，抑制INS误差发散，主要包括ZUPT/ZIHR。
以文章（零速更新(ZUPT)辅助INS定位）一样，流程1.零速检测；2.零速更新成立，进行误差修正。
本文采用固定长度滑动窗口内的加速度、速度和角速度三个量的峰峰值来和阈值对比，判断载体是否处于静止状态。实际功能工程应用中，可以针对不同的运动载体和环境选用不同阈值。

当判断零速更新成立时，imu速度观测值可以表示为：

零速更新的观测方程可以表示为：

由误差扰动分析，imu推算得到的载体姿态矩阵和航向角可以分别表示为：

静止航向锁定的观测方程可以表示为：

三、基于车轮速度信息构建的三种观测约束算法讨论

从以上推导可以看出，三种观测值更新算法本质上都是利用载体车轮速度信息对系统状态进行修正。其中：
1.车轮中心速度约束算法将imu速度经杠臂和姿态矩阵C_b2n)投影到车轮中心，联合NHC与陀螺数据推算的v系下的车轮速度构建约束。引起观测误差的系统误差状态主要包括速度误差、姿态误差、陀螺误差、杠臂误差和里程计比例因子误差。
2.里程计增量约束算法是将载体速度投影在n系下进行积分得到一段时间内的位移增量，并假设在积分区间内系统误差状态不变。引起观测误差的系统状态包括速度误差、姿态误差、陀螺误差、杠臂误差、加速度计误差和车轮比例因子误差。
3.车轮触地点投影约束并不需要利用陀螺数据计算车轮中心的速度，而是利用“当载体不发生颠簸、侧滑等现象时，车轮触地点相对于n系的速度为零”这一条件构件约束。引起观测误差的系统状态包括速度误差、姿态误差、陀螺误差、杠臂误差和车轮比例因子误差。

同时，三种算法都依赖非完整性约束和载体水平面运动假设。对于车轮中心速度约束算法和里程增量约束算法比较直观，因其观测方程建立时需要载体在v系下的速度和载体姿态。而对于车轮触地点投影约束，载体水平面运动约束提现在利用车轮旋转角和半径进行速度投影。此时若载体运动在斜面上，则速度投影还应该在imu横滚角上补偿载体俯仰角。对于里程计增量约束算法，在载体动态较大时，不满足积分区间内系统保持不变这一假设，同时速度观测噪声也在无形中进行了积分，一定程度上破坏了卡尔曼中观测噪声服从高斯分布这一条件，并且里程计增量约束对NHC的假设更强一些，需要保证在积分区间内载体垂向速度和侧向速度均为零。对于车轮中心约束，由于载体航向也来自于imu姿态，因此在速度投影时引入了额外的航向误差。而对于车轮触地点零点约束，则在速度投影引入了横滚角误差。

综上，三种观测值约束算法利用的信息本质上是相同的，只是观测值加工的手段不同，不同的观测值引入不同的微小误差，因此，最终的导航结果也不应有明显的差异。

缺陷：1.单体车轮安装imu受限于水平面运动假设，在起伏不平的路段精度会下降，同时单个系统的鲁棒性有待提高。

改进：针对缺陷1，可以通过车体安装imu，检测载体的水平姿态信息；针对缺陷2，考虑在两个非转向轮同时安装imu，进行两次观测去平均。同时车身安装imu，可以用imu之间的空间位姿关系来修正系统误差。