融合信賴域的SLAM后端優化算法*

王華宇，雷 斌

(武漢科技大學 a.機械傳動與制造工程湖北省重點實驗室；b.機器人與智能系統研究院，武漢 430081)

0 引言

視覺與慣性導航融合里程計(Visual-Inertial Odometry, VIO)是當前解決SLAM高精地圖與多場景適用的主流方案之一。VIO可以根據傳感器數據融合框架分為松耦合與緊耦合[1]，也可以根據優化方式分為濾波法與優化法[2]。松耦合忽略不同傳感器數據之間的相關性，視覺運動估計和IMU運動估計結果相互獨立，將估計結果進行融合，損失了部分數據信息，精度較低；緊耦合則使用不同傳感器原始數據共同進行估計，一次性建模所有的運動和測量信息，更容易得到最優結果，是目前研究的重點。

MSCKF[3]是基于卡爾曼濾波的緊耦合方法，將多幀圖像的相機位姿加入卡爾曼狀態向量。該方法魯棒性高，計算復雜度低，但沒有全局優化和回環檢測。MSF[4]是基于濾波法的EKF松耦合框架，該方法極大地提升了系統運行速度，系統拓展性好，可接入GPS、激光雷達、碼盤等其它傳感器，但精度很差。ROVIO[5]是基于稀疏圖像塊和EKF濾波的VIO，慣性測量用于狀態傳遞過程，視覺信息用于更新；其速度與MSCKF相似，精度較高，計算量小，但不同設備需要調整參數，沒有回環檢測。OKVIS[6]將視覺觀測與IMU觀測一起優化求解位姿與3D地圖點，前端采用Harris角點檢測和BRISK特征描述子進行匹配，計算資源消耗大，不支持重定位和閉環檢測。該方法采用緊耦合策略和非線性優化，精度高但速度慢。

VINS-Mono[7]在精度上可以媲美OKVIS，具有比OKVIS更加魯棒和完善的初始化以及閉環檢測過程。但隨著SLAM輕量化趨勢的發展，一些小型設備開始支持SLAM技術[8]，要在有限的計算資源條件下，發揮出更加優秀的實時性能，VINS-Mono仍然存在提升的空間。

文章在保證原系統精度的前提下，對VINS-Mono系統優化過程中耗時較多的部分進行優化，使用融合信賴域改進的TR算法替換Levenberg-Marquardt算法，對最小二乘問題求解部分進行優化，采用并行運算對信息矩陣拼接部分進行加速。實驗結果表明，優化后的算法提高了系統運行速度、改善了系統的實時性能，更加適合大規模的后端優化問題。

1 后端優化速度提升原理分析

經典視覺SLAM系統主要包括4個模塊：前端視覺里程計(Visual Odometry)、后端優化、回環檢測、地圖構建。后端非線性優化過程中的集束調整[10](Bundle Adjustment, BA)是除前端特征提取與跟蹤之外，最消耗計算資源的部分。

BA可以歸結為最小化圖像點觀測值和預測值之間的重投影誤差。重投影誤差表現為大量非線性實值函數的平方和，通過Levenberg-Marquardt[11](LM)等最小二乘算法來最小化重投影誤差是當前最常見的做法。相對容易的實現過程和有效的阻尼策略能夠讓LM在大范圍的初始估計中迅速收斂，使得LM算法成為眾多BA實現的基準。通過迭代線性化函數，使其在當前估計鄰域內最小化，LM算法涉及到線性系統的解，即正規方程。在求解BA中的最小化問題時，由于不同的三維點和相機參數之間缺乏相互作用，使得正規方程矩陣具有稀疏塊結構。利用正規方程矩陣的稀疏性，可以加速正規方程的求解過程，從而加快BA的優化速度。

LM算法是Gauss-Newton和最速下降法結合的衍生算法，通過迭代的方式求解多個非線性實值函數的平方和組成的多元函數的局部極小值。LM算法在當前解遠離局部極小值時表現出最速下降法的性質：收斂緩慢，但保證收斂性；在當前解靠近局部極小值時表現出Gauss-Newton法的性質：迅速收斂。

忽略二次項，將殘差函數進行一階泰勒近似展開：

f(p+δ)≈l(p)≡f(p)+J(p)δ

(1)

J(p)=?f(p)/?p為殘差函數的雅克比(Jacobian)矩陣，δ為迭代步長。將殘差函數代入損失函數：

(2)

如此，就將損失函數近似成一個二次函數線性模型L，如果雅克比矩陣滿秩，則JTJ正定，損失函數有最小值。令損失函數一階導為零，可得高斯牛頓方程：

