Robust Initialization of Monocular Visual-Inertial Estimation on Aerial Robots

VINS初始化論文詳解

I.Motivation

單目相機在視覺系統應用中的短板

• 尺度不確定性
• 光照變化，弱紋理以及運動模糊等導致視覺跟蹤丟失

IMU作為視覺互補的傳感器的好處

• 恢復尺度，同時roll和pitch都可以直接獲取
• 可以幫助系統在高速飛行下有更好的穩定性，彌補視覺跟蹤丟失等情況

視覺慣導系統是一個非線性的系統，一個良好的初始值嚴重影響系統的性能，一個不良的初始值可能使估計器發散或者陷入局部最小化；

由于初始狀態的$R,t$是一個可變的原點狀態（即不需要考慮位置），因此實際需要估計的是重力向量，速度，傳感器偏差和特征點深度；

視覺慣導初始化的難點：

• 單目無法恢復尺度
• IMU需要非零加速度運動才可估計尺度
  • 因為IMU只有加速度計，需要積分才能得到速度和位移
  • 但由于加速度計獲得的數據是包含重力向量的，因此加速度與重力向量是耦合的
    • 因為是加速度運動，所以無法估計重力向量
    • 無法估計重力向量就無法得到得到加速度
• 任務中由于時間等限制因素，難以保證無人機能靜止或者按設定的運動進行初始化
  • 特別是在運動過程中，發生估計器失效需要重啟的情況
  • 因此凸顯了飛行中自動重新初始化的重要性

文章貢獻點：

• 未知先驗信息情況下，實現運動中的初始化速度，重力向量，尺度和陀螺儀的偏置
• 將中初始化算法應用與基于滑窗的單目視覺慣導系統并開源

II.Related Work

早期的視覺慣導系統大多假設某些初始狀態是已知的或者忽略某些初始狀態，抑或是需要系統在啟動前先保持靜止或者水平狀態。在沒有先驗信息下，大多數系統都無法適應動態起飛或運動中的初始化

S Shen 等人早期利用陀螺儀短暫的積分獲取相對旋轉實現對線性估計器的初始化，該法能很好的應用與室內環境，然而在特征點分布在大范圍的室外由于沒能覆蓋傳感器噪聲而表現不理想。

[7] S. Shen, Y. Mulgaonkar, N. Michael, and V. Kumar, “Initialization-free monocular visual-inertial estimation with application to autonomous MAVs,” in Proc. of the Int. Sym. on Exp. Robot., Marrakech, Morocco, Jun. 2014.

[12] Z. Yang and S. Shen, “Monocular visual–inertial state estimation with online initialization and camera–imu extrinsic calibration,” IEEE Transactions on Automation Science and Engineering, vol. 14, no. 1, pp. 39–51, 2017.

通過分析SfM和慣導積分實現初始化

[23] L. Kneip, S. Weiss, and R. Siegwart, “Deterministic initialization of metric state estimation filters for loosely-coupled monocular visioninertial systems,” in Proc. of the IEEE/RSJ Int. Conf. on Intell. Robots and Syst., Sep. 2011, pp. 2235–2241.

基于ORB-SLAM的慣導初始化，通過視覺估計得到估計值后進行初始化，然而據說其尺度收斂耗時往往多于10s，因此并不適合于一開始就需要尺度信息的機器人導航

[24] R. Mur-Artal and J. D. Tardos, “Visual-inertial monocular SLAM with map reuse,” arXiv preprint arXiv:1610.05949, 2016.

本文的方法于上一個方法比較相似，我們忽略了加速度計的偏置以確保初始化過程的快速性，同時發現加速度計的偏置與重力向量耦合，往往不可觀。關于加速度計偏置的估計會在Sect. IV-E中討論

III.Overview總體思想

純視覺可通過八點法，五點發，找本質矩陣或基礎矩陣獲得幀間相對位姿

[22] A. Heyden and M. Pollefeys, “Multiple view geometry,” Emerging Topics in Computer Vision, 2005.

[27] D. Nister, “An efficient solution to the five-point relative pose prob- ′ lem,” IEEE transactions on pattern analysis and machine intelligence, vol. 26, no. 6, pp. 756–770, 2004.

然后使用BA去優化，以得到尺度未知的地圖

[28] B. Triggs, P. F. McLauchlan, R. I. Hartley, and A. W. Fitzgibbon, “Bundle adjustmenta modern synthesis,” in International workshop on vision algorithms. Springer, 1999, pp. 298–372.

通過與imu測量值進行對齊進而提取初始化值（重力向量，速度和偏置）

構造一個松耦合的視覺慣導初始化過程

• 先進行純視覺的位姿估計
• 然后跟IMU預積分的值對齊恢復所需初始化參數

IV.Methodology

符號定義：

• $(?{)}^v$表示以SfM結果為參考系
• $\widehat{(?)}$表示以傳感器測量數據
• $\overline{(?)}$表示SfM尺度未知的估計參數

