本文深入探讨了VINS-Mono中的IMU预积分概念，详细介绍了IMU模型和基于世界坐标系的运动模型，包括连续和离散形式。通过推导误差递推方程和协方差矩阵，阐述了预积分误差、协方差以及雅克比矩阵的递推方程。文章旨在帮助理解IMU预积分在非线性优化中的作用和重要性，以及如何处理误差和不确定性。
文章目录
1. 前言
本博客借鉴了崔华坤的《VINS论文推导及代码解析》和 VINS-Mono理论学习——IMU预积分 Pre-integration (Jacobian 协方差)的内容，因为确实写得太好了，然后有些地方加入自己一些理解。
VINS-MONO论文中的IV-B. IMU Pre-integration介绍了IMU预积分模型，Foster的倆篇论文对IMU预积分理论进行详细分析。
值得注意的是，本文使用了四元素中Hamilton（可参考《Quaternion kinematics for the error state Kalman filter》，右手系）的表示和JPL（可参考《Indirect Kalman Filter for 3D Attitude Estimation》，左手系）表示，因为Vins-mono里面也是用了俩种表示混用，俩者的具体区别可以看《Quaternion kinematics for the error state Kalman filter》的第17页，且论文中用到的JPL的右乘左乘定义还与《Indirect Kalman Filter for 3D Attitude Estimation》不同，这种问题估计让无数人心塞。／（ㄒｏㄒ）／～～
为什么需要对IMU进行预积分？
传统捷联惯性导航算法，在已知 k k k 时刻下的IMU状态量(姿态、速度和位移)情况下，利用IMU测量的线加速度和角速度，通过积分预算得到 k + 1 k+1 k+1时刻下的状态量。
然而在非线性优化的VIO中，各个节点的状态量都是估计值，当算法对这些状态量优化时，每次调整都需要在它们之间重新积分，导致绝对位姿被优化时对状态量进行重复积分。IMU预积分的提出使得优化算法可对IMU的相对测量进行处理，使它与绝对位姿解耦，或者只要线性运算就可以进行矫正。
2. IMU模型
IMU测量值包括加速度计得到的 (测量值) 线加速度 a t ^ \hat{a_{t}} at^ 和陀螺仪得到的角加速度 w ^ t \hat{w}_{t} w^t 【论文式(1)】
a ^ t = a t + b a t + R w t g w + n a \hat{a}_{t}=a_{t}+b_{at}+R_{w}^{t}g^{w}+n_{a} a^t=at+bat+Rwtgw+na w ^ t = w t + b w t + n w \hat{w}_{t}=w_{t}+b_{wt}+n_{w} w^t=wt+bwt+nw其中 t t t下标表示在IMU的体(body)坐标系下， a t a_{t} at、 w t w_{t} wt分别表示IMU真实的线加速度和角速度，并受到加速度偏置(bias) b a t b_{at} bat、陀螺仪偏置 b w t b_{wt} bwt和附加噪声 n a n_a na、 n n w n_{n_{w}} nnw的影响。计算得到的线加速度 a t ^ \hat{a_{t}} at^是重力加速度和物体加速度的合矢量。
假设附加噪声为高斯噪声：
n a ∼ ( 0 , σ a 2 ) , n a ∼ ( 0 , σ w 2 ) n_{a}\sim(0,\sigma_{a}^{2}), \ n_{a}\sim(0,\sigma_{w}^{2}) na∼(0,σa2), na∼(0,σw2) 加速度计偏置和陀螺仪偏置被建模为随机游走，其导数为高斯分布：【论文式(2)】
b ˙ a t = n b a , b ˙ w t = n b w \dot{b}_{at}=n_{ba}, \ \dot{b}_{wt}=n_{bw} b˙at=nba, b˙wt=nbw n b a ∼ N ( 0 , σ b a 2 ) , n b w ∼ N ( 0 , σ b w 2 ) n_{ba}\sim N(0,\sigma_{ba}^{2}), \ n_{bw} \sim N(0, \sigma_{bw}^{2}) nba∼N(0,σba2), nbw∼N(0,σbw2)
3. 基于世界坐标系下的IMU运动模型
3.1 连续形式下的IMU运动模型
对于图像帧 k k k和 k + 1 k+1 k+1, IMU body坐标系对应为 b k b_{k} bk和 b k + 1 b_{k+1} bk+1，位置、速度和姿态状态值PVQ(Pose、Velocity、Quaternion)可以根据 [ t k , t k + 1 ] [t_{k}, t_{k+1}] [tk,tk+1]时间间隔内的IMU测量值，在世界坐标系下进行传递：【论文式(3)(4)】
p b k + 1 w = p b k w + v b k w Δ t k + ∫ ∫ t ∈ [ t k , t k + 1 ] ( R t w ( a ^ t − b a t − n a ) − g w ) d t 2 p^{w}_{b_{k+1}}=p^{w}_{bk}+v_{b_{k}}^{w}\Delta t_{k}+\int \int_{t\in[t_{k}, t_{k+1}]}(R_{t}^{w}(\hat{a}_{t}-b_{at}-n_{a})-g^{w})dt^{2} pbk+1w=pbkw+vbkwΔtk+∫∫t∈[tk,tk+1](Rtw(a^t−bat−na)−gw)dt2 v b k + 1 w = v b k w + ∫ t ∈ [ t k , t k + 1 ] ( R t w ( a ^ t − b a t − n a ) − g w ) d t v_{b_{k+1}}^{w}=v_{bk}^{w}+\int_{t\in[t_{k},t_{k+1}]}(R^{w}_{t}(\hat{a}_{t}-b_{at}-n_{a})-g^{w})dt vbk+1w=vbkw+∫t∈[tk,tk+1](Rtw(a^t−bat−na)−gw)dt q b k + 1 w = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] 1 2 q t b k ⊗ [ ( w ^ t − b w t − n w ) 0 ] d t = q b k ⊗ ∫ t ∈ [ t k , t k + 1 ] 1 2 Ω ( w ^ t − b w t − n w ) q t b k d t (1) q_{b_{k+1}}^{w} = q_{b_{k}}^{w} \otimes \int_{t\in[t_{k}, t_{k+1}]} \frac{1}{2} q_{t}^{b_{k}} \otimes \begin{bmatrix} (\hat{w}_{t}-b_{wt}-n_{w})\\ 0 \end{bmatrix}dt \\ = q_{bk} \otimes \int_{t\in[t_{k}, t_{k+1}]} \frac{1}{2} \Omega (\hat{w}_{t}-b_{wt}-n_{w})q_{t}^{bk}dt \tag{1} qbk+1w=qbkw⊗∫t∈[tk,tk+1]21qtbk⊗[(w^t−bwt−nw)0]dt=qbk⊗∫t∈[tk,tk+1]21Ω(w^t−bwt−nw)qtbkdt(1) Ω ( w ) = [ − [ w ] × w − w T 0 ] , [ w ] × = [ 0 − w z w y w z 0 − w x − w y w x 0 ] \Omega(w)=\begin{bmatrix} -[w]_{\times} & w\\ -w^{T}& 0 \end{bmatrix}, \ [w]_{\times} = \begin{bmatrix} 0 & -w_{z} & w_{y}\\ w_{z} & 0 & -w_{x}\\ -w_{y} & w_{x} & 0 \end{bmatrix} Ω(w)=[−[w]×−wTw0], [w]×=⎣⎡0wz−wy−wz0wxwy−wx0⎦⎤其中 Δ t k \Delta t_{k} Δtk是 [ t k , t k + 1 ] [t_{k}, t_{k+1}] [tk,tk+1]之间的时间间隔， R t w R_{t}^{w} Rtw为t时刻IMU body坐标系到世界坐标系的旋转矩阵， q t b k q_{t}^{bk} qtbk为用四元素表示的 t t t 时刻从IMU body坐标系到 k k k 时刻IMU body坐标系的旋转，这里的四元素实部在后，虚部在前, 为了与论文保持一致，即四元素JPL表达。这里的 Ω ( w ) \Omega(w) Ω(w)表示四元素右乘。
关于公式 ( 1 ) (1) (1)的推导，这里首先引入四元素左乘右乘及导数定理：
这里与《Indirect Kalman Filter for 3D Attitude Estimation》中公式（10）不同，主要是因为左右手系不同，我们引入左乘和右乘符号如下：
q a ⊗ q b = R ( q b ) q a = [ s b z b − y b x b − z b s b x b y b y b − x b s b z b − x b − y b − z b s b ] [ x a y a z a s a ] = L ( q a ) q b = [ s a − z a y a x a z a s a − x a y a − y a x a s a z a − x a − y a − z a s a ] [ x b y b z b s b ] q_{a} \otimes q_{b} = R(q_{b})q_{a} = \begin{bmatrix} s_{b} & z_{b} & -y_{b} & x_{b}\\ -z_{b} & s_{b} & x_{b} & y_{b}\\ y_{b}& -x_{b} & s_{b} & z_{b}\\ -x_{b}& -y_{b} & -z_{b} & s_{b} \end{bmatrix}\begin{bmatrix} x_{a}\\ y_{a}\\ z_{a}\\ s_{a} \end{bmatrix} \\ = L(q_{a})q_{b} = \begin{bmatrix} s_{a} & -z_{a} & y_{a} & x_{a}\\ z_{a} & s_{a} & -x_{a} & y_{a}\\ -y_{a}& x_{a} & s_{a} & z_{a}\\ -x_{a}& -y_{a} & -z_{a} & s_{a} \end{bmatrix}\begin{bmatrix} x_{b}\\ y_{b}\\ z_{b}\\ s_{b} \end{bmatrix} qa⊗qb=R(qb)qa=⎣⎢⎢⎡sb−zbyb−xbzbsb−xb−yb−ybxbsb−zbxbybzbsb⎦⎥⎥⎤⎣⎢⎢⎡xayazasa⎦⎥⎥⎤=L(qa)qb=⎣⎢⎢⎡saza−ya−xa−zasaxa−yaya−xasa−zaxayazasa⎦⎥⎥⎤⎣⎢⎢⎡xbybzbsb⎦⎥⎥⎤
为了简化，令 q = [ x y z s ] = [ w s ] q=[x \ y \ z \ s] = [w \ s] q=[x y z s]=[w s], 则有：
R ( q ) = Ω ( w ) + s I 4 × 4 = [ − [ w ] × w − w T 0 ] + s I 4 × 4 R(q) = \Omega(w)+sI_{4\times4} = \begin{bmatrix} -[w]_{\times} & w\\ -w^{T}& 0 \end{bmatrix} + sI_{4\times4} R(q)=Ω(w)+sI4×4=[−[w]×−wTw0]+sI4×4 L ( q ) = Ψ ( w ) + s I 4 × 4 = [ [ w ] × w − w T 0 ] + s I 4 × 4 L(q) = \Psi(w)+sI_{4\times4} = \begin{bmatrix} [w]_{\times} & w\\ -w^{T}& 0 \end{bmatrix} + sI_{4\times4} L(q)=Ψ(w)+sI4×4=[[w]×−wTw0]+sI4×4
对于四元素的求导，我们定义 q t q_{t} qt为 t t t时刻下的单位四元素(这里的四元素实部在后，虚部在前)， w w w为 q t q_{t} qt确定的角速度，则关于 q t q_{t} qt的导数为： q ˙ t = 1 2 [ − [ w ] × w − w T 0 ] q t = 1 2 Ω ( w ) q t = 1 2 R ( [ w 0 ] ) q t = 1 2 q t ⊗ [ w 0 ] \dot{q}_{t} = \frac{1}{2} \begin{bmatrix} -[w]_{\times} & w\\ -w^{T}& 0 \end{bmatrix}q_{t} = \frac{1}{2}\Omega(w)q_{t}=\frac{1}{2}R(\begin{bmatrix} w\\ 0 \end{bmatrix})q_{t} = \frac{1}{2}q_{t} \otimes \begin{bmatrix} w\\ 0 \end{bmatrix} q˙t=21[−[w]×−wTw0]qt=21Ω(w)qt=21R([w0])qt=21qt⊗[w0]
当看到 q ˙ = 1 2 q t ⊗ [ 0 w ] \dot{q} = \frac{1}{2} q_{t} \otimes \begin{bmatrix} 0 \\ w \end{bmatrix} q˙=21qt⊗[0w] 说明该四元素实部在前，虚部在后（Hamilton表示），只是表示不同而已。
因此对于IMU连续形式下的旋转状态(用四元素表示)推导，我们有：
q b k + 1 w = q b k w ⊗ q b k + 1 b k = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] q t ˙ b k d t = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] 1 2 q t b k ⊗ [ w t b k 0 ] d t = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] 1 2 q t b k ⊗ [ w t ^ − b w t − n w 0 ] d t = = q b k ⊗ ∫ t ∈ [ t k , t k + 1 ] 1 2 Ω ( w ^ t − b w t − n w ) q t b k d t q_{b_{k+1}^{w}}=q_{b_{k}}^{w} \otimes q_{b_{k+1}}^{b_{k}} = q_{b_{k}}^{w} \otimes \int _{t\in [t_{k}, t_{k+1}]} \dot{q_{t}}^{b_{k}} dt = q_{bk}^{w} \otimes \int_{t\in[t_{k}, t_{k+1}]} \frac{1}{2} q_{t}^{b_{k}}\otimes \begin{bmatrix} w_{t}^{b_{k}}\\ 0\end{bmatrix}dt \\ =q_{b_{k}}^{w} \otimes \int_{t \in [t_{k}, t_{k+1}]} \frac{1}{2}q_{t}^{b_{k}} \otimes \begin{bmatrix} \hat{w_{t}}-b_{wt}-n_{w}\\ 0\end{bmatrix} dt \\ = = q_{bk} \otimes \int_{t\in[t_{k}, t_{k+1}]} \frac{1}{2} \Omega (\hat{w}_{t}-b_{wt}-n_{w})q_{t}^{bk}dt qbk+1w=qbkw⊗qbk+1bk=qbkw⊗∫t∈[tk,tk+1]qt˙bkdt=qbkw⊗∫t∈[tk,tk+1]21qtbk⊗[wtbk0]dt=qbkw⊗∫t∈[tk,tk+1]21qtbk⊗[wt^−bwt−nw0]dt==qbk⊗∫t∈[tk,tk+1]21Ω(w^t−bwt−nw)qtbkdt
3.2 离散形式下的IMU运动模型
3.2.1 欧拉法离散形式
使用欧拉法，即 k + 1 k+1 k+1时刻的位姿是用第 k k k时刻的测量值 a ^ b k \hat{a}_{b_{k}} a^bk, w ^ b k \hat{w}_{b_{k}} w^bk来计算的：
p b k + 1 w = p b k w + v b k w Δ t k + 1 2 a ^ b k δ t 2 p^{w}_{b_{k+1}} = p_{b_{k}}^{w} + v_{b_{k}}^{w} \Delta t_{k} + \frac{1}{2}\hat{a}_{b_{k}}\delta t^{2} pbk+1w=pbkw+vbkwΔtk+21a^bkδt2 v b k + 1 w = v b k w + a ^ b k δ t v_{b_{k+1}}^{w} = v_{b_{k}}^{w} + \hat{a}_{b_{k}} \delta t vbk+1w=vbkw+a^bkδt q b k + 1 w = q b k w ⊗ [ 1 1 2 w ^ b k δ t ] q^{w}_{b_{k+1}} = q^{w}_{b_{k}} \otimes \begin{bmatrix} 1\\ \frac{1}{2}\hat{w}_{b_{k}}\delta t \end{bmatrix} qbk+1w=qbkw⊗[121w^bkδt] 其中 a ^ b k = q b k w ( a b k − b a k ) − g w \hat{a}_{b_{k}} = q_{b_{k}}^{w}(a_{b_{k}}-b_{ak})-g^{w} a^bk=qbkw(abk−bak)−gw w ^ b k = w b k − b w k \hat{w}_{b_{k}} = w_{b_{k}}-b_{wk} w^bk=wbk−bwk
3.2.2 中值法离散形式
使用中值法，即 k + 1 k+1 k+1时刻的位姿是用俩个时刻 k k k和 k + 1 k+1 k+1测量值 a a a, w w w的平均值来计算的：
p b k + 1 w = p b k w + v b k w Δ t k + 1 2 a ^ ˉ t δ t 2 p^{w}_{b_{k+1}} = p_{b_{k}}^{w} + v_{b_{k}}^{w} \Delta t_{k} + \frac{1}{2}\bar{\hat{a}}_{t}\delta t^{2} pbk+1w=pbkw+vbkwΔtk+21a^ˉtδt2 v b k + 1 w = v b k w + a ^ ˉ t δ t v_{b_{k+1}}^{w} = v_{b_{k}}^{w} + \bar{\hat{a}}_{t} \delta t vbk+1w=vbkw+a^ˉtδt q b k + 1 w = q b k w ⊗ [ 1 1 2 w ^ ˉ t δ t ] q^{w}_{b_{k+1}} = q^{w}_{b_{k}} \otimes \begin{bmatrix} 1\\ \frac{1}{2}\bar{\hat{w}}_{t}\delta t \end{bmatrix} qbk+1w=qbkw⊗[121w^ˉtδt] 其中 a ^ ˉ t = 1 2 [ q b k w ( a b k − b a k ) − g w + q b k + 1 w ( a b k + 1 − b a k + 1 ) − g w ] \bar{\hat{a}}_{t} = \frac{1}{2}[q_{b_{k}}^{w}(a_{b_{k}}-b_{ak})-g^{w} + q_{b_{k+1}}^{w}(a_{b_{k+1}}-b_{ak+1})-g^{w}] a^ˉt=21[qbkw(abk−bak)−gw+qbk+1w(abk+1−bak+1)−gw] w ^ ˉ t = 1 2 ( w b k − b w k + w b k + 1 − b w k + 1 ) = 1 2 ( w b k + w b k + 1 ) − b w k \bar{\hat{w}}_{t} = \frac{1}{2}(w_{b_{k}}-b_{wk}+w_{b_{k+1}}-b_{wk+1}) \\ = \frac{1}{2}(w_{b_{k}}+w_{b_{k+1}})-b_{w{k}} w^ˉt=21(wbk−bwk+wbk+1−bwk+1)=21(wbk+wbk+1)−bwk 假设在短时间内加速度计和陀螺仪的偏置不变，则有： b a k = b a k + 1 , b w k = b w k + 1 b_{ak}=b_{ak+1}, \ b_{wk} = b_{wk+1} bak=bak+1, bwk=bwk+1
4.IMU预积分 (基于第K帧IMU body坐标系下的运动模型)
通过公式 ( 1 ) (1) (1)可以看到，IMU 的积分需要依赖与第 k k k 帧的 v v v 和 R R R (基于世界坐标系下的)，当我们在后端进行非线性优化时，需要迭代更新第 k k k 帧的 v v v 和 R R R，这将导致我们需要根据每次迭代后的值重新进行积分，这将非常耗时。我们考虑将优化变量从第 k k k 帧到第 k + 1 k+1 k+1 帧的 IMU 积分项中分离开来。
4.1 连续形式下的IMU运动模型
IMU预积分的思想就是将参考坐标系
最低0.47元/天 解锁文章
扫一扫
478
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
