基于多智能體的圖優化SLAM構建方法

宋燕燕，秦 軍,2，邢艷芳，汪晨曦，周洪萍

(1.中國傳媒大學南廣學院，江蘇 南京 211172； 2.南京郵電大學，江蘇 南京 210003)

0 引 言

增強現實(augmented reality，AR)是一種結合虛擬化技術觀察世界的方式，它利用計算機技術，將虛擬的物體實時地疊加到一個真實畫面或空間，形成具有實時交互的三維圖像畫面，給用戶帶來更真實的體驗與感受[1]。增強現實由于其在真實環境和虛擬環境中起到了一個連續體的作用，為相關的應用提供了一種新的人機交互方式，滿足用戶在現實世界中真實地感受虛擬空間中模擬的事物，增強使用的趣味性和互動性。近年來，隨著同步定位與地圖構建(SLAM)算法的不斷更新，已經成為一個研究熱點問題，定位(運動追蹤)和建圖(稀疏或者稠密地圖)構成了SLAM的核心功能。因此，SLAM可以說是AR中最基礎的模塊，屬于設備感知周邊環境的范疇[2-4]。

在目前的研究中，SLAM一方面充分利用圖像信息來提取圖像特征，另一方面也通過融合深度相機、IMU等傳感器滿足視覺SLAM在困難條件時，仍然可以穩健定位建圖的需要，在機器人自動化、智能化區域發揮著很重要的作用。在針對多機器人基于深度學習的SLAM算法中，國外學者提出了基于稀疏編碼的遷移學習SLAM算法，這樣就可以有效減小訓練模型的數據量，并對模型的通用化進行了完善，使人們可以理解不同類型的數據，進而更好地對底層進行規劃。在目前的研究中，對于避障問題，學者提出將導航策略轉變為一個深度的神經網絡，將障礙物信息設為輸入量，速度控制信號設為輸出量，通過大量數據訓練模型，從而完成避障。異構多機器人搭載多種傳感器相對于單一種類機器人具有更大的優勢，基于多傳感器數據融合SLAM的研究在搭載多機器人時可以更加有效地對復雜環境進行全面的地圖構建[5-6]。

1 基于圖優化的SLAM框架

視覺SLAM就是一個帶有特定相機的承載主體，它可以在沒有任何的提前探測的情況下，對自己接下來的路徑進行規劃。前端主要涉及到計算機視覺領域的問題，而后端主要與優化算法有關。

圖1 整體視覺SLAM流程

整個視覺SLAM框架如圖1所示，主要包含五個步驟：傳感器信息讀取、視覺里程計、后端優化、回環檢測、建圖。圖優化是把常規的優化問題采用圖的方式來表示，圖優化中的圖就是數據結構里的圖，一個圖由若干個頂點以及連接著這些頂點的邊組成，如圖2所示，可以更直觀地看到其結構。

圖2 圖優化例子

圖優化SLAM方法是先通過前端的圖構建得到初始狀態估計值，在前端得到初始地圖[7-8]。但由于傳感器的噪聲等，使得最后所得到的結果具有較大的誤差，需要通過后端對初始狀態進行估計來進一步優化求解，使得地圖的一致性和精度都得以提高。基于圖優化的方法利用了這些信息來優化完整的地圖路徑，也稱為全SLAM方法，如圖3所示。

圖3 基于圖優化的SLAM框架

2 SLAM相機位姿的估計和優化

(1)

