高速無人機穩健雙目VIO演算法
編者序：相比於成熟的激光定位，視覺和IMU融合定位是未來的趨勢，尤其在無人機上是一種低成本的狀態估計解決方案。本文提出了雙目版的MSCKF緊耦合演算法，並在多個數據集上與現有的三種state of the art演算法進行了比較，在低cpu佔用率的同時獲得了與它們可比擬的估計精度。此外，本文開源了代碼，更值得為作者打call。
推薦指數
一、引言
精確穩健的狀態估計對機器人和無人機MAV至關重要，MAV需要正確的位姿估計保持空中平衡。結合視覺信息和IMU測量的VIO可以在沒有GPS的環境中工作良好，僅僅需要輕量級的感測器組合，因此是無人機平台的理想選擇。
在搜救和快速響應等場景，無人機需要飛行很遠的環境，光照條件、不均勻照射、低紋理場景和由風或操控導致的高度劇烈變化對VIO位姿估計產生了很大挑戰，要求VIO演算法不僅要精確，而且要穩健。
為了實現完全自主飛行，從低層次的感測器驅動到高層次的規劃演算法，所有的軟體模塊必須在類似於筆記本電腦的功耗熵實時運行，無人機載荷受限，FALCON如下圖所示。從而VIO演算法需要與其它模塊共享機上資源，必須高效且不能產生CPU佔用尖峰。
本文提出了一種基於濾波的雙目VIO演算法，比基於優化的演算法運算效率更高。我們選擇精度高、一致性好的MSCKF演算法作為演算法起點，在不同環境和運動下，雙目比單目更穩健。本文主要貢獻為：（1）本文是第一個開源的基於濾波的開源VIO演算法，能夠在沒有GPU加速下實時運行；（2）將本文的S-MSCKF和state-of-the-art開源VIO演算法OKVIS、ROVIO、VINS-MONO進行了精度、效率和穩健性的詳細比較，數據集包括EuRoc和FALCON採集的數據集；（3）公開了數據集。
二、相關工作
有大量關於VO的工作，從純VO、松耦合VIO到緊耦合VIO，本文則聚焦於緊耦合VIO。現有的緊耦合VIO可分為基於優化的方法和基於濾波的方法，前者通過最小化來自IMU和圖像的殘差獲得最優估計，大部分使用圖像上的稀疏特徵作為測量，該類方法可叫做非直接法。Usenko等人提出了利用圖像更多信息的直接法，最小化光度誤差，展示了基於優化的方法可以獲得更高精度，但由於迭代優化需要很多計算資源，近年來的一些高效求解器能夠在線實時運行。
基於濾波的方法一般使用EKF或UKF，更高效且能獲得與基於優化方法類似的精度。Huang等人也提出了第一估計雅可比FEJ和觀測約束OC以提高VIO演算法的一致性，可以反過來提高估計精度。最近也有直接法的基於濾波的VIO演算法以進一步提高精度和穩健性。
相對於單目，雙目或多目VIO演算法比較少，可能是由於圖像特徵帶來的運算量的增加。Leutenegger提出了一種能夠實時運行的多目VIO完整優化框架，Usenko在雙目VIO中引入了直接法以獲得精度的進一步提高，它們實時運行都需要強大的CPU進行優化。Paul等人提出了一種基於均方根逆濾波的雙目VIO濾波方法，能夠在移動設備上運行，但沒有開源。
三、實驗
為了評估所提演算法，進行了三個實驗。受限在EuRoC數據集上與OKVIS、ROVIO和VINS-MONO進行比較，然後在最高速度17.5m/s的高速數據集上驗證穩健下，這兩個實驗中，VINS-MONO的閉環檢測禁掉。最後，給出了S-MSCEKF的一個典型應用，在非結構化和未知環境中的飛行。
3.1 EuRoC數據集
OKVIS是基於優化的雙目演算法，ROVIO是基於濾波的單目演算法，VINS-MONO是基於優化的單目演算法。RMSE和平均CPU佔用率如下圖所示，CPU是NUC6i7KYK帶的i76770HQ雙核CPU。所提演算法並不適合V2_03_difficult數據，因為在時間跟蹤和雙目匹配時使用了KLT光流演算法以提高效率，而該組數據存在連續亮度變化使得雙目匹配錯誤，導致濾波器發散。在其餘的數據上，除了machine hall數據集上的ROVIO外四種方法精度類似。對於CPU佔用率，基於濾波的方法比基於優化的演算法更高效。OKVIS比VINS-MONO的CPU佔用率更高是由於使用了Harris角點和BRISK描述子進行匹配，OKVIS後端全速運行而VINS-MONO後端固定10Hz。S-MSCKF中80%的運算是特徵檢測、跟蹤、匹配前端佔用的，濾波器本身在單核20Hz時佔用10%。因此，本文演算法在精度和CPU佔用率上取得很好的折中。
3.2 高速飛行數據集
採集設備包括兩個40Hz解析度為960800的PointGrey CM3-U3-13Y3M-CS前視相機，一個200Hz的VectorNav VN-100 Rugged IMU，根據IMU觸發信號進行同步。為了在不同光照條件下得倒合適的圖像曝光，禁用相機的內部自動曝光，使用快速自適應外部控制器維持圖像常量平均亮度，控制器僅使用左圖進行亮度測量，然後對右圖應用同樣的曝光時間和增益，一些圖像如下圖所示。數據集包括5m/s、10m/s、15m/s和17.5m/s四組機場跑道上空，飛到300m外並返回。
精度和CPU佔用率如下圖所示，ROVIO有明顯的尺度漂移精度非常低而沒有可比性。在時間和偏航角對齊後根據GPS測量的x、y進行精度評估。S-MSCKF的CPU佔用率最低，且精度與其他演算法類似。與EuRoC數據集比，本文演算法在改組數據集上圖像處理前端計算量更大，一是由於幀頻和解析度更高，二是由於高速飛行導致更短的特徵周期，需要更頻繁的提取新特徵，17.5m/s時的軌跡如下圖所示。
3.3 自主飛行
無人機需要首先導航到樹叢中，找到入口並進入倉庫，找到目標然後返回起點。該試驗包括了不同類型的環境，缺少特徵、連續操控、光照明顯變化等。激光點雲和雙程軌跡如下圖所示，藍色、紅色和黃色的大圓點分別為起點、目標點和倉庫唯一入口，點雲使用VIO位姿配准。激光僅用於建圖，狀態估計僅僅使用雙目和IMU，超過700m的軌跡最終漂移僅為3m，低於0.5%的平移距離。
英文名稱：RobustStereo Visual Inertial Odometry for Fast Autonomous Flight
源碼：https://github.com/KumarRobotics/msckfvio
本公眾號專註於提供計算機視覺、機器學習、深度學習、智能感測器技術等人工智慧、機器人、無人車、無人機相關領域省時的知識轉述，敬請各位看官關注、評論、轉發、分享。
關注該公眾號，一起創造奇蹟。
