多智能體協同視覺SLAM技術研究進展

王 璐，楊功流，蔡慶中，陳 威，閆旭亮

(北京航空航天大學儀器科學與光電工程學院，北京 100191)

0 引言

多智能體協同視覺即時定位與地圖構建(Simultaneous Localization and Mapping，SLAM)[1-2]指搭載一個或多個相機等視覺傳感器的運動主體，通過對環境信息的感知估計出自身位姿的變化并重建未知環境的三維地圖，對自身位姿變化的估計以及三維地圖的重建對增強現實和機器人的自主導航都至關重要[3]。近年來，視覺SLAM技術日漸成為計算機視覺、機器人、深度學習、增強現實等領域的研究熱點，與使用激光雷達等傳感器的傳統SLAM技術相比，視覺SLAM技術使用的相機具有輕量化、低成本并且包含豐富視覺信息的特點，因此適用于有成本限制、載荷有限的平臺。

經過十余年的迅速發展，視覺SLAM技術已經成功地應用于很多商業產品中，例如微軟的Hololens、Google的Project Tango，還有兩大手機操作系統Android[4]和iOS[5]也利用視覺SLAM技術為消費者提供增強現實體驗。另外，無人機上也使用了視覺SLAM技術，例如DJI的四旋翼無人機的幾種型號——Mavic Air和Phantom 4，使得它們可以在全球定位系統(Global Positioning System，GPS)拒止環境下進行可靠的導航。因此，視覺SLAM技術憑借其廉價的傳感器和巨大的感知潛力受到了越來越廣泛的關注。

目前，國內外對視覺SLAM的研究形成了一些理論方法，但是對多智能體視覺SLAM研究的方法較少，這些智能體可能是無人機群或者使用不同AR設備的人[6]。在這樣的環境下，每個智能體在獨立工作的同時，還要時刻關注其他智能體的運動信息，這就要求不同智能體中的視覺SLAM系統相互協作、共享信息[7]，從而為多個智能體同時解決定位和地圖構建的問題。

1 多智能體協同視覺SLAM發展現狀

在SLAM技術研究初期，主要的關注點在于以激光雷達為主要傳感器的SLAM系統。隨著計算機能力和三維重建(Structure from Motion, SfM)技術[8]的發展，相機以其成本低、質量小、信息豐富等優點逐漸成為解決SLAM問題的理想傳感器。隨著單目視覺SLAM技術的迅速發展和日益強大，是否可以將視覺SLAM技術應用于多個相機，特別是一組獨立運動的相機在學術界引起廣泛關注。

多數協同視覺SLAM系統采用的是中心式架構，第一部分(智能體前端)利用視覺傳感器信息對智能體的實時狀態進行計算；第二部分(服務器后端)利用前端結果，根據多視圖幾何理論，對地圖進行融合、簡化和優化，輸出位姿及三維地圖，后端算法主要通過濾波[9-11]和非線性最小二乘優化[12-14]兩種方法來實現。此外，回環檢測(Loop Closure Detection)[15]通過重復探測可以消除累積誤差，從而在視覺SLAM系統中構成閉環。

2003年，第一個視覺SLAM系統[16]問世，該系統主要是基于擴展卡爾曼濾波(Extend Kalman Filter，EKF)算法[17]，利用從圖像上提取的特征點實現對運動主體狀態的估計和三維地圖的構建。Georg Klein等采用了優化的方法，提出了基于加速分割檢測(Features from Accelerated Segment Test，FAST)特征的并行跟蹤和建圖(Parallel Tracking and Mapping，PTAM)[18]。作為PTAM的擴展，并行跟蹤與多重映射(Parallel Tracking and Multiple Mapping，PTAMM)[19]將單目視覺SLAM擴展到了2個獨立移動的相機上，一個相機放在肩上，另一個在手上，二者同時運行，將關鍵幀發送到同一個地圖發生器，從而優化了運動主體的位姿和特征點的3D坐標，同時，系統還存儲了再臨同一位置時使用的局部地圖。

CoSLAM[20]是一種應用于一組獨立運動相機的協同視覺SLAM。該系統支持12個獨立運動的相機并把其分為不同的組，同一組中的相機需要具有共同的視域，通過采用相機間特征匹配、姿態估計和地圖構建的方式，實現同一組內相機之間的協同。該系統通過不同視域中的特征點匹配來三角化3D運動點，實現相機間的建圖，從而估計出運動特征點的3D軌跡[21]。

