自动驾驶之定位基本理论-电子工程世界

经常开车的老司机应该都遇到过，当车辆开过高楼林立的路段或隧道时，我们的导航就会显示GPS信号弱，甚至GPS信号中断的“丢星工况”，这个问题也是自动驾驶技术工程实践中关心的重点。为了让车辆可以继续导航，此时的融合算法该如何处理呢？

答案是航迹推算（DR）或惯性定位（INS/IMU）。两者的区别在于航迹推算算法是开环定位算法，主要是通过车轮脉冲、方向盘角度，结合车辆角速度、车速，推演出车辆在每个时刻的位置和姿态；是路径规划和路径跟踪环节的依赖项。而惯性定位系统是一种利用加速度计和陀螺仪等惯性传感器来测量物体加速度和旋转速率，并通过这些数据计算物体的位置、速度和姿态的导航系统。IMU通过积分运算将加速度和角速度转换为速度和位置，从而实现导航定位。DR更适用于低成本、简单导航需求，而INS则适用于高精度、高可靠性要求的场景。IMU在目前自动驾驶车辆上使用的较多。

目前高精度定位技术可以大致分为：卫星导航、惯性导航、环境特性匹配定位。卫星信号定位相信大家都知晓其中的原理，而环境特性匹配定位是利用车载摄像头、激光雷达等传感器感知周边的环境，用观测到的特征和数据在高精地图匹配，从而得到车辆的位置和姿态。本文接下来讲讲定位技术融合的原理，主要针对目前界内研究的热点：卫星导航和惯性导航组合进行探讨。

一、首先我们聊一聊卫星定位和航迹推算的优点和局限性：

GNSS的优势和局限性：其实目前为了提高定位精度，通常会引入一些天地基的辅助手段，也就是实时动态差分技。GNSS可以提供全球范围内的精确位置信息，尽管提高了精度，是全局定位中的主心骨，在开阔地区和无遮挡的视线下表现出色。然而，在密闭空间、城市高密建筑群和隧道等环境中，GNSS信号仍然可能受到遮挡，导致其性能下降甚至断连。

IMU的优势和局限性：IMU由陀螺仪和加速度计组成，可以提供相对运动信息，如加速度和旋转变化。与GNSS不同，IMU不受外部信号的影响，因此在GNSS受限的环境中非常有用。然而，IMU容易因误差积累而产生漂移，导致位置和速度估计不准确。因为解算模块存在积分计算，因此存在累积误差，随着时间的延长，误差会越来越大。

综上，我们把两者结合到一起，信息互补，用俗语来描述可谓之“男女搭配，干活不累”。有望成为主要的定位技术解决方案。

二：惯性导航的工作原理是什么样的呢？

惯性导航定位的工作原理是基于众所周知的牛顿第二定律：F=ma。

惯性导航系统利用载体先前的位置、惯性测量单元测量的加速度和角速度来确定其当前位置。其中，速度v 和偏移量s都可以通过对加速度a 的积分得到。如下式所示，加速度a 经过积分得到速度v，经过二重积分得到偏移量s。相反，速度和加速度也可以通过对位移的微分而估算得到。

类似地，汽车的俯仰、偏航、翻滚等姿态信息都可以通过对角加速度的积分得到。利用姿态信息可以把导航参数从惯性坐标系变换到导航坐标系中。综上，惯性导航系统可以说是一个由惯性测量单元和积分器组成的积分系统。该系统通过陀螺仪测量载体旋转信息求解得到载体的姿态信息，再将加速度计测量得到的载体比力信息转换到导航坐标系进行加速度信息的积分运算，就能推算出汽车的位置和姿态信息。

惯性导航定位基于一个简单的原理，那就是位置的差异可以由一个加速度的双重积分得到，可以被描述为在一个稳定坐标系下并且被明确定义的与时间相关的函数，可表述为:

式中，s0为初始位置，at是在st规定的坐标系中的惯性测量单元测量得到的沿运动方向的加速度。

惯性导航分为一维惯性、二维惯性和三维惯性。通俗解释就是将车辆放入一个坐标系中，一维只考虑X轴（运动方向）的加速度即可，相对简单。二维需考虑X轴（与汽车航向保持一致）、Y轴两个方向上加速度和航向角的位置变化。

下图是将曲线运动近似为直线运动的捷联式惯性导航二维航迹递推示意图，其中黑色圆点表示汽车位置，θ表示汽车与北向间的夹角，圆柱体表示加速度计与陀螺仪，陀螺仪敏感轴垂直于纸面向外。在进行类似一维航迹递推中的积分运算前，需要将惯性测量单元的输出转换到导航坐标系中。汽车转弯将使陀螺仪产生一个相对于导航坐标系方向角变化的角速度ω，结合初始航向角 A0，对陀螺仪测量得到的角速度进行积分可以得到航向角At。

三维即考虑X、Y、Z轴三个方向的加速度和姿态。而三维递推还需考虑重力因素。形成三维旋转矩阵，三个角分别对应3个姿态角翻滚角、俯仰角、航向角。

三：卫惯组合的耦合方式都有哪些？

卫惯组合有三种耦合方式：松耦合、紧耦合和深耦合。从命名方式上，目前大众普遍有个误解，认为这三种方式的耦合程度从低到高，定位精度也是从低到高，其实不然。

松耦合：又称标准耦合模型，是紧/深耦合技术的基础和前提。GNSS/RTK定位结果和IMU原始数据来实现融合，卫惯系统各自解算载体方位和运动情况信息（卫星输出位置与速度等、惯性输出位置速度姿态等），所得结果导入滤波器进行进一步结算，估算INS误差情况。

紧耦合：与标准耦合（SFM）导航系统不同的是， GNSS 接收机输出的是伪距与多普勒频移这些原始采样数据。将 GNSS 接收采样得到的伪距与多普勒频移由 INS 系统预测得到的伪距与多普勒频移的预测值相减即可得到伪距与多普勒频移的测量残余作为数据融合滤波器的观测量，将观测量输入到数据融合滤波器之后，同标准耦合（SFM）系统一样可以得到INS计算误差以及传感器偏差以完成对INS系统的对准并获得位置与速度的最优估计值。与松耦合相比的优势是紧耦合在干扰环境中工作时能更好地保持对卫星的锁定，不过除了干扰一信号比以外，这一优势很难定量表示。

深耦合：深耦合的应用是在高动态和射频干扰环境下，且无差分修正（无RTK），此时无论是 GNSS/INS 标准耦合（SFM）系统还是 GNSS/INS 紧耦合系统都无法正常工作，比如：炮弹、导弹、火箭等非载人装备。深耦合导航系统相对于紧耦合导航系统增加了 INS 单元对 GNSS 接收机的辅助。深耦合模型其优势在于：在高动态条件下，可提升跟踪环路的稳定性，提高了接收机的动态性，进而提高了接收机跟踪环路的灵敏度，即使在强射频干扰环境下。劣势在于：由于两者的之间的耦合很深，因此当断开两个系统时，GNSS 接收机不但无法完成定位、定速的工作，同时也无法继续完成对卫星信号的跟踪，此时如果没有相应的切换方法来控制，则会导致卫星信号的失锁。