VINS_mono總結

對預積分的理解

預積分 是vins_mono中比較關鍵的一步，其主要目的是預測和處理IMU的實時數據，以得到機器人當前位置速度和姿態，並將數據傳遞給後端進行處理。其計算過程涉及到剛體座標變換、微積分、數值積分等相關數學知識。

座標系的選擇

vins_mono整體採用的是緊耦合的方式，即當前的圖像和imu信息直接參與當前位姿的計算，包括後端的建圖、重定位和迴環檢測。imu的輸出量爲原始加速度和角速度，考慮到重力因素，在後續的計算中會將重力加速度分離出來，剩下的部分爲imu實際的加速度。爲降低轉換座標產生的計算量，因此在計算中，由圖像信息計算出的當前位姿全部統一到imu的剛體座標系下（body座標系），在實際應用中，姑且認爲imu（而非相機）在世界座標系中的座標即是機器人的實際座標。

相機與慣導對齊

圖像採集和imu數據採集都是離散的過程，在實際應用中，如果要獲得當前時刻的機器人的位置速度和姿態，那就需要得到當前時刻的圖像和imu信息。imu的幀率一般比相機的幀率要高很多，在實際應用中，不可能針對每一幀的數據進行處理，這樣計算量會非常大，一般選取和圖像幀時刻對應時刻的imu信息進行處理，認爲兩個圖像幀之間的imu信息線性變化，用兩個圖像幀端點的imu信息的積分形式表示兩個圖像幀之間的變化信息。因此需要得到當前的相機和imu的幀率，得到兩個硬件幀率之間的線性關係，以選取相應的imu信息。
因此，固定的幀率對於vins_mono來說非常關鍵，最好的情況是兩個硬件保持時鐘同步，但是在工程應用中，兩個硬件很難做到理論上的同步，即使是visensor，其採用相同的硬件時鐘，也會出現很多問題，比如圖像的時間戳的選擇，不同設備廠家有不同的選取方式，有曝光前，有曝光後，也有曝光的中間時刻。論文的的算法採用了曝光的中間時刻，如此一來，如果設備採用曝光前或曝光後的時刻作爲圖像時間戳，那麼，在幀率爲20的情況下，延時就會達到50/2即25毫秒，會導致算法初始化失敗。當然，通過算法中的時間校準可以補償這個延時，這個在後面討論。

當前位置、速度、姿態的連續形式

已知當前機器人在世界座標系中位置和從imu採集到的第$k$幀（圖像幀）的原始加速度raw acc （ax ay az），角速度raw gyro（gx gy gz），計算得到第$k+1$幀的位置、速度和姿態。
vins_mono將初始化後的第一幀所在位置設爲世界座標系的原點。
位置：第$k+1$幀時刻imu在世界座標系中的座標，等於第$k$幀時刻imu在世界座標系中的位置，加上第$k$幀到第$k+1$幀時刻時移動的距離（第$k$幀時刻imu的速度乘以兩幀間的時間，加上兩幀之間的加速度的二次積分）。這裏$\hat{a}_t-b_{a_t}-n_a$是imu在兩幀之間運動時的加速度，用測量值減去偏移和噪聲得到，實際中測量值$\hat{a}$是不斷變化的，後面會提到，爲簡化計算，用數值積分的方法讓$\hat{a}$爲定值：
$p^w_{b_{k+1}} =p^w _{b_k}+v^w_{b_k}\Delta t_k+\int\int_{t\in[k,k+1]}[\bm R_t^w(\hat{a}_t-b_{a_t}-n_a)-g^w] dt^2 \tag{1}$

速度：第$k+1$幀時刻imu在世界座標系中的速度，等於第$k$幀時刻imu在世界座標系中的速度加上兩幀之間加速度的積分：
$v^w_{b_{k+1}} =v^w _{b_k}+\int_{t\in[k,k+1]}[\bm R_t^w(\hat{a}_t-b_{a_t}-n_a)-g^w]dt \tag{2}$

姿態：第$k+1$幀時刻imu在世界座標系中的姿態，這裏均用四元數表示，等於第$k$幀時刻imu在世界座標系中的姿態乘以兩幀之間的姿態變化量，其中，$\otimes$爲四元數乘法符號，$\bm\Omega(\omega)$是一個運算，這裏的$\omega$是$\hat{\omega}_t-b_{\omega_t}-n_a$，即兩幀間的角速度減去偏移和噪聲，和加速度一樣，$\hat{\omega}$也是不斷變化的：

