VINS-Mono理论推导1

Reference

  1. VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator.
  2. Quaternion Kinematics for Error-State KF.

 

预积分的推导

预积分是VINS-Mono中十分重要的一个步骤,主要涉及的是IMU的运动方程,下面对这部分进行一个简单的推导,主要使用的是ESKF中的方法,采用简单的欧拉法在连续时域下进行推导。


ESKF

这部分详细可以参照参考【2】,这里做一个记录,ESKF的主要精髓在于分析的是error-state,并且获得的是error-state的状态更新方程,但是对于优化问题,优化的变量其实并不是error-state,而是state,那为什么会有这个理论的诞生,这里先埋个坑(坑1),后面会解释。

  1. ESKF有两个变量,一个是normal-state X X X,一个是truth-state X t X_t Xt,两者的关系如下:
    X t = [ p t v t q t a b t ω b t ] = [ p + δ p v + δ v q ⊗ δ q a b + δ a b ω b + δ ω b ] = X ⊞ δ X (1) \mathbf{X_t}=\left[\begin{array}{c} \mathbf{p_t} \\ \mathbf{v_t} \\ \mathbf{q_t} \\ \mathbf{a}_{bt} \\ \boldsymbol{\omega}_{bt} \end{array}\right]= \left[\begin{array}{c} \mathbf{p}+\delta{p} \\ \mathbf{v}+\delta{v} \\ \mathbf{q}\otimes\delta{q} \\ \mathbf{a}_{b}+\delta{a_b} \\ \boldsymbol{\omega}_{b}+\delta{\omega_b} \end{array}\right]=\mathbf{X}\boxplus\mathbf{\delta{X}} \tag{1} Xt=ptvtqtabtωbt=p+δpv+δvqδqab+δabωb+δωb=XδX(1)

  2. 进一步,通过对两边求微分得到:
    δ p ˙ = δ v δ v ˙ = − R [ a m − a b ] × δ θ − R δ a b − R a n δ θ ˙ = − [ ω m − ω b ] × δ θ − δ ω b − ω n δ a b ˙ = a w δ ω ˙ b = ω w (2) \begin{aligned} \dot{\delta \mathbf{p}} &=\delta \mathbf{v} \\ \dot{\delta \mathbf{v}} &=-\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\mathbf{R} \delta \mathbf{a}_{b}-\mathbf{R} \mathbf{a}_{n} \\ \dot{\delta \boldsymbol{\theta}} &=-\left[\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\delta \boldsymbol{\omega}_{b}-\boldsymbol{\omega}_{n} \\ \dot{\delta \mathbf{a}_{b}} &=\mathbf{a}_{w} \\ \delta \dot{\boldsymbol{\omega}}_{b} &=\boldsymbol{\omega}_{w} \end{aligned} \tag{2} δp˙δv˙δθ˙δab˙δω˙b=δv=R[amab]×δθRδabRan=[ωmωb]×δθδωbωn=aw=ωw(2)

  3. 对公式2进行离散化
    δ p ← δ p + δ v Δ t δ v ← δ v + ( − R [ a m − a b ] × δ θ − R δ a b ) Δ t + v i δ θ ← I − [ ω m − ω b ] × Δ t δ θ − δ ω b Δ t + θ i δ a b ← δ a b + a i δ ω b ← δ ω b + ω i (3) \begin{aligned} \delta \mathbf{p} & \leftarrow \delta \mathbf{p}+\delta \mathbf{v} \Delta t \\ \delta \mathbf{v} & \leftarrow \delta \mathbf{v}+\left(-\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\mathbf{R} \delta \mathbf{a}_{b}\right) \Delta t+\mathbf{v}_{\mathbf{i}} \\ \delta \boldsymbol{\theta} & \leftarrow I-\left[\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right]_{\times} \Delta t \delta \boldsymbol{\theta}-\delta \boldsymbol{\omega}_{b} \Delta t+\boldsymbol{\theta}_{\mathbf{i}} \\ \delta \mathbf{a}_{b} & \leftarrow \delta \mathbf{a}_{b}+\mathbf{a}_{\mathbf{i}} \\ \delta \boldsymbol{\omega}_{b} & \leftarrow \delta \boldsymbol{\omega}_{b}+\boldsymbol{\omega}_{\mathbf{i}} \end{aligned} \tag{3} δpδvδθδabδωbδp+δvΔtδv+(R[amab]×δθRδab)Δt+viI[ωmωb]×ΔtδθδωbΔt+θiδab+aiδωb+ωi(3)
    其中:

    • R R R表示从机体系到世界坐标系的旋转;
    • a m a_m am表示IMU的测量减去重力加速度的值;
  4. 将公式3写作矩阵形式,得到kalman滤波的预测阶段:
    δ ^ x ← F x ( x , u m ) ⋅ δ ^ x P ← F x P F x ⊤ + F i Q i F i ⊤ (4) \begin{array}{l} \hat{\delta} \boldsymbol{\mathbf { x }} \leftarrow \mathbf{F}_{\mathbf{x}}\left(\mathbf{x}, \mathbf{u}_{m}\right) \cdot \hat{\boldsymbol{\delta}} \mathbf{x} \\ \mathbf{P} \leftarrow \mathbf{F}_{\mathbf{x}} \mathbf{P} \mathbf{F}_{\mathbf{x}}^{\top}+\mathbf{F}_{\mathbf{i}} \mathbf{Q}_{\mathbf{i}} \mathbf{F}_{\mathbf{i}}^{\top} \end{array} \tag{4} δ^xFx(x,um)δ^xPFxPFx+FiQiFi(4)
    其中:

    • F x = ∂ f ∂ δ x ∣ x , u m = [ I I Δ t 0 0 0 0 0 I − R [ a m − a b ] × Δ t − R Δ t 0 I Δ t 0 0 I − [ ω m − ω b ] × Δ t 0 − I Δ t 0 0 0 0 I 0 0 0 0 0 0 I 0 0 0 0 0 0 I ] \mathbf{F}_{\mathbf{x}}=\left.\frac{\partial f}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}, \mathbf{u}_{m}}=\left[\begin{array}{ccccc} \mathbf{I} & \mathbf{I} \Delta t & 0 & 0 & 0 & 0 \\ 0 & \mathbf{I} & -\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \Delta t & -\mathbf{R} \Delta t & 0 & \mathbf{I} \Delta t \\ 0 & 0 & I-\left[\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right]_{\times} \Delta t & 0 & -\mathbf{I} \Delta t & 0 \\ 0 & 0 & 0 & \mathbf{I} & 0 & 0 \\ 0 & 0 & 0 & 0 & \mathbf{I} & 0 \\ 0 & 0 & 0 & 0 & 0 & \mathbf{I} \end{array}\right] Fx=δxfx,um=I00000IΔtI00000R[amab]×ΔtI[ωmωb]×Δt0000RΔt0I0000IΔt0I00IΔt000I

    • F i = ∂ f ∂ i ∣ x , u m = [ 0 0 0 0 I 0 0 0 0 I 0 0 0 0 I 0 0 0 0 I 0 0 0 0 ] \mathbf{F}_{\mathbf{i}}=\left.\frac{\partial f}{\partial \mathbf{i}}\right|_{\mathbf{x}, \mathbf{u}_{m}}=\left[\begin{array}{cccc} 0 & 0 & 0 & 0 \\ \mathbf{I} & 0 & 0 & 0 \\ 0 & \mathbf{I} & 0 & 0 \\ 0 & 0 & \mathbf{I} & 0 \\ 0 & 0 & 0 & \mathbf{I} \\ 0 & 0 & 0 & 0 \end{array}\right] Fi=ifx,um=0I000000I000000I000000I0

    • Q i = [ V i 0 0 0 0 Θ i 0 0 0 0 A i 0 0 0 0 Ω i ] \mathbf{Q}_{\mathbf{i}}=\left[\begin{array}{cccc} \mathbf{V}_{\mathbf{i}} & 0 & 0 & 0 \\ 0 & \mathbf{\Theta}_{\mathbf{i}} & 0 & 0 \\ 0 & 0 & \mathbf{A}_{\mathbf{i}} & 0 \\ 0 & 0 & 0 & \Omega_{\mathbf{i}} \end{array}\right] Qi=Vi0000Θi0000Ai0000Ωi


