9.基于轨迹相似性理论进行自动驾驶车辆IMU和车身的外参标定

0. 说明

本文部分参考这篇文章，本文相关公式不清晰可以去看这篇博客。

1. 组合导航产品介绍

自动驾驶车辆一般会装有有GNSS/INS组合导航器件，比如NOVATEL，导远这些公司的产品。它可以提供高精度的车辆定位信息。组合定位系统的相关知识可以参考组合定位系统。

我们这里IMU和车体坐标系的标定，就用到了组合导航给出的高精度位姿信息。注意：不同的组合导航产品可能输出位置的参考坐标系不同，在编程的时候也要注意坐标系统一。比如我使用的novatel产品输出的位置信息是在WGS84坐标系下，应该把它转换到ENU导航坐标系。相应坐标系转换可以参考这里。

2. 标定原理

一方面，我们通过novatel得到厘米级定位信息，可以获得车辆的运行轨迹“真值”。另一方面，我们也可以通过车速，IMU和车体之间的安装角误差以及novatel得到的IMU到导航系的位姿得到车辆运行轨迹在导航系下的表示。
显然，在各项系数都标定完毕，且忽略一些误差的情况下，两个轨迹应该是重合的。详细情况会在后面讲到。

3. 轨迹相似性原理

假设车体坐标系为m系，IMU系为b系，两者之间有较小的安装角误差。两者之间的外参可以如下表示：

另外，里程计的测量中还可能存在刻度系数误差 $\delta K_D$，其输出速度大小 $\tilde{v}$与理论速度大小$\upsilon$之间的关系为：
所以，在导航坐标系中里程计的实际速度输出应该为:

由于车辆只有前向速度，所以${\upsilon }_D^m$ = 0，${\upsilon }_D$, 0，重新整理公式得：

从上式可以得出，翻滚角不影响里程计的测量值。

3.1 航位推算

假设车辆在行驶中水平的姿态角很小。

对上式进行积分，可以得到位置方程：

可以看到，上式包含里程计的刻度因子，组合导航的姿态失准角，IMU到车身的安装角（pitch和yaw），理论上可以标定这几个参数。我们忽略组合导航的姿态失准角，便可以标定IMU到车身的安装角以及里程计的刻度因子。

3.2 轨迹相似的可视化

可见，真实轨迹和解算轨迹相差一个旋转以及缩放因子。
两点说明：

1. 缩放是有里程计刻度因子导致；
2. 相对旋转有导航姿态失准角以及IMU和车身安装角导致；

4. 选择车速源

那我们的车辆速度选择什么来源呢？一个是轮速计，但是他会有刻度因子标定的问题，会把误差引入到我们安装角估计当中来；再就是可以使用NHC（车辆约束假设），这里参考了武汉大学多源智能导航实验室的一篇论文。论文在这。

基于NHC假设，我们可以将GNSS观测的速度（GNSS处于固定解状态时）取模，当做车辆的前向速度，这样就不用估计里程计的刻度因子了。
这样的话，我们就可以用novatel给出的轨迹“真值”和DR轨迹作差，得到误差方程了。

5. 标定效果

上图是高程上反应pitch角标定的效果。

---

下面是分别给pitch和yaw角添加了5度的偏差，看能否标回来。可以看到yaw角能标定回来，pitch角标定效果较差。




最后一张放大图表明车辆频繁上下坡会显著降低pitch的标定精度。