$q^w_{b_{k+1}} =q^w _{b_k}\otimes\int_{t\in[k,k+1]}\frac12\bm \Omega (\hat{\omega}_t-b_{\omega_t}-n_w)q^{b_k}_t dt \tag{3}$
式中$\bm \Omega(\omega)=\begin{bmatrix} -\lfloor\omega\rfloor_{\times} & \omega \\ -\omega^T & 0 \\ \end{bmatrix}$，$\lfloor\omega\rfloor_{\times}=\begin{bmatrix} 0 & -\omega_z & \omega_y\\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \\ \end{bmatrix}$¹

兩幀之間的增量形式

上面3個公式是求解imu在世界座標系中位置速度姿態的迭代公式，通過觀察可見，如果要求第$k+1$幀世界座標系的位置、速度和姿態，需要先求第$k$幀世界座標系的位置、速度和姿態，如果之前某一幀的狀態發生變化，比如第3幀出現了重定位，優化更新了第3幀的世界座標，那麼3到$k$幀之間每幀的位置速度和姿態需要進行逐個計算。由於積分計算非常耗時，作者將公式修改爲增量形式，儘量簡化積分計算，簡單說就是把公式中的積分部分去除與世界座標系相關的變量，而且把重力加速度$g$提取出來放到外面。通過轉換座標系可以實現，將3個公式都轉換到imu（body）座標系下，這樣可以在系統尚未獲得與世界座標系關係時進行數據處理：
$\begin{aligned} \bm R^{b_k}_wp^w_{b_{k+1}} &=\bm R^{b_k}_w(p^w _{b_k}+v^w_{b_k}\Delta t_k-\frac12g^w\Delta t^2_k)+\alpha^{b_k}_{b_{k+1}}\\[2ex] \bm R^{b_k}_wv^w_{b_{k+1}} &=\bm R^{b_k}_w(v^w_{b_k}-g^w\Delta t)+\beta^{b_k}_{b_{k+1}}\\[2ex] q^{b_k}_w\otimes q^w_{b_{k+1}} &=\gamma^{b_k}_{b_{k+1}} \end{aligned}\tag{4}$
其中，
$\begin{aligned} \alpha^{b_k}_{b_{k+1}}& =\int\int_{t\in[k,k+1]}\bm R_t^{b_k}(\hat{a}_t-b_{a_t}-n_a) dt^2\\[2ex] \beta^{b_k}_{b_{k+1}} &=\int_{t\in[k,k+1]}\bm R_t^{b_k}(\hat{a}_t-b_{a_t}-n_a)dt\\[2ex] \gamma^{b_k}_{b_{k+1}} &=\int_{t\in[k,k+1]}\frac12\bm \Omega (\hat{\omega}_t-b_{\omega_t}-n_w)\gamma^{b_k}_t dt \end{aligned}\tag{5}$
公式（5）所示的變量僅與當前的測量值和偏移量有關，與重力無關，直觀來看類似一個在自由落體運動中的座標系。

當前幀的預積分

公式（5）描述了imu在兩幀之間位置、速度和姿態的增量，之前提到的測量值$\hat{a}$和$\hat{\omega}$在實際中是不斷變化的，處理這兩個變量用到了數值積分的相關知識，論文中提到了歐拉積分、中值積分、4階龍格庫塔積分等。論文中對歐拉積分進行了詳細說明，而代碼實現則用了中值法。
代碼實現（中值積分） 第i個imu時刻與第i+1個imu時刻的變量關係：
$\begin{aligned} \overline{\hat{a}_i}=&\frac12(\hat{a}_i+\hat{a}_{i+1})\,\,,\,\,\overline{\hat{\omega}_i}=\frac12(\hat{\omega}_i+\hat{\omega}_{i+1})\\[2ex] \hat\alpha^{b_k}_{i+1}& =\alpha^{b_k}_i+\hat\beta^{b_k}_i\delta t+\frac12\overline{\hat{a}_i}\delta t^2\\[2ex] \hat\beta^{b_k}_{i+1} &=\beta^{b_k}_i+\overline{\hat{a}_i}\delta t\\[2ex] \hat\gamma^{b_k}_{i+1} &=\hat\gamma^{b_k}_i \otimes \hat\gamma^i_{i+1} =\hat\gamma^{b_k}_i \otimes \left[\begin{array}{c} 1\\[1ex] \frac12\overline{\hat{\omega}_i}\delta t \end{array} \right] \end{aligned}\tag{6}$

未完待續。。。

---

1. 四元數的一階龍格-庫塔公式：$q(t+T)=q(t)+T\dot{q}(t)$，其中，$\dot{q}(t)=\frac12\Omega(t) q(t)$，具體公式推導可以查找飛控姿態解算等相關資料 ↩︎