文章：GVINS: Tightly Coupled GNSS-Visual-Inertial Fusion for Smooth and Consistent State Estimation

作者：Shaozu Cao, Xiuyuan Lu, and Shaojie Shen

代碼：https://github.com/HKUST-Aerial-Robotics/GVINS

編譯：點云PCL

本文僅做學術分享，如有侵權，請聯系刪除。歡迎各位加入免費知識星球，獲取PDF論文，歡迎轉發朋友圈。內容如有錯誤歡迎評論留言，未經允許請勿轉載！

公眾號致力于分享點云處理，SLAM，三維視覺，高精地圖相關的文章與技術，歡迎各位加入我們，一起每交流一起進步,有興趣的可聯系微信：920177957。本文來自點云PCL博主的分享，未經作者允許請勿轉載，歡迎各位同學積極分享和交流。

摘要

視覺慣性里程計（VIO）眾所周知在長期的運行中會有累計誤差。在本文中提出了GVINS，一個基于非線性優化的系統，它將GNSS原始測量值與視覺和慣導信息緊密地融合起來，用于實時和無漂移的狀態估計。本文的系統的目標是在復雜的室內外環境下提供精確的全局6自由度姿態估計，在這種環境下，GNSS信號可能被大量丟失甚至完全不可用。為了將全局測量與局部狀態聯合起來，本文提出了一種由粗到精的初始化方法，可以有效地在線標定變換，并在很短的測量滑動窗口內對GNSS狀態進行初始化。然后在因子圖框架下，結合視覺和慣導約束，對GNSS偽距和多普勒頻移測量進行建模和優化。對于復雜和GNSS不友好的區域，對退化場景進行了討論和處理，以保證里程計的魯棒性。該系統所涉及的工程挑戰也包括在內，以便于相關的GNSS融合研究。由于采用了緊耦合的多傳感器方法和系統設計，我們的系統充分利用了三種傳感器的優點，能夠無縫地應對室內和室外環境之間的過渡，即便在衛星丟失和重新捕獲的情況下。我們通過仿真和實際的實驗對所提出的系統進行了廣泛的評估，結果表明，盡管GNSS測量有噪聲，我們的系統仍然有效地消除了VIO的漂移，并保持了系統的局部精度。此外，實驗還表明，我們的系統甚至可以從一顆衛星獲得增益，而傳統的GNSS算法至少需要四顆衛星。

介紹

基于多傳感器融合的狀態估計方法已經被證明是有效和魯棒的，并且在這方面有大量的文獻。其中，我們特別感興趣的是將小型低成本的傳感器，如相機、IMU和GNSS接收機結合起來，在未知環境下進行實時準確的估計。視覺與慣性測量的緊密耦合融合可以分為基于濾波的方法和基于優化的方法。MSCKF是一種優秀的基于濾波器的狀態估計算法，它利用多攝像機姿態之間的幾何約束來有效地優化系統狀態。與基于濾波器的方法相比，非線性批處理優化方法通過重新線性化，以犧牲計算量為代價，可以獲得更好的性能。OKVIS利用基于關鍵幀的滑動窗口優化方法進行狀態估計。VINS Mono還優化了滑動窗口內的系統狀態，但在線重新定位和姿態圖優化更為完善。但是無法避免的在長期運行中的累計誤差。

圖1。我們的系統在復雜的室內外環境中的快照。全局估計結果直接繪制在Google地圖上，并與第（a）部分所示的真值RTK軌跡很好地吻合。（b）部分描述了衛星的分布，切線方向表示方位角，徑向方向表示仰角。藍色箭頭是一個類似指南針的應用程序，指示相機的全局偏航方向。子圖（c）和（d）分別表示姿態信息和局部偏航偏移。每顆被跟蹤衛星的測量噪聲級如（e）部分所示。注意，當我們走室內樓梯時，RTK軌跡上有一個明顯的故障，而我們的系統即使在室內環境中也可以進行全局估計。

本文效果如下圖1 ，其貢獻：

由于緊耦合的方法和系統設計，我們的系統充分利用了全球導航衛星系統、視覺和慣性測量之間的互補性，即使在復雜的環境中也能夠提供局部平滑和全局一致的估計，如圖1所示，本文的貢獻如下：

