惯性导航一般集成在GPS设备中，都是由供应商集成，那在这里有什么讨论的必要呢，要知道在车辆行驶中，我们可以拿到GPS的yawrate和speed信号，而且车辆本身还有一套传感器获取yawrate和speed，又因为航迹推算是自动驾驶很重要的一部分，所以理解惯性导航的工作原理，能很好地帮助我们做基于车身的航迹推算。
惯性导航
目前GNSS+IMU构成的组合导航系统是主流的定位系统方案，惯性导航系统是唯一可以输出完备的六自由度数据的设备、数据更新频率高、是定位信息的融合中心。
惯导中使用的核心算法主要包括3种：1. 惯性导航解算算法；2. 组合导航的卡尔曼滤波器的耦合。3. 环境特征信息与惯性导航融合。
组合导航系统核心算法框架
硬件及原理
惯性导航系统（INS）是利用惯性传感器（IMU）测量载体的比力及角速度信息，结合给定的初始条件，与 GNSS等系统的信息融合，从而进行实时推算速度、位置、姿态等参数的自主式导航系统。具体来说惯性导航系统属于一种推算导航方式。即从一已知点的位置根据连续测得的运载体航向角和速度推算出其下一点的位置，因而可连续测出运动体的当前位置。
惯性系统工作原理图
惯性导航系统采用加速度计和陀螺仪传感器来测量载体的运动参数。其中三个垂直布置的陀螺仪用于测量载体绕自身三个坐标轴的转动角速度，同时也敏感地球自转的角速度。
加速度计基于牛顿第二定律，采用电容式、压阻式或热对流原理，通过在加速过程中对质量块对应惯性力的测量来获得加速度值。用来测量运动体坐标系上各轴的加速度。
惯性系统工作原理图
惯导通过对陀螺仪测量的角速度进行积分运算和坐标变换，计算车体的姿态角（横滚、俯仰角）和方位角。根据姿态角可以计算出重力加速度在各个坐标轴上的分量，加速度计测量得的各轴加速度，减去重力加速度分量后积分，得到速度和位置。惯导计算得到的状态，用于预测车辆当前的位置，再和卫星定位接收机得到的位置（或观测数据）进行比较。比较的偏差包含了惯导的推算误差和卫星接收机的定位误差，通过数据融合算法进行加权后，用于修正惯导的预测，让惯导的预测越来越准确。
惯性导航解算算法
通常分以下几步：
- 姿态更新：对陀螺仪输出的角速度进行积分得到姿态增量，叠加到上次的姿态上；
- 坐标转换：从IMU载体坐标系到位置、速度求解坐标系（惯性坐标系）；
- 速度更新：需要考虑重力加速度的去除，得到惯性系下的加速度，通过积分得到速度；
- 位置更新：通过速度积分得到位置。
惯性导航解算算法原理图
在惯性导航中，导航方程的每一次迭代都需要利用上一次的导航结果作为初始值，因此惯导的初始化是比较重要的部分之一。姿态对准是指得到IMU的roll, pitch, yaw。roll, pitch的对准过程一般称为调平。使当车静止时，加速度计测量的比力仅由重力导致，可以通过f=C*g来求解；对于非常高精度的IMU可通过罗经对准的方式，车静止时，通过测量载体系中的地球自转来确定载体的方位(yaw)。
惯性导航初始化原理图
组合导航的卡尔曼滤波器的耦合
使用Kalman滤波器的耦合，对IMU和GNSS即点云定位结果进行融合。可分为松耦合和紧耦合两种方法。
松耦合滤波器采用位置、速度量测值和解算的位置速度之差作为组合导航滤波器输入，也即卡尔曼滤波器的量测量。紧耦合的数据包括GNSS的导航参数、定位中的伪距、距离变化等。
卡尔曼滤波器的松耦合原理图
卡尔曼滤波器的紧耦合原理图
卡尔曼滤波器松耦合和紧耦合的优点和不足对比
以百度阿波罗使用的惯导系统为例，采用了松耦合的方式，并且使用了一个误差卡尔曼滤波器。惯性导航解算的结果用于Kalman滤波器的时间更新，即预测；而GNSS、点云定位结果用于Kalman滤波器的量测更新。Kalman滤波会输出位置、速度、姿态的误差用来修正惯导模块，IMU期间误差用来补偿IMU原始数据。
百度阿波罗卡尔曼滤波器的松耦合
卡尔曼滤波融合示意
环境特征信息与惯性导航融合
目前常用的GNSS+IMU组合惯导方案在一些场景的定位精度稳定性仍不能完全满足自动驾驶的要求。例如，城市楼宇群、地下车库等GNSS长时间信号微弱的场景下，依靠GNSS信号更新精确定位稳定性不足，因此必须引入新的精确定位更新数据源，在组合惯导中引入并融合激光雷达/视觉传感定位等环境信息进行融合定位成为必然趋势。
一种组合导航和环境感知信息融合的架构示意图
以百度阿波罗的多传感器融合定位系统解决方案为例，惯性导航系统处于定位模块的中心位置，模块将IMU、GNSS、Lidar等定位信息进行融合，通过惯性导航系统解算修正后最终输出满足自动驾驶需求的6个自由度的高精度位置信息。
百度阿波罗的惯性融合定位模块框架
