自动驾驶中的障碍物时间对齐法

描述

自动驾驶算法使用的系统往往不是实时系统，因此每个节点间拿到的数据可能不是同一时间的数据，从而造成系统误差，针对这一现象，工程上往往采用时间对齐内插外推法。这里我们用感知障碍物来举例。

自动驾驶系统有许多重要模块，假设每个模块占用一个进程，即节点，那么进程与进程间需要相互通信来传递数据。决策规划节点需要感知节点传来的障碍物信息来做决策规划。假设在$t1$时间点，决策规划收到了感知障碍物的数据，但这个障碍物的位姿一定是在$t1$时间点的位姿吗，不一定，因为不是实时系统，数据传输存在延时，所以决策规划在$t1$时间点收到的感知障碍物数据很可能是更早的$t0$时间点的数据，这个时间错位可能是几毫秒到几十毫秒，这个时间错位对高速场景可能最终导致障碍物实际距离差出了几米。所以针对这种非实时系统导致的时间错位的情况，我们需要进行时间对齐，内插外推出感知障碍物更准确的位姿。

具体做法

因为感知和定位的需要，自动驾驶系统中往往存在一个全局里程计odometry，odometry是个相对概念，并不代表车辆真实的utm位姿，只是反映了不同时间点位姿的变化过程，这个里程计由translation和rotation组成，反映了车辆x y z yaw pitch roll六个维度的变化。

1. 在决策规划节点里，我们需要记录odometry历史到现在的一段时间轴，时间轴设置几秒即可，因为延时不会太大，时间轴长了反而会降低程序效率。

2. 假设在$t1$时刻，决策规划拿到了感知障碍物的数据，那么先读取感知障碍物自带的时间戳，假设为$t0$，这个时间戳是感知发布那一帧信息时打的，这个时间戳往往比此时决策规划的现在的时间更早，因为信息传递需要时间。拿到这个时间戳后，在第1步里记录的时间轴里去找对应的odometry的位姿，并记录为$pose0$

3. 根据此时的$t1$时刻，在时间轴里去找对应的odometry的位姿，并记录为$pose1$

4. 由$pose0$和$pose1$可计算出从$t0$时刻到$t1$时刻的位姿转移变化矩阵，记为$tf$

5. 将$tf$施加到$t0$时刻感知障碍物的位姿上，得到的结果是$t0$时刻感知障碍物在$t1$时刻时相对于本车的位姿

6. 因为障碍物可能存在速度，因此在做完tf转换后还需预测推理更准确的位置，这里我们采用最简单的cv预测(恒定速度预测)，注意在预测前还需对障碍物速度方向施加tf转换。预测时间为$t1-t0$

7. 预测的距离方向施加在第5步得到的位姿上，即可得到$t1$时刻，相对于本车，更准确的障碍物车辆的位姿坐标