由于CoSLAM中所有的圖像信息需要進行實時的發送，因此通信負載會隨著相機數量的增加而增大。為了降低通信負載，C2TAM[22]中的智能體都配備相機并把提取的關鍵幀圖像信息發送到云端；云端利用這些關鍵幀圖像信息構建每個智能體所在的3D局部地圖，并在識別出相同位置時將這些地圖信息進行融合；最后利用光束平差法對構建好的地圖進行優化。

為了進一步降低通信負載，CSfM[23]中每個智能體獨立運行單目視覺里程計，其發送的關鍵幀數據不僅包含關鍵幀圖像上的特征點，而且還有這些關鍵幀之間的相對姿態。服務器接收到關鍵幀數據后為每個智能體構建3D局部地圖，為了獲得局部地圖中新到達的關鍵幀的絕對姿態，CSfM通過比較車載和服務器上計算的關鍵幀姿態之間的相對差異，從車載視覺SLAM系統中估計地圖的未知比例因子。

在CCM-SLAM[24]中，每個智能體只運行具有有限關鍵幀數的視覺里程計，智能體將檢測到的關鍵幀信息發送到服務器；服務器根據這些信息進行局部地圖的構建，并通過位置識別的方法將局部地圖信息進行融合；姿態估計和光束平差[25]在服務器中被應用于對地圖的細化[26]。

為了獲得更好的魯棒性和準確性，可以將慣性測量單元(Inertial Measurement Unit，IMU)與相機等視覺傳感器進行組合，使得兩類傳感器進行互補。IMU輔助的視覺SLAM系統通常被稱為視覺慣性導航系統[27-29]，MOARSLAM[30]就在智能體中采用了單目視覺慣性SLAM的框架。多智能體協同視覺SLAM方案對比如表1所示。

表1 多智能體協同視覺SLAM方案對比

目前，雖然大多數視覺SLAM算法都集中在單個平臺上的應用，但是隨著無人機、機器人、多用戶設備等多智能體應用的發展，尤其是5G[31]移動網絡的出現，越來越多的科研人員將投入到多智能體協同視覺SLAM技術的研究當中。

2 多智能體協同視覺SLAM的關鍵技術

多智能體就是由多個相互作用的單智能體構成的系統，每個智能體都是多智能體系統的重要組成部分。在多智能體協同視覺SLAM系統中，智能體之間相互通信,相互協調,并行地求解問題,不僅可以有效地利用空間分布的信息資源，提高問題求解效率；而且系統中單個智能體的損壞一般不會影響到其他個體的運行，與單智能體相比具有更好的容錯性和抗干擾性。

然而，多智能體協同視覺SLAM的研究還存在諸多技術挑戰，相對于對單智能體的控制，多智能體之間交換的信息量會增加，大量的計算與通信會擁塞有限的通信信道，造成延遲；而有限的通信資源在智能體成員之間如何分配也是多智能體協同視覺SLAM系統需要解決的一個重要問題。智能體在運動過程中，重復經過某場景時，該場景能否被其他智能體快速匹配也會對匹配正確率與位姿精度產生影響。

與經典單目視覺SLAM相似，多智能體協同視覺SLAM也可以大致分為位姿跟蹤和地圖構建兩部分。然而不同的是，多智能體協同視覺SLAM主要是對來自不同智能體的信息的處理，下面對其關鍵技術進行討論。

2.1 視覺前端

視覺前端是多智能體協同視覺SLAM中不可缺少的一部分，一般分為直接法和特征點法。直接法[32-33]不需要進行特征提取而是直接對像素進行操作，近年來，因其在精度和效率上的優勢而受到廣泛關注。雖然在單個智能體上使用直接法會收到很好的效果，但是很難直接匹配不同智能體上的相機之間的像素以建立對應關系。

2.1.1 特征檢測與匹配

特征點的檢測和匹配是特征點法中的關鍵一步，特征點由關鍵點和描述子組成。關鍵點的檢測算法有很多種，其中，FAST角點[34]是早期視覺SLAM[35-36]中最常用的關鍵點檢測算法之一，在提取出關鍵點后對其描述子進行計算，通過描述子的匹配來完成特征匹配。為了在不同的視角之間匹配相應的特征點，需要使用對大視角變化魯棒的描述子[37]。其中，ORB描述子[38]是BRIEF描述子[39]的一個擴展，具有旋轉不變性和對大視角變化的魯棒性，對位姿估計、地圖構建和環路檢測來說具有較高的可靠性；SURF描述子[40]因具有較高的效率也得到了廣泛的應用。

