具身感知的数据基本预处理｜惯视跨模态时序对齐系列（一）

1. 引言

具身感知指的是智能体以第一视角观察世界，以交互的方式感知学习。而第一视角所具备的最独特信息之一，就是时序运动信息。将运动信息与视觉感知信息跨模态融合，才能得到第一视角下真实的物理世界感知。然而，就算人脑获得视觉的处理信息也会有秒级别的延迟，导致与运动产生不一致而头晕目眩。在现实世界中采集视觉和惯性传感器数据，也难免会有时序上随机的延迟噪声。这些延迟噪声的影响会在高动态的运动下放大，造成视觉感知和惯性运动模态的不一致，在数据层面上就有了误差。因此，惯视跨模态的时序对齐在具身感知的数据预处理上至关重要。而惯视系统的时序对齐在里程计中的研究已经有了良好的基础，本文将根据惯视里程计中的时序对齐进行总结和讨论。

2. 时序延迟噪声的特征

2.1 随机性

对于视觉惯性数据采集设备，由于相机与惯性传感器（IMU）内部的晶振频率有细微扰动差异，以及相机的采集流程需要曝光，数据传输需要时间，因此实际相机的采样时刻和IMU的采样时刻存在一个随机的时间偏移，如下式所示：

$$t_{cam}=t_{imu}+t_d$$

对于时间偏移的分布，一部分是由相机与IMU晶振的频差所决定的固定偏移量，另一部分为随机的数据传输实验噪声。

2.3 累计性

时间偏差除了存在随机性，其还存在累计特性。时间偏差会随着系统运行时间的偏移而不断累积，这是由于传感器的下一个采样时刻间隔高度依赖上一时刻采样结束的时刻，这就造成了时间偏移分布的不确定性。一个实际系统的时间偏移如下图所示：

图1｜实际系统中传感器间采样时间偏移误差©️【深蓝AI】

因此，离线的时间偏差标定只能够给系统提供一个良好的初始值，实际视惯系统的采样时间偏差一般都需要在线标定。

图2｜硬件触发时刻示意图©️【深蓝AI】

对于具有硬件触发的系统而言，同步触发能够消除一部分累计误差，但是随着系统运行时间的增长，随机噪声的干扰也会对系统造成累计性的影响。

3. 时序对齐的基本思想

视惯系统对齐的思路直观上分为两类，一类是将运动感知向视觉特征的观测对齐，另一类是将视觉观测的特征向运动感知对齐，下面分别对其进行介绍。

3.1 运动感知向视觉特征对齐

这种方法实际上就是利用当前系统所估计的角速度和线速度过程进行时间偏移参量$t_d$的估计，在原时刻的基础上再以匀速状态向前积分$t_d$时间段，导出$t_d$延迟后的特征观测状态或位姿状态，如下两式所示。

$${}^C{\mathbf{p}}_{f_i}\left(t+t_d\right)={}_I^C\mathbf{R}{}_G^I\mathbf{R}\left(t+t_d\right)\left({}^G{\mathbf{p}}_{f_i}-{}^G{\mathbf{p}}_I\left(t+t_d\right)\right)+{}^C{\mathbf{p}}_I$$

$${\hat{\mathbf{c}}}_{\text{new }}=\left[\begin{array}{c}{}_G^C\widehat{\mathbf{q}\left(t+t_d\right)}\\ {}_G{\mathbf{p}}_C\left(t+t_d\right)\end{array}\right]=\left[\begin{array}{c}{}_I^C\hat{\mathbf{q}}\otimes {}_G^I\widehat{\hat{\mathbf{q}}}\left(t+{\hat{t}}_d\right)\\ {}^G{\hat{\mathbf{p}}}_I\left(t+{\hat{t}}_d\right)+{}_G^I\hat{\mathbf{R}}{\left(t+{\hat{t}}_d\right)}^{TI}{\hat{\mathbf{p}}}_C\end{array}\right]$$

除了假定匀速状态向前积分，还有将运动轨迹用多项式拟合的方法，使得估计状态在所拟合的状态流形上移动$t_d$时间段，从而将运动观测对齐至视觉观测特征。

图3｜运动感知向视觉特征对齐示意图©️【深蓝AI】

