《 无人驾驶原理与实践》一3.4.2不同传感器的定位融合实现

3.4.2不同传感器的定位融合实现

      本节主要介绍GPS如何结合INS实现定位，以及数据融合的一些方法技巧。
       上文中我们已经了解到基本的GPS运作原理。通常GPS设备输出的信号主要包括经度、纬度、高度等，以及一些表示时间同步、信号等控制信息。通过经纬度坐标，通过ECEF（地心地固坐标系）、UTM（横轴墨卡托）等坐标系，根据场景需要可将经纬度信息转换为投影平面坐标x、y、z等位置信息，由此就得到平面位置信息。
       INS设备输出的主要数据包含x、y、z方向的加速度，以及对应旋转角速度信息，通过对这些测量值进行时间积分操作，我们可以得到x、y、z方向的速度信息，以及相对位置信息和roll、pitch、yaw等角度信息。
       现在，有了x、y、z（两组，GPS和INS输出）、vx、vy、vz，以及roll、pitch、yaw，相当于我们有了至少9个状态向量，来表示车辆此时的姿态和位置信息。有了这些状态向量，就可以根据扩展卡尔曼算法的原理（扩展卡尔曼的原理在此不赘述，其基本原理即根据不断输入的测量值，基于贝叶斯概率，来更新计算当前目标的姿态位置等信息），进行状态向量的数据融合。对于前面所讲的x、y、z（两组）变量，我们知道对于这两组数据，相对GPS来说这组值更可靠些，因为IMU（Inertial Measurement Unit，惯性测量单元）随着时间累积和发热，会存在明显、较大的漂移，因此对定位精度的影响较大（精度极高的IMU除外）。我们可以将GPS输出的x、y、z作为主要的位置状态向量参考量，而将IMU的x、y、z作为次要的位置状态向量参考量给予较低的权重。由于前面所述原因，可以不直接使用IMU的x、y、z测量值，而是通过dx/dt、dy/dt、dz/dt计算得出，因为我们知道在这段时间内其相对速率的变化是相对准确的。通过建立扩展卡尔曼方程，并解决不同设备数据输出时间同步的问题，就可以得到两个不同设备之间的数据融合结果，计算出一个较为精确的状态向量融合结果，即［x, y, z, vx, vy, vz, roll, pitch, yaw］，从而得到车辆当前基于多传感器融合的新的位置和姿态信息。这就是多传感器定位融合原理的本质所在。
同理，我们还可以增加其他传感器设备，如摄像头、超声波、里程计、轮速计等，因为这些传感器都有其各自的优点和缺点，都能提供不同维度度量的测量值，通过融合多传感器不同维度的测量数据，可以计算得到一组可靠的车辆姿态数据，这也是采用多传感器融合能够提升定位精度的原因所在。
       系统状态向量融合的简要流程示意图如图39所示。

       如图310［9］所示，还可以扩展到其他更多传感器的融合上，每个传感器都可以提供各种维度的测量值，最终通过图39所述融合逻辑，计算出车辆最终估计姿态和位置。