IMU與視覺資訊融合—手寫VIO課程筆記2（下）

BlueAkoasm發表於2020-12-19

VIO殘差函式構建

帶權重（方差）的殘差計算： $r = \parallel f(x)\parallel ^2_\sum$ ，其中f(x)服從高斯分佈，協方差為 $\sum$

協方差可以將所有殘差變化到一個統一的無量綱的範圍內，可以將不同殘差進行相加

協方差還能起到一個權重的作用，協方差越小，則它的逆就越大，該測量值就更可信

基於滑動視窗的VIO Bundle Adjustment

為了節約計算量採用滑動視窗形式的Bundle Adjustment，在 i時刻，滑動視窗內待優化的系統狀態量定義如下：

其中，xi包含i時刻IMU機體的在慣性座標系中的位置，速度，姿態，以及IMU機體座標系中的加速度和角速度的偏置量估計

n，m分別是機體狀態量，路標在滑動視窗裡的起始時刻。

N滑動視窗中關鍵幀數量

M是被滑動視窗內所有關鍵幀觀測到的路標數量

視覺重投影誤差

定義：一個特徵點在歸一化相機座標系下的估計值和觀測值的差

$r_c=\begin{bmatrix} \cfrac{x}{z}-u\\ \cfrac{y}{z}-v\ \end{bmatrix}$ ,其中，待估計的狀態量為特徵點的三維空間座標(x,y,z)T,觀測值(u,v)T為特徵在相機歸一化平面的座標。

逆深度引數化：特徵點在歸一化相機座標系與在相機座標系下的座標關係為：

$\begin{bmatrix} x\\ y\\ z \end{bmatrix}=\frac{1}{\lambda }\begin{bmatrix} u\\ v\\ 1 \end{bmatrix}$ ，其中 $\lambda$ =1/z是逆深度。

VIO中基於逆深度的重投影誤差：特徵點逆深度在第i幀中初始化得到，在第j幀又被觀測到，預測其在第j中的座標為

$\begin{bmatrix} x_c_j\\ y_c_j\\ z_c_j\\ z \end{bmatrix}=T_b_c^{-1}T_{wbj}^{-1}T_{wbi}T_{bc}\begin{bmatrix} \frac{1}{\lambda }u_c_i\\ \frac{1}{\lambda }v_c_i\\ \frac{1}{\lambda }\\ 1 \end{bmatrix}$

相機座標系c1→body座標系b1→body座標系b2→相機座標系c2

視覺重投影誤差為：

$r_c=\begin{bmatrix} \frac{x_c_j}{z_c_j}-u_c_j\\ \frac{y_c_j}{z_c_j}-v_c_j \end{bmatrix}$

IMU測量值積分：

上標g表示gyro，a表示acc，w表示在世界座標系world，b表示imu機體座標系body

PVQ對時間的導數為：

$\dot{p}_{wbt}=v_t^w$ , $\dot{v}_t^w=a_t^w$ , $\dot{q}_{wbt}=q_{wbt}\otimes \begin{bmatrix} 0\\ \frac{1}{2}\omega ^b^t \end{bmatrix}$

從第i時刻的PVQ對IMU的測量值進行積分得到第j時刻的PVQ：（前面第一次筆記已經推導過）

用此種方法，每次 $q_{wbt}$ 優化更新後，都需要重新進行積分，計算量大，所以採用預積分的方法

IMU預積分

一個很簡單的公式轉換，就可以將積分模型轉為預積分模型：

$q_{wbt}=q_{wbi}\otimes q_{bibt}$

那麼，PVQ積分公式中的積分項則變成相對於第i時刻的姿態，而不是相對於世界座標系的姿態

預積分量：預積分量僅僅跟IMU測量值有關，它將一段時間內的IMU資料直接積分起來就得到了預積分量

重新整理得到：

預積分誤差：一段時間內IMU構建的預積分量作為測量值，對兩時刻之間的狀態量進行約束，

其中，位移、速度和偏置都是直接相減得到，第二項是關於四元數的旋轉誤差，其中[]xyz表示只取四元數的虛部（x,y,z）組成的三維向量。

預積分的離散形式

使用mid-point方法，兩個相鄰時刻k到k+1的位姿是用兩個時刻的測量值a，w的平均值來計算

預積分量方差的計算

協方差的傳遞：已知一個變數y = Ax， $x\in N(0,\Sigma _x)$ ，則 $\Sigma _y=A\Sigma _xA^T$

假設已知相鄰時刻誤差的線性傳遞方程： $\eta _i_k=F_{k-1}\eta _{ik-1}+G_{k-1}n_{k-1}$

誤差的傳遞由兩部分組成：當前時刻的誤差傳遞給下一時刻，當前時刻測量噪聲傳遞給下一時刻。

協方差矩陣可以通過遞推計算得到： $\Sigma _i_k=F_{k-1}\Sigma _{ik-1}F_{k-1}^T+G_{k-1}\Sigma _nG_{k-1}^T$ ，其中 $\Sigma _n$ 是測量噪聲的協方差矩陣，方差從i時刻開始進行遞推， $\Sigma _i_i$ = 0

狀態誤差線性遞推公式的推導

通常對於狀態量之間的遞推關係是非線性的方程，如 $x_k=f(x_{k-1},u_{k-1})$ ，其中狀態量為x，u是系統的輸入量

用兩種方法來推導狀態誤差傳遞的線性遞推關係：

一種是基於一階泰勒展開的誤差遞推方程

一種是基於誤差隨時間變化的遞推方程

基於一階泰勒展開的誤差遞推方程

令狀態量為 $x = \hat{x} + \delta x$ ，其中真值為 $\hat{x}$ ，誤差為 $\delta x$ ，另外輸入量u的噪聲為n

基於泰勒展開的誤差傳遞（應用於EKF的協方差預測）：

非線性系統 $x_k=f(x_{k-1},u_{k-1})$ 的狀態誤差的線性遞推關係如下：

$\delta x_k=F\delta x_{k-1}+Gn_{k-1}$ ，其中F是狀態量xk對狀態量xk-1的雅克比矩陣，G是狀態量xk對輸入量uk-1的雅克比矩陣

證明：進行一階泰勒展開有

其中， $\hat{x}=f(\hat{x}_{k-1},\hat{u}_{k-1})$ ，所以兩邊可以約掉，即可導上述遞推關係。

基於誤差隨時間變化的遞推方程

如果我們能夠推導狀態誤差隨時間變化的導數關係，比如 $\delta \dot{x}=A\delta x+Bn$

則誤差狀態的傳遞方程為：

$\delta x_k=\delta x_{k-1}+\delta \dot{x}_{k-1}\Delta t$

$\rightarrow \delta x_k=(I+A\Delta t)\delta x_{k-1}+B\Delta tn_{k-1}$

通過這兩種推導方式可以看出： $F = I + A\Delta t$ , $G = B\Delta t$

為什麼會要弄誤差隨時間的變化呢？

這是因為VIO系統中已經知道了狀態的導數和狀態之間的轉移矩陣，如我們已經知道了速度和狀態量之間的關係： $\dot{v}=Ra^b+g$

那麼就可以推導速度的誤差和狀態誤差之間的關係，再每一項上都加上各自的誤差就有：

由此就可以類推，輕易寫出整個A和B其他方程了。