自动驾驶TPM技术杂谈 ———— 惯性导航定位技术

简介

 惯性是所有质量体本身的基本属性。惯性导航系统不与外界发生任何光电联系，仅靠系统本身就能对汽车进行连续的三维定位和三维定向。惯性导航系统是一种不依赖于外部信息，也不向外辐射能量的自主式导航系统，其主要由惯性测量单元、信号预处理和机械力学编排3个模块组成。

 一个惯性测量单元包括3个相互正交的单轴加速度计和3个相互正交的单轴陀螺仪，结构单元组成如下。信号预处理部分对惯性测量单元输出信号进行信号调理、误差补偿并检查输出量范围等，以确保惯性测量单元正常工作。

 惯性导航系统根据机械力学编排形式不同可分为平台式惯性导航系统（Gimbaled Inertial Navigation System，GINS）和捷联式惯性导航系统（Strap-down Inertial Navigation System，SINS）。平台式惯性导航系统是将陀螺仪和加速度计等惯性测量单元通过支架平台与载体固连的惯性导航系统。惯性测量单元固定在平台台体上，系统的敏感轴能直接模拟导航坐标系，这就保证了敏感轴的准确指向且隔离了载体的角运动。由于需要平台台体，这就决定了系统结构复杂、体积大、制造成本高等不足。捷联式惯性导航系统则把惯性测量单元直接固连在载体上，用计算机来完成导航平台功能的惯性导航系统，载体转动时系统的敏感轴也随之转动，通过计算载体的姿态角就能确定出惯性测量单元敏感轴的只想，然后将惯性测量单元测量得到的载体运动信息变换到导航坐标系上进行轨迹递推。从成本的角度出发，捷联式惯性导航系统广泛应用于自动驾驶方案。

工作原理

 惯性导航系统是一种以陀螺仪和加速度计为感知元件的导航参数解算系统，应用轨迹递推算法提供位置、速度、姿态等信息。汽车行驶数据的采集由以陀螺仪和加速度计组成的惯性测量单元完成。这里需要说明的是加速度计测量的是载体相对惯性空间的绝对加速度和重力加速度之和，称作“比力”。从加速度计的工作原理可知，加速度计可以输出沿敏感轴方向的比力，其中含有载体的绝对加速度。同样地，陀螺仪可以输出车体相对于惯性坐标系的角加速度信号。以上两个惯性传感器组的敏感轴是相互平行的，共享惯性传感器组的原点和敏感轴。惯性导航系统工作原理是基于牛顿第二定律，其说明了加速度的大小与作用力成正比，方向与作用力方向相同，表达式如下：
$$F=ma$$
 惯性导航系统利用载体先前的位置、惯性测量单元的加速度和角速度来确定其当前位置，其中，速度v 和偏移量s 都可以通过对加速度a 的积分得到。加速度a 经过积分得到速度v ，经过二重积分得到偏移量s 。相反，速度和加速度也可以通过对位移的微分而估算得到。
