在本文中，我們提出了一種完全分布式的點線協同視覺慣性導航系統。我們通過蒙特卡羅模擬和真實環境數據集，在稠密特征或稀疏特征環境下將所提出的算法與其他四種算法進行了比較。所有結果表明，我們的PL-CVIO優于獨立的MSCKF和CVIO。

01 摘要

稀疏特征環境是幾何計算機視覺（CV）算法的主要挑戰之一。在許多人工場景中，通常存在稀疏特征情況，而線條可以被視為點特征的補充。本文提出了一種利用點和線特征的多機器人協同視覺慣性導航系統（VINS），通過在多狀態約束Kalman濾波器（MSCKF）框架內使用協方差交集（CI）更新，每個機器人不僅利用自身的點和線測量數據，還利用鄰近機器人觀察到的共同點和共同線特征進行約束，采用最近點表示對線特征進行參數化和更新。該算法（PL-CVIO）經過蒙特卡羅模擬和真實環境數據集廣泛驗證，在稀疏特征和稠密特征環境中均表現出優于獨立MSCKF以及我們先前CVIO工作成果的結果。

02 簡介

近年來，SLAM技術受到了廣泛關注，已經成為許多機器人和計算機視覺應用的核心技術，例如增強現實/虛擬現實、自動駕駛和機器人導航等。在GPS信號受限的環境中，通過使用低成本、輕便的車載攝像頭和慣性測量單元（IMU），視覺慣性導航系統（VINS）及其相關算法已經獲得了廣泛的認可。不過事實上，多個機器人能夠比單個機器人能夠更有效地完成任務并實現更高的精度。但對于多機器人群體，有一個關鍵問題就是如何最好地利用環境信息和其他機器人的信息。

在人工環境中，線可以被視為對點的良好補充，特別是在稀疏特征環境中只能提取到少量點特征的情況下。在VINS中，有兩種主要方法來處理點和線：間接（基于特征）和直接方法。具體而言，間接方法通過提取特征描述符并沿著序列進行匹配來預處理圖像流。間接方法是通過最小化幾何誤差來優化系統。直接方法跳過了特征提取步驟，并直接使用行像素來優化光度誤差。相比較，直接方法非常高效，但需要假設亮度恒定（忽略曝光變化），而實際環境中曝光會發生較大變化。

在先前的基于特征的VINS文獻中，解決方案可以大致分為兩類：基于濾波器的方法和基于圖的方法。基于濾波器方法中最具代表性的是多狀態約束卡爾曼濾波（MSCKF）,它通過使用相同特征點的測量值形成了一個多約束更新。另一種領先技術是緊耦合單目圖像VIO（VINS-Mono），它引入了魯棒初始化和非線性優化. 此外，還有一些同時使用點特征和線特征的VINS算法。其中，點線視覺慣性里程計（PL-VIO） 是對VINS-Mono 的擴展，它可以在滑動窗口內優化點和線特征的重投影誤差。PL-SLAM 提出了一個基于ORB-SLAM 的點線SLAM框架。還有研究者設計了適用于快門相機的Plucker 表示下的直線特征和提出了兩種直線三角測量算法，以及對三種不同直線表示形式(Plucker, Quaternion, Closest Point) 及其相應可觀測性進行深入分析.然而，以上所有方法都是集中在單個機器人情況下進行研究。

協作式VINS（C-VINS）的一個優勢在于多個機器人共享共同特征，從而引入更多共同特征的幾何約束。具體來說，群組中的每個機器人不僅觀察其自己的測量值，還會收集來自多機器人群組的測量值。通過利用共同特征約束，機器人應用更新以改善定位性能。也存在一些集中式多機器人解決方案，但它們通常需要昂貴的計算和通信成本，分布式算法在這方面有著一些優勢。最近，研究者采用M-Space表示不同類型特征提出了分布式點線協作SLAM（CSLAM）算法,但由于重復使用相同信息導致估計一致性無法保證.在 [Localization of a group of communicating vehicles by state exchange] 中,群組中每個機器人處理自己可獲得的測量，并且只在特定時間步內與通信范圍內其他機器人融合估計和協方差.DOOR-SLAM 提出了一個完全分布式CSLAM算法,包含姿態圖優化模型和類似于[Data-efficient decentralized visual SLAM] 的數據高效分布式SLAM前端.文章 [Distributed maximum a posteriori estimation for multi-robot cooperativelocalization]提出了使用最大后驗概率（MAP）進行完全分布式算法.我們之前的工作CVIO , 利用協方差交叉(CI)更新提供了一個完全分布式合作算法并可以保證一致性,但稀疏特征環境未被考慮進去.

