一文读懂TTC碰撞时间算法

一文读懂TTC碰撞时间算法

在当今的自动驾驶车上，需要更先进和复杂的驾驶辅助系统。大多数车通过一个前向摄像头来实现如LDW（lane departure warning）、TSR（traffic sign recognition）、FCW（forward collision warning）等功能。由于应用环境的复杂性，FCW是一个相当具有挑战的任务，一种鲁邦、可靠的ttc估计方法将显得尤为重要。

TTC为自车与前车发生碰撞的时间，定义为自车与障碍物之间的距离除以相对速度。在单目系统中，测距和测速并不是一个简单的任务。而基于单目视觉的TTC估计是在不需要计算实际距离和速度的前提下，算得自车与前车的碰撞时间。

TTC计算理论

在视频流中，在一个短暂的时间内，TTC可以通过目标的尺寸除以尺寸变化来计算。证明如下：

令Z为自车与目标的物理距离，为目标物理宽度，D=x2-x1为目标在图像上的像素宽度。

根据相机投影模型 可得，

,         

目标实际的物理宽度有

由于在实际的物理环境中，目标的物理宽度是不变的，对物理宽度求导可得：

但是在实际应用场景，相对速度是实时变化的，那么图像中目标的尺寸D和尺寸变化率也是实时变化的，那么，准确地计算出目标的尺寸和尺寸变化率并不是一件容易的事情。为鲁邦、可靠的估算TTC，仅靠上述公式，算得的ttc是不稳定的。

《Time To Contact Estimation Using Interest Points》提供了一种简单、稳定的TTC估算方法。以下内容将围绕该文章介绍TTC的估算方式。

Time To Contact Estimation Using Interest Points

该方法的主要思想：

• 利用目标关键点估计目标的尺寸变化S；
• 然后利用S建立目标的匀速运动模型和加速运动模型；
• 用扩展Kalman滤波跟踪模型参数；
• 采用策略融合多种运动模型的计算结果。

在实际应用中，本人认为采用匀加速模型（CA）和匀速模型（CV）即可。

运动模型建立

利用相似三角形原理，可得：

，其中，为带参数的运动模型。

那么，在不同时刻，我们有：

令。假定已测得若干组s，

则有

假定运动模型为：

其中，，。

利用最小二乘拟合该2阶曲线，即可求得、。

计算尺寸变化S

令、为不同帧中的对应点(该对应点可通过关键点匹配或者LK光流算得，对于关键点计算，在本文中不做讨论。欢迎评论或者留言)。采用放射变换计算s。

利用最小二乘算法，可求得：

是s的有偏估计，目标尺寸w越小，估计误差越大。

多模型跟踪

采用扩展卡尔曼模型跟踪运动模型的参数，运动模型为CA模型和CV模型。再进行多模型的融合决策，得出最终的TTC。

（该论文中，作者没有采用IMM Kalman (Interactive Multi model kalman filter algorithm）对多个模型进行融合。）

（对于Kalman、扩展Kalman、IMM等模型，将在后续文章中更新，欢迎关注、评论和留言）

状态向量为：（见公式7）

对CA模型，一步预测方程为：

雅克比矩阵为：

对CV模型，一步预测方程为：

雅克比矩阵为：

采用上述方程，对CA模型和CV模型进行跟踪。

多模型融合决策

该论文中，作者并没有采用IMM模型进行融合。而是采用了一种简单的逻辑策略，对各个模型计算的TTC进行融合。

融合策略如下：

• 计算观测值在各个模型中的似然估计p1和p2，估计原理戳here；

令，可求得ttc的值，即公式7的根。

实验结果