VINS-Mono-VIO初始化 (三:陀螺仪零偏初始化)

之前估计的旋转等我外参,加速度计的零偏不在这里进行估计,会在后面滑窗优化的时候进行估计
q b k c 0 q^{c_{0}}_{b_{k}} qbkc0 表示枢纽帧 c 0 c_{0} c0 到第 k k k 帧imu的变换
q c k c 0 q^{c_{0}}_{c_{k}} qckc0 表示枢纽帧 c 0 c_{0} c0 到第k帧相机的变换
q c b q^{b}_{c} qcb 表示旋转外参的估计量,上一节推导的

通过公式 q b k c 0 = q c k c 0 ⊕ ( q c b ) − 1 q^{c_{0}}_{b_{k}}=q^{c_{0}}_{c_{k}}⊕(q^{b}_{c})^{-1} qbkc0=qckc0(qcb)1就能获得枢纽帧到第k帧imu的变换,这个第k是按相机的顺序来算的

然后平移公式如下: s P b k c 0 = s P c k c 0 − R b k c 0 P c b sP^{c_{0}}_{b_{k}}=sP^{c_{0}}_{c_{k}}-R^{c_{0}}_{b_{k}}P^{b}_{c} sPbkc0=sPckc0Rbkc0Pcb s s s 是尺度
通过公式 T c 0 c k = T c 0 b k ⋅ T b c T_{c_{0}c_{k}}=T_{c_{0}b_{k}}·T_{bc} Tc0ck=Tc0bkTbc 展开推一下就出来了

上来两个公式等于是用已知量算出来了 T c 0 b k T_{c_{0}b_{k}} Tc0bk,这个是用视觉算出来旋转乘上外参算出来的,这样就和imu坐标系有关联了,后面才乘上个 T b k + 1 c 0 T_{b_{k+1}c_{0}} Tbk+1c0 ,就能获得两个imu之间的旋转 T b k + 1 b k T_{b_{k+1}b_{k}} Tbk+1bk

整体思想就是,通过相机计算出来的旋转(得通过外参转换到imu坐标系下)理论上是等于预积分出来的旋转的
求陀螺仪零偏的公式如下
m i n ∑ ∣ ∣ ( q b k + 1 c 0 ) − 1 ⊕ q b k c 0 ⊕ γ b k + 1 b k ∣ ∣ 2 min∑||(q^{c_{0}}_{b_{k+1}})^{-1}⊕q^{c_{0}}_{b_{k}}⊕γ^{b_{k}}_{b_{k+1}}||^{2} min∣∣(qbk+1c0)1qbkc0γbk+1bk2
里面的 ( q b k + 1 c 0 ) − 1 ⊕ q b k c 0 = q b k b k + 1 (q^{c_{0}}_{b_{k+1}})^{-1}⊕q^{c_{0}}_{b_{k}}=q^{b_{k+1}}_{b_{k}} (qbk+1c0)1qbkc0=qbkbk+1 ,这个本质上是和 γ b k + 1 b k γ^{b_{k}}_{b_{k+1}} γbk+1bk 的逆是一样的,他们两个相乘结果理论上是单位矩阵, γ b k + 1 b k γ^{b_{k}}_{b_{k+1}} γbk+1bk是预积分量。
由于零偏的存在,所以他们的相乘不会是单位阵
更新的时候采用以下公式
γ b k + 1 b k ≈ γ b k + 1 b k ^ ⊕ [ 1 1 2 J b w γ ∆ b w ] γ^{b_{k}}_{b_{k+1}}≈\hat{γ^{b_{k}}_{b_{k+1}}}⊕\begin{bmatrix}1\\\frac{1}{2}J^{γ}_{b_{w}}∆b_{w}\end{bmatrix} γbk+1bkγbk+1bk^[121Jbwγbw]
上述的意思是当零偏经过优化发生变化是,通过获取零偏变化的微量 ∆ b w ∆b_{w} bw 乘上旋转量对零偏的雅克比矩阵(这个在前面零偏建模的时候有过推导是怎么来的),然后通过这样的方式更新旋转量

然后计算公式变为如下
[ 1 0 0 0 ] = ( q b k + 1 c 0 ) − 1 ⊕ q b k c 0 ⊕ γ b k + 1 b k ^ ⊕ [ 1 1 2 J b w γ ∆ b w ] \begin{bmatrix} 1\\0\\0\\0 \end{bmatrix}=(q^{c_{0}}_{b_{k+1}})^{-1}⊕q^{c_{0}}_{b_{k}}⊕\hat{γ^{b_{k}}_{b_{k+1}}}⊕\begin{bmatrix}1\\\frac{1}{2}J^{γ}_{b_{w}}∆b_{w}\end{bmatrix} 1000 =(qbk+1c0)1qbkc0γbk+1bk^[121Jbwγbw]
这样就可以更新零偏然后知道满足结果为0为止,前面计算的对零偏的雅克比也是在这里这样用的

由于 ( q b k + 1 c 0 ) − 1 ⊕ q b k c 0 ⊕ γ b k + 1 b k ^ (q^{c_{0}}_{b_{k+1}})^{-1}⊕q^{c_{0}}_{b_{k}}⊕\hat{γ^{b_{k}}_{b_{k+1}}} (qbk+1c0)1qbkc0γbk+1bk^ 是已知的,都拿到左边去
则有
[ ( γ b k + 1 b k ^ ) − 1 ⊕ ( q b k c 0 ) − 1 ⊕ q b k + 1 c 0 ] v e c = 1 2 J ∆ b (\hat{γ^{b_{k}}_{b_{k+1}}})^{-1}⊕(q^{c_{0}}_{b_{k}})^{-1}⊕q^{c_{0}}_{b_{k+1}}]_{vec}=\frac{1}{2}J∆b (γbk+1bk^)1(qbkc0)1qbk+1c0]vec=21Jb
上面的 v e c vec vec 指的是取虚部,因为只有需要发生变化,实部一直都是1,整个公式的未知量只有 ∆ b ∆b b ,整个问题可以认为是 Ax=b 的模型, Ax=0的话就可以用 SVD来进行分解。
这里会有很多组数据,会把 A , b A,b A,b 矩阵累积扩大
代码中进行小矩阵累加,就是为了避免大矩阵的运算,代码中用+=来对矩阵进行累加
只要最小化 ∣ ∣ A x − b ∣ ∣ m i n ||Ax-b||_{min} ∣∣Axbmin 即可
可以这样去进行记忆
A x = b Ax=b Ax=b
= > A T A x = A T b =>A^{T}Ax=A^{T}b =>ATAx=ATb
= > x = = ( A T A ) − 1 ⋅ A T ⋅ b =>x==(A^{T}A)^{-1}·A^{T}·b =>x==(ATA)1ATb
这里的 x x x 结果就是我们的 ∆ b ∆b b ,这个结果就是我们要的零偏估计

求解出零偏后会对滑窗中的预积分根据新的零偏重新进行传播,因为第一次估计的 ∆ b ∆b b 会比较大,所以才会重新传播,后面都是用雅克比矩阵进行一阶近似来传播的,算出来的是 ∆ b ∆b b ,所以代码中更新零偏也是采用+=的方式

你可能感兴趣的:(VINS系列专栏,算法,自动驾驶,线性代数,c++,人工智能)