然而，这类方法基本上都依赖较为精准的先验状态信息，尤其是角速度和线速度的估计。在高动态系统中，很难维持良好的先验状态初值。对于整个系统而言，一旦角速度和线速度的估计出现了突变，那么对于时间偏移$t_d$的观测就会失效。因此，这类方法在系统稳定时效果很好，但在无法时刻保持稳定的高动态运动系统中，难以维持有效的时间偏移估计。

3.2 视觉特征向运动感知对齐

该方法通过计算视觉特征在平面内的移动速度，将视觉特征向运动感知的观测时刻上移动，如下图4所示。

图4｜视觉特征向运动感知对齐示意图©️【深蓝AI】

对于观测到的特征，将其以特征平面速度移动$t_d$段时间，使得视觉观测特征与运动感知的状态对齐，模型如下公式所示：

$$\begin{array}{rl}& {\mathbf{e}}_l^k={\mathbf{z}}_l^k\left(t_d\right)-\pi \left({\mathbf{R}}_{c_k}^{w^T}\left({\mathbf{P}}_l-{\mathbf{p}}_{c_k}^w\right)\right)\\ & {\mathbf{z}}_l^k\left(t_d\right)={\left[\begin{array}{ll}{u}_l^k& v_l^k\end{array}\right]}^T+t_d{\mathbf{V}}_l^k.\end{array}$$

这类方法对于时间偏移估计方法的核心点就在于对特征速度的计算，这里的特征速度通常依赖特征的连续稳定观测。然而，在高动态运动过程中，特征更新是非常迅速的，而很多特征点都无法进行长期稳定的观测，这就给此类方法提出了挑战。

4. 时序对齐的代表方法

4.1 VINS-Mono中的时序对齐

VINS-Mono中采用的是视觉特征向运动感知对齐的策略，其特征速度计算的方式采用的是匀速模型。在实际代码实现中，其计算方式如下式：

$${\mathbf{V}}_l^k=\left(\left[\begin{array}{l}{u}_l^{k+1}\\ v_l^{k+1}\end{array}\right]-\left[\begin{array}{l}{u}_l^k\\ v_l^k\end{array}\right]\right)/\left(t_{k+1}-t_k\right)$$

在验证过程中，VINS-Mono的验证方式均为恒定时间偏移。这与人们实时自采数据时间偏移情况略有出入。但在恒定时间偏移或者短程低速的实际场景中性能还是比较出色的，但对于实际运行较长、具有实际累计噪声的视惯系统中，没有进行验证。在Euroc中人为设定的恒定时间偏移的估计结果如下图5所示。

图5｜对与恒定时间偏移VINS-Mono中时间偏移估计Euroc数据集的结果。©️【深蓝AI】

4.2 OpenVINS中的时序对齐

OpenVINS中采用的是运动感知向视觉特征对齐的策略，实现方法为将时间偏移考虑进IMU的预积分过程中，其雅可比矩阵如下所示。

$${\mathbf{J}}_t=\left[\begin{array}{c}{{}_G^I{\hat{\mathbf{R}}}^T\left(t+{\hat{t}}_d\right))}^I\hat{\omega }\left(t+{\hat{t}}_d\right)\\ {}_G^I\hat{\mathbf{R}}{\left(t+{\hat{t}}_d\right)}^T{\lfloor {}^I\hat{\omega }\left(t+{\hat{t}}_d\right)\times \rfloor }^I{\hat{\mathbf{p}}}_C+{}^G{\hat{\mathbf{v}}}_I\left(t+{\hat{t}}_d\right)\end{array}\right]$$

OpenVINS中的时序对齐方法性能并没有系统在各个数据集上的公开测试结果，笔者会在后续的技术分享中分析得到的评价结果。

5. 总结与思考

惯视跨模态时序的对齐对于视惯系统的具身感知十分重要，通常对于时间偏移的估计会在里程计中在线进行。现有方法已经对恒定时间偏移以及低速短程估计问题有了比较完备的解决，但需要对于实际系统的时变偏移以及高动态运动的情况继续进行研究，以消除时间偏移误差对系统估计精度所造成的影响。

引用：

【1】https://arxiv.org/abs/1808.00692

【2】https://pgeneva.com/downloads/papers/Geneva2020ICRA.pdf

【3】https://intra.ece.ucr.edu/~mourikis/papers/Li2014IJRR_timing.pdf

笔者｜Frank

审核｜Los

移步公众号【深蓝AI】，第一时间获取自动驾驶、人工智能与机器人行业最新最前沿论文和科技动态。