一種完全分布式的點(diǎn)線協(xié)同視覺慣性導(dǎo)航系統(tǒng)

在本文中，我們提出了一種完全分布式的點(diǎn)線協(xié)同視覺慣性導(dǎo)航系統(tǒng)。我們通過蒙特卡羅模擬和真實(shí)環(huán)境數(shù)據(jù)集，在稠密特征或稀疏特征環(huán)境下將所提出的算法與其他四種算法進(jìn)行了比較。所有結(jié)果表明，我們的PL-CVIO優(yōu)于獨(dú)立的MSCKF和CVIO。

01 摘要

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

02 簡(jiǎn)介

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

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

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

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

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

03問題概述

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

A. 視覺-慣性里程計(jì)狀態(tài)向量

為了執(zhí)行PL-CVIO，每個(gè)機(jī)器人的狀態(tài)向量被定義為：

B. 動(dòng)態(tài)系統(tǒng)模型

對(duì)于每個(gè)機(jī)器人，IMU的線性加速度和角速度的測(cè)量被建模為：

C. 點(diǎn)-線測(cè)量模型

在稀疏特征環(huán)境中，線條是點(diǎn)的良好補(bǔ)充。因此，在本文中我們考慮了點(diǎn)和線測(cè)量。機(jī)器人的點(diǎn)測(cè)量可以描述為：

D. 獨(dú)立點(diǎn)和線特征更新

為了進(jìn)行獨(dú)立的點(diǎn)或線特征更新，將對(duì)每個(gè)機(jī)器人應(yīng)用標(biāo)準(zhǔn)的MSCKF更新。具體來說，我們會(huì)收集當(dāng)前滑動(dòng)窗口內(nèi)所有的點(diǎn)和線測(cè)量，通過堆疊一個(gè)點(diǎn)或一條線的測(cè)量，我們可以利用IMU姿勢(shì)的估計(jì)來三角化這個(gè)點(diǎn)特征或線特征。為簡(jiǎn)化符號(hào)表示，令表示一個(gè)點(diǎn)特征或一條線特征，并且機(jī)器人的測(cè)量殘差可以被線性化為：

E. 共同點(diǎn)和線特征更新

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

Robot及其鄰近機(jī)器人將對(duì)共同特征應(yīng)用線性化和左零空間投影。與第II-D節(jié)類似，機(jī)器人將使用進(jìn)行EKF更新。然而，機(jī)器人不會(huì)放棄，而是利用鄰近機(jī)器人的共享信息。它將構(gòu)建一個(gè)新的殘差系統(tǒng)，該系統(tǒng)依賴于與自身和鄰近機(jī)器人相關(guān)聯(lián)的頂部部分堆疊在一起的共同點(diǎn)或線特征。

04模擬和實(shí)驗(yàn)

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

A. 蒙特卡羅模擬

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

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

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

每個(gè)機(jī)器人的方向和位置的均方根誤差（RMSE）以及每種算法在各個(gè)環(huán)境中的平均RMSE都記錄在表II中。RMSE結(jié)果顯示，在所有模擬場(chǎng)景中，我們的PL-CVIO算法優(yōu)于P-CVIO和P-VIO。特別是在稀疏特征環(huán)境中，PL-CVIO顯著改善了姿態(tài)和位置的RMSE。

B. 實(shí)驗(yàn)

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

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

05 總結(jié)

在本文中，我們提出了一種完全分布式的點(diǎn)線協(xié)同視覺慣性導(dǎo)航系統(tǒng)。我們通過蒙特卡羅模擬和真實(shí)環(huán)境數(shù)據(jù)集，在稠密特征或稀疏特征環(huán)境下將所提出的算法與其他四種算法進(jìn)行了比較。所有結(jié)果表明，我們的PL-CVIO優(yōu)于獨(dú)立的MSCKF和CVIO。此外，我們驗(yàn)證了線特征可以改善獨(dú)立情況下定位的準(zhǔn)確性，并且共享線特征在協(xié)作情況下表現(xiàn)更好。