算法改进2：时间同步与同步误差估计

感谢：紫薯萝卜 https://zhuanlan.zhihu.com/p/77996744

 

1. 同步误差 定义

不同传感器之间的数据同步对融合算法至关重要，在VIO中，相机与IMU之间的数据同步对VIO精度影响非常大。如下图所示，假设相机和IMU在同一时刻（设为 ）采样，由于曝光、数据传输等需要时间，相机数据在经过 时间后才收到，IMU数据在经过 时间后收到，一般在收到传感器数据后才会打时间戳，所以相机和IMU数据的时间戳分别为 ，由于 与 的不一致，使得本来在同一时刻采集的相机和IMU数据被打上了不同的时间戳，从而造成了数据不同步，定义同步误差（也叫Time Delay） ，由于我们只关心两者之间的相对时间，所以只需要在图像时间戳上加上 来补偿掉图像与IMU之间的误差，IMU时间戳保持不变。

 

 

2. 估计理论推导

通常不是一个恒定的值，所以没法事先标定补偿，需要加入到状态向量中进行实时估计（Online Temporal Calibration）。加入 后最主要的改动就是State Augmentation：当图像在 时刻采集，需要将 时刻的相机状态加入到状态向量中，而原始MSCKF加入的是 时刻相机状态。先IMU预测到 时刻（而不是原来的 时刻），再根据外参计算 时刻相机位姿并扩展到状态向量：

最后扩展协方差矩阵：

是 对状态向量的Jacobian，这里只推导 对 的Jacobian ，其他项在之前的章节已经推导过了。（注意：推导的Jacobian和论文[1]中的不一致，这是因为[1]对旋转误差量使用的右乘，而这里统一使用的左乘）

推导过程如下：

上述推导最关键的那一步替换是旋转 和 与 之间的关系（旋转对时间的导数是角速度）即：

注意这里是坐标变换，只能用左乘。

 

3. 估计实测

总结下来，引入 估计后算法的改动如下：

1. 状态向量中添加1维状态 ，初值可以直接设为0，根据对实际系统的同步误差的大致估计，设一个初始协方差。
2. 补偿图像时间戳：每次接收到图像信息后，从估计器获取当前的 估计，将其加到图像时间戳上。这样后面IMU Propagation会自动积分到最新的图像时间戳 ，State Augmentation会自动扩增 时刻的相机状态，这两部分都不需要再做任何改动。
3. 设置 ：State Augmentation的Jacobian中填写新相机对 的Jacobian 。

估计看上去很复杂，其实代码改动量非常少。在EuROC上测试效果，EuROC的数据同步误差比较小，为了凸显改进效果，先人工将图像时间戳加10ms，可以看到 能随着运动渐渐收敛到-10ms。

 

 

参考文献

[1] Li M, Mourikis A I. Online temporal calibration for camera-IMU systems[M]. Sage Publications, Inc. 2014