自动驾驶定位算法：基于多传感器融合的状态估计(muti-Sensors Fusion)

附赠自动驾驶学习资料和量产经验：链接

1、传感器(Sensor)选取

自动驾驶系统中用于状态估计(State Estimation)的常用传感器包括GPS/GNSS、IMU、激光雷达(Lidar)。

状态估计(State Estimation)选用传感器需要考虑哪些因素：

1）误差不相关性。也就是说，用于Sensor Fusion的传感器其中单个传感器(Sensor Measurement)测量失败，不会导致其它传感器(Sensor)由于相同的原因而同时失败。

2）传感器的相互补充性。比如IMU可以填充GPS两次定位间隔期间的定位输出，用于平滑GPS/GNSS的定位结果；GPS为IMU提供初值，消除IMU单独使用出现的偏移(Drift)的问题；Lidar可以弥补定位精度的问题，而GNSS可以为Lidar定位地图匹配提供地图范围数据。

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

2、传感器的标定(Sensor Calibration)

如果想要各个传感器能够相互协同，无间配合，传感器的标定是必不可少的。传感器的标定通常分为三种: 内参标定(Intrinsic Calibration)、外参标定(Extrinsic Calibration)和时间校准(Temporal Calibration)。

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

2.1 内参标定(Intrinsic Calibration)

传感器或者车辆的内参在传感器制造的时候就已经固定下来，传感器模型中的固定参数都是内参，都需要通过Intrinsic Calibration事先确定。

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

比如估计车辆运动距离的轮速计模型 �=�� 中，r就一个内参。另外激光雷达(Lidar)中扫描线的角度，在激光雷达计算模型中需要事先知道这个参数，以实现激光雷达扫描线(Scan Line)的拼接。

如何获取传感器的内参呢？实践中有几种方法:

1）从传感器制造商的使用说明书中获取。这种方法往往只能获取大概的参数，每个设备的内参都是不同，所以并不能获取比较精确的参数。

2）手工测量内参。比如车轮的半径，可以通过手工测量的方法获取。但是类似于激光雷达的内参无法通过手工测量获取。

3）Estimate as part of State。这种方式不仅可以获取精确的传感器内参，而且可以解决内参随时间变化的情况。比如汽车的轮胎半径漏气导致半径变小等。

2.2 外参标定(Extrinsic Calibration)

传感器的外参主要表达各个传感器之间的位置相对姿态，它是把各个传感器的数据坐标统一起来的必不可少的参数。

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

如何获取传感器的外参呢？实践中有几种方法:

1、CAD图纸。如果你能获取传感器安装的CAD图纸，那你就可以获得比较准确的传感器外参。

2、手动测量。当然手动测量的难度也非常高，因为传感器的中心往往在传感器内部，难以精确测量。

3、Estimate as part of State。这也是一个研究的方向。可以比较好的应对外参标定问题，但难度也比较高。

2.3 时间校准(Temporal Calibration)

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

时间校准对于各个传感器的数据融合至关重要。比如IMU的输出频率是200HZ，Lidar的输出频率是20HZ，只有按照最相近的时间进行对齐，才能将IMU和Lidar数据准确融合起来。

在实际应用中，各个传感器的相对时间误差是未知的，这些误差可能是由于各个传感器的预处理耗时不同导致的，也可能是由于各个传感器的计时器精度不同造成的。

如何校准传感器的时间呢？实践中有几种方法:

1）假设这些传感器的时间相对误差为0。当然忽略这些误差，会导致最终的融合结果比预期要差。

2）硬件同步。在硬件设计上保证各个传感器的时间戳对齐。

3、EKF-多传感器融合(Multi-Sensors Fusion)

自动驾驶汽车一般包含多个Camera、3D 激光雷达(Lidar)、惯性测量单元(IMU)、多个Radar、GPS/GNSS Reciver、轮速计(Wheel Odmetry)，这些传感器在运行过程中时刻都在以不同的频率发送不同类型的数据，多传感器融合模块需要将这些信息融合起来，不断更新自动驾驶车辆的状态(Vehicle State)。多传感器融合进行状态估计(State Estimation)的流程如下：

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