A. Vision-Only Structure

搜索出與最新幀有足夠視差的關鍵幀

通過五點法（即用五對點求解本質矩陣）求解兩幀間的相對位姿

隨機設置一個尺度$s$并將特征點全部三角化

利用PnP（Perspective-n-Point，3D-2D）來恢復滑窗內的其他幀的位姿

進行全局BA最小化重投影誤差

根據外參，將變量都轉移到imu坐標系下

$(?{)}^v$is the reference frame in SfM, which is an arbitrarily fixed frame in visual structure, irrelevant to inertial measurement.

注意這里都是在基于SfM的參考坐標系下

B. IMU Pre-Integration

IMU反饋的頻率比視覺的高許多，因此在兩幀間存在著大量的imu數據；對這些數據進行預積分，同時以預積分的結果作為增量度量約束

IMU數據測量方程：

受偏置bias $b$ 和噪聲 $\eta$ 影響

對于兩連續幀$b_k$和$b_{k+1}$，可以對線加速度和角速度在參考坐標系$b_k$下進行預積分，相對位移${\alpha }_{b_{k+1}}^{b_k}$，相對速度${\beta }_{b_{k+1}}^{b_k}$和相對旋轉${\gamma }_{b_{k+1}}^{b_k}$表示如下

可以知道這些相對變量獨立于參考系$b_k$的初始的位置和速度

IMU預積分補充知識（參考VINS-MONO）

IMU測量方程：

兩幀間$[t_k,t_{k+1}]$世界坐標系下的普通積分方程：

補充：基于擾動模型的四元數導數

Sola J. Quaternion kinematics for the error-state Kalman filter[J]. arXiv preprint arXiv:1711.02508, 2017.

P44, P7

從上述世界坐標系下的積分可以看出，每當初始狀態改變都需要重新積分，特別是基于優化的算法，意味著每調整一次位姿都需要重新進行積分，極其消耗算力，因此預積分方法應運而生。

通過將參考坐標系由世界坐標系改為相對參考系（local frame）$b_k$，即兩幀間積分中的前一幀為參考系，這樣只需要預積分一次在該參考系下的線加速度和角速度則在以后都可以復用。

這些預積分項現在只與偏置和噪聲有關，如果偏置變化很小，我們可以通過一階近似調整這些預積分項；如果變化較大的話，還是需要重新進行積分。

離散時間預積分

• 歐拉法（Euler）
• 中值積分法（mid-point）
• 龍格庫塔積分法（RK4）

以歐拉法為例作講解，代碼使用的是中值積分法：

初始狀態下，${\alpha }_{b_k}^{b_k}=0$, ${\beta }_{b_k}^{b_k}=0$, ${\gamma }_{b_k}^{b_k}=[1,0_v{]}^T$

噪聲項未知，設其為0

后續協方差傳播和動態誤差方程有緣再更，可自行看vins-mono的論文

C. Visual-Inertial Alignment

前面已經獲得了由SfM得到的尺度未知的相機姿態和IMU的預積分。

1) Gyroscope Bias Calibration

已知：

• 由視覺估計獲取的相對姿態${\mathbf{q}}_{b_k}^v$和${\mathbf{q}}_{b_{k+1}}^v$
• 由預積分得到的姿態變換約束${\gamma }_{b_{k+1}}^{b_k}$

最小化誤差以獲取陀螺儀偏置

稍微解釋一下誤差函數的構造
$$\begin{gathered} {{\mathbf{q}}_{b_{k+1}}^v}^{?1}?{\mathbf{q}}_{b_k}^v?{\gamma }_{b_{k+1}}^{b_k} \\ ={{\mathbf{q}}_{b_{k+1}}^v}^{?1}?{\hat{\mathbf{q}}}_{b_{k+1}}^v \\ ={{\bar{\gamma }}_{b_{k+1}}^{b_k}}^{?1}?{\gamma }_{b_{k+1}}^{b_k} \end{gathered}$$
綜上可以獲得偏置的更新量$\delta b_g$，將此更新量加到初始偏置上$b_{g_0}=0$上獲得估計的陀螺儀偏差，使用估計的陀螺儀偏差重新更新預積分

至此視覺與IMU已經完成了初略的對準，即姿態匹配上了

2) Initializing Velocity, Gravity Vector and Metric Scale

定義待估計變量：

對應關鍵幀的速度，重力向量和尺度

定義殘差為預積分（IMU測量值）與視覺估計的誤差

其中${\hat{\mathbf{z}}}_{b_{k+1}}^{b_k}=\left[\begin{array}{l}{\hat{\mathbf{\alpha }}}_{b_{k+1}}^{b_k}\\ {\hat{\mathbf{\beta }}}_{b_{k+1}}^{b_k}\end{array}\right]$為預積分數據，${\mathbf{H}}_{b_{k+1}}^{b_k}{\mathcal{X}}_I+{\mathbf{n}}_{b_{k+1}}^{b_k}$為SfM得到的數據，這里依然設定噪聲項${\mathbf{n}}_{b_{k+1}}^{b_k}=0$，其中尺度$s$隱含在SfM測量函數中，由以下公式關聯