*一種從粗到精的在線方法來初始化系統

*一種基于優化的GNSS+VIO緊耦合方法，在概率框架下將視覺慣性數據與多星座GNSS原始測量數據融合一種實時估計器，

*能夠在GNSS信號可能被大量丟失甚至完全不可用的復雜環境中提供無漂移的6自由度全局估計

*在仿真和真實環境中對所提出的系統進行了評估。

主要內容

本文提出的系統結構下圖5所示。估計器以原始GNSS、IMU和相機測量值作為輸入，然后對每種類型的測量進行必要的預處理。IMU測量值被預積分，整個圖像被概括為一系列稀疏的特征點。對于GNSS原始數據，首先過濾掉容易出錯的低俯仰角和不良的衛星信號。為了抑制不穩定的衛星信號，只允許連續鎖定一定時間的衛星進入系統。由于星歷數據是通過慢速衛星接收器無線鏈路（50位/秒）獲取的，因此在相應的星歷完全傳輸之前，全球衛星導航系統的測量是不可用的。在預處理階段之后，所有的測量都為估計器準備好了，但是在進入優化部分之前，需要一個初始化階段來正確初始化非線性估計器的系統狀態。初始化過程從一個純視覺的SfM開始，由該SfM聯合估計一個最接近的運動和結構，然后將IMU的運動軌跡與SfM結果對齊，以恢復IMU的尺度、速度、重力和IMU偏差。

圖5 上圖顯示了本文提出的系統的工作流程。首先，所有傳感器的測量值在進入后續程序之前都經過預處理。在初始化階段，視覺慣性初始化是通過將慣性信息與純視覺SfM的結果對齊來完成的。如果視覺慣性系統成功對準，則執行從粗到精的過程，以初始化GNSS狀態。一旦全球衛星導航系統介入，該系統將監測和處理全球衛星導航系統退化情況。最后在非線性優化框架下對滑動窗口內的測量約束進行優化。注意，如果GNSS不能初始化，我們的系統仍然可以在視覺慣性模式下工作。同時采用邊緣化策略，保證實時估計。

GNSS初始化和退化
本流程的狀態估計過程相對于系統狀態是非線性的，因此其性能在很大程度上依賴于初始值。通過在線初始化，可以很好地從未知情況中恢復初始狀態，無需任何假設或人工干預。在系統運行過程中，估計器還可能遇到一些傳感器發生故障或退化的不完美情況。由于關于視覺慣性系統的初始化和退化已經有大量的文獻。

初始化

初始化是需要一個已知全局和局部坐標的錨點，將全球GNSS測量與局部視覺和慣性信息進行融合。由于錨點已經設置到局部世界框架的原點，因此需要事先校準局部的世界原點的ECEF坐標。在本文中，我們提出了一個多階段GNSS-VI初始化程序來在線校準錨點和偏航偏移ψ 在ENU和局部世界框架之間。在GNSS-VI初始化之前，假設VIO已成功初始化，即重力矢量、初始速度、初始IMU偏差和比例已獲得初始值。之后，在局部世界框架中形成一條平滑的軌跡，并準備用于GNSS-VI初始化階段。如圖7所示，在線GNSS-VI初始化以從粗到精的方式進行，包括以下三個步驟：

1）粗定位點定位；
2）偏航角偏移校準；
3）錨點優化。

圖7。本文提出的從粗到精初始化過程的圖示。該模塊從VIO中獲取局部位置和速度結果，并在全局ECEF幀中輸出相應的軌跡。

GNSS實際案例
毫無疑問，我們的系統將在GNSS系統信號穩定、衛星分布均勻的開闊地區發揮最佳性能。下面我們將討論幾種可能會降低系統性能的情況

1） 低速運動：由于多普勒頻移測量的噪聲級比偽距測量的噪聲級低一個數量級，因此局部幀和ENU幀之間的偏航偏移可以通過多普勒頻移測量的短窗口得到很好的約束。一旦GNSS接收機的速度低于多普勒頻移的噪聲水平，估計的偏航偏移量就可能被測量噪聲所破壞。此外，低速運動也意味著窗口內的平移距離較短，因此偏航估計也可能受到偽距的影響。在極端情況下，當平臺經歷純旋轉運動時，GNSS不能提供任何關于旋轉方向的信息，而偏航分量又會像VIO那樣漂移。因此，如果窗口內的平均速度低于閾值vths，我們固定偏航偏移變量。在我們的系統中，vths設置為0.5m/s，即使行人也能很容易地滿足要求。