车辆运动模型(Motion Model Input)如下，它的信息一般来自于IMU，包含x、y、z三个方向上的加速度和角速度，是一个6维向量。

车辆运动模型的计算过程如下:

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

为了应用EKF，我们定义Error State如下，其中是3x1的矩阵。

EKF的Motion Model如下:

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

EKF中的GNSS测量模型:

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

EKF中的Lidar测量模型:

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

这里假设激光雷达(Lidar)的测量结果和GNSS的测量结果都在同一个坐标系下(注意，实际情况下，需要经过坐标变换才能达到这种效果)

EKF的IMU+GNSS+Lidar多传感器融合流程如下:

1）Update State With IMU Inputs

2、Propagate Uncertainty

3、当有GNSS或者LIDAR测量结果到达时，进入步骤4），否则进入步骤1）。

4、计算GNSS/Lidar的卡尔曼增益(Kalman Gain)。

4、计算Error State。

5、Correct Predicted State。

6、Compute Corrected Covariance。

4.状态估计(State Estimation)的精度需求(Accuracy Requirements)

不同的应用场景对State Estimation的精度的要求不同，比如高速场景下的Lane Keeping一般要求亚米级级精度。如下图所示的场景，车辆宽度为1.8m，机动车道宽度为3m，所以车辆两侧有约60cm的冗余空间，在这种场景下，如果要实现Lane Keeping的功能，只要状态估计的精度小于60cm就可以满足实际应用的需求。

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

但在拥挤的城市道路交通场景下，对State Estimation的精度要求是越高越好，状态估计的精度越高，自动驾驶就越安全。

5、状态估计(State Estimation)的更新频率要求

以人类驾驶汽车为例，一个人开车过程中闭着眼睛，但为了保证行车安全，她每间隔1s睁开一次眼睛，以确定自己所在的位置。在空旷的道路场景下，1HZ的位置确认频率就可以保证，但是在繁忙的交通的道路上，1s确认一次位置的做法就非常不靠谱了。

但是，越高的定位频率带来的越高的计算资源消耗，而车载计算资源是有限的，并且还是感知、控制、决策、路径规划等所有功能共享的，所以在更新频率和计算资源之间需要有一个trade-off。

根据经验，15HZ-30HZ的状态更新的频率就能够满足自动驾驶的应用需求，当然在计算资源允许的情况下，状态更新(State Estimation)频率越高越好。

6、Sensor Failures

自动驾驶使用的传感器系统可能由于外部环境因素而失效，比如恶劣天气状况、硬件故障、系统连接线松了等等；也可能由于传感器自身的短板导致，比如GNSS在隧道场景下无法定位、在城市环境下定位的误差达到数十米，IMU容易收到温度变换的影响等。

即使在没有传感器异常的情况下，我们依然能够从多传感器的使用中收益。如下图所示，各个传感器的功能相互补充，构建安全的自动驾驶系统。

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

各个传感器各有所长，比如短距测量传感器可以在停车场景下，检测附近的障碍物，避免发生碰撞；中距测量传感器在车道保持场景下，检测周围的行人、机动/非机动车辆；长距测量传感器帮助我们检测和预测远距离障碍物的运动等等。在实际应用要充分考虑到这些传感器的长处和短板，并增加一定的冗余系统，保证在部分系统无法工作的情况下，仍然可以保证车辆的正常运行。

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

7、多传感器融合的代码实战

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

Couresas上的Multi-Sensors Fusion Project效果如下:

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

参考链接:

https://medium.com/@wilburdes/sensor-fusion-algorithms-for-autonomous-driving-part-1-the-kalman-filter-and-extended-kalman-a4eab8a833ddmedium.com/@wilburdes/sensor-fusion-algorithms-for-autonomous-driving-part-1-the-kalman-filter-and-extended-kalman-a4eab8a833dd

https://www.coursera.org/learn/state-estimation-localization-self-driving-carswww.coursera.org/learn/state-estimation-localization-self-driving-cars