由此可以構造最小二乘問題求解得速度，重力向量以及尺度

這里省略億點點過程，主要是如何從SfM估計的位姿轉化為預積分對應項，以及代碼中如何將最小二乘問題轉化為構造并求解$Ax=b$，基本原理還是很簡單的，有空再更新下過程。

3) Gravity Refinement

由于一個地方的重力是可以測量得到的，也即重力的模已知的；因此，優化重力向量從三維變為二維。然而若把重力的模作為約束加入到上述問題中將會使其轉變為非線性問題，也即無法利用$Ax=b$來進行高效的求解。

將重力向量在切線空間（tangent space）上進行參數化
$$g?\overline{\hat{\mathrm{g}}}+w_1{\mathbf{b}}_1+w_2{\mathbf{b}}_2$$
$g$為重力的模，$\overline{\hat{\mathrm{g}}}$為當前估計的重力方向，${\mathbf{b}}_1,{\mathbf{b}}_2$為切線空間中的兩個正交基，${\omega }_1,{\omega }_2$為對應方向${\mathbf{b}}_1,{\mathbf{b}}_2$上的位移

將$g?\overline{\hat{\mathrm{g}}}+w_1{\mathbf{b}}_1+w_2{\mathbf{b}}_2$，帶回${\hat{\mathbf{z}}}_{b_{k+1}}^{b_k}={\mathbf{H}}_{b_{k+1}}^{b_k}{\mathcal{X}}_I+{\mathbf{n}}_{b_{k+1}}^{b_k}$，依然可以得到一個線性的最小二乘問題，通過迭代或者構造$Ax=b$問題求解出${\omega }_1,{\omega }_2$，即可以得到精細化后的重力向量

根據估計的重力向量，可以將之前估計的所有狀態都轉換到世界坐標系下，綜上完成了VIO的初始化過程。

D. Nonlinear VINS Estimator

基于圖優化的緊耦合視覺慣導系統

對應的狀態表達如下（忽略轉置）：

${\mathbf{X}}_k$對應IMU在第$k^{th}$幀的狀態，$n$為滑窗內關鍵幀總數，$m$為特征點個數，用逆深度${\lambda }_i$表示，${\mathbf{x}}_c^b$為外參

將狀態估計問題構造為非線性最小二乘問題如下：

第一項為邊緣化的先驗信息，第二項為IMU測量殘差，第三項為視覺重投影殘差。具體看VINS-MONO論文

E. Discussions

為了保證系統的能觀性（除了全局位置漂移和yaw角），視覺及IMU初始化都需要充分的激勵。

其中視覺可以選取擁有足夠視差的關鍵幀來保證，而對于IMU測量來說，能保證SfM成功的那些幀對應的IMU測量數據并不一定能保證對應IMU初始化的成功，對于某些直線或勻速運動是無法避免的。因此直覺上，可以通過檢查加速度及角速度的方差或者標準差來判斷IMU的激勵程度，來決定是否需要進行IMU的初始化

然而代碼里即便在IMU未完全激勵下，仍不會阻止系統進行初始化，而對應結果是運動狀態初始化無太大影響，而在靜止狀態強行進行初始化，會因為加速度計無有效數據進行優化導致系統漂移，此時補充一個靜止狀態下的加速度計偏置估計能有效解決問題

論文進行對加速度計偏置也做了與角速度計偏置對應的處理來進行求解，通過仿真實驗發現需要在短時間內至少30°的旋轉才能很好的估計加速度計的偏置，論證了初始化階段難以對加速度計偏置進行估計

而另一個仿真實驗，忽略了加速度計偏置，考慮角速度計偏置，發現即便忽略加速度計的偏置，仍能獲得一個良好的初始估計

綜上，在由足夠運動激勵下，能夠在初始化階段忽視加速度計偏置

V. EXPERIMENTAL RESULTS

A. Performance on Public Datasets

1) Initial Values Recovery

分別進行初始時刻初始化以及隨機選時間進行初始化以驗證飛行途中重新初始化的性能，下圖展示了在MH 01 easy數據集上進行25次隨機時間初始化結果，若以尺度估計的誤差低于10%為成功，則成功率高達84%，同時，即便誤差高達30%非線性估計器也能夠成功運行

2) Overall Performance

以提出的VINS系統對比了OKVIS，一個雙目視覺慣導系統

可以看到即便VINS是單目系統，但總體性能優于雙目的OKVIS；

同時即便初始化階段忽略了加速度計偏置的估計，但相應偏差項都能夠收斂于真值。

B. Real World Experiment

1) Indoor Closed-Loop Control

2) Outdoor Environments

總結

以上是生活随笔為你收集整理的Robust Initialization of Monocular Visual-Inertial Estimation on Aerial Robots的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。