一種融合外部姿態信息的SLAM算法

李醒飛，李婧嫻，郭鳳志

(天津大學 精密測試技術及儀器國家重點實驗室，天津 300072)

近年來，SLAM 技術由于其環境感知和自我定位能力，成為了自主導航領域的研究熱點[1]。降低估計誤差、提升系統穩定性一直是SLAM技術發展的方向。目前，SLAM 技術已從基于概率和統計方法[2]，發展至基于非線性優化的圖優化方法。在苛刻條件下，SLAM 系統的累計誤差成為了限制定位和建圖精度的關鍵因素，嚴重影響了SLAM 系統的穩定性。盡管視覺已經擺脫了輪速計較為嚴重的漂移問題[3]，但機器人在長期運行過程中，視覺位姿和地圖的不確定度增大，誤差逐步累積依然會影響系統的穩定性。

對此，研究學者傾向于采用多傳感融合的方案加以解決。如2018 年香港科技大學秦通等人實現了視覺與IMU 的緊耦合位姿估計[4]，夏琳琳等人則在融合IMU 基礎上又結合了光流-線特征方法[5]，另外張福斌等人避免了IMU 二次積分的距離誤差，轉而融合里程計與陀螺儀降低累積誤差[6]，王立玲等人則額外融合了IMU 和關節編碼器實現機器人的自主定位[7]。P.Gakne 等人設計了基于GNSS 與空天相機緊耦合的位姿估計算法，定位精度有明顯提升[8]。此外，還有學者利用地磁傳感器獲得實時測量的地磁序列修正SLAM 的姿態漂移[9]。但這些方法受制于使用時間或者環境，無法在特殊場景下校正累積誤差。

為在嚴苛條件下能夠降低累積誤差，SLAM 系統需要找到準確可靠且不產生累計誤差的傳感器或傳感系統。在導航和定位領域，尋北儀作為直接準確測量地理真實姿態的設備，已被廣泛應用于導彈發射等軍事領域以及采礦、測繪和天文學等民用領域[10]。故在本文中使用自研捷聯式尋北儀與靶標構成外部測姿系統，為SLAM 系統在嚴苛條件下提供地理真實姿態信息作為姿態約束，從而有效降低SLAM 系統的累積誤差。

本文針對地下、室內等特殊應用場景，提出了一種融合外部姿態信息的SLAM 算法。相機在采集過程中通過標定靶獲得外部姿態信息，在解算圖像幀的相對位姿不確定度后，通過稀疏的外部姿態信息和密集的圖像幀建立了融合姿態信息的姿態圖和全局優化模型，在特殊應用場景下依然能夠有效降低SLAM 系統的累計誤差，提高SLAM 系統的穩定性，對機器人定位和建圖具有重要意義。

1 框架搭建

為降低SLAM 系統的累計誤差，在原有框架中加入外部姿態信息，以連續三幀相機位姿和兩個路標點為例，可構建如圖1 所示的貝葉斯網絡。由圖1 可知，除原本的起始點和相機約束外，測姿系統的觀測量z5和z6為相機位姿添加了額外約束關系，通過約束關系中的姿態信息可對相機位姿和路標點位置進行優化，以減小姿態和位置的漂移。

圖1 貝葉斯網絡Fig.1 Bayesian network

在實際應用場景下，測姿系統的觀測量較圖像觀測量顯著稀疏。此外，為保證姿態的測量與相機運動過程不沖突，測姿系統和相機相互獨立。由于測姿系統能便捷地獲取地理真實姿態信息，相機可在需要校正時拍攝測姿系統的靶標，通過視覺進行位姿傳遞，獲取相機姿態。x 為相機位姿，p 為路標點位置，z 為觀測值，其中z0為固定的起始點，z1～z4為相機觀測量，z5和z6為測姿系統觀測量。

2 融合外部姿態信息的SLAM 算法

2.1 協方差估計算法

相對姿態的協方差矩陣描述了相對姿態的不確定度大小，外部姿態信息相比于圖像關鍵幀十分稀疏，兩者比值可達到兩個數量級，但前者給出的是世界坐標系下不含累積誤差的三維姿態，后者則是通過相對位姿的迭代獲取世界坐標系下六維姿態。由于前者不含累積誤差，得到的相機相對姿態的協方差行列式遠小于后者姿態的協方差行列式，這為優化方程的參數設計提供了參考。另一方面，對于后者不同關鍵幀間的相對姿態的協方差矩陣也各不相同，因此，為能夠根據不確定度進行準確的姿態圖優化，需要估計姿態圖優化中各個邊的協方差。

