IMU初始化 - 爱码网

IMU初始化是为了给局部BA和全局BA提供一个更好的初值从而减少IMU噪声积累。

IMU初始化分解为多个子问题：

估计陀螺仪偏置
忽略加速度计偏置，估计尺度和重力矢量
估计加速度计偏置，进一步优化尺度和重力
估计速度

1. 陀螺仪偏置估计

初始化过程假定 $b^g$ 保持不变，每帧的偏置恒定： $b_i^g=b^g$ 。优化 $b^g$ ,最小化所有相邻关键帧对之间的陀螺仪（旋转）预积分及从纯视觉ORB-SLAM2直接视觉计算的旋转之间的误差。
$^*b^g=\mathop{\arg\min}_{\theta} \sum_{k=1}^{N-1}\|Log((\Delta\tilde{R}_{k,k+1}Exp(J_{\Delta}^{g}b^g))^TR_{BW}^kR_{WB}^{k+1})\|^2$ 其中 $\Delta\tilde{R}_{k,k+1}=Exp((\tilde{\omega_k}-b^g)\Delta t)$ ,为陀螺仪的预积分值。N为关键帧的数量。 $R_{WB}^{(·)}=R_{WC}^{(·)}\times R_{CB}^{(·)}$ , $R_{WC}^{(·)}$ 为ORB-SLAM视觉计算的位姿， $R_{CB}^{(·)}$ 由标定得到。 $b^g$ 从0开始迭代。

2.尺度及重力估计

忽略加速度计偏置 $b_i^a=b^a=0$ ，由第一步得到的陀螺仪偏置，可以预积分速度及位移 $\Delta{v_{ij}},\Delta{p_{ij}}$ ：
$\Delta{v_{ij}}=\sum_{k=i}^{j-1}{\Delta R_{ik}\tilde{a}_k\Delta t} \\ \Delta{p_{ij}}=\sum_{k=i}^{j-1}{\Delta v_{ik} \Delta t+\frac{1}{2} \Delta R_{ik}\tilde{a}_k\Delta t^2} \\ \Delta{R_{ij}}=\prod_{k=i}^{j-1}{Exp((\tilde{w}_k-b^g)\Delta t)}$

视觉观测尺度不确定，有 $T_{WC}=\left[ \begin{array}{} R_{WC} & s_Wp_C\\ 0^T & 1 \end{array}\right ]$ ,因此，由camera系到body系的转化过程中，有：
$T_{WB}=\left[ \begin{array}{} R_{WB} & _Wp_B\\ 0^T & 1 \end{array}\right ]=T_{WC}T_{CB}=\left[ \begin{array}{} R_{WC} & s_Wp_C\\ 0^T & 1 \end{array}\right ]·\left[ \begin{array}{} R_{CB} & _Cp_B\\ 0^T & 1 \end{array}\right ]=\left[ \begin{array}{} R_{WC}R_{CB} & R_{WC}{_Cp_B}+{s_Wp_C}\\ 0^T & 1 \end{array}\right ]$ 其中 $T_{CB}$ 为标定得到，即 $_Wp_B=R_{WC}{_Cp_B}+{s_Wp_C}$ 代入位置关联方程：
$_Wp_{B}^{i+1}=_Wp_{B}^{i}+_Wv_{B}^{i}\Delta t_{i,i+1}+\frac12g_W\Delta t_{i,i+1}^2+R_{WB}^i\Delta p_{i,i+1}$ 得到： $s_Wp_{C}^{i+1}=s_Wp_{C}^{i}+_Wv_{B}^{i}\Delta t_{i,i+1}+\frac12g_W\Delta t_{i,i+1}^2+R_{WB}^i\Delta p_{i,i+1}+(R_{WC}^i-R_{WC}^{i+1}){_Cp_B}$ 其中， $_Wp_{C}^{i},R_{WC}^i,R_{WC}^{i+1},_Wp_{C}^{i+1}$ 由视觉计算得到， $\Delta t_{i,i+1}$ 已知， $\Delta p_{i,i+1}$ 由预积分得到, $R_{CB},_Cp_B$ 为标定得到。解该线性方程组得到 $s、g_W$ ，然而方程中含有 $_Wv_B^i$ ,为避免计算这N个速度，降低复杂度。我们将连续的三帧提取两个关系，利用速度关联方程： $_Wv_{B}^{i+1}=_Wv_{B}^{i}+g_W\Delta t_{i,i+1}+R_{WB}^i\Delta v_{i,i+1}$ 为简化表述，使用1，2，3代替 $i,i+1,i+2$ ，有：
$s_Wp_{C}^{3}=s_Wp_{C}^{2}+_Wv_{B}^{2}\Delta t_{23}+\frac12g_W\Delta t_{23}^2+R_{WB}^2\Delta p_{23}+(R_{WC}^2-R_{WC}^{3}){_Cp_B} \\ {s_Wp_{C}^{2}=s_Wp_{C}^{1}+_Wv_{B}^{1}\Delta t_{12}+\frac12g_W\Delta t_{12}^2+R_{WB}^1\Delta p_{12}+(R_{WC}^1-R_{WC}^{2}){_Cp_B}} \\ {_Wv_{B}^{2}=_Wv_{B}^{1}+g_W\Delta t_{12}+R_{WB}^1\Delta v_{12}}$ 第一式乘 $\Delta t_{12}$ 与第二式乘 $\Delta t_{23}$ 做差，结合代入第三式，得到：
$\left( \begin{array}{} \lambda(i)&\beta(i) \end{array}\right )\left( \begin{array}{} s\\g_W \end{array}\right )=\gamma(i)$