引導搜索是一種提高特征匹配效率的方法[41]，通過計算由當前狀態變量預測的路標的可能位置范圍來降低匹配時的計算量。這一思想自早期的視覺SLAM系統以來就一直被采用，尤其是一些基于濾波器的視覺SLAM系統[42]中，這一范圍通常由預測不確定性或投影協方差矩陣決定；而在基于關鍵幀的系統中，這一范圍為極線。在PTAM和ORB-SLAM[43]中，執行極線搜索以在不同關鍵幀之間進行三角化而得到新點。在CoSLAM中，對不同的相機應用極線搜索來生成新的地圖點。

2.1.2 外點剔除

匹配過程中會產生外點，通常有兩類，第一類主要是由錯誤的特征匹配引起的；而第二類則來自環境中的運動特征點，如果把運動特征點作為靜態點來處理，就會導致位姿估計的不一致。

外點剔除一般通過隨機抽樣一致(Random Sample Consensus, RANSAC)算法[44-45]或者魯棒估計[46]來實現。其中RANSAC算法通過隨機抽取最小的2D-3D對應點集，采用PnP算法計算運動主體的位姿，并用當前的位姿估計值檢驗剩余對應點的一致性，將對應點一致性最好的一組位姿估計結果作為最優解，而不一致的對應點則作為外點被剔除。該算法的優點是魯棒性好，但是計算量大。

魯棒估計通過對現有非線性最小二乘優化中的殘差函數稍作修改來實現，相比于RANSAC算法，魯棒估計計算量小，但是當外點數量較大時，其魯棒性會變差。因此，為了在不影響系統魯棒性的前提下減小計算量，可以采用將RANSAC算法和魯棒估計相結合的方法。

2.2 協同位姿的估計

對六自由度位姿的實時估計是多智能體協同視覺SLAM的一項基本任務，相機位姿估計可以通過融合來自多個相機的信息來實現，主要有基于濾波的(filter-based)和基于優化的(optimization-based)兩種不同方法。

在基于濾波的方法中，目前有基于EKF和基于粒子濾波的方法。在基于EKF的多智能體協同視覺SLAM中，未知變量通過擴展卡爾曼濾波器進行估計，通常把運動主體的自身運動參數和路標的三維坐標選為狀態變量[47]，通過EKF的迭代解決位姿估計和路標三維坐標的更新問題。由此可見，該方法的效率隨著路標數量的增多而降低，因此，在實際的應用中，為了保證系統的實時性，路標的數量應該被限制在一定的范圍內[48]。其中，一種降低計算量的方法是從狀態變量中移除路標，但是保留運動主體在當前時刻和過去時刻的位姿，使得觀測模型只依賴于運動主體的位姿，這種方法被稱為多狀態約束卡爾曼濾波(Multi-State Constraint Kalman Filter，MSCKF)，是設計視覺慣性里程計的主要方法。

在基于優化的方法中，則主要是基于關鍵幀優化的方法。關鍵幀是一種數據結構，通常存儲圖像上檢測到的特征點及其對應的3D點，使用最近的關鍵幀中得到的3D點，及其在當前圖像中匹配的特征點，能夠通過3D-2D對準的方式，如PnP算法或者非線性最小二乘優化[49-50]來求解當前姿態。通過對匹配特征點的三角化，在幀間進行地圖構建，再利用光束平差法對關鍵幀位姿和三維地圖進行優化。為了使視覺SLAM具有較高的效率，一般將姿態計算與地圖構建任務交替求解，也就是說，姿態估計是在地圖構建的基礎上完成的，反之亦然。

2.3 協同地圖的構建

關鍵幀是多智能體協同視覺SLAM中表示地圖的最佳方法，它可以緊湊地存儲和地圖相關的任務(如地圖融合和優化)所需的必要信息，包括特征點坐標及其描述子、特征點三維坐標、關鍵幀的相機位姿及其拓撲關系(通常稱為位姿圖[51])等。

構建地圖的核心工作是對特征點進行三角化，從而得到新的3D點。在多智能體協同視覺SLAM中，地圖構建可以進一步分為相機內地圖構建和相機間地圖構建。其中前者與單目視覺SLAM中的建圖類似，僅使用同一個相機的圖像；后者在使用不同相機的圖像構建地圖時，首先要確定相機視野是否相同，然后再利用特征匹配建立不同相機之間的點對應關系。