當對一個旋轉矩陣R2(李代數為φ2)左乘一個微小旋轉矩陣R1(李代數為φ1時，可以近似地看作在原有的李代數上加上了一項J1(φ2)-1φ1。在給近似添加了一個微小擾動的情況下,分成了左乘擾動和右乘擾動兩種擾動方式。在使用時，須注意使用的是左乘模型還是右乘模型。

左乘BCH近似雅克比Jl為：

(2)

它的逆為：

(3)

而右乘雅克比只需要將右乘中的自變量取負號即可得到：

Jr(φ)=Jl(-φ)

(4)

為了討論位姿的導數，會引入構建位姿函數調整當前的值，以調整整體的估計值，使用李代數表示位姿，來對其進行求導，再對左乘或者右乘進行擾動，最后對其求導。這時把結果稱為左擾動和右擾動模型。

3 非線性優化SLAM問題

即使相機的運動方程可以很好地符合針孔模型，但由于噪聲的存在，數據依舊會受到很大的影響，運動方程和觀測方程會對測量結果有偏差。所以需要在有噪聲的數據中進行準確的狀態估計，從噪聲數據中恢復出所需要的東西，優化處理噪聲數據[11]。

3.1 狀態估計問題

SLAM模型一般由兩個方程——運動方程和觀測方程組成：

(5)

觀測方程在SLAM中具有特殊性，所以當在xk對yj進行觀測，對應到圖像中的像素位置zk,j,那么，便可得出觀測方程：

szk,j=Kexp(ξ∧)yi

(6)

其中，s表示像素點之間的距離，k表示相機的內參。

在運動和觀測設有的兩個噪聲項wk,vk,j滿足以下條件：

wk～N(0,Rk),vk,j～N(0,Qk,j)

(7)

在噪聲的干擾下，希望能夠準確地計算出相機的位姿以及得到完整的地圖(算出它們在有噪聲的條件下的概率分布)。目前是采用非線性優化來解決這一狀態估計問題，對每個時刻的數據中的噪點進行采集，然后對它進行狀態估計。了解每個時刻中的誤差點，這種方法已經成為了目前解決視覺SLAM中噪聲問題的主要方法。在這里，把所有待估的變量放在一起：

x={x1,…,xn,y1,…,yn}

所以可得到x的條件概率分布：P(x|z,u)，在沒有先驗的情況下，沒有告知剛體的運動方向，只有由像素點構成的圖片時，就沒有運動方程帶來的具體數據，這樣就只能依靠觀測方程所產生的數據來判斷，不必考慮在時間上的順序。這時只能分析空間上物體的運動來得到三維結構信息，通過找到三維空物體圖像的對應點(在圖像中找到角點特征)，來恢復立體圖像結構問題，這種方法被稱為運動恢復結構SFM(structure from motion)。為了進一步了解此時的條件分布特征，利用貝葉斯法則，可以得到:

(8)

其中，P(x)為先驗。很難找到后驗分布，因此需要先求解該狀態下的最優估計。使得在該狀態下后驗概率最大化(maximize a posterior，MAP)：

(9)

可以直接忽略貝葉斯法則的分母部分(與狀態值x無關)。當無法判斷剛體的位姿在什么地方時，就說明不能準確地得到先驗概率。那么，此時可以求解x的最大似然估計:

(10)

似然是指在當前的情況下可能得到的數據，所以最大似然估計的直接意義就是“在什么樣的狀態下，最有可能產生的現在觀測到的數據”。

觀測方程中，只有在xi點可以看到yi時，才會產生觀測數據。由于視覺SLAM的特征點非常多，所以在實際中觀測方程的量會遠大于運動方程。如果認定沒有運動方程，整個優化問題就只能由多個觀測方程得到，相當于通過一組圖像來恢復運動的結構。這就比較類似于SFM，但SLAM中的圖像有時間上的先后順序，而SFM允許使用完全無關的圖像。

3.2 非線性優化方案

非線性優化最常見的兩種方案是高斯牛頓法和列文伯格—馬夸爾特方法。由于目標函數復雜，對問題提供不同的初始值都容易陷入局部最小值，所以使用高斯牛頓法和列文伯格-馬夸爾特法優化初始值。

3.2.1 高斯牛頓法

高斯牛頓法將f(x)進行一階的泰勒展開：

f(x+Δx)≈f(x)+J(x)Δx

(J(x)為f(x)的導數)

(11)

它的目標是尋找下降矢量Δx,使得‖f(x+Δx)‖2達到最小，所以求解線性最小二乘：

(12)

將目標函數對Δx求導，并令導數為零，可得：

J(x)TJ(x)Δx=-J(x)Tf(x)

(13)

求解的變量為Δx，一般稱為增量方程或者高斯牛頓方程(Gauss Newton equation)。將左邊的系數項設為H，右邊定義為g：

HΔx=g

(14)

將JTJ作為二階Hessian矩陣的近似，省略掉了計算H的過程。所以可得高斯牛頓法的步驟為：

(1)給定初始值x0。

(2)對第k次迭代，求出當前的雅克比矩陣J(xk)和誤差f(xk)。

(3)求解增量方程HΔx=g。

(4)直到Δxk足夠小，則停止。否則，令xk+1=xk+Δxk，繼續第二步。

從算法中可以看出，增量方程求解占據主要位置。它要求所用的近似H矩陣是可逆的(而且是正定的)。但是可能出現的JTJ卻只有半正定性。在使用高斯牛頓法時，可能會出現JTJ為奇異矩陣或者病態的情況，導致算法不收斂。即使排除這一情況，在求步長Δx時，若它過大，也會使得局部近似不夠準確，無法保證它的迭性收斂。

3.2.2 列文伯格-馬夸爾特法

高斯牛頓法在運算時具有局限性，它只能展開附近較好的近似效果，因此需要給Δx添加信賴區域(trust region)來提高它的準確性。在非線性優化中，這類方法被稱為信賴區域法(trust region method)。一般區域的大小與近似模型和實際函數的差異成反比，用ρ來代替兩個函數的差異性:

(15)

其中，分子表示實際函數的下降值，分母表示近似模型的下降值。若ρ接近于1，則近似值是正好的；若ρ太小，則近似性比較差，需要縮小范圍；反之，則說明實際下降范圍比較大，這時，需要放大范圍。

因此構建了一個改良的非線性優化框架，即：

(1)給定初始值x0以及優化半徑μ。

(2)對于第k次迭代，可得：

(3)計算ρ。

(6)ρ大于閾值，則認為近似可行，令xk+1=xk+Δxk。

(7)判斷算法是否收斂，若不收斂，則繼續第二步；否則結束。

馬夸爾特提出將D取成非負數對角陣—通常用JTJ的對角元素平方根，這樣使得在梯度維度上的約束范圍會變得大一些。用拉格朗日乘子將它轉化為一個無約束化問題：

(16)

可以發現計算增量的線性方程仍然是該問題的核心：

(H+λDTD)Δx=g(以下用I代替DTD)

(17)

在這個增量方程中，相比于高斯牛頓法，多了一項λI。當λ較小時，H占主要地位；另一個方面，當λ較大時，λI將占據主要地位。列文伯格—馬夸爾特法為優化問題提供更穩定、準確的增量Δx，在一定程度上避免了方程組的非奇異性和病態問題。

4 多智能體與SLAM構建

多智能體技術可以為SLAM提供很好的穩定性和魯棒性，使視覺SLAM得到更加有效可靠的特征。具有無監督學習機制的SLAM已經在現實應用中有很大的市場，將以上非線性優化方案結合多智能體技術使得實時追蹤能夠具有自身的協調能力，更好地應用于適應未來的市場發展機制[12-13]。視覺SLAM則是增強現實(AR)中一個很好的搭載輔助工具。它可以自主地識別空間中的墻壁，障礙物以及其他物品。當出現大規模的數據集時，基于數據的圖優化SLAM能夠自主提取特征，而非手動提取。但當出現大規模數據時，它的魯棒性還需要進一步提升[14]。

基于深度學習圖優化的SLAM也具有很強的靈活性，它不僅可以估計位置和構建三維點云地圖，并且可以產生估計的不確定性，為后續的決策所服務[15]。但是它也存在誤差很大，數據增長很快的情況。該文提供了一種解決方法：核函數。它能保證每條邊的誤差不會大到掩蓋其他的邊。把原先誤差的二范數度量替換成一個增長沒有那么快的函數，保證自己的光滑性質(否則無法求導)，使得整個優化過程更為穩健，所以又叫魯棒核函數(robust kernel)。

魯棒核函數最常用的Huber核為：

(18)

當誤差e大于某個閾值δ時，函數增加由二次形式變成了一次形式，相當于限制了梯度的最大值。如圖4所示，可見誤差較大時Huber核函數增長明顯低于二次函數。在生成場景語義圖時，如何進一步提高改善定位性和魯棒性還需不斷探索，期待場景語義圖可以幫助定位，并進一步減少數據存儲量，從而有助于大規模大尺度環境構圖。

圖4 Huber核函數

通過該文構建的方法，將非線性優化方案結合多智能體進行SLAM構建，實現移動平臺真實瀏覽虛擬樣板間的可視化與交互，運動跟蹤是通過識別和跟蹤用戶周圍的可識別特征來完成的，然后利用這些點以及設備的朝向和加速度傳感器來保持跟蹤更新。如果沒有這些信息，就無法實現準確跟蹤。隨后通過USB或遠程將設備與開發機器相連，然后在設備上構建和運行應用程序。繪制點云數據需要花費一段時間。并且隨著點的個數的增加，在一個方向保持設備的時間就越長。這些點表示用于跟蹤和解釋環境的可識別特征點。放置對象用光線投射技術，光線投射技術可在兩個維度上獲取觸摸點，并向場景投射光線，然后該射線測試場景中產生碰撞的其他對象。檢測到平面的效果如圖5所示，圖6為點擊平面后疊加出現另一個虛擬樣板間的效果。

圖5 檢測到平面

圖6 點擊平面并出現門

5 結束語

基于圖優化SLAM算法與多智能體結合，提出一種構建方法，并對增強現實進行應用討論。現在人工智能和增強現實技術在社會中的地位穩步上升，這些領域的核心關鍵技術在逐步實現突破，視覺識別技術也遙遙領先。圖優化SLAM與多智能體的結合，給增強現實在構建完整地圖提供了良好的開端，實現了SLAM與增強現實的結合。視覺SLAM系統在大尺度地圖中的速度，優化算法能得到提升。在大環境下系統性將會變得脆弱，魯棒性也隨之降低。如果遇到特征點很少的場景、交叉的白平衡、圖片失真等情況，這些給對特征檢測增加很大的難題，數據會變大，導致無法很好地存儲與處理地圖。當將多智能體技術與其相結合，通過智能體之間的相互協調性提高算法的速度，通過智能體之間的相互通信實現對每一個零散的點進行整合，能得到一個更好的三維地圖，在未來的自動駕駛等領域會得到良好的應用。