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

作者 | 韦韦道来  编辑 | 韦心雕AI

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心【多传感器融合】技术交流群

后台回复【多传感器融合综述】获取图像/激光雷达/毫米波雷达融合综述等干货资料！

    最近的事情比较多，已经将近两个月没有好好维护了。很有幸迎来一位新的常驻小伙伴，贝塔。贝塔也将不定期的推送一些论文笔记。今天来叨叨自动驾驶多传感器融合系列的相机与IMU同步内容。

在自动驾驶中，往往认为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维度状态量可以表示为：

 

其中，每个是D维时间函数，是DxB的矩阵。为了估计，只需要估计D维向量C即可。

当从测量数据估计时间差时，令误差项如下：

 

其中，是时刻的测量值，是由状态量计算测量值的模型，d为待估计的时间差。将前面提到的状态变量代入上式可得：

 

通过优化误差结果可以得到最优时间差d的结果。为了求解上式的最小二乘问题，需要知道相应的jacobian矩阵，对于给定的初值，对应的变化量为，那么上式可以表示为：

 

其中:

 

这样做有两个好处，首先可以将时间差估计问题转换为最大似然估计问题来处理，其次将离散的量变为连续的，可以很容易得到timeoffsets的jacobian矩阵。所以，多传感器之间的估计问题就可以统一了，也就形成了统一的处理框架。

2）camera-imu的时空标定

这里来介绍一下camera-imu的实际应用。收集一到两分钟的数据，时间为，这里有三个坐标系，分别为：世界坐标系用于状态估计，imu系下进行加速度和角速度估计，以及图像坐标系。假设设备在标签前运动，那么有如下几个估计量：世界坐标系下的重力加速度，imu到camera的外参，以及camera与imu时间戳之间的时间偏差d。

这里IMU的位姿是一个6维向量函数，包含转向姿态和平移向量，是随时间变化的状态量。使用B-spline函数来表示状态量，如下：

 

其中，表示转向姿态参数，表示从参数化函数到旋转矩阵的映射。表示位移，速度和加速度在世界坐标系中的表示如下：

 

那么角速度可以表示成：

 

其中表示旋转向量到旋转角的映射。

将IMU数据与相机数据结合得到如下模型：

 

其中和分别是时刻的加速度和角速度的测量值，是世界坐标系下的m个观测点，其对应图像上的像素坐标为，是图像时间戳，是需要估计的时间偏差，并假设相互独立，表示相机投影模型。

上述模型中误差项考虑了图像相对IMU数据的时间延迟d，这个时间延迟可正可负，假设IMU零偏满足随机游走，那么模型如下：

 

并假设彼此独立，即（由于高斯分布是独立性和不相干性等价，所以可以使用不相关性条件来表示）。

那么对上述的几个变量构建估计模型，根据测量值和估计值之间建立误差项，那么IMU的bias模型给出积分误差项，如下：

 

最后，使用LM算法(Levenberg-Marquardt)进行最小化下式即可：

 

这里给出整体的标定流程图：

 

其整个位姿标定的效果图如下：

 

    写在最后的话：惊奇的发现，公众号居然可以贴公式了。其实关于相机与IMU，最难的还是如何进行时间的同步。这里通过姿态的校正来同步两个传感器，1）通过视觉匹配完成回环检测与回环校正；2）修正IMU的累计漂移误差。

往期回顾

史上最全综述 | 3D目标检测算法汇总！（单目/双目/LiDAR/多模态/时序/半弱自监督）

视频课程来了！

自动驾驶之心为大家汇集了毫米波雷达视觉融合、高精地图、BEV感知、传感器标定、传感器部署、自动驾驶协同感知、语义分割、自动驾驶仿真、L4感知、决策规划、轨迹预测等多个方向学习视频，欢迎大家自取（扫码进入学习）

（扫码学习最新视频）

国内首个自动驾驶学习社区

近1000人的交流社区，和20+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、Occpuancy、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！

【自动驾驶之心】全栈技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向；

添加汽车人助理微信邀请入群

备注：学校/公司+方向+昵称