2.4 回環檢測

回環檢測是視覺SLAM系統中的一個重要組成部分，在多智能體協同視覺SLAM中，有兩種回環檢測的方式：相機內閉環和相機間閉環。其中前者通過檢測單個相機曾經到達過的場景，減小漂移誤差；后者通過檢測多個獨立地圖之間的重疊區域，進行地圖融合。它們在實現時有著相同的步驟，即位置識別和位姿圖優化，從而優化關鍵幀的位姿和特征點的3D坐標。

2.4.1 位置識別與相對位姿的計算

位置識別(Place recognition)[52]用來識別曾經到達過的場景，是閉環或地圖融合的基礎，一般通過下采樣圖像等作為全局描述子來檢測2幅圖像是否匹配。在小場景中，使用存儲的關鍵幀的小圖像即可完成任務[53]；當場景變大時，為確保系統的實時性，通常使用詞袋技術進行位置識別，如DBoW2[54]等。DBoW2方法使用二進制描述子(BRIEF或ORB)和FAST角點作為特征點，具有很好的高效性和實時性，是目前單目視覺SLAM位置識別的主流方法。在多智能體協同視覺SLAM中，通常采用基于深層神經網絡的方法[55]來實現。

在完成位置識別后，為了進一步優化，需要獲取2幅圖像之間的相對位姿，目前，有2D-2D匹配、2D-3D匹配以及3D-3D匹配三種方法來實現相對位姿的計算。在基于2D-2D匹配的方法中，只需要用到相匹配特征點的2D坐標信息，將平移向量的尺度作為未知變量，利用五點算法[56]對本質矩陣進行估計，利用RANSAC方法來處理異常值，再通過矩陣分解從基本矩陣中提取兩幀之間的相對位姿；在基于2D-3D匹配[57]的方法中，使用PnP算法或非線性最小二乘優化的方法，利用當前幀中2D特征點與另一幀中3D特征點的匹配關系獲得相對位姿；而在基于3D-3D匹配的方法中，可以直接利用三維點云計算得到相對位姿，這一方法通常用于RGB-D SLAM[58]和半稠密視覺SLAM系統。

2.4.2 位姿和場景三維坐標的優化

在完成位置識別后，一般使用光束平差法對運動主體的位姿和場景3D結構進行優化，光束平差法通過Gaussian-Newton迭代或Levenberg-Marquardt迭代對殘差進行非線性最小二乘的求解，但是當問題規模較大或初始值與真實值差距較大時，很容易得到局部極小值。因此，為了獲得更好的初始值，可以在使用光束平差法之前進行位姿圖的優化[59]。

在位姿圖中，節點表示關鍵幀的相機位姿，邊表示不同位姿之間的約束關系，通常為剛性變換。將位置識別引入的新邊插入到位姿圖中，然后把新邊和舊邊的所有約束集合在一起，通過非線性最小二乘迭代求解，使得約束誤差平方和最小。事實證明，采用剛性變換作為位姿約束對旋轉和平移漂移的校正效果較好，但對尺度漂移的校正效果較差。因此，更好的方法是使用相似變換作為位姿約束。

位姿圖優化中的一個重要問題是優化過程中，可能存在一些錯誤的回環檢測，也就是將不同位置錯誤地識別為相同位置。這些錯誤大多是由于描述不完善或感知混淆造成的，從而導致了錯誤的相對位姿約束，如果處理不當會對地圖構建造成較大的影響。為了避免錯誤的發生，可以通過匹配幀之間相對變換的先驗信息，或者一組回環檢測的一致性檢驗[60]結果來識別異常值。一致性檢驗過程如圖1所示,紅線表示閉環測量值，黑色實線表示連續幀之間的相對變換。

圖1 a、b兩機器人對閉環測量的一致性檢驗Fig.1 Consistency test of closed-loop measurement between robot a & b

經過位姿圖優化后，通過光束平差法進一步優化關鍵幀的相機位姿和點云三維坐標[61]。其中，光束平差問題可通過開源工具[62-64]進行求解，這一過程與單目視覺SLAM大致相同。一些系統[65-66]使用視覺慣性SLAM作為前端，在這種情況下，光束平差的代價函數中還需要包含慣性測量值，為了避免重復積分和大量計算，慣性測量值可以通過預積分獲得[67]。