1）相機連續幀的協方差估計

在前段的跟蹤環節，非線性優化問題的量測方程可寫為含有高斯噪聲的形式：

式中，x 是相機位姿， f （x ）是量測方程，y 是被觀測量， N （0,Σ ）表示高斯噪聲， y 具有無偏高斯噪聲，噪聲協方差矩陣為半正定矩陣Σ 。因此，在觀測量為y 的條件下，x 的最大似然估計為：

式(2)實際是Mahalanobis 距離的平方[11]。當f （x）在 x*處的雅克比矩陣 J（ x*）是列滿秩時，相機位姿的協方差矩陣為：

2）組合位姿協方差估計

在關鍵幀非線性優化模型中，需要考慮兩關鍵幀之間的相對位姿的不確定度，進而獲得用于姿態圖優化的信息矩陣。本文通過遞推關鍵幀間的相機位姿來計算得到關鍵幀間的相對位姿不確定度估計。首先，給出連續兩幀相機姿態，分別為：

因此，當3D-2D 匹配點數目較多時，協方差矩陣Σ1和Σ2的行列式較小，對于連續兩幀的組合相機位姿，可以直接保留二階項用于估計組合相機位姿的協方差矩陣：

對于連續多次的相機位姿組合問題，推導可得邊（m, n ）的相對位姿不確定度：

2.2 姿態圖算法

1）姿態圖優化

考慮到常用的位姿圖優化需要六自由度的信息，由于地理姿態信息不含對相機位置的約束，因此推導融合地理真實姿態的姿態圖優化算法。首先，定義相對姿態李群表達形式如下：

將其表達為李代數形式：

誤差表達式為：

對式(12)添加左擾動，可得：

根據恒等式：

繼續整理可得：

式中：

式(16)和式(17)即為姿態圖優化的雅克比矩陣子塊，將其帶入高斯牛頓方程可迭代求解相機的姿態。在獲得用于姿態優化的雅克比矩陣子塊后，給出誤差方程表達式：

式中，雅克比矩陣 J（ x ）為：

式中，M 為圖像共視關鍵幀所組成的邊的個數；邊的集合為MΘ ，m 為序號下標；N 為測姿幀所組成的邊的個數；邊的集合為NΘ ，n 為序號下標。且滿足：

式中，（ i ,j ）∈ΘM以及（ p ,q ）∈ΘN，下面給出誤差項的協方差矩陣：

式中，oΣ 為對角分塊矩陣。通過非線性優化方法可迭代求解關鍵幀姿態。

通過融合地理真實姿態的姿態圖優化算法，不僅能夠在運算中避免計算大量路標點，提升優化效率，還能夠有效避免路標點本身所具有的累積誤差對于相機位姿估計和路標點位置的影響，從而正確利用地理真實姿態準確校正各個關鍵幀的相機位姿和路標點位置，避免直接進行全局優化而導致相機位姿和路標點位置收斂于局部最小值。此外，進一步考慮到姿態圖優化后的相機位姿和路標點的位置還存在一定的偏差，需要在下一步進行全局優化以獲得更加準確的估計結果。

2）全局優化

在全局優化中，相機位姿和路標點位置同時作為優化變量，路標點重投影誤差和測姿幀誤差表達式分別為：

由此，可以將姿態圖優化后的相機位姿和路標點位置作為初始值，利用雅克比矩陣求解高斯牛頓方程，實現相機位姿和路標點位置的全局優化。

3仿真分析及試驗結果

3.1 仿真分析

為驗證算法的有效性，本文利用標準數據集KITTI中編號為00的數據集中部分數據進行仿真分析，該數據集提供了真實位姿，仿真中提取部分關鍵幀的真實姿態作為尋北系統所檢測到的地理真實姿態信息。

圖2中，X 軸為數據集序列號，Y 軸為融合前后的絕對軌跡誤差（AbsolutePosition Error,APE）。可以看出，當相機運動到拐彎處，即關鍵幀序號為35幀之后時，誤差變化劇烈，盡管在(40,50)階段APE在減少，實際上是由于姿態發生了嚴重的漂移而導致相機估計軌跡與實際軌跡在三維空間形成了較大的相交角度，因此在接近區域誤差減小，但此后誤差逐漸增大；在融合優化后，如藍色曲線所示，相機的位置誤差顯著減少。

