自动驾驶多传感器融合之相机与IMU的同步

智驾最前沿 2023-12-24 08:21

【免注册下载】创新MCU提升高压系统的实时性能 如何增强电动汽车的实时控制能力？

--关注、星标、回复“40429”--

↓↓免费领取：《汽车驾驶自动化分级》（GB/T 40429-2021）↓↓

在自动驾驶中，往往认为IMU所在的位置为车体坐标系的原点（不绝对），所以需要计算相机到IMU之间的时空同步。相机与IMU具有互补性：相机在高速运动、光照改变情况下容易失效，但能够获得丰富的环境信息；而IMU能够获得车辆的运动信息，并且不受周围环境的影响，从而弥补相机的不足。可以通过视觉匹配完成回环检测与回环校正，有效的修正IMU的累计漂移误差，即所谓的相机与IMU的相对位姿。

1）相机与IMU的相对位姿：

相机与IMU之间的相对位姿，就是相机坐标系与IMU坐标系之间的变换，包括相对旋转角和相对平移量。其变换关系满足如下：

其中wc表示相机坐标系，wb表示IMU坐标系，cb表示相对坐标系。将上式展开可以得到相机坐标系与IMU坐标系之间旋转角和平移量的变换关系：

2）相机与IMU的时间同步：

由于触发、传输等延时的存在，相机与IMU采样的时间和时间戳的时间并不匹配，如下图，这会导致相机与IMU之间存在时间差

。

这里时间差的计算为：

即将相机的时间戳平移td后，相机与IMU之间实现了同步。

相机与IMU的时空同步目前有很多开源的框架，如熟知的VINS就是集合了相机与IMU状态估计和标定的系统框架，也是目前个人比较推荐的一个应用框架。首先给出VINS的系统图。

首先进行相机与IMU之间相对旋转角的标定，标定完成之后进行初始化和相机与IMU之间相对平移量的标定。标定和初始化完成后进行视觉惯性联合的状态估计，并且把标定量也当做待估计的状态放入联合优化中。VINS除了可以标定相对姿态，还能够进行时间标定。原理如下：假设在较短时间内相机从

匀速运动到

，特征点在像素平面上的投影也从

匀速运动至

,像素匀速运动的速度为

。如下图：

通过平移像素坐标，将时间差作为优化变量加入投影误差的表达式中，如下式：

此投影误差项与IMU误差项和初始值误差项相加进行联合非线性优化。每一次优化结束后对时间差进行补偿，最终时间差的标定量逐渐趋于0。

当然，为了获取更精确的标定结果，还可以将时间差加入到IMU预积分表达式中，作为优化变量参与联合非线性优化。

关于VINS的其他应用，后续有机会再来介绍，这里再来介绍另外一种方法，即基于basis-functions来估计时间偏移的方法，用于统计相机与IMU之间固定延迟timeoffset的统一框架，然后将离散的数据在时间上连续化，最后基于最大似然估计得出矫正后的结果。并由此衍生的camera-imu的外参标定和时间同步方法。

1）基于basis-functions的时间偏移估计

使用一组基函数加权和来表示随时间变换的状态量，如D维度状态量