2.5 系統初始化

目前，多智能體協同視覺SLAM有兩種初始化的方法，一種典型的初始化方法是對前2個關鍵幀之間的相對位姿使用五點算法，并對相應的特征點進行三角化，得到初始的3D點，為每個智能體構建獨立的初始化局部地圖，一旦通過位置識別發現局部地圖之間有重疊時，就將局部地圖進行融合；第二種方法要求所有的相機能夠在場景中看到同一個地方，從不同相機捕獲的圖像中估計出運動主體的相對位姿，由所有的智能體一起完成。

3 分布式架構

現有的多智能體視覺SLAM多采用中心式架構，該架構中包含能夠處理與地圖優化有關的多種計算任務的服務器，例如：回環檢測、姿態優化、光束平差等。但是當服務器出現故障或通信受限時，這種中心式架構是不可取的，因此對分布式架構[68]的研究成為一大熱點。

分布式架構增加了單個智能體的功能，相應智能體的軟硬件結構更為復雜。但由于這種任務協調機制，使得系統的魯棒性更強，幾乎不受單個智能體的約束，且可根據不同的條件需求擴充或減少智能體的個數，系統靈活可調。

分布式架構一般有兩種：一種是每一個智能體都需要與其他智能體進行信息交流，如圖2所示；另一種是每一個智能體只需要與其附近的智能體進行信息交流，這種分布式架構效率更高，如圖3所示。

圖2 一種普通的分布式架構Fig.2 A common kind of distributed architecture

圖3 一種效率更高的分布式結構Fig.3 A more efficient distributed architecture

DDF-SAM[69]中的分布式架構可以使機器人交換概要地圖。概要地圖是通過變量消除得到的且只包含環境特征信息，每個機器人利用其自身和相鄰機器人的概要地圖，以一種非線性約束最優化的方式進行鄰域地圖的構建，直至鄰域地圖中包括所有機器人的概要地圖信息，從而得到全局地圖。為了避免循環更新的問題，DDF-SAM中采用了基于 RANSAC 算法的數據關聯方法。DDF-SAM2.0[70]中引入了一種抗因子(Anti-factor)，使得機器人在避免循環更新的同時，降低了計算復雜度。目前，DDF-SAM和DDF-SAM2.0中的分布式架構已經在二維平面場景中進行了測試。

蘇黎世大學采用了一種新的分布式算法來解決位姿優化的問題[71]，其思想是在非線性最小二乘優化的迭代過程中，不采用矩陣分解的直接求解方法，而是采用逐次超松弛(Successive Over-Relaxation，SOR)和雅可比超松弛(Jacobi Over-Relaxa-tion，JOR)等迭代方法求解正規方程，減少了數據傳輸量，推動了分布式架構在多智能體協同視覺SLAM領域的發展。

分布式架構因其獨特的任務協調機制，在車聯網應用場景中，可以對道路環境等數據進行全網實時更新，具有廣泛的應用前景；在多機器人系統、編隊控制等應用場景中，對提高系統魯棒性和智能性具有重要研究價值。與中心式架構相比，將分布式架構應用到多智能體視覺SLAM系統中要復雜得多，主要的技術難點可以歸結如下2個方面：

1)分布式架構需要將計算任務分配給每一個具有有限計算能力的智能體，并設計相應的分布式算法來完成計算任務；另外，由于不同的智能體之間交換的信息量不完全相同，所以在設計分布式算法時需要考慮通信負載和帶寬限制。

2)分布式架構需要考慮不同智能體之間數據的同步和一致性，這個問題隨著智能體數量的增加而變得更加復雜。

4 總結與展望

隨著計算機和人工智能等技術的發展，多智能體協同視覺SLAM得到了廣泛的關注。通過實現運動主體的實時精確定位和地圖構建，多智能體協同視覺SLAM對機器人領域和增強現實領域產生了積極的影響。目前，國內外對多智能體協同視覺SLAM的研究已經取得了很大進展，但是仍有一些問題需要解決，例如：如何設計有效的分布式算法來解決相機間的協同和地圖優化問題；如何在算法上解決高速運動情況下導致的視覺信息魯棒性降低的問題。而且，我國在多智能體協同視覺SLAM的研究中與發達國家仍有較大差距，所以要進一步加強與先進國家的技術交流，縮小與國外先進水平的差距。