(JTJ)δgn=-JTf

(3)

式(3)也可以稱為正規方程，即用Gauss-Newton步長δgn作為正規方程的解。雅克比矩陣J與海塞(Hessian)矩陣H=JTJ均具有稀疏結構，求解正規方程是整個優化問題的核心。

Levenberg和Marquardt對Gauss-Newton法進行的改進，是在式(3)的基礎上引入阻尼因子μ：

(JTJ+μI)δlm=-JTf,μ≥0

(4)

式(4)也被稱為增廣正規方程，I為單位矩陣。對JTJ做特征值分解后有：

(5)

半正定的信息矩陣特征值{λi}和對應的特征向量為{Vj}。阻尼因子μ的大小是相對于信息矩陣JTJ的元素而言的，一個簡單的初始值策略為μ0=τ·max{(JTJ)ii}，通常τ～[10-8,1]。

使用增益ρ控制迭代更新：

(6)

2 融合信賴域改進的后端優化方法

當前大部分的集束調整方法均是采用的LM算法，很少有除利用正規方程矩陣稀疏性之外的集束調整加速方法被提出。但LM算法需要對每一個待估計參數求偏導，而且求解過程中存在矩陣求逆運算，如果目標函數f非常復雜，或者有大量的待估計參數，LM算法就不再是集束調整最優的選擇。針對這一問題，提出了一種融合信賴域(Trust Region)的優化方法—TR算法，用于SLAM系統中的后端優化過程。

2.1 TR算法

在一個信賴域框架內，構造目標函數的二次函數模型，該函數模型在當前點附近的行為與目標函數相似，但該函數模型僅對以當前點為中心、半徑為Δ的超球面上的點精確表示目標函數，因此被稱為信賴區域，Δ為信賴域半徑。近似線性化模型為：

f(p+αδ)≈f(p)+αJδ

(7)

(8)

其中，

(9)

Gauss-Newton法的迭代步δgn由正規方程(3)給出，最速下降法的迭代步δsd由δsd=-JTf給出，此時改進的LM算法迭代步長δtr有兩個候選迭代步可供選擇：δtr=αδsd與δtr=δgn。在信賴域半徑為Δ的條件下使用以下策略來選擇迭代步：

如果δgn≤Δ,δtr=δgn；

LM算法中使用增益ρ控制阻尼參數更新，而在TR算法中，增益ρ被用來控制信賴域半徑Δ更新，如式(10)所示：

(10)

其中，

(11)

p的更新策略為：

(12)

其中，ρ的取值范圍劃分采用的是經驗值。ρ比較大，說明線性化模型效果很好，可以適當增大信賴域半徑來增大迭代步長，靠近Gauss-Newton迭代方向；若ρ比較小，則需要減小信賴域半徑來減小迭代步長，使迭代靠近最速下降迭代方向。

相較于原有的LM算法，TR算法不僅能從正規方程矩陣的稀疏性當中受益，在大規模問題求解上，TR算法無需對待估計參數求偏導，所需的計算量更低，這一點對于SLAM中一幅圖像數百個特征點構建的超大維度線性方程求解尤為重要。

在迭代的過程中，如果遇到當前迭代步沒有使計算結果收斂的情況，LM算法需要重新計算Hessian矩陣來進行下一次迭代；而TR算法分別求解了Gauss-Newton法和梯度下降法的迭代步，在信賴域內使用一定的策略來進行選擇，當遇到壞的迭代步時不再需要像LM算法一樣重新計算Hessian矩陣。因此，TR算法更適用于SLAM問題中的大規模BA優化過程。

2.2 并行運算加速信息矩陣拼接

隨著SLAM系統在新環境中的探索不斷深入，越來越多的相機與IMU姿態以及環境圖像特征被加入到殘差函數中，最小二乘殘差就會不斷增多，信息矩陣數量越來越大，計算量也隨之加大。為了保持優化變量的個數在一定范圍內，采用滑窗優化動態增加或移除優化變量，是當前流行的處理方法。滑窗優化中，增加或者移除優化變量，需要將對應殘差的信息矩陣拼接起來。而在進行矩陣拼接的時候需要進行大量計算和頻繁的存儲器訪問。

如圖1a所示，當前的計算機處理器是基于x86指令集的串行架構設計的，在設計思路上是為了盡快地完成一個任務，因此在處理矩陣拼接這種需要高度并行、運算密集、分多個階段執行的問題時，其缺陷顯而易見。使用并發編程模型可以有效解決這個問題。如圖1b所示，并發編程可以通過共享變量進行數據交換來共享內存，按照for循環將單線程的單個任務使用多線程進行處理，能夠大幅提升信息矩陣的拼接速度，減少運算時間，提升程序實際運行效率。

