融合信賴域的SLAM后端優(yōu)化算法*

王華宇，雷 斌

(武漢科技大學(xué) a.機(jī)械傳動(dòng)與制造工程湖北省重點(diǎn)實(shí)驗(yàn)室；b.機(jī)器人與智能系統(tǒng)研究院，武漢 430081)

0 引言

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

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

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

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

1 后端優(yōu)化速度提升原理分析

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

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

LM算法是Gauss-Newton和最速下降法結(jié)合的衍生算法，通過迭代的方式求解多個(gè)非線性實(shí)值函數(shù)的平方和組成的多元函數(shù)的局部極小值。LM算法在當(dāng)前解遠(yuǎn)離局部極小值時(shí)表現(xiàn)出最速下降法的性質(zhì)：收斂緩慢，但保證收斂性；在當(dāng)前解靠近局部極小值時(shí)表現(xiàn)出Gauss-Newton法的性質(zhì)：迅速收斂。

忽略二次項(xiàng)，將殘差函數(shù)進(jìn)行一階泰勒近似展開：

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

(1)

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

(2)

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

(JTJ)δgn=-JTf

(3)

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

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

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

(4)

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

(5)

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

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

(6)

2 融合信賴域改進(jìn)的后端優(yōu)化方法

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

2.1 TR算法

在一個(gè)信賴域框架內(nèi)，構(gòu)造目標(biāo)函數(shù)的二次函數(shù)模型，該函數(shù)模型在當(dāng)前點(diǎn)附近的行為與目標(biāo)函數(shù)相似，但該函數(shù)模型僅對以當(dāng)前點(diǎn)為中心、半徑為Δ的超球面上的點(diǎn)精確表示目標(biāo)函數(shù)，因此被稱為信賴區(qū)域，Δ為信賴域半徑。近似線性化模型為：

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

(7)

(8)

其中，

(9)

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

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

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

(10)

其中，

(11)

p的更新策略為：

(12)

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

相較于原有的LM算法，TR算法不僅能從正規(guī)方程矩陣的稀疏性當(dāng)中受益，在大規(guī)模問題求解上，TR算法無需對待估計(jì)參數(shù)求偏導(dǎo)，所需的計(jì)算量更低，這一點(diǎn)對于SLAM中一幅圖像數(shù)百個(gè)特征點(diǎn)構(gòu)建的超大維度線性方程求解尤為重要。

在迭代的過程中，如果遇到當(dāng)前迭代步?jīng)]有使計(jì)算結(jié)果收斂的情況，LM算法需要重新計(jì)算Hessian矩陣來進(jìn)行下一次迭代；而TR算法分別求解了Gauss-Newton法和梯度下降法的迭代步，在信賴域內(nèi)使用一定的策略來進(jìn)行選擇，當(dāng)遇到壞的迭代步時(shí)不再需要像LM算法一樣重新計(jì)算Hessian矩陣。因此，TR算法更適用于SLAM問題中的大規(guī)模BA優(yōu)化過程。

2.2 并行運(yùn)算加速信息矩陣拼接

隨著SLAM系統(tǒng)在新環(huán)境中的探索不斷深入，越來越多的相機(jī)與IMU姿態(tài)以及環(huán)境圖像特征被加入到殘差函數(shù)中，最小二乘殘差就會不斷增多，信息矩陣數(shù)量越來越大，計(jì)算量也隨之加大。為了保持優(yōu)化變量的個(gè)數(shù)在一定范圍內(nèi)，采用滑窗優(yōu)化動(dòng)態(tài)增加或移除優(yōu)化變量，是當(dāng)前流行的處理方法?；皟?yōu)化中，增加或者移除優(yōu)化變量，需要將對應(yīng)殘差的信息矩陣拼接起來。而在進(jìn)行矩陣拼接的時(shí)候需要進(jìn)行大量計(jì)算和頻繁的存儲器訪問。

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

(a) 串行運(yùn)算示意圖

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

3 實(shí)驗(yàn)

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

3.1 LM算法改進(jìn)前后對比實(shí)驗(yàn)

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

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

表1 絕對軌跡誤差對比實(shí)驗(yàn) (m)

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

圖2 TR算法與地面真值軌跡對比實(shí)驗(yàn)結(jié)果

圖3 改進(jìn)前后LM算法與TR算法時(shí)間消耗對比

3.2 并行加速對比實(shí)驗(yàn)

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

如表2和圖4所示，并行加速以極小的系統(tǒng)精度為代價(jià)，獲得了明顯的速度提升。實(shí)驗(yàn)結(jié)果表明，并行運(yùn)算能夠加速后端優(yōu)化過程中的矩陣拼接過程，提高系統(tǒng)運(yùn)行速度。

表2 并行加速實(shí)驗(yàn)絕對軌跡誤差結(jié)果 (m)

圖4 并行加速優(yōu)化前后時(shí)間消耗對比

4 結(jié)論

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