2） 少于4顆被跟蹤的衛星：如果被跟蹤的衛星數量少于4顆，SPP或松散耦合的方法將無法解析接收器的位置。然而，在緊耦合結構的幫助下，我們的系統仍然能夠利用可用的衛星并隨后更新狀態向量。

3） 無GNSS信號：在GNSS信號完全不可用的室內或雜亂環境中，與全局信息有關的狀態，即偏航偏移ψ, 接收機時鐘偏差δt和漂移率˙δ它們不再是可見的。然而，在優化過程中，仍然保持等式（24）和（25）的約束。

實驗

我們進行了模擬和真實世界的實驗來驗證我們提出的系統的性能。在本節中，我們的系統與VINS Mono、VINS Fusion[（Monocular+IMU+GNSS）和RTKLIB進行比較。由于我們只對實時估計結果感興趣，因此禁用了VINS Mono和VINS融合的循環函數。我們使用RTKLIB3來計算GNSS SPP解，并將得到的GNSS位置反饋給VINS融合，得到松散耦合的結果。

A 仿真環境

1） 設置：模擬環境為30m× 30米 具有隨機生成的三維地標的30m立方體。這些地標被投影到一個10hz的虛擬攝像機上，這個攝像機有75度的水平視場和55度的垂直視場，然后每幀產生大約100個可見特征。在所有特征點上增加一個標準偏差為0.5像素的白噪聲項。一個虛擬的200hz IMU與攝像機剛性連接，并沿著預先設計的3D路徑移動。

圖9.在仿真環境中，GVINS、VINS融合和VINS-mono相對于評估距離的相對姿態誤差。上面兩個圖對應VIO的四個不可觀察方向（x、y、z和偏航），下面的圖是整體相對旋轉誤差。

圖10描繪了絕對軌跡誤差（ATE）隨行駛距離的變化。

2） 結果：圖9顯示了相對于評估距離的相對位姿誤差（RPE）。從圖中可以看出，在平移和旋轉方向上，VINS Mono的相對誤差都隨著評估距離的增加而增加。其中轉動誤差主要來自偏航分量。這表明VINS Mono在四個不可觀察的方向（即x、y、z和偏航）上遭受累積漂移。當評價距離較短時，VINS融合的誤差表現出相似的趨勢，當評價距離進一步增大時，融合誤差保持在一個恒定水平。這意味著VINS融合能夠通過松耦合GNSS解決方案來限制累積漂移。然而，相對于VINS-Mono和GVINS的估計結果，其相對誤差要大得多，因此GNSS測量噪聲對估計的平滑度有很大的影響。由于采用了緊耦合的方法，我們提出的系統結合了VINS Mono和VINS融合的優點。一方面，在較短的范圍內，相對誤差與VINS-Mono相當，從而保持了平滑度。另一方面，誤差不再在各個方向累積，全局一致性也得到了保證。

B 實際環境中的測試

1） 設置：如圖11所示，我們在實際實驗中使用的設備是，帶有VI傳感器和一個u-blox ZE-F9P GNSS接收器。在圖像傳感器方面，實驗中只使用了VI傳感器的左攝像頭。u-blox ZED-F9P是一款低成本的多波段接收機，支持多種星座。此外，ZED-F9P擁有一個內部RTK引擎，能夠在開闊區域提供精度為1厘米的接收器定位。來自3km外基站的實時RTCM流被饋送到ZED-F9P接收機，用于真值RTK解決方案。

圖11. 實際實驗中使用的設備。

圖12.運動場實驗中GVINS、VINS融合、VINS-MONO和RTKLIB的定位誤差。三個子圖對應于ENU框架的三個方向。GVINS、VINS-Fusion和RTKLIB的結果直接與RTK真值進行比較，而VINS-Mono的結果事先與真值軌跡對齊。

圖13.運動場實驗中RTK、GVINS、VINS融合、VINS Mono和RTKLIB的運動軌跡。本文提出的系統的軌跡是平滑的，與RTK的軌跡吻合良好。

圖14.在鎖定衛星數量不足的情況下，本文提出的系統的定位誤差。

左圖復雜室內外實驗中GVINS、VINS Mono和RTKLIB的定位誤差。這里只與RTK-fix解決方案進行比較，因此圖中的差距對應于無法獲得基本事實的情況。由于存在較大的誤差和振蕩，VINS融合的結果沒有顯示出來。右圖 RTK和GVINS在復雜室內外實驗中的定位結果。

圖18。復雜的室內外實驗的最終軌跡。RTKLIB和VINS融合的結果由于噪聲和抖動較大而沒有繪制出來。RTK路徑的不連續性是由于GNSS信號錯誤和修復丟失事件造成的。

總結

在本文中，我們提出了一個基于非線性優化框架下的緊耦合系統來融合來自相機、IMU和GNSS接收機的測量。我們的系統從一個初始化開始，在初始化階段采用從粗到精的過程來在線校準局部和全局幀之間的轉換。在優化階段，在概率因子圖框架下對GNSS原始測量值進行建模和表達。為了保證系統在復雜環境中的魯棒性，對退化情況進行了仔細的分析和處理。此外，還討論了系統集成過程中遇到的工程難題，以便于其他GNSS融合研究。我們在仿真和真實環境下進行了實驗，結果表明，我們的系統有效地消除了累積漂移，保持了典型VIO系統的局部精度。為此，我們聲明我們的系統可以實現局部平滑和全局一致性。

資源

三維點云論文及相關應用分享

【點云論文速讀】基于激光雷達的里程計及3D點云地圖中的定位方法

3D目標檢測：MV3D-Net

三維點云分割綜述（上）

3D-MiniNet: 從點云中學習2D表示以實現快速有效的3D LIDAR語義分割（2020）

win下使用QT添加VTK插件實現點云可視化GUI

JSNet：3D點云的聯合實例和語義分割

大場景三維點云的語義分割綜述

PCL中outofcore模塊---基于核外八叉樹的大規模點云的顯示

基于局部凹凸性進行目標分割

基于三維卷積神經網絡的點云標記

點云的超體素(SuperVoxel)

基于超點圖的大規模點云分割

更多文章可查看：點云學習歷史文章大匯總

SLAM及AR相關分享

【開源方案共享】ORB-SLAM3開源啦！

【論文速讀】AVP-SLAM：自動泊車系統中的語義SLAM

【點云論文速讀】StructSLAM:結構化線特征SLAM

SLAM和AR綜述

常用的3D深度相機

AR設備單目視覺慣導SLAM算法綜述與評價

SLAM綜述(4)激光與視覺融合SLAM

Kimera實時重建的語義SLAM系統

SLAM綜述(3)-視覺與慣導，視覺與深度學習SLAM

易擴展的SLAM框架-OpenVSLAM

高翔：非結構化道路激光SLAM中的挑戰

SLAM綜述之Lidar SLAM

基于魚眼相機的SLAM方法介紹

如果你對本文感興趣，請后臺發送“知識星球”獲取二維碼，務必按照“姓名+學校/公司+研究方向”備注加入免費知識星球，免費下載pdf文檔，和更多熱愛分享的小伙伴一起交流吧！

以上內容如有錯誤請留言評論，歡迎指正交流。如有侵權，請聯系刪除

掃描二維碼

? ? ? ? ? ? ? ? ? ?關注我們

讓我們一起分享一起學習吧！期待有想法，樂于分享的小伙伴加入免費星球注入愛分享的新鮮活力。分享的主題包含但不限于三維視覺，點云，高精地圖，自動駕駛，以及機器人等相關的領域。

分享及合作方式：微信“920177957”（需要按要求備注） 聯系郵箱：dianyunpcl@163.com，歡迎企業來聯系公眾號展開合作。

點一下“在看”你會更好看耶

總結

以上是生活随笔為你收集整理的GVINS：基于GNSS视觉惯性紧耦合的平滑状态估计方法的全部內容，希望文章能夠幫你解決所遇到的問題。

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