(a) 串行運算示意圖

(b) 并行運算示意圖圖1 串并行運算示意圖

3 實驗

實驗基于VINS-Mono開源框架完成，實驗所需視覺及IMU數據采用EuRoC公開數據集中的MH_05_difficult序列。實驗平臺為Intel i7-8750H@2.20GHz (16GB RAM)。采用絕對軌跡誤差(ATE)測試系統在精度和速度上的變化。絕對軌跡誤差(ATE)是估計值與地面真實軌跡之間的絕對距離，將估計值與地面真值根據位姿時間戳對齊，計算每對位姿之間的差值并以圖表的形式輸出。ATE具有直觀的可視化性能，非常適合用于評估視覺SLAM系統的性能。

3.1 LM算法改進前后對比實驗

該部分主要進行了三組對比實驗，第一組為原始LM算法與地面真值(GT)，第二組為TR算法與地面真值，第三組為TR算法與原始LM算法。使用SLAM性能評估工具EVO對系統軌跡絕對誤差進行測試，測試項目為最大軌跡誤差max、軌跡誤差平均值mean、軌跡誤差中值median、軌跡誤差最小值min、均方根誤差rmse、誤差平方和sse以及標準差std。表1中給出了算法改進前后絕對軌跡對比實驗的各項數據。

如表1所示，原始LM算法與地面真值的最大軌跡誤差約為0.483 m，誤差平均值約為0.226 m，均方根誤差約為0.242 m；TR算法與地面真值的最大軌跡誤差約為0.426 m，誤差平均值約為0.226 m，均方根誤差約為0.239 m；改進前后的軌跡誤差最大值約為0.105 m，誤差平均值約為0.033 m。改進前后的系統精度幾乎保持不變。

表1 絕對軌跡誤差對比實驗 (m)

圖2所示為TR算法與地面真值的軌跡對比結果。虛線為地面軌跡真值，靠右部分柱狀圖中刻度從下往上依次為絕對軌跡誤差最小值、中值、最大值。圖3所示為融合信賴域改進前后的算法時間消耗對比結果。橫坐標為圖像幀序列，縱坐標為處理該幀所消耗的時間。原始LM算法的平均時間消耗約為89.34 ms，TR算法的平均時間消耗為62.43 ms，速度提升了近30.12%。實驗結果表明，TR算法在以極小的精度損失為代價的情況下，節省了大量計算資源，極大地減少了優化過程的時間消耗，提升了后端優化算法的性能。

圖2 TR算法與地面真值軌跡對比實驗結果

圖3 改進前后LM算法與TR算法時間消耗對比

3.2 并行加速對比實驗

使用多線程編程對系統代碼進行優化，提升信息矩陣拼接速度，使用EVO工具測試優化前后系統的精度和速度變化。表2所示為并行加速優化算法與TR算法、并行加速優化算法與原始LM算法兩組對比實驗的結果。由表可知，并行加速后的軌跡與地面真值的最大軌跡誤差約為0.426 m，平均誤差約為0.226 m，均方根誤差約為0.238 m；與TR算法的軌跡相比對，最大誤差約為0.105 m，平均誤差約為0.033 m，均方根誤差約為0.035 m；即并行計算加速前后系統的精度損失極小。圖4所示為并行運算加速前后的算法Hessian矩陣拼接部分耗時對比結果。加速前Hessian矩陣拼接平均耗時32.09 ms，加速后平均耗時為7.90 ms，速度提升近75.38%。

如表2和圖4所示，并行加速以極小的系統精度為代價，獲得了明顯的速度提升。實驗結果表明，并行運算能夠加速后端優化過程中的矩陣拼接過程，提高系統運行速度。

表2 并行加速實驗絕對軌跡誤差結果 (m)

圖4 并行加速優化前后時間消耗對比

4 結論

本研究針對輕量化SLAM系統中，后端優化消耗大量計算資源與時間這一問題，提出了一種融合信賴域改進的后端優化方法。該方法通過優化最小二乘法求解步驟，能夠顯著提升最小二乘問題求解速度，加快后端優化速度；同時，使用并行運算加快信息矩陣拼接速度，能夠在不降低精度的前提下，提升SLAM系統的實時性能。實驗結果表明，經該方案優化的集束調整更加適合大規模的SLAM后端優化問題，優化后的算法迭代求解速度提升了近30.12%，實時性能表現更佳。