圖2 融合優化前后的絕對位置誤差Fig.2 Absolute position error beforeand after fusion optimization

此外，相對位置誤差能夠有效體現相鄰關鍵幀位置估計誤差的變化情況，圖3給出了相同序列上關鍵幀相對位置誤差和相對位姿協方差行列式對數值。其中，藍色圓圈標記了相對位置誤差較大的點，綠色圓圈標記了相對位置協方差較大的曲線段，可以看出兩者基本呈負相關，即協方差大的位置，相對位姿誤差較大，證明了協方差估計的合理性。通過在姿態圖中融入協方差進行優化，能夠正確設置圖優化中邊的權重，實現累積誤差的有效消除。

圖3 關鍵幀相對位置誤差和相對位姿協方差行列式對數值Fig.3 Relative position error of key frameand relative pose covariancedeterminant log value

仿真分析表明，在沒有回環檢測環節參與的情況下，經過融合優化后相機位姿的絕對軌跡誤差下降至優化前的30%以下。此外，根據實際測量需求可以降低閾值，以獲得更好的優化效果。在下一節中，將該算法應用于經典框架ORB-SLAM2以分析算法性能。

3.2 試驗結果

3.2.1 室內試驗

在室內試驗中，將本文算法搭載于ORB-SLAM2，外部測姿系統如圖4所示，由轉臺、光纖陀螺儀和加速度計構成，航向精度在30s內達到0.3°，水平精度在30s內達到0.05°。通過靶標進行姿態傳遞，相機能夠較為準確的獲得地理真實姿態。

圖4 測姿系統Fig.4 Attitudemeasurement syste m

本試驗中，具有尋北信息的圖像幀共有5幀?？紤]到相機絕對位置在當前實驗條件下不易獲得，本文利用實驗場地處于空間內一個平面這一限制條件分析算法的效果。融合優化前后相機在y 軸方向上的軌跡誤差值如表1所示。

從表1可知，融合后各項誤差均顯著降低，其中均值誤差僅為融合前的67.6%，最大值為融合前的68.2%。最后，將y 軸誤差呈現在相機運動的xz 平面內，如圖5所示。

表1 融合前后相機在y軸上的軌跡誤差（單位：米）Tab.1 The trajectory error of thecamera onthe y-axis b efore andafter the fusion(Unit:m)

圖5中，右側色帶從藍色過渡到紅色表示軌跡在y 軸的誤差逐漸變大，在靠近起點同時也是回環處的誤差相對較小，軌跡顯示為藍色，這是由于開始運動時累積誤差較小，同時回環檢測可以有效降低誤差。同時，從兩幅曲線圖也可以發現，在距離起點較遠的位置處，融合前的軌跡誤差達到了整個估計值誤差中的最大值，這是由于回環檢測對距離回環距離較遠的位置校正能力十分有限。在通過融合算法后，距離校正點最遠的位置曲線顏色從紅色變為了黃綠色，同時整個曲線的顏色向藍色偏移，即全局誤差整體顯著降低，在一定程度上驗證了算法對于減小累積誤差的有效性。為進一步準確地評估融合優化前后的誤差情況，下一節將融合視覺和地理真實姿態的ORB-SLAM2 算法應用在標準數據集KITTI 上進行實驗。

圖5 運動軌跡xz 平面的y軸誤差Fig.5 The y-axiserror of the xz planeof the trajectory

3.2.2 基于標準數據集KITTI的試驗

本節利用KITTI 數據集進行實驗，該數據集是國際上最大的自動駕駛場景下計算機視覺算法評測數據集，部分數據集還提供了相機的真實位姿，本實驗所需要的地理真實姿態信息來自數據集所提供的真實位姿。為體現在稀疏尋北幀下的優化效果并保證均勻性，每200個圖像關鍵幀添加一次尋北關鍵幀，以進行融合優化。此外，為驗證算法在不同情況下的性能，本實驗選取KITTI 數據集中具有多次回環的00號數據集和不存在回環的01號數據集。

1）具有多次回環的00號數據集實驗

在本實驗中，共添加了6幀尋北關鍵幀，利用軌跡真實值對融合前后軌跡的估計值進行評定，融合前后誤差值如表2所示。

表2 融合前后誤差Tab.2 Error before andafter fusion

