激光雷达TTC估计

Camera系列文章

传感器融合是将多个传感器采集的数据进行融合处理，以更好感知周围环境；这里首先介绍Camera的相关内容，包括摄像头及图像知识基本介绍，OpenCV图像识别（特征提取，目标分类等），融合激光点云和图像进行TTC估计。

系列文章目录
1. 摄像头基础及校准
2. Lidar TTC估计
3. Camera TTC估计

---

前言

提示：本章将介绍基于激光雷达（Lidar）距离测量估计与前车的TTC。首先我们需要理解如何通过传感器测量数据和运动模型计算TTC。然后再讨论如何从激光雷达点云数据中提取前车最近点的距离信息，并计算TTC。

碰撞时间TTC（Time To Collision），保持当前时刻的运动状态，VUT（被测测量） 与VT（目标车） 或PTA（成年假人目标） 发生碰撞所需的时间，即TTC = 两车车距 / 两车相对车速。
TTC通常用于FCW前碰撞预警（Forward Collision Warning）中，作为报警时刻的参考。

---

下面我们将假设ADAS车辆配置激光雷达传感器对前车进行距离测量。

一、问题定义

我们首先定义要解决的碰撞检测问题，交通场景如下图所示，$t_0$时刻绿色前车开始减速，自车传感器检测到距离为$d_0$，随后$t_1$时刻探测距离$d_1$。我们的目标是计算当前TTC以确定报警和制动时机，TTC=两车相对距离/两车相对速度，而两车相对速度无法直接获得（假设这里没有Radar），因此需要通过联系2次探测间接测量。

为测量TTC，我们需要假设两车的相对运动模型。主要有两种：①constant velocity model (CVM)；②constant acceleration model (CAM)。$v_0$为相对速度，$a_0$为相对加速度。

在大部分时候，CVM可以很好描述两车相对运动，但某些场景，比如急减速场景，CVM不是很精确，比如急减速，两个采样时间间隔的相对距离变化量不一致，此时需要采用CAM模型。考虑到采样时间间隔较短，我们通常用CVM。

二、TTC计算

在此我们首先讨论如何基于CVM（Constant Velocity Model）模型计算TTC。这里通过计算连续两次测量的相对距离信息获取相对速度$v_0$信息，从而获取TTC。


下一节再介绍如何从Lidar点云数据中提取最近点的距离信息。

三、Lidar点云数据获取相对距离信息

1.Lidar点云分割

下图是高速场景下Lidar点云数据俯视图叠加Camera图像，其中绿色→红色由远及近。

从上图可以看到，Lidar激光反射的点云数据不仅包括有用前车车距信息，还包括地面反射信息，低反射率的点云信息（如下图所示，绿色为高反射率点云，红色为低反射率点云），所以这里我们首先要对点云进行预处理，即点云分割。

这里主要有两步：
1.去除路面反射点云，通常需要建立道路平面（通常假设为平面，坡道需要道路坡度信息），去除道路平面附近的反射点云；
2.去除低反射率点云；

下图左侧是去除路面反射点云和低反射率点云后的点云俯视图，右侧是处理后的激光点云叠加在Camera图像上。预处理后获取前车距离信息更为方便。

2.距离信息提取

激光点云数据结构如下，x (forward), y (left) an z (up)为三维坐标，r为反射率。

struct LidarPoint { // single lidar point in space
    double x, y, z; // point position in m
    double r; // point reflectivity in the range 0-1
};

Lidar点云数据分割及提取的前车后尾门反射点云信息如下图所示。

由图可知，即使激光雷达是一个非常可靠的传感器，也有一些点位于后尾门外侧，与车辆无连接，这就会出现距离预估错误，因此需要更可靠的方法提取距离信息。由于篇幅的信息，本章暂不介绍，后续会对此进行介绍。

为提取距离信息，需要获取当前车道前车最近点的距离信息。这里我们需要根据道路宽度（如假设Lane width=4）去除非关联的点云。
以下是从$t_0$时刻点云（lidarPointsPrev）及$t_1$时刻点云（lidarPointsCurr）中获取最近点信息的代码，然后基于上述TTC计算公式计算TTC。

void computeTTCLidar(std::vector<LidarPoint> &lidarPointsPrev, std::vector<LidarPoint> &lidarPointsCurr, double &TTC)
{
    // auxiliary variables
    double dT = 0.1;        // time between two measurements in seconds
    double laneWidth = 4.0; // assumed width of the ego lane

    // find closest distance to Lidar points within ego lane
    double minXPrev = 1e9, minXCurr = 1e9;
    for (auto it = lidarPointsPrev.begin(); it != lidarPointsPrev.end(); ++it)
    {        
        if (abs(it->y) <= laneWidth / 2.0)
        { // 3D point within ego lane?
            minXPrev = minXPrev > it->x ? it->x : minXPrev;
        }
    }

    for (auto it = lidarPointsCurr.begin(); it != lidarPointsCurr.end(); ++it)
    {
        if (abs(it->y) <= laneWidth / 2.0)
        { // 3D point within ego lane?
            minXCurr = minXCurr > it->x ? it->x : minXCurr;
        }
    }

    // compute TTC from both measurements
    TTC = minXCurr * dT / (minXPrev - minXCurr);
}