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维状态 ,初值可以直接设为0,根据对实际系统的同步误差的大致估计,设一个初始协方差。
- 补偿图像时间戳:每次接收到图像信息后,从估计器获取当前的 估计,将其加到图像时间戳上。这样后面IMU Propagation会自动积分到最新的图像时间戳 ,State Augmentation会自动扩增 时刻的相机状态,这两部分都不需要再做任何改动。
- 设置 :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