积分与预积分

积分比较简单,拿到IMU的数据进行常规的积分操作,就可以得到两节点之间的相对位姿和速度。这里祭出IMU的运动模型:
{ p ˙ = v v ˙ = R i k i 0 ( a m − b a − n a ) q ˙ = q ⊗ q { w m − b w − n w } b a ˙ = w a b w ˙ = w w (5) \begin{cases} \dot{p}=v \\ \dot{v}=R_{i_k}^{i_0}(a_m-b_a-n_a) \\ \dot{q}=q\otimes q\{w_m-b_w-n_w\} \\ \dot{b_a}=w_a \\ \dot{b_w}=w_w \end{cases} \tag{5} p˙=vv˙=Riki0(ambana)q˙=qq{wmbwnw}ba˙=wabw˙=ww(5)
可以看到,在进行积分的时候,公式中不仅使用了各个时刻的IMU测量值,而且用到零偏值,因此当零偏值变化的时候,整个微分数值就产生了变化,于是最后的相对状态值就会产生变化,此时一种方法是将修改后的零偏值带入到积分过程中得到新的相对位姿和速度。

重新带入的方法是可行的,但是这样会比较浪费时间和算力。于是预积分的方法就提出来了。

承接上面的ESKF,把正常的积分过程看做是normal-state,这部分积分仅与这段时间的IMU测量值以及零偏bias有关,与其他量都无关,当零偏bias被优化之后,此时并不用修改normal-state的量,而是认为变化的这部分在error-state中,下面有个简略的示意图:

VINS-Mono理论推导1_第1张图片
图中X表示使用IMU数据进行积分的过程,IMU认为在这段时间内,坐标系的变化为从 I k I_k Ik I k + 1 I_{k+1} Ik+1,而当状态变量进行了优化,零偏变化 δ b \delta b δb时,积分过程需要加上 Δ X \Delta X ΔX,即假如零偏变化了 δ b \delta b δb,那么IMU认为坐标系的变化应该到 I k + 1 ′ I^{'}_{k+1} Ik+1,所以剩下就是求解上图中的黄色部分的变化了,这里采用一阶近似来表示这个变化,推导如下:
{ p n t = p n + ∂ p ∂ b δ b = p n + p + δ p − p δ b δ b = p n + δ p δ b δ b v n t = v n + ∂ v ∂ b δ b = v n + v + δ v − v δ b δ b = v n + δ v δ b δ b q n t = q n ⊗ [ 1 1 2 ∂ θ ∂ b δ b ] = q n ⊗ [ 1 1 2 ∂ θ ∂ b δ b ] = q n ⊗ [ 1 1 2 θ + δ θ − θ δ b δ b ] = q n ⊗ [ 1 1 2 δ θ δ b δ b ] (6) \begin{cases} p_{nt}=p_{n}+\frac{\partial{p}}{\partial{b}}\delta{b}=p_{n}+\frac{p+\delta{p}-p}{\delta{b}}\delta{b}=p_{n}+\frac{\delta{p}}{\delta{b}}\delta{b} \\ v_{nt}=v_{n}+\frac{\partial{v}}{\partial{b}}\delta{b}=v_{n}+\frac{v+\delta{v}-v}{\delta{b}}\delta{b}=v_{n}+\frac{\delta{v}}{\delta{b}}\delta{b} \\ q_{nt}=q_{n}\otimes\begin{bmatrix}1\\ \frac{1}{2}\frac{\partial{\theta}}{\partial{b}}\delta{b} \end{bmatrix}=q_{n}\otimes\begin{bmatrix}1\\ \frac{1}{2}\frac{\partial{\theta}}{\partial{b}}\delta{b} \end{bmatrix}=q_{n}\otimes\begin{bmatrix}1\\ \frac{1}{2}\frac{\theta+\delta{\theta}-\theta}{\delta{b}}\delta{b} \end{bmatrix}=q_{n}\otimes\begin{bmatrix}1\\ \frac{1}{2}\frac{\delta{\theta}}{\delta{b}}\delta{b} \end{bmatrix} \end{cases} \tag{6} pnt=pn+bpδb=pn+δbp+δppδb=pn+δbδpδbvnt=vn+bvδb=vn+δbv+δvvδb=vn+δbδvδbqnt=qn[121bθδb]=qn[121bθδb]=qn[121δbθ+δθθδb]=qn[121δbδθδb](6)

其中需要说明的几个点为:

  • 偏导这里采用了求导的原始定义;
  • 相对姿态使用四元数进行表示,这里使用旋转向量来表示误差量,这样的好处就是能在 R 3 \R^3 R3空间上直接进行向量的加减操作,十分方便;

剩下的就是如何求解上述推导中的 δ p δ b , δ v δ b , δ q δ b \frac{\delta{p}}{\delta{b}},\frac{\delta{v}}{\delta{b}},\frac{\delta{q}}{\delta{b}} δbδp,δbδv,δbδq了,特别注意的这里的三个导数和公式4中的变量并不一样,因为公式(4)是两个IMU周期之间的递推公式,而公式(6)是两个相机周期(一般都是N倍于IMU周期)之间的递推公式,那这个数值是多少呢?这里其实是一个连乘的操作,如下:
δ p n δ b = F n − 1 δ p n − 1 δ b = F n − 1 F n − 2 δ p n − 2 δ b = ∏ i = 0 n − 1 F i δ p 0 δ b = ∏ i = 0 n − 1 F i  where  δ p 0 δ b = I \frac{\delta{p_n}}{\delta{b}}=\frac{F_{n-1}\delta{p_{n-1}}}{\delta{b}}=\frac{F_{n-1}F_{n-2}\delta{p_{n-2}}}{\delta{b}}=\frac{\prod_{i=0}^{n-1}F_{i}\delta{p_0}}{\delta{b}}=\prod_{i=0}^{n-1}F_{i} \text{ where } \frac{\delta{p_{0}}}{\delta{b}}=I δbδpn=δbFn1δpn1=δbFn1Fn2δpn2=δbi=0n1Fiδp0=i=0n1Fi where δbδp0=I
所以为什么要用ESKF也就很明显了,同时使用ESKF进行error-state状态的更新还有另一个好处就是能够得到这次观测的协方差,如何得到呢?其实也很简单,使用协方差的原始定义就好啦,这里以位置为例:
P p = E ( ( p t − p i n t ) T ( p t − p i n t ) ) = E ( δ p T δ p ) = P δ p (7) P_p=E((p_t-p_{int})^T(p_t-p_{int}))=E(\delta{p}^T\delta{p})=P_{\delta{p}} \tag{7} Pp=E((ptpint)T(ptpint))=E(δpTδp)=Pδp(7)


VINS中的中点法(mid-point)

在VINS-Mono的代码中,作者使用的是中点法得到上面的公式4的,其实和参考【2】中的中点法不太一样,这里的中点法使用的是两端点的平均,下面进行简单的推导,先给出离散时间下的迭代步骤:
{ p k + 1 = p k + v k Δ t + 1 2 R k ( a k − b a k ) + R k + 1 ( a k + 1 − b a k ) 2 Δ t 2 v k + 1 = v k + R k ( a k − b k ) + R k + 1 ( a k + 1 − b k + 1 ) 2 Δ t q k + 1 = q k ⊗ q { ( ( w k + w k + 1 ) 2 − b w k ) Δ t } b a k + 1 = b a k + n b a Δ t b w k + 1 = b w k + n b w Δ t (8) \begin{cases} p_{k+1}=p_{k}+v_{k}\Delta{t}+\frac{1}{2}\frac{R_k(a_k-b_{ak})+R_{k+1}(a_{k+1}-b_{ak})}{2}\Delta{t}^2 \\ v_{k+1}=v_k+\frac{R_k(a_k-b_k)+R_{k+1}(a_{k+1}-b_{k+1})}{2}\Delta{t}\\ q_{k+1}=q_{k}\otimes q\{(\frac{(w_k+w_{k+1})}{2}-b_{wk})\Delta{t}\}\\ b_{ak+1}=b_{ak}+n_{ba}\Delta{t} \\ b_{wk+1}=b_{wk}+n_{bw}\Delta{t} \end{cases} \tag{8} pk+1=pk+vkΔt+212Rk(akbak)+Rk+1(ak+1bak)Δt2vk+1=vk+2Rk(akbk)+Rk+1(ak+1bk+1)Δtqk+1=qkq{(2(wk+wk+1)bwk)Δt}bak+1=bak+nbaΔtbwk+1=bwk+nbwΔt(8)

整个过程中稍微有些奇怪的是VINS在表示陀螺仪的真值的时候,使用的是 w t = w m − b w − δ b w + n w w_t=w_m-b_w-\delta{b_w}+n_w wt=wmbwδbw+nw,不过因为噪声是高斯白噪声,加和减是一样的,所以这里按这样推导也无妨。

旋转的error-state

这里先推导旋转的误差项是因为后面的位移和速度会用到这部分结论,整体来说和上面的ESKF一样,使用truth-state=normal-state+error-state的方法进行推导,这部分沿用微分的方式:
q t ˙ = ( q ⊗ δ q ) ˙ = 1 2 q t ⊗ Ω t = q ˙ ⊗ δ q + q ⊗ δ q ˙ = 1 2 q t ⊗ Ω t = 1 2 q ⊗ Ω ⊗ δ q + q ⊗ δ q ˙ = 1 2 q ⊗ δ q ⊗ Ω t = [ Ω ] L δ q + 2 δ q ˙ = [ Ω t ] R δ q = 2 δ q ˙ = ( [ Ω t ] R − [ Ω ] L ) δ q (9) \begin{aligned} \dot{q_{t}}&=\dot{(q\otimes \delta{q})}=\frac{1}{2}q_t\otimes \Omega_t \\ &=\dot{q}\otimes \delta{q}+q \otimes \dot{\delta{q}}=\frac{1}{2}q_t\otimes \Omega_t \\ &=\frac{1}{2}q \otimes \Omega \otimes \delta{q}+q\otimes \dot{\delta{q}}=\frac{1}{2}q\otimes\delta{q}\otimes \Omega_t \\ &=[\Omega]_L\delta{q}+2\dot{\delta{q}} = [\Omega_t]_R \delta{q} \\ &=2\dot{\delta{q}} = ([\Omega_t]_R-[\Omega]_L)\delta{q} \end{aligned} \tag{9} qt˙=(qδq)˙=21qtΩt=q˙δq+qδq˙=21qtΩt=21qΩδq+qδq˙=21qδqΩt=[Ω]Lδq+2δq˙=[Ωt]Rδq=2δq˙=([Ωt]R[Ω]L)δq(9)

下面对其中的一些符号进行说明:

  • δ q = [ 1 1 2 δ θ ] \delta{q}=\begin{bmatrix} 1 \\ \frac{1}{2}\delta{\theta} \end{bmatrix} δq=[121δθ]

  • [ Ω ] R = [ 0 − Ω T Ω − [ Ω ] × ] [ Ω ] L = [ 0 − Ω T Ω [ Ω ] × ] \begin{aligned} [\Omega]_R&= \begin{bmatrix}0 & -\Omega^T \\ \Omega & -[\Omega]_{\times} \end{bmatrix} \\ [\Omega]_L&= \begin{bmatrix}0 & -\Omega^T \\ \Omega & [\Omega]_{\times} \end{bmatrix} \end{aligned} [Ω]R[Ω]L=[0ΩΩT[Ω]×]=[0ΩΩT[Ω]×]

  • Ω = w m k + w m k + 1 2 − b w k Ω t = w m k + n w k + w m k + 1 + n w k + 1 2 − b w k − δ b w k = Ω − δ b w k + 1 2 n w k + 1 2 n w k + 1 \begin{aligned} \Omega &=\frac{w^k_m+w^{k+1}_m}{2}-b^k_w \\ \Omega_t&=\frac{w^k_m+n^k_w+w^{k+1}_m+n^{k+1}_{w}}{2}-b^k_w-\delta{b^k_w}=\Omega-\delta{b^k_w}+\frac{1}{2}n^{k}_{w}+\frac{1}{2}n^{k+1}_{w} \\ \end{aligned} ΩΩt=2wmk+wmk+1bwk=2wmk+nwk+wmk+1+nwk+1bwkδbwk=Ωδbwk+21nwk+21nwk+1

至此,把上述的展开代入到公式9最后一行中中:
[ 0 δ θ ˙ ] = [ 0 − ( Ω t − Ω ) T ( Ω t − Ω ) − ( [ Ω t ] × + [ Ω ] × ) ] [ 1 1 2 δ θ ] = [ 0 − ( − δ b w k + 1 2 n w k + 1 2 n w k + 1 ) T − δ b w k + 1 2 n w k + 1 2 n w k + 1 − ( 2 [ Ω ] − [ δ b w k × + 1 2 n w k + 1 2 n w k + 1 ] × ) ] [ 1 1 2 δ θ ] = [ . . . − [ Ω ] × δ θ − δ b w k + 1 2 n w k + 1 2 n w k + 1 ] δ θ ˙ → − [ Ω ] × δ θ − δ b w k + 1 2 n w k + 1 2 n w k + 1 (10) \begin{aligned} \begin{bmatrix}0 \\ \dot{\delta{\theta}} \end{bmatrix}&=\begin{bmatrix}0 & -(\Omega_t-\Omega)^T \\ (\Omega_t-\Omega) & -([\Omega_t]_{\times}+[\Omega]_{\times}) \end{bmatrix} \begin{bmatrix}1 \\ \frac{1}{2}\delta{\theta}\end{bmatrix} \\ &=\begin{bmatrix}0 & -(-\delta{b^k_w}+\frac{1}{2}n^{k}_{w}+\frac{1}{2}n^{k+1}_{w})^T \\ -\delta{b^k_w}+\frac{1}{2}n^{k}_{w}+\frac{1}{2}n^{k+1}_{w} & -(2[\Omega]-[\delta{b^k_w}_{\times}+\frac{1}{2}n^{k}_{w}+\frac{1}{2}n^{k+1}_{w}]_{\times})\end{bmatrix}\begin{bmatrix}1 \\ \frac{1}{2}\delta{\theta}\end{bmatrix} \\ &=\begin{bmatrix}... \\ -[\Omega]_{\times}\delta{\theta}-\delta{b^k_w}+\frac{1}{2}n^{k}_{w}+\frac{1}{2}n^{k+1}_{w} \end{bmatrix} \\ \dot{\delta{\theta}}&\rightarrow -[\Omega]_{\times}\delta{\theta}-\delta{b^k_w}+\frac{1}{2}n^{k}_{w}+\frac{1}{2}n^{k+1}_{w} \end{aligned} \tag{10} [0δθ˙]δθ˙=[0(ΩtΩ)(ΩtΩ)T([Ωt]×+[Ω]×)][121δθ]=[0δbwk+21nwk+21nwk+1(δbwk+21nwk+21nwk+1)T(2[Ω][δbwk×+21nwk+21nwk+1]×)][121δθ]=[...[Ω]×δθδbwk+21nwk+21nwk+1][Ω]×δθδbwk+21nwk+21nwk+1(10)
所以离散化之后:
δ θ k + 1 = δ θ k − [ Ω ] × Δ t δ θ k − δ b w k Δ t + 1 2 n w k Δ t + 1 2 n w k + 1 Δ t (11) \delta{\theta}_{k+1}=\delta{\theta}_k-[\Omega]_{\times}\Delta{t} \delta{\theta}_k-\delta{b^k_w}\Delta{t}+\frac{1}{2}n^{k}_{w}\Delta{t}+\frac{1}{2}n^{k+1}_{w}\Delta{t} \tag{11} δθk+1=δθk[Ω]×ΔtδθkδbwkΔt+21nwkΔt+21nwk+1Δt(11)

位移的error-state

取公式8中的位移迭代公式,并使用truth-state=normal-state+error-state的方法进行推导,有:
p k + 1 t = p k + 1 + δ p k + 1 = p k t + v k t + 1 2 R k t ( a k t − b a k t ) + R k + 1 t ( a k + 1 t − b a k t ) 2 Δ t 2 = p k + 1 + δ p k + 1 = p k + δ p k + ( v k + δ v k ) Δ t + 1 2 R k t ( a k t − b a k t ) + R k + 1 t ( a k + 1 t − b a k t ) 2 Δ t 2 \begin{aligned} p_{k+1}^{t}&=p_{k+1}+\delta{p_{k+1}}=p^{t}_{k}+v_{k}^t+\frac{1}{2}\frac{R_k^t(a_k^t-b_{ak}^t)+R_{k+1}^t(a_{k+1}^t-b_{ak}^t)}{2}\Delta{t}^2 \\ &=p_{k+1}+\delta{p_{k+1}}=p_{k}+\delta{p_k}+(v_{k}+\delta{v_k})\Delta{t}+\frac{1}{2}\frac{R_k^t(a_k^t-b_{ak}^t)+R_{k+1}^t(a_{k+1}^t-b_{ak}^t)}{2}\Delta{t}^2 \end{aligned} pk+1t=pk+1+δpk+1=pkt+vkt+212Rkt(aktbakt)+Rk+1t(ak+1tbakt)Δt2=pk+1+δpk+1=pk+δpk+(vk+δvk)Δt+212Rkt(aktbakt)+Rk+1t(ak+1tbakt)Δt2
p k + 1 p_{k+1} pk+1的迭代公式代入之后有:
δ p k + 1 = δ p k + δ v k Δ t + 1 4 { [ R k t ( a k t − b a k t ) − R k ( a k − b a k ) ] + [ R k + 1 t ( a k + 1 t − b a k t ) − R k + 1 ( a k + 1 − b a k ] } Δ t 2 (12) \begin{aligned} \delta{p_{k+1}}&=\delta{p_k}+\delta{v_k}\Delta{t}+\frac{1}{4}\{[R_k^t(a_k^t-b_{ak}^t)- R_k(a_k-b_{ak})]+[R_{k+1}^t(a_{k+1}^t-b_{ak}^t)-R_{k+1}(a_{k+1}-b_{ak}]\}\Delta{t}^2 \end{aligned} \tag{12} δpk+1=δpk+δvkΔt+41{[Rkt(aktbakt)Rk(akbak)]+[Rk+1t(ak+1tbakt)Rk+1(ak+1bak]}Δt2(12)
把后面的大括号中的部分设为E,单独拿出来化简,有:
E = R k E x p ( δ θ k ) ( a k − n a k − b a k − δ b a k ) − R k ( a k − b a k ) + R k + 1 E x p ( δ θ k + 1 ) ( a k + 1 − n a k + 1 − b a k + 1 − δ b a k + 1 ) − R k ( a k + 1 − b a k + 1 ) = R k ( − δ b a k − n a k ) + R k [ δ θ k ] × ( a k − n a k − b a k − δ b a k ) + R k + 1 ( − δ b a k + 1 − n a k + 1 ) + R k + 1 [ δ θ k + 1 ] × ( a k + 1 − n a k + 1 − b a k + 1 − δ b a k + 1 ) = R k ( − δ b a k − n a k ) + R k [ δ θ k ] × ( a k − b a k ) + O ( ∣ ∣ δ θ k ∣ ∣ 2 ) + R k + 1 ( − δ b a k + 1 − n a k + 1 ) + R k + 1 [ δ θ k + 1 ] × ( a k + 1 − b a k + 1 ) + O ( ∣ ∣ δ θ k + 1 ∣ ∣ 2 ) ≈ R k ( − δ b a k − n a k ) + R k [ δ θ k ] × ( a k − b a k ) + R k + 1 ( − δ b a k + 1 − n a k + 1 ) + R k + 1 [ δ θ k + 1 ] × ( a k + 1 − b a k + 1 ) ≈ R k ( − δ b a k − n a k ) − R k [ a k − b a k ] × δ θ k + R k + 1 ( − δ b a k + 1 − n a k + 1 ) − R k + 1 [ a k + 1 − b a k + 1 ] × δ θ k + 1 where equation(11) ≈ R k ( − δ b a k − n a k ) − R k [ a k − b a k ] × δ θ k + R k + 1 ( − δ b a k + 1 − n a k + 1 ) − R k + 1 [ a k + 1 − b a k + 1 ] × ( δ θ k − [ Ω ] × Δ t δ θ k − δ b w k Δ t + 1 2 n w k Δ t + 1 2 n w k + 1 Δ t ) ≈ R k ( − δ b a k − n a k ) − R k [ a k − b a k ] × δ θ k + R k + 1 ( − δ b a k + 1 − n a k + 1 ) − R k + 1 [ a k + 1 − b a k + 1 ] × ( [ I − [ Ω ] × Δ t ] δ θ k − δ b w k Δ t + 1 2 n w k Δ t + 1 2 n w k + 1 Δ t ) where  δ b a ˙ = 0 → δ b a k + 1 = δ b a k ≈ − ( R k [ a k − b a k ] × + R k + 1 [ a k + 1 − b a k ] × ( [ I − [ Ω ] × Δ t ] ) δ θ k ⏟ δ θ + ( − R k + R k + 1 ) δ b a k ⏟ b a k + ( R k + 1 [ a k + 1 − b a k + 1 ] × Δ t ) δ b w k ⏟ b w k + ( − R k + 1 [ a k + 1 − b a k + 1 ] × 1 2 Δ t ) n w k ⏟ n w k + ( − R k + 1 [ a k + 1 − b a k + 1 ] × 1 2 Δ t ) n w k + 1 ⏟ n w k + 1 + ( − R k ) n a k ⏟ n a k + ( − R k + 1 ) n a k + 1 ⏟ n a k + 1 (13) \begin{aligned} E&=R_kExp(\delta{\theta_k})(a_k-n^k_{a}-b_{ak}-\delta{b_{ak}})-R_k(a_k-b_{ak}) \\ &+R_{k+1}Exp(\delta{\theta_{k+1}})(a_{k+1}-n^{k+1}_{a}-b_{ak+1}-\delta{b_{ak+1}})-R_k(a_{k+1}-b_{ak+1}) \\ \\ &=R_k(-\delta{b}_{ak}-n_{a}^k)+R_k[\delta{\theta_k}]_{\times}(a_k-n^k_{a}-b_{ak}-\delta{b_{ak}})\\ &+R_{k+1}(-\delta{b}_{ak+1}-n_{a}^{k+1})+R_{k+1}[\delta{\theta_{k+1}}]_{\times}(a_{k+1}-n^{k+1}_{a}-b_{ak+1}-\delta{b_{ak+1}}) \\ \\ &=R_k(-\delta{b}_{ak}-n_{a}^k)+R_{k}[\delta{\theta_k}]_{\times}(a_k-b_{ak})+O(||\delta{\theta_k}||^2)+R_{k+1}(-\delta{b}_{ak+1}-n_{a}^{k+1}) \\ &+R_{k+1}[\delta{\theta_{k+1}}]_{\times}(a_{k+1}-b_{ak+1})+O(||\delta{\theta_{k+1}}||^2) \\ \\ &\approx R_k(-\delta{b}_{ak}-n_{a}^k)+R_{k}[\delta{\theta_k}]_{\times}(a_k-b_{ak})\\ &+R_{k+1}(-\delta{b}_{ak+1}-n_{a}^{k+1})+R_{k+1}[\delta{\theta_{k+1}}]_{\times}(a_{k+1}-b_{ak+1}) \\ \\ &\approx R_k(-\delta{b}_{ak}-n_{a}^k)-R_{k}[a_k-b_{ak}]_{\times}\delta{\theta_k}\\ &+R_{k+1}(-\delta{b}_{ak+1}-n_{a}^{k+1})-R_{k+1}[a_{k+1}-b_{ak+1}]_{\times}\delta{\theta_{k+1}}\\ &\text{where equation(11)} \\ \\ &\approx R_k(-\delta{b}_{ak}-n_{a}^k)-R_{k}[a_k-b_{ak}]_{\times}\delta{\theta_k}+R_{k+1}(-\delta{b}_{ak+1}-n_{a}^{k+1})\\ &-R_{k+1}[a_{k+1}-b_{ak+1}]_{\times}(\delta{\theta}_k-[\Omega]_{\times}\Delta{t} \delta{\theta}_k-\delta{b^k_w}\Delta{t}+\frac{1}{2}n^{k}_{w}\Delta{t}+\frac{1}{2}n^{k+1}_{w}\Delta{t}) \\ \\ &\approx R_k(-\delta{b}_{ak}-n_{a}^k)-R_{k}[a_k-b_{ak}]_{\times}\delta{\theta_k}+R_{k+1}(-\delta{b}_{ak+1}-n_{a}^{k+1})\\ &-R_{k+1}[a_{k+1}-b_{ak+1}]_{\times}([I-[\Omega]_{\times}\Delta{t}]\delta{\theta}_k-\delta{b^k_w}\Delta{t}+\frac{1}{2}n^{k}_{w}\Delta{t}+\frac{1}{2}n^{k+1}_{w}\Delta{t}) \\ &\text{where } \dot{\delta{b_a}}=0 \rightarrow \delta{b_{ak+1}}=\delta{b_{ak}} \\ \\ &\approx \underbrace{-(R_k[a_k-b_{ak}]_{\times}+R_{k+1}[a_{k+1}-b_{ak}]_{\times}([I-[\Omega]_{\times}\Delta{t}])\delta{\theta}_k}_{\delta{\theta}}\\ &+\underbrace{(-R_{k}+R_{k+1})\delta{b_{ak}}}_{b_{ak}}+\underbrace{(R_{k+1}[a_{k+1}-b_{ak+1}]_{\times}\Delta{t})\delta{b_{wk}}}_{b_{wk}}\\ &+\underbrace{(-R_{k+1}[a_{k+1}-b_{ak+1}]_{\times}\frac{1}{2}\Delta{t})n_{w}^k}_{n_{w}^k}+\underbrace{(-R_{k+1}[a_{k+1}-b_{ak+1}]_{\times}\frac{1}{2}\Delta{t})n_{w}^{k+1}}_{n_{w}^{k+1}} \\ &+\underbrace{(-R_{k})n_{a}^k}_{n_{a}^k}+\underbrace{(-R_{k+1})n_{a}^{k+1}}_{n_{a}^{k+1}} \end{aligned} \tag{13} E=RkExp(δθk)(aknakbakδbak)Rk(akbak)+Rk+1Exp(δθk+1)(ak+1nak+1bak+1δbak+1)Rk(ak+1bak+1)=Rk(δbaknak)+Rk[δθk]×(aknakbakδbak)+Rk+1(δbak+1nak+1)+Rk+1[δθk+1]×(ak+1nak+1bak+1δbak+1)=Rk(δbaknak)+Rk[δθk]×(akbak)+O(δθk2)+Rk+1(δbak+1nak+1)+Rk+1[δθk+1]×(ak+1bak+1)+O(δθk+12)Rk(δbaknak)+Rk[δθk]×(akbak)+Rk+1(δbak+1nak+1)+Rk+1[δθk+1]×(ak+1bak+1)Rk(δbaknak)Rk[akbak]×δθk+Rk+1(δbak+1nak+1)Rk+1[ak+1bak+1]×δθk+1where equation(11)Rk(δbaknak)Rk[akbak]×δθk+Rk+1(δbak+1nak+1)Rk+1[ak+1bak+1]×(δθk[Ω]×ΔtδθkδbwkΔt+21nwkΔt+21nwk+1Δt)Rk(δbaknak)Rk[akbak]×δθk+Rk+1(δbak+1nak+1)Rk+1[ak+1bak+1]×([I[Ω]×Δt]δθkδbwkΔt+21nwkΔt+21nwk+1Δt)where δba˙=0δbak+1=δbakδθ (Rk[akbak]×+Rk+1[ak+1bak]×([I[Ω]×Δt])δθk+bak (Rk+Rk+1)δbak+bwk (Rk+1[ak+1bak+1]×Δt)δbwk+nwk (Rk+1[ak+1bak+1]×21Δt)nwk+nwk+1 (Rk+1[ak+1bak+1]×21Δt)nwk+1+nak (Rk)nak+nak+1 (Rk+1)nak+1(13)
把公式(13)的结论代入到公式(12)中,就可以得到关于位移的error-state的离散形势下的状态转移方程了。

速度的error-state

和位移同理:
δ v k + 1 = δ v k + { [ R k t ( a k t − b a k t ) − R k ( a k − b a k ) ] + [ R k + 1 t ( a k + 1 t − b a k t ) − R k + 1 ( a k + 1 − b a k ] } Δ t (14) \begin{aligned} \delta{v_{k+1}}&=\delta{v_k}+\{[R_k^t(a_k^t-b_{ak}^t)- R_k(a_k-b_{ak})]+[R_{k+1}^t(a_{k+1}^t-b_{ak}^t)-R_{k+1}(a_{k+1}-b_{ak}]\}\Delta{t} \end{aligned} \tag{14} δvk+1=δvk+{[Rkt(aktbakt)Rk(akbak)]+[Rk+1t(ak+1tbakt)Rk+1(ak+1bak]}Δt(14)
可以看到,这次后面的部分和公式13是一模一样的,这里就不进行过多的推导了。

零偏Bias的error-state

同样的方式有:
b k + 1 t = b k + 1 + δ b k + 1 = b k t + n t Δ t = b k + n Δ t + δ b k + 1 = b k + δ b k + n t Δ t → δ b k + 1 = δ b k + ( n t − n ) ⏟ n b Δ t (15) \begin{aligned} b_{k+1}^t&=b_{k+1}+\delta{b_{k+1}}=b_{k}^t+n^t\Delta{t} \\ &=b_{k}+n\Delta{t}+\delta{b_{k+1}}=b_{k}+\delta{b_k}+n^t\Delta{t} \\ & \rightarrow \delta{b_{k+1}}=\delta{b_k}+\underbrace{(n^t-n)}_{n_b}\Delta{t} \end{aligned} \tag{15} bk+1t=bk+1+δbk+1=bkt+ntΔt=bk+nΔt+δbk+1=bk+δbk+ntΔtδbk+1=δbk+nb (ntn)Δt(15)

整个状态转移方程的矩阵形式

结合上述的公式(11)(12)(14)(15),可以得到最终的状态转移方程的矩阵形式为下图,相比于之前的欧拉法复杂了很多:

VINS-Mono理论推导1_第2张图片

 

visual-inertial alignment

这部分属于VINS中初始化阶段,我个人认为是一个不错的部分,因为VINS没有引入任何先验性的东西来初始化,而是真正的进行了各个量的估计,主要包括:

  1. 对陀螺仪初始零偏的估计;
  2. 对初始重力的估计;
  3. 对机体坐标系速度的估计;
  4. 对初始机体坐标系到世界坐标系的转移;

下面各个击破,这部分的前提是整个系统已经通过足够的激励得到了较好的IMU积分和Visual位姿估计(这些位姿没有尺度)。


对陀螺仪初始零偏的估计

这部分主要涉及到旋转,因此和尺度没有关系,因此作为第一步进行计算。

整体的思路为:

  1. 算法通过视觉SFM得到了一系列初始姿态 { q i ∣ i = 0 , . . . , N } \{q_i|i=0,...,N\} {qii=0,...,N}

  2. 同时预积分阶段得到了每两帧之间的相对姿态 { r j + 1 j ∣ j = 0 , . . . , N − 1 } \{r^j_{j+1}|j=0,...,N-1\} {rj+1jj=0,...,N1}

  3. 构建优化问题为:视觉帧间的相对姿态 q i + 1 i q_{i+1}^i qi+1i与IMU积分相对姿态 r j + 1 j r_{j+1}^j rj+1j的差的和最小,即:
    E = ∣ ∣ e i + 1 i ∣ ∣ 2 = ∣ ∣ ( q i − 1 ⊗ q i + 1 ⏟ v i s u a l ) − 1 ⊗ r i i + 1 ⏟ I M U ∣ ∣ 2 (16) E=||e_{i+1}^i||^2=||(\underbrace{q_i^{-1}\otimes q_{i+1}}_{visual})^{-1} \otimes \underbrace{r_{i}^{i+1}}_{IMU} ||^2 \tag{16} E=ei+1i2=(visual qi1qi+1)1IMU rii+12(16)

针对公式16,下面构建最小二乘问题有:
E = ∣ ∣ e i + 1 i ( b w ) ∣ ∣ 2 = ∣ ∣ [ 1 ϕ ( b w ) ] ∣ ∣ 2  where  ϕ = 1 2 θ = 1 + ϕ ( b w ) T ϕ ( b w ) (17) \begin{aligned} E=||e_{i+1}^{i}(b_w)||^2&=||\begin{bmatrix}1 \\ \phi(b_w) \end{bmatrix} ||^2 \text{ where } \phi=\frac{1}{2}\theta \\ &=1+\phi(b_w)^T \phi(b_w) \end{aligned} \tag{17} E=ei+1i(bw)2=[1ϕ(bw)]2 where ϕ=21θ=1+ϕ(bw)Tϕ(bw)(17)
δ b w \delta{b_w} δbw的扰动之后有:
E = ∣ ∣ e i + 1 i ( b w + δ b w ) ∣ ∣ 2 = 1 + ( ϕ ( b w ) + J δ b w ) T ( ϕ ( b w ) + J δ b w )  where  J = ∂ ϕ ∂ b w = ϕ + δ ϕ − ϕ δ b w = 1 2 δ θ δ b w = 1 2 J b w θ = 1 + ϕ ( b w ) T ϕ ( b w ) + ϕ ( b w ) T J δ b w + δ b w T J T ϕ ( b w ) + δ b w T J T J δ b w ∂ E ∂ δ b w = 0 → J T ϕ ( b w ) + J T J δ b w = 0 → 2 ( J b w θ ) T ϕ ( b w ) + ( J b w θ ) T ( J b w θ ) δ b w = 0 → δ b w = − ( ( J b w θ ) T ( J b w θ ) ) − 1 ( 2 ( J b w θ ) T ϕ ( b w ) ) (18) \begin{aligned} E=||e_{i+1}^{i}(b_w+\delta{b_w})||^2&=1+(\phi(b_w)+J\delta{b_w})^T(\phi(b_w)+J\delta{b_w}) \text{ where } J=\frac{\partial\phi}{\partial{b_w}}=\frac{\phi+\delta{\phi}-\phi}{\delta{b_w}}=\frac{1}{2}\frac{\delta{\theta}}{\delta{b_w}}=\frac{1}{2}J^{\theta}_{b_w} \\ &=1+\phi(b_w)^T\phi(b_w)+\phi(b_w)^TJ\delta{b_w}+\delta{b_w}^TJ^T\phi(b_w)+\delta{b_w}^TJ^TJ\delta{b_w} \\ \frac{\partial{E}}{\partial{\delta{b_w}}}=0 &\rightarrow J^T\phi(b_w)+J^TJ\delta{b_w}=0 \\ & \rightarrow 2(J^{\theta}_{b_w})^T\phi(b_w)+(J^{\theta}_{b_w})^T(J^{\theta}_{b_w})\delta{b_w}=0 \\ & \rightarrow \delta{b_w}=-((J^{\theta}_{b_w})^T(J^{\theta}_{b_w}))^{-1}(2(J^{\theta}_{b_w})^T\phi(b_w)) \tag{18} \end{aligned} E=ei+1i(bw+δbw)2δbwE=0=1+(ϕ(bw)+Jδbw)T(ϕ(bw)+Jδbw) where J=bwϕ=δbwϕ+δϕϕ=21δbwδθ=21Jbwθ=1+ϕ(bw)Tϕ(bw)+ϕ(bw)TJδbw+δbwTJTϕ(bw)+δbwTJTJδbwJTϕ(bw)+JTJδbw=02(Jbwθ)Tϕ(bw)+(Jbwθ)T(Jbwθ)δbw=0δbw=((Jbwθ)T(Jbwθ))1(2(Jbwθ)Tϕ(bw))(18)
这部分推导和代码中的基本上是一模一样了,唯一需要特别注意的是代码中的 ϕ \phi ϕ是IMU与visual的差,而这里的推导使用的是visual与IMU的差,所以差了一个负号(下面注释中的注意1),这里简单贴一下代码:

void solveGyroscopeBias(map &all_image_frame, Vector3d* Bgs)
{
    Matrix3d A;
    Vector3d b;
    Vector3d delta_bg;
    A.setZero();
    b.setZero();
    map::iterator frame_i;
    map::iterator frame_j;
    // 构建增量方程
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
    {
        frame_j = next(frame_i);
        MatrixXd tmp_A(3, 3);
        tmp_A.setZero();
        VectorXd tmp_b(3);
        tmp_b.setZero();
        Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
        tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
        // 注意1. 这个地方是对IMU的预积分取转置,根据四元数的转置的性质,后面的所有的相关变量都多了个负号
        tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
        A += tmp_A.transpose() * tmp_A;
        b += tmp_A.transpose() * tmp_b;

    }
    
    // 相当于对所有的状态求了一个公共的bias
    // 个人认为初始化部分的时间较短,所有共享一个bias是没有问题的
    delta_bg = A.ldlt().solve(b);
    ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());

    for (int i = 0; i <= WINDOW_SIZE; i++)
        Bgs[i] += delta_bg;
	
    // 重新预积分
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
    {
        frame_j = next(frame_i);
        frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
    }
}

Visual Inertial Alignment

经过了上一步骤之后,算法开始进行整个的视觉位姿和惯导位姿的对齐工作,算法在这个过程中主要求解的变量为每个预积分时间间隔的速度、整个视觉的尺度以及在视觉初始帧上的重力加速度(这个地方也是笔者认为VINS-Mono工作很细致且合理的地方,因为整个过程中算法并没有依靠任何的先验信息),求解的状态变量如下:
X I = [ v b 0 b 0 , v b 1 b 1 , ⋯ v b n b n , g c 0 , s ] \mathcal{X}_{I}=\left[\mathbf{v}_{b_{0}}^{b_{0}}, \mathbf{v}_{b_{1}}^{b_{1}}, \cdots \mathbf{v}_{b_{n}}^{b_{n}}, \mathbf{g}^{c_{0}}, s\right] XI=[vb0b0,vb1b1,vbnbn,gc0,s]

还是先看算法的输入:

  • 相机参考系 C 0 C_0 C0下的每个时刻的位姿 T c k c 0 = [ R c k c 0 ∣ p c k c 0 ] , k = 0 , . . . , N T^{c0}_{c_k}=[R^{c0}_{c_k}|p^{c0}_{c_k}],k=0,...,N Tckc0=[Rckc0pckc0],k=0,...,N
  • IMU坐标系 b b b下的每个时间间隔的积分值 p b k + 1 b k , v b k + 1 b k , k = 0 , . . . , N − 1 {p_{bk+1}^{bk}, v_{bk+1}^{bk},k=0,...,N-1} pbk+1bk,vbk+1bk,k=0,...,N1

于是可以在相机的参考系 C 0 C_0 C0构建如下的模型:
{ s c 0 p b k + 1 = s c 0 p b k + c 0 v b k Δ t − 1 2 c 0 g Δ t 2 + c 0 p b k + 1 b k c 0 v b k + 1 = c 0 v b k − c 0 g Δ t + c 0 v b k + 1 b k (18) \begin{aligned} \begin{cases} s{^{c0}}p_{bk+1} &= s^{c0}p_{bk}+{^{c0}}v_{bk}\Delta{t}-\frac{1}{2}{^{c0}}g\Delta{t}^2+{^{c0}}p_{bk+1}^{bk} \\ {^{c0}}v_{bk+1} &= {^{c0}}v_{bk}-{^{c0}}g\Delta{t}+{^{c0}}v_{bk+1}^{bk} \end{cases} \end{aligned} \tag{18} {sc0pbk+1c0vbk+1=sc0pbk+c0vbkΔt21c0gΔt2+c0pbk+1bk=c0vbkc0gΔt+c0vbk+1bk(18)
在上述公式中,笔者特别把参考系写在了符号的左上角,例如 c 0 p b k ^{c0}p_{bk} c0pbk表示在 C 0 C_0 C0坐标系下,IMU在K时刻的位置;但是我们仅仅有K时刻Camera的位置,这个也很简单,基本上就是一个简单的三角转换关系,图形表示的话如下:

VINS-Mono理论推导1_第3张图片

其中:

  • a = c 0 p c k a={^{c0}}p_{ck} a=c0pck b = c 0 p b k c k b={^{c0}}p^{ck}_{bk} b=c0pbkck c = c 0 p b k c={^{c0}}p_{bk} c=c0pbk

  • 那么c是需要求解的量,a是视觉得到的量,那么剩下的关键就是b如何得到了,其实也比较简单,因为通常情况下我们知道了IMU与Camera之间的外参 T c b = [ R c b ∣ t c b ] = [ R c k b k ∣ t c k b k ] T_{c}^{b}=[R_{c}^{b}|t_{c}^{b}]=[R_{ck}^{bk}|t_{ck}^{bk}] Tcb=[Rcbtcb]=[Rckbktckbk],那可以看到,外参对应的参考系是K时刻的IMU系,即 b k b_k bk,那转到 C 0 C_0 C0坐标系下也简单,直接转一下就OK了,于是有:
    s c 0 p b k = s c 0 p c k + ( − R b k c 0 p c k b k ⏟ c 0 p c k b k ) = s c 0 p c k − R c k c 0 ( R c b ) T ⏟ R b k c 0 p c b (19) \begin{aligned} s{^{c0}}p_{bk}&=s{^{c0}}p_{ck}+(-\underbrace{R^{c0}_{bk}p_{ck}^{bk}}_{{^{c0}}p_{ck}^{bk}}) \\ &=s{^{c0}}p_{ck}-\underbrace{R^{c0}_{ck}(R_c^b)^T}_{R_{bk}^{c0}} p_{c}^{b} \end{aligned} \tag{19} sc0pbk=sc0pck+(c0pckbk Rbkc0pckbk)=sc0pckRbkc0 Rckc0(Rcb)Tpcb(19)

所以公式(18)可以进一步写作:
{ p b k + 1 b k = R c 0 b k [ s ( c 0 p b k + 1 − c 0 p b k ) − R b k c 0 v b k Δ t + 1 2 c 0 g Δ t 2 ] = R c 0 b k [ s c 0 p c k + 1 − R b k + 1 c 0 p c b − s c 0 p b k + R b k c 0 p c b − R b k c 0 v b k Δ t + 1 2 c 0 g Δ t 2 ] = R c 0 b k [ s ( c 0 p c k + 1 − c 0 p b k ) − R b k c 0 v b k Δ t + 1 2 c 0 g Δ t 2 ] − R b k + 1 b k p c b + p c b v b k + 1 b k = R c 0 b k ( c 0 v b k + 1 − c 0 v b k + c 0 g Δ t ) = R c 0 b k ( R b k + 1 c 0 v b k + 1 − R b k c 0 v b k + c 0 g Δ t ) (20) \begin{aligned} \begin{cases} p_{bk+1}^{bk} &= R_{c0}^{bk} \left[s({^{c0}}p_{bk+1}-{^{c0}}p_{bk})-R_{bk}^{c0}v_{bk}\Delta{t}+\frac{1}{2}{^{c0}}g\Delta{t}^2\right] \\ &= R_{c0}^{bk} \left[s{^{c0}}p_{ck+1}-R_{bk+1}^{c0}p_{c}^{b}-s{^{c0}}p_{bk}+R_{bk}^{c0}p_{c}^{b}-R_{bk}^{c0}v_{bk}\Delta{t}+\frac{1}{2}{^{c0}}g\Delta{t}^2\right]\\ &= R_{c0}^{bk} \left[s({^{c0}}p_{ck+1}-{^{c0}}p_{bk})-R_{bk}^{c0}v_{bk}\Delta{t}+\frac{1}{2}{^{c0}}g\Delta{t}^2\right]-R_{bk+1}^{bk}p_{c}^{b}+p_{c}^{b}\\ v_{bk+1}^{bk} &= R_{c0}^{bk}({^{c0}}v_{bk+1} - {^{c0}}v_{bk}+{^{c0}}g\Delta{t}) \\ &= R_{c0}^{bk}(R_{bk+1}^{c0}v_{bk+1} - R_{bk}^{c0}v_{bk}+{^{c0}}g\Delta{t}) \\ \end{cases} \end{aligned} \tag{20} pbk+1bkvbk+1bk=Rc0bk[s(c0pbk+1c0pbk)Rbkc0vbkΔt+21c0gΔt2]=Rc0bk[sc0pck+1Rbk+1c0pcbsc0pbk+Rbkc0pcbRbkc0vbkΔt+21c0gΔt2]=Rc0bk[s(c0pck+1c0pbk)Rbkc0vbkΔt+21c0gΔt2]Rbk+1bkpcb+pcb=Rc0bk(c0vbk+1c0vbk+c0gΔt)=Rc0bk(Rbk+

你可能感兴趣的:(VINS,预积分,SLAM,SLAM,算法,VINS-Mono,VIO)