$$\{\begin{array}{c}v=\int adt\\ s=\int vdt=\iint adtdt\end{array}$$
$$v=\frac{ds}{dt},a=\frac{dv}{dt}=\frac{d^{2}s}{d{t}^2}$$
 类似的，汽车的俯仰、偏航、翻滚等姿态信息都可以通过对角加速度的积分得到。利用姿态信息可以把导航参数从惯性坐标系变换到导航坐标系中。综上，惯性导航系统是一个惯性测量单元和积分器组成的积分系统。该系统通过陀螺仪测量载体旋转信息求解到载体的姿态信息，再将加速度计测量得到的载体比力信息转换到导航坐标系进行加速度信息的积分运算，就可推算出汽车的位置和姿态信息。
 从一个已知位置的开始，根据载体在该点的航向、航速和航行时间，推算下一时刻该坐标位置的导航过程称为轨迹递推。惯性导航系统基于位置的差异可以由一个加速度的双重积分得到的原理，可以被描述为在一个稳定坐标系下并且被明确定义的与时间相关的函数，可表达为：
$$\Delta s=s_t-s_0={\int }_0^t{\int }_0^t{a}_{t}dtdt$$
 其中，$s_0$ 为初始位置，$a_t$ 是在 $s_t$ 规定的坐标系中的惯性测量单元测量得到的沿运动方向的加速度。

一级轨迹递推

 对于一级轨迹递推，考虑在直线行驶的场景。在此场景下，只需要将一个加速度计安装在汽车上，并使加速度计的敏感轴方向与汽车行驶方向一致，就可得到速度和位置。已知汽车的初始位置$s_0$、初始速度$v_0$通过对加速度a 进行积分就可得到汽车在t时刻的速度$v_t$：
$$v_t=\int adt=at+v_0$$
 对速度$v_t$进行积分得到汽车在t时刻的位置$s_t$：
$$\begin{array}{rl}{s}_t& =\int v_{t}dt\\ & =\int (at+v_0)dt\\ & =\frac{1}{2}a{t}^2+v_{0}t+s_0\end{array}$$

二级轨迹递推

 轨迹递推从一维拓展到二维的难点主要在于需要将惯性坐标系（坐标轴为x、y，x轴与汽车航向保持一致）下的加速度变换到下一个与地球固连的坐标系下，常用的是地理坐标系，也成为导航坐标系（坐标轴为E、N，N轴与地理北向保持一致）。在二维轨迹递推中，将汽车看作在二维平面（x,y）上的运动，需要已知汽车的起始点$(x_0,y_0)$和起始航向角$A_0$。通过实时检测汽车在x、y方向上的行驶距离和航向角变化，即可实时推算汽车的二维位置。下图是将曲线运动近似为直线运动的捷联式惯性导航二维轨迹递推示意图，其中黑色圆点表示汽车位置，θ表示汽车与北向间的夹角。$a_x$、$a_y$分别代表着加速度计和陀螺仪，陀螺仪敏感轴垂直于纸面向外。汽车转弯将使陀螺仪产生一个相对于导航坐标系方向角变化的角速度$\omega$ ，结合初始航向角$A_0$，对陀螺仪测量得到的角速度进行积分可以得到航向角$A_t$。
$$A_t=\int \omega dt+A_0$$

 汽车速度变化将产生IMU坐标系下的加速度$a_y$，但是推算过程中需要的是在导航坐标系中的加速度$a_N$，使用航向角$A_t$可以将惯性测量单元的测量信息转换到导航坐标系中。坐标系的转换如下图所示。

 IMU坐标轴x、y与导航坐标轴E、N存在夹角θ，因此加速度$a_E$和加速度$a_N$可以写为：
$$\{\begin{array}{c}{a}_E=a_y\sin \theta +a_x\cos \theta \\ a_N=a_y\cos \theta -a_x\sin \theta \end{array}$$
 矩阵形式为
$$\left(\begin{array}{c}{a}_E\\ a_N\end{array}\right)=\left(\begin{array}{cc}\cos \theta & \sin \theta \\ -\sin \theta & \cos \theta \end{array}\right)\left(\begin{array}{c}{a}_x\\ a_y\end{array}\right)$$
 其中，$\left(\begin{array}{cc}\cos \theta & \sin \theta \\ -\sin \theta & \cos \theta \end{array}\right)$ 为坐标转换的二位旋转矩阵。
 得到导航坐标系中的加速度，即可对其积分得到速度
$$\{\begin{array}{c}{v}_E=\int (a_y\sin \theta +a_x\cos \theta )dt\\ v_N=\int (a_y\cos \theta -a_x\sin \theta )dt\end{array}$$
 再进行积分，得到导航坐标系中的位置：
$$\{\begin{array}{c}{x}_E=\iint (a_y\sin \theta +a_x\cos \theta )dtdt\\ x_N=\iint (a_y\cos \theta -a_x\sin \theta )dtdt\end{array}$$
 矩阵形式为
$$\left(\begin{array}{c}{x}_E\\ x_N\end{array}\right)=\iint \left(\begin{array}{cc}\cos \theta & \sin \theta \\ -\sin \theta & \cos \theta \end{array}\right)\left(\begin{array}{c}{a}_x\\ a_y\end{array}\right)dtdt$$

三维轨迹递推

 三维轨迹递推需要3个陀螺仪来测量载体相对于惯性空间的旋转角速率，需要3个加速度计来测量载体相对惯性空间受到的比力。如下图所示，载体的合加速度是重力加速度和其他外力产生的加速度的合成。为了消除重力加速度分量，需知道加速度计相对于重力加速度方向的角度（可由解算的姿态矩阵给出）。与二维轨迹递推相似，对陀螺仪测量的角速度进行积分可以得到3个姿态角，去掉重力加速度的同时通过三维旋转矩阵将加速度计测量值投影到导航坐标系中。

 下面给出基础三维旋转矩阵，其中γ、α、β角分别对应3个姿态角翻滚角、俯仰角、航向角。下式中分别表示x、y、z轴旋转γ、α、β角的旋转矩阵。
$$R_x(\gamma )=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \gamma & \sin \gamma \\ 0& -\sin \gamma & \cos \gamma \end{array}\right)$$

$$R_y(\alpha )=\left(\begin{array}{ccc}\cos \alpha & 0& -\sin \alpha \\ 0& 1& 0\\ \sin \alpha & 0& \cos \alpha \end{array}\right)$$

$$R_z(\beta )=\left(\begin{array}{ccc}\cos \beta & -\sin \beta & 0\\ -\sin \beta & \cos \beta & 0\\ 0& 0& 1\end{array}\right)$$
 对于上述三个基础旋转矩阵，其旋转次序（也称顺规）不可忽略，顺规可以自由组合。γ、α、β角在不同顺规中有不同的复合旋转矩阵结果。一般情况下，不同顺规完成的旋转效果相同，但当y 轴旋转90° 时，会导致x 轴和z 轴重合而失去x 轴的自由度，即万向节死锁。下面举例说明复合旋转矩阵的计算，zyx顺规下的复合旋转矩阵为
$$\begin{array}{rl}R(\gamma ,\alpha ,\beta )& =R_x(\gamma )R_y(\alpha )R_z(\beta )\\ & =\left(\begin{array}{ccc}\cos \alpha \cos \beta & \cos \alpha \sin \beta & -\sin \alpha \\ -\cos \alpha \sin \beta +\sin \gamma \sin \alpha \cos \beta & \cos \gamma \cos \beta +\sin \gamma \sin \alpha \sin \beta & \sin \gamma \cos \alpha \\ \sin \gamma \sin \beta +\cos \gamma \sin \alpha \cos \beta & -\sin \gamma \cos \beta +\cos \gamma \sin \alpha \sin \beta & \cos \gamma \cos \alpha \end{array}\right)\end{array}$$
 结合初始航向角，对这三个加速度做一次积分运算可得到三维的速度信息，通过两次积分运算可得到三维的位置信息。

误差分析

 惯性导航系统不与外界发生任何光电联系，仅靠系统本身对汽车进行连续三维定位和定向，其通过内部所感知到的情况来推断外面的情况，使得惯性导航被称为“黑盒导航”。如下图所示，惯性导航系统中既存在电子设备，又存在机械结构，在外部冲击、振动等力学环境中，除了需要的加速度和角速度之外，还有很多误差源。

随机误差

 1. 传感器白噪声误差：通常与电子噪声合在一起，可能来源于电源、半导体设备内部的噪声或数字化过程中的量化误差；
 2. 变温误差：由外部环境温度变化或内部热分布变化引起的；
 3. 传感器随机游动误差：惯性测量单元中对随机游动噪声有具体的要求，但是大多数针对其输出的积分，而非输出本身。随机游动误差随着时间线性增大，其功率谱密度也随之下降；
 4. 谐波误差：由于热量传输延迟，所以温度控制方法经常引入循环误差，这些都可能在输出中引入谐波误差。谐波周期取决于设备的尺寸大小；
 5. 闪烁噪声误差：闪烁噪声时陀螺仪零偏随时间漂移的主要因素。多数电子设备中都存在这种噪声，通常模型化为白噪声和随机游动的组合。

固定误差

 与随机误差不同，固定误差是可重复的传感器输出误差。常见的传感器误差模型如图所示。

 固定误差主要包括：
   1. 偏差 —— 输入为零时传感器的任何非零的输出；
   2. 尺度因子误差 —— 常来自于标定偏差；
   3. 非线性 —— 不同程度地存在于多种传感器中；
   4. 尺度因子符号不对称性 —— 来自不匹配地推挽式放大器；
   5. 死区误差 —— 通常由机械静摩擦力或死锁引起；
   6. 量化误差 —— 数字系统中固有的误差。