在本文中，我們提出了一種利用點特征和線特征的完全分布式多機器人姿態估計算法。每個機器人不僅利用自己的點和線測量，還相互進行合作（見圖1）。特別是在稀疏特征環境中，也就是缺乏穩健的地標，可以通過整合每個機器人獨立獲取到的點和線特征，并利用協方差交叉更新來更精確地估計它們的位置。為此，我們開發了PL-CVIO算法，并將其應用于最先進的OpenVINS系統中，該系統采用單目攝像頭-IMU架構。通過在蒙特卡羅模擬和現實環境下進行測試，結果表明，在稀疏特征和稠密特征環境下，我們的算法都表現出更精確的定位能力。

03問題概述

點線協同式視覺-慣性里程計的目標是在全局坐標系?中跟蹤每個機器人的3D姿態，其中= 1, · · · ,n。與單個機器人不同，多個機器人可以相互共享共同特征。本文利用常見的點和線特征來提高定位精度。

A. 視覺-慣性里程計狀態向量

為了執行PL-CVIO，每個機器人的狀態向量被定義為：

B. 動態系統模型

對于每個機器人，IMU的線性加速度和角速度的測量被建模為：

C. 點-線測量模型

在稀疏特征環境中，線條是點的良好補充。因此，在本文中我們考慮了點和線測量。機器人的點測量可以描述為：

D. 獨立點和線特征更新

為了進行獨立的點或線特征更新，將對每個機器人應用標準的MSCKF更新。具體來說，我們會收集當前滑動窗口內所有的點和線測量，通過堆疊一個點或一條線的測量，我們可以利用IMU姿勢的估計來三角化這個點特征或線特征。為簡化符號表示，令表示一個點特征或一條線特征，并且機器人的測量殘差可以被線性化為：

E. 共同點和線特征更新

需要注意，相鄰的機器人可能會觀察到共同的點或線特征。因此，我們將進一步利用相鄰機器人之間的點和線特征約束來提高定位精度。機器人之間可以通信以共享信息。

Robot及其鄰近機器人將對共同特征應用線性化和左零空間投影。與第II-D節類似，機器人將使用進行EKF更新。然而，機器人不會放棄，而是利用鄰近機器人的共享信息。它將構建一個新的殘差系統，該系統依賴于與自身和鄰近機器人相關聯的頂部部分堆疊在一起的共同點或線特征。

04模擬和實驗

在本節中，我們利用蒙特卡羅模擬和真實環境數據集驗證了協作情況下常見線特征對定位精度的提高以及獨立情況下線特征對精度的改善。我們將在兩種不同環境下通過表I比較我們的PL-CVIO算法與先前工作。稀疏特征場景包含少量特征，而稠密特征場景則包含足夠多的特征。如表I所示，PVIO代表獨立MSCKF算法，PLVIO代表獨立點線MSCKF算法，P-CVIO代表我們之前的CVIO工作，IPLCP-CVIO是指不僅利用每個機器人的獨立點、線特征還收集鄰近機器人的共享點特性的算法，并且PL-CVIO使用了本文介紹的獨立和共享點線功能。

A. 蒙特卡羅模擬

在蒙特卡羅模擬中，我們利用了一個由三個機器人組成的小組。該小組中的機器人0按照數據集的真實軌跡運動，而機器人1和機器人2的軌跡則是通過向真實軌跡添加位置和方向偏移來創建。之后，如果當前幀中點或線測量數量低于閾值，則生成3D特征及其相應的2D測量。然后，收集并利用來自一個機器人共同特征以及鄰近機器人共有特征的約束來更新當前狀態。