根據表2，融合后的軌跡誤差顯著下降，均值下降為融合前的62.9%，標準差下降為53.6%；融合后旋轉角誤差同樣顯著下降，均值下降為融合前的55.9%，除標準差微弱上升外，其余各項誤差均顯著下降。此外，將相機的軌跡誤差呈現在運動軌跡的xz平面中，如圖6所示，彩色線為估計軌跡，虛線為真實軌跡。

如圖6(a)所示，在距離回環檢測位置較遠的位置，回環檢測的校正能力有限，導致誤差較大，因此呈現紅色；如圖6(b)所示，在經過融合優化后，全局的軌跡誤差明顯降低，誤差最嚴重的區域也從紅色變為了黃綠色。綜上，對于00號數據集，融合后軌跡誤差和旋轉角誤差明顯降低，證明了優化算法的有效性。

圖6 運動軌跡xz 平面的軌跡誤差Fig.6 The y-axiserror of the xz planeof the trajectory

2）無回環的01號數據集實驗

上述實驗所利用的KITTI 的00號數據集具有多次回環，為進一步驗證算法的綜合性能，對KITTI 的01號數據集進行測試，該數據集并未存在回環，因此可以很好檢測在沒有回環情況下融合算法的性能，其中尋北關鍵幀共有7幀，融合前后誤差值如表3所示。

表3 融合前后誤差Tab.3 Error beforeand after fusion

根據表3，融合后的軌跡誤差有所下降，最大值下降為融合前的79.7%，標準差下降為80.9%；融合后旋轉角誤差顯著下降，均值僅為融合前的8.8%，標準差僅為16.7%?？紤]到旋轉角誤差下降明顯，進一步分析融合算法對姿態角誤差的校正情況，姿態角變化情況如圖7所示，X軸為數據集序列號，綠線為融合前角度，藍線為融合后角度，虛線為真實角度。

圖7 姿態角Fig.7 Attitude angle

從整體和局部姿態角曲線可知，藍色曲線與虛線基本重合，而它們與綠色曲線有一定偏離，這代表融合后的俯仰角、翻滾角和偏航角與它們的真實值基本相同，而融合前角度則與真實值有一定差別。因此，融合優化算法顯著提升了相機旋轉角的估計精度，能夠有效降低相機的姿態誤差。此外，將相機的軌跡誤差呈現在運動軌跡的xz 平面中，如圖8所示，彩色線為估計軌跡，虛線為真實軌跡。

圖8 運動軌跡xz平面的軌跡誤差圖Fig.8 The trajectory error of the xz planeof the motion trajectory

如圖8(a)所示，在融合前，軌跡末端（右下角所示）明顯偏離于軌跡真實值，而如圖8(b)所示，在經過融合優化后，軌跡曲線中代表誤差最大的深紅色變為了橙色，全局的軌跡誤差有效降低。綜上，對于不存在回環的01 號數據集，融合后軌跡誤差在一定程度上被減小，姿態角誤差幾乎被消除，證明了融合優化算法在數據集不存在回環情況下對于減小累積誤差的有效性。

4 結 論

本文設計了融合外部姿態信息的SLAM 算法，以消除系統的累積誤差。為有效利用真實位姿信號減小累積誤差，設計了融合視覺和地理真實姿態的SLAM算法架構。通過標定靶傳遞外部姿態信息，推導了姿態組合后的不確定度表達式，并提出了姿態圖和全局優化的算法，獲得了校正累積誤差后的全局最優軌跡和地圖。實驗驗證了融合視覺和地理真實姿態對于累計誤差消除的有效性。

考慮在室內建模、地下救援、災后重建等場景下，常規傳感器易受到環境的制約，無法穩定的消除SLAM 系統中的累積誤差，導致系統在長時間工作過程中定位和建圖精度嚴重下降，甚至失效。本文提出的視覺和外部姿態測量系統融合方案，則能在這些嚴苛環境下依然為SLAM系統提供真實準確的外部姿態信息，同時通過算法解決了外部姿態信息相比于圖像幀過于稀疏而帶來的優化難題，對SLAM 的應用領域拓展具有重要意義。

此外，本文所使用的測姿系統體積較大，不能靈活移動，會在一定程度上限制SLAM 系統的應用，且室內實驗未能提供用于評估誤差的完整的相機運動軌跡。未來計劃選型其他更加便捷的測姿系統，并利用激光跟蹤儀等設備獲取相機的真實軌跡，開展進一步驗證工作。