其中，

\lambda(i)

为3×1的矩阵，

\beta(i)

为3×3矩阵，

\left( \begin{array}{} s\\g_W \end{array}\right )

为4×1矩阵，

\gamma(i)

为3×1矩阵。写成

AX=B

的形式，

A_{3×4},X_{4×1},B_{3×1}

，四个未知数，一组关键帧（3帧）有三个方程，所以至少需要两个关键帧组（至少4帧）。若N个关键帧，有N-2组，则

A_{3(N-2)×4},X_{4×1},B_{3(N-2)×1}

,可以使用最小二乘或SVD求解。

3.加速度计偏置估计及尺度、重力修正

由于在之前的估计中忽略了加速度计偏置，导致我们计算的重力实际上也混杂了偏置的影响，与实际的重力方向有偏差。引入惯性系 $I$ 以及实际重力 $G$ ，在惯性系下实际重力的方向向量坐标为 $\hat{g}_I=(0,0,-1)$ ，我们估计的重力在世界系下的方向为 $\hat{g_W}=\frac{g_W^*}{\|g_W^*\|}$ .由此，我们可以计算惯性系到世界系的旋转矩阵：
$R_{WI}=Exp(\hat v\theta) \\ \hat v=\frac{\hat g_I×\hat{g}_W}{\|\hat g_I×\hat{g}_W\|} \ , \ \theta=atan2({\|\hat g_I×\hat{g}_W\|},\hat g_I·\hat{g}_W)$ 将实际重力惯性系坐标旋转到世界系中，即实际重力在世界系下的坐标为 $g_W=R_{WI}\hat{g}_IG$ 但是由于我们估计的重力与实际重力是有偏差的，所以旋转矩阵 $R_{WI}$ 并不是实际两坐标系的旋转矩阵，需要加一个修正量 $\delta\theta$ 。
另外由于惯性系中重力方向沿z方向，所以沿z轴的旋转是没有影响的。旋转轴在xy平面，只有沿x轴与y轴的旋转分量，没有z轴的旋转分量，即 $\delta\theta=(\delta\theta_x \ \delta\theta_y \ 0)^T$ ,综上，有： $g_W=R_{WI}Exp(\delta\theta)\hat{g}_IG \\ \delta\theta=(\delta\theta_{xy}^T \ 0)^T=(\delta\theta_x \ \delta\theta_y \ 0)^T$ 一阶近似 $g_W\simeq R_{WI}(I+\delta\theta^{\wedge})\hat{g}_IG=R_{WI}\hat{g}_IG-R_{WI}\hat{g}_I^{\wedge}G\delta\theta$ 除了引入重力修正，我们同时考虑加速度计偏置的影响。代入位置关联方程： $s_Wp_{C}^{i+1}=s_Wp_{C}^{i}+_Wv_{B}^{i}\Delta t_{i,i+1}-\frac12R_{WI}\hat{g}_I^{\wedge}G\Delta t_{i,i+1}^2\delta\theta+R_{WB}^i(\Delta p_{i,i+1}+J_{\Delta p}^ab^a)+(R_{WC}^i-R_{WC}^{i+1}){_Cp_B}+\frac12R_{WI}\hat{g}_IG\Delta t_{i,i+1}^2$ 这样，我们就引入了重力方向的修正和加速度计偏置。
同样地，我们考虑连续的三个连续帧，避免速度计算,并简化表述：
$s_Wp_{C}^{3}=s_Wp_{C}^{2}+_Wv_{B}^{2}\Delta t_{23}-\frac12R_{WI}\hat{g}_I^{\wedge}G\Delta t_{23}^2\delta\theta+R_{WB}^2(\Delta p_{23}+J_{\Delta p23}^ab^a)+(R_{WC}^2-R_{WC}^{3}){_Cp_B}+\frac12R_{WI}\hat{g}_IG\Delta t_{23}^2 \\ s_Wp_{C}^{2}=s_Wp_{C}^{1}+_Wv_{B}^{1}\Delta t_{12}-\frac12R_{WI}\hat{g}_I^{\wedge}G\Delta t_{12}^2\delta\theta+R_{WB}^1(\Delta p_{12}+J_{\Delta p12}^ab^a)+(R_{WC}^1-R_{WC}^{2}){_Cp_B}+\frac12R_{WI}\hat{g}_IG\Delta t_{12}^2 \\ {_Wv_{B}^{2}=_Wv_{B}^{1}+g_W\Delta t_{12}+R_{WB}^1(\Delta v_{12}+J_{\Delta v12}^ab^a)}$ sWpC3=sWpC2+WvB2Δt23−21RWIg^I∧GΔt232δθ+RWB2(Δp23+JΔp23aba)+(RWC2−RWC3)CpB+21RWIg^IGΔt232sWpC2=sWpC1+WvB1Δt12−21RWIg^I∧GΔt122δθ+RWB1(Δp12+JΔp12aba)+(RWC1−RWC2)CpB+21RWIg^IGΔt122WvB2=WvB1+gWΔt12+RWB1(Δv12+JΔv12aba) 得到

