多传感器时间同步方法
多传感器时间同步方案
硬件方案
举个例子
- 开机fpga便依靠自身的晶振时钟对imu和视觉进行分频触发，保证了imu与视觉的时间同步；
- 1pps信号输入fpga时间同步板；
- 当1pps信号出现时，a. fpga会进入中断记录此时自身的时间戳；b. 从gnss接收机获得当前1pps触发时的gnss时间戳;c. 得到fpga与gnss的时间差，完成gnss与vio的时间同步；
此处需要强调gnss接收机发出的1pps信号是一个整秒的脉冲，因此在获得一次gnss定位解算时同时获得当前解算时间戳之后便可以推得整秒脉冲的时间戳。
软件同步方案
openvins中使用的imu与相机的时间偏移估计方法
在相机时间戳t时刻观测到的第i个特征的像素坐标，是特征投影到imu时间戳\(t+t_d\)时刻的成像平面上的。即
\[ \begin{align} z_i(t) &= h({}^C p_{f_i}(t+t_d)) \\ &= \frac{1}{ {}^{C} {z_i}(t+t_d) } \left[\begin{array}{cc}{}^Cx_i(t+t_d)\\{}^Cy_i(t+t_d) \end{array}\right]+n_i(t+t_d) \end{align} \]
其中\(h(.)\)归一化投影函数，\({}^Cp_{f_i}(t+t_d)\)是在相机时间戳\(t+t_d\)时刻特征在相机坐标系下的三维坐标。 \[ {}^Cp_{f_i}(t+t_d) = {}_I^CR\ {}_G^IR(t+t_d)({}^Gp_{f_i}-{}^Gp_I(t+t_d))+{}^Cp_I \] 重投影误差为： \[ r_i = z_{mi}(t) - h({}^Cp_{f_i}(t+t_d)) \]
如上，在读入一个时间戳t的图像后，为了计算重投影残差，我们需要将imu状态积分传播到\(t+t_d\)时刻。
重投影误差的雅克比为： \[ H_{x,i}(t+t_d) = [H_{\theta,i}, H_{p,i}, 0_{2\times 9}, H_{\phi,i}, H_{p_c,i}, H_{t_d,i}] \] 其中非零项分别是重投影残差对imu姿态、imu位置、相机到imu的外参姿态，相机到imu的外参旋转，时间偏移的雅克比。其中只有最后一项是此次需要重点介绍的。其为 \[ H_{t_d,i} = H_{\theta,i} {}_G^I R^T(t+t_d)w(t+t_d) + H_{p,i} {}^G v_I(t+t_d) \] 使用链式法则，其中位姿关于时间的导数是速度！
这种方法是一值在系统维护着一个\(t_d\)，下面会介绍一种逐渐修正式的方法，来自vins-mono
vins-mono中使用的时间偏移估计方法
vins-mono使用了另外一种思路来进行时间偏移估计。(注意下面使用vins论文中的公式，所以与上面openvins中可能稍有不同)
首先时间偏移的定义依旧是 \[ t_{IMU} = t_{cam} + t_d \] 本文相对于移动整个相机或IMU序列，我们选择在时间线上移动特征的观察点，效率更高，也更容易推导雅克比。为此引入了特征速度，用于建模和补偿时间偏移。在很短的时间内（几毫秒），相机运动可以被视为匀速运动。因此，特征在短时间内在图像平面上以近似恒定速度移动。特征点在像素平面上的运动速度为 \[ V_l^k = (\left[\begin{array}{cc}u_l^{k+1}\\v_l^{k+1} \end{array}\right]- \left[\begin{array}{cc}u_l^{k}\\v_l^{k} \end{array}\right])/(t_{k+1}-t_k) \]
- 重投影误差定义为：
\[ e_l^k = z_l^k(t_d) - \pi ({ {R}_{c_k}^w}^T \ (P_l-p_{c_k}^w)) \]
其中\(P_l\)表示第l个特征在世界坐标系中的位置，w表示world，c表示camera，\(\pi\)是投影函数。其中 \[ z_l^k(td) = \left[\begin{array}{cc}u_l^{k}\\v_l^{k} \end{array}\right] + t_d V_l^k \]
- 使用\(t_d\)参数化特征（深度参数化表示）位置之后的重投影误差定义
\[ e_l^j = z_l^j(t_d) - \pi ({R_{c_i}^w}^T(R_{c_j}^w \lambda_i \left[\begin{array}{cc}z_l^i(t_d)\\1 \end{array}\right]+p_{c_i}^w-p_{c_j}^w)) \]
上述\(e_l^j\)表示第l个特征在第j帧上的重投影误差，可以发现第l个特征的anchored帧是第i帧。其中在第i帧与第j帧上特征的归一化坐标位置为 \[ z_l^i(td) = \left[\begin{array}{cc}u_l^{i}\\v_l^{i} \end{array}\right] + t_d V_l^i \] \[ z_l^j(td) = \left[\begin{array}{cc}u_l^{j}\\v_l^{j} \end{array}\right] + t_d V_l^j \] 其中残差关于\(t_d\)的雅克比求导十分简单（线性的），此处就不继续讨论了。
每次优化后，移动后续视觉流的时间戳来补偿时间偏移。然后系统估计后续补偿过的视觉测量和惯性测量之间的时间偏移\(δt_d\) ，然后再补偿再估计，最后趋向0。随着δt_d 越来越趋向0，论文基本假设：特征在短时间内以恒定速度在图像平面上移动。越来越合理。即使开始存在巨大时间偏移，该过程也将逐渐从粗略到精细的修正补偿它。