稀疏特征和稠密特征環境通過提取不同數量的點特征進行劃分。在稠密特征環境中，每幀的點特征數量為150，線特征數量為50。對于稀疏特征環境，我們將點特征數量減少到50。對于這兩種環境，我們使用了First-Estimation
Jacobian(FEJ)和在線相機-IMU校準。在運行了30次蒙特卡羅迭代后，在稠密特征或稀疏特征環境的Udel Gore數據集下，相對姿態誤差（ROE）和相對位置誤差（RPE）的統計數據分別如圖2和圖3所示。可以看到，我們的PL-CVIO算法在兩種環境中都優于其他所有算法。特別是在稀疏特征環境下，我們發現共同線可以明顯降低ROE和RPE（如圖3中的藍色和紅色條形圖所示）。此外，有趣的發現是，如果每幀只觀察到有限數量的點，PL-VIO將優于P-CVIO。在此情況下，共同點特征的數量也有限，因此僅依賴于共同點特征的協同方法P-CVIO可用資源有限。相比之下，進一步利用線特征的方法PL-VIO和PL-CVIO表現出更好的性能，而PL-CVIO則取得了最佳性能，因為它不僅利用了點和線特征，還利用了與鄰近點的協作。

此外，我們還在所有的EuRoC V1數據集中對我們的PL-CVIO算法進行了模擬，并將其與P-VIO和P-CVIO在稀疏特征和稠密特征環境下進行了比較。

每個機器人的方向和位置的均方根誤差（RMSE）以及每種算法在各個環境中的平均RMSE都記錄在表II中。RMSE結果顯示，在所有模擬場景中，我們的PL-CVIO算法優于P-CVIO和P-VIO。特別是在稀疏特征環境中，PL-CVIO顯著改善了姿態和位置的RMSE。

B. 實驗

在現實環境的實驗中，每個機器人的位置和方向都初始化為地面真值。我們使用FAST[37]從每個幀中提取點特征，并通過ORB與8點RANSAC算法進行跟蹤以跨越幀或與其他機器人的點觀測匹配。同時，利用LSD提取線段，并使用LBD進行跟蹤。此外，我們還添加了一些異常值剔除策略，以刪除滿足以下條件的線段：(1)
LBD距離大于50；(2) 線段長度小于50像素；(3) 線段起點到該線的距離小于0.1或大于100；(4) 當應用SVD對該線進行三角測量時，線條視差太小會導致奇異性。

我們在TUM
Visual Inertial Dataset Rooms 1, 3和5中評估了我們的PL-CVIO算法，其中IMU頻率為200Hz，相機頻率為20Hz。我們加載同一房間的所有三個數據集，并分別使用三臺機器人運行所有五種算法。此外，我們提取不同數量的點特征來模擬稀疏特征和稠密特征環境。因此，在圖4中展示了我們的PL-CVIO算法與其他四種算法在稠密特征環境下的實驗結果，并在圖5中展示了稀疏特征環境下的實驗結果。同時，在表III中展示了利用不同算法在TUM數據集上每個機器人方向和位置的均方根誤差（RMSE）。從ROE/RPE和RMSE結果可以明顯看出，線特征可以提高P-VIO的準確性，而共享點-線功能則可以改善P-CVIO的性能。另外，在稀疏特征場景中通過比較P-VIO和PL-VIO以及P-CVIO和IPL-CP-CVIO可見線條顯著改善了性能,如表III所示.最終,在所有實驗案例中,我們的PL-CVIO算法均優于其他四種算法。

05 總結

在本文中，我們提出了一種完全分布式的點線協同視覺慣性導航系統。我們通過蒙特卡羅模擬和真實環境數據集，在稠密特征或稀疏特征環境下將所提出的算法與其他四種算法進行了比較。所有結果表明，我們的PL-CVIO優于獨立的MSCKF和CVIO。此外，我們驗證了線特征可以改善獨立情況下定位的準確性，并且共享線特征在協作情況下表現更好。