其中， $\lambda(i)$ 为3×1的矩阵， $\phi(i)$ 为3×2矩阵， $\zeta(i)$ 为3×3矩阵， $\left( \begin{array}{} s\\\delta\theta_{xy}\\b^a \end{array}\right )$ 为6×1矩阵， $\psi(i)$ 为3×1矩阵。写成 $AX=B$ 的形式， $A_{3×6},X_{6×1},B_{3×1}$ ，六个未知数，一组关键帧（3帧）有三个方程，所以至少需要两个关键帧组（至少4帧）。若N个关键帧，有N-2组，则 $A_{3(N-2)×6},X_{6×1},B_{3(N-2)×1}$ ,可以使用最小二乘或SVD求解。

4.估计速度

尺度，重力，陀螺仪、加速度计偏置已知，可以代入原位置关联方程求解速度
$s_Wp_{C}^{i+1}=s_Wp_{C}^{i}+_Wv_{B}^{i}\Delta t_{i,i+1}-\frac12R_{WI}\hat{g}_I^{\wedge}G\Delta t_{i,i+1}^2\delta\theta+R_{WB}^i(\Delta p_{i,i+1}+J_{\Delta p}^ab^a)+(R_{WC}^i-R_{WC}^{i+1}){_Cp_B}+\frac12R_{WI}\hat{g}_IG\Delta t_{i,i+1}^2$ $s_Wp_{C}^{i+1}=s_Wp_{C}^{i}+_Wv_{B}^{i}\Delta t_{i,i+1}+\frac12g_W\Delta t_{i,i+1}^2+R_{WB}^i\Delta p_{i,i+1}+(R_{WC}^i-R_{WC}^{i+1}){_Cp_B}$ 求解邻近关键帧速度，使用速度关联方程： $_Wv_B^{i+1}=_Wv_B^{i}+g_W\Delta t_{i,i+1}+R_{WB}^{i}(\Delta v_{i,i+1}+J_{\Delta v}^gb^g+J_{\Delta v}^ab^a)$

5.重定位后偏置重新初始化

当系统运行一段时间后根据位置辨识重定位，需要对偏置进行重新初始化。陀螺仪偏置仍然是求解最优旋转误差方程，加速度偏置通过求解简化的3中的方程组，其中s和 $g_W$ 已知。