基于雅克比域零空間邊緣化的視覺SLAM*

蘆 濤，金 馨，廖毅霏，黃圣杰，楊依琳，謝國濤，，秦曉輝，3

（1.汽車車身先進設計制造國家重點實驗室，湖南大學機械與運載工程學院，長沙 410082；2.山東省科學技術情報研究院，濟南 250101；3.湖南大學無錫智能控制研究院，無錫 214115）

前言

自動駕駛技術包含環(huán)境感知、自主定位、決策規(guī)劃以及運動控制等多項關鍵技術［1］，其中自主定位技術是智能車輛在復雜環(huán)境下自主運動的基礎保障，是車輛決策與規(guī)控的重要前提［2］。同時定位與建圖（simultaneous localization and mapping，SLAM）是指智能車輛在沒有周圍環(huán)境的先驗信息前提下，僅利用自身傳感器完成位姿估計與地圖構建的過程，是自主定位技術中的重要組成部分［3］。近年來，隨著自動駕駛系統(tǒng)的推廣，SLAM 技術得到廣泛研究。根據(jù)傳感器方案不同，SLAM 技術主要分為視覺SLAM 與激光SLAM，相較于激光雷達，視覺傳感器具有成本低、易于安裝和信息豐富等特點，常用于智能車自動駕駛實際落地的部署方案中［4］。

視覺SLAM 算法基于PTAM 架構日漸成熟［5］，主要分為前端跟蹤模塊和后端優(yōu)化模塊。目前主流視覺SLAM 研究工作主要集中在前端跟蹤及系統(tǒng)功能創(chuàng)新［6］，而對于自動駕駛落地任務，如何在保證精度及魯棒性的基礎上，提升高算力負擔下后端模塊的求解速度，保證其在搭載低算力平臺的設備上穩(wěn)定運行成為了一個難題。

后端優(yōu)化本質(zhì)上是一個狀態(tài)估計模塊，待估計狀態(tài)量為相機曝光時刻位姿以及特征像素點空間位置［7］。由于待估計像素點數(shù)量一般較多，導致優(yōu)化問題維度較大，需要利用一定技巧將優(yōu)化問題降階。根據(jù)狀態(tài)估計方法不同，主要分為濾波框架以及基于非線性優(yōu)化框架?；跒V波框架的代表作有MSCKF［8］，該系統(tǒng)在濾波更新階段將觀測方程的路標點雅克比投影至其左零空間使路標點邊緣化，形成僅含位姿狀態(tài)的新誤差觀測方程，從而在不擴充狀態(tài)向量前提下對系統(tǒng)進行觀測更新；基于優(yōu)化框架的代表作有ORB-SLAM［9］等方法，此種優(yōu)化框架在進行狀態(tài)估計時常常會利用海森矩陣獨有的稀疏性結合舒爾消元將待估計問題進行降階處理［10］。相較于非線性優(yōu)化，基于濾波的后端估計存在著一些局限性，例如其需要將待估計狀態(tài)建模成具有高斯分布的隨機變量，在預測更新過程中實時維護其均值和協(xié)方差，導致系統(tǒng)擴展性較差且沒有異常檢測機制，在遇到離群數(shù)據(jù)時系統(tǒng)容易發(fā)散。此外，因為濾波器方法在一定程度上假設了馬爾科夫性，在觀測信息的使用上不像優(yōu)化方法可以使用歷史數(shù)據(jù)；最重要的是，由于濾波方法無法在狀態(tài)更新后重新線性化，導致其精度會受到線性化誤差的影響，而基于優(yōu)化的方法在迭代中可以基于新的狀態(tài)量多次線性化，在系統(tǒng)精度方面存在一定優(yōu)勢，同時由于目前優(yōu)化問題已有一些成熟的工具，如ceres［11］、g2o［12］等，使得基于非線性優(yōu)化的視覺SLAM 在近些年受到廣泛關注。

基于非線性優(yōu)化的后端模塊最主要功能就是實時執(zhí)行光束法平差（bundle adjustment，BA），從一系列視覺圖像中提煉出最優(yōu)像素點空間位置和相機外參［13］。由于像素路標點數(shù)量眾多，BA問題優(yōu)化的算力要求很容易達到低算力平臺難以接受的程度。BA 優(yōu)化中較為代表性的工作有ICE-BA［14］，其在迭代求解時的線性化階段使用增量式狀態(tài)線性化策略，避免反復線性化變化不大的狀態(tài)量，從而降低系統(tǒng)算力負擔，但帶來的結果是梯度不準確從而導致定位精度有一定下降。此外，也存在諸如MegBA 這種利用GPU 并行化求解BA 問題的工作［15］，但使用如此高成本設備求解自主定位模塊中一個小功能不利于工程化應用。在求解BA問題時，由于對增量方程（normal equation）直接求解會導致計算量較高，往往會利用海森矩陣稀疏性結合舒爾消元將優(yōu)化問題降階，即邊緣化（marginalization）［16］。但基于舒爾消元的邊緣化方法不可避免地需要顯式構建海森矩陣（Hession matrix），即信息矩陣，為濾波框架中協(xié)方差矩陣的逆。對于一般視覺SLAM 系統(tǒng)，海森矩陣很容易達到成千上萬的維度，這為將BA 問題移植到低算力平臺實時求解留下了隱患。同時，由于目前基于非線性優(yōu)化的視覺SLAM 框架一般使用通用的非線性優(yōu)化器求解BA 問題，若針對具體系統(tǒng)設計優(yōu)化器，在性能方面還存在一定的提升空間［17］。

為解決上述問題，受到基于濾波框架的MSCKF系統(tǒng)在觀測方程中使用左零空間邊緣化路標點的思想啟發(fā)［18］，加之濾波框架本身存在的局限性，本文提出一種利用雅克比域零空間在非線性優(yōu)化視覺SLAM 系統(tǒng)中邊緣化路標點的方法，相較于傳統(tǒng)舒爾消元邊緣化方法，其能在不構建海森矩陣前提下將BA問題進行邊緣化降階，使得后端優(yōu)化模塊在執(zhí)行BA 過程中具備更好的數(shù)值穩(wěn)定性。本文主要貢獻如下：

（1）提出一種利用雅克比域零空間在非線性優(yōu)化視覺SLAM 系統(tǒng)中邊緣化路標點的方法，在代數(shù)上詳細推導了該邊緣化方法的過程，使得后端優(yōu)化模塊可在不構建海森矩陣前提下將BA問題降階，提升了后端優(yōu)化模塊的求解速度。

（2）在代數(shù)上證明了本文方法和基于舒爾消元的邊緣化方法的等價性，同時在數(shù)值分析的角度上證明本文方法具備更好的數(shù)值穩(wěn)定性，從而支持求解器使用單精度浮點數(shù)求解，進一步提升運算效率，為后端優(yōu)化模塊在一些精度和算力有限的低成本設備上運行提供了可能性。

（3）基于該邊緣化方法實現(xiàn)了非線性優(yōu)化BA求解器，并將其應用在了主流的視覺SLAM框架［9］中。

（4）通過城市自動駕駛場景公開數(shù)據(jù)集［19］及實車試驗，證明本文方法具備更好的精度和處理速度。

1 系統(tǒng)架構

本文提出的基于雅克比域零空間邊緣化視覺SLAM 系統(tǒng)架構如圖1 所示，功能模塊主要分為3 個部分：前端跟蹤、后端優(yōu)化和回環(huán)檢測。其中前端跟蹤部分接收圖像數(shù)據(jù)，經(jīng)過與上一幀圖像數(shù)據(jù)關聯(lián)及位姿估計后［20］，將判定為關鍵幀的對象送到后端優(yōu)化模塊進行優(yōu)化，同時為優(yōu)化問題提供初值；在后端模塊，首先利用關鍵幀與地圖點共視關系維護局部地圖，利用最小化重投影誤差對該局部地圖執(zhí)行BA，獲得相機姿態(tài)以及路標點空間位置的最優(yōu)解；最后再經(jīng)過回環(huán)檢測模塊，進行回環(huán)矯正［21］，消除累積誤差。

圖1 基于雅克比域零空間邊緣化的視覺SLAM系統(tǒng)架構

該系統(tǒng)在ORB-SLAM2［22］基礎上，將基于第三方通用優(yōu)化庫g2o 實現(xiàn)的舒爾消元邊緣化局部BA模塊替換成本文提出的基于雅克比域零空間邊緣化局部BA 模塊，如圖2 所示。該模塊在求解BA 時利用雅克比矩陣的左零空間投影將邊緣化操作從構建海森矩陣后提前到了線性化狀態(tài)量后，避免構建具有大維度海森矩陣同時降低了求解過程中的矩陣條件數(shù)，相較于基于舒爾消元邊緣化的局部BA模塊具備更好的精度和處理速度。

圖2 基于雅克比域零空間邊緣化的局部BA流程

2 基于零空間邊緣化的BA

本節(jié)介紹BA問題的數(shù)學形式，在代數(shù)上推導基于海森域舒爾消元邊緣化方法以及基于雅克比域零空間邊緣化方法，同時證明了兩種方法的等價性，并且從數(shù)值分析角度表明本文方法具備更好的運算效率和數(shù)值穩(wěn)定性，最后介紹基于該邊緣化方法實現(xiàn)的求解器細節(jié)。

2.1 BA問題構建

本文的BA優(yōu)化問題狀態(tài)量如下。

相機曝光時刻世界坐標系到相機坐標系的變換∈SE(3)，i∈[0，np]，李代數(shù)表示為ξi∈SE(3)，c表示相機坐標系，w表示世界坐標系，np表示相機狀態(tài)數(shù)量。

基于重投影誤差定義的代價函數(shù)為

式中：uij表示第i個相機對第j個地圖點的實際二維觀測；K為針孔相機的內(nèi)參矩陣。

對于該最小二乘問題，一般利用高斯牛頓法迭代求解出最佳狀態(tài)量增量?x使得代價函數(shù)最小，即

式中：e(x)表示狀態(tài)量殘差；J(x)表示殘差對狀態(tài)量的雅克比矩陣；?x為狀態(tài)量增量。將式（2）展開后對增量?x求導并令其為零后生成增量方程：

式中H(x)=J(x)TJ(x)為海森矩陣（信息矩陣）。

令Pc=TcwPw=[xc yc zc]T，則Jp、Jl如下：

式中：fx、fy為相機模型參數(shù)；Rcw∈SO(3)為的旋轉(zhuǎn)矩陣。

2.2 基于海森域舒爾消元邊緣化

若要對BA問題進行求解，如何快速求解增量方程是關鍵。觀察式（4）可以發(fā)現(xiàn)，海森矩陣維度為(6np+3nl)，并且由于待優(yōu)化的路標點數(shù)量遠遠大于相機數(shù)量（nl?np），很容易導致其維度達到成千上萬的級別，直接求逆解增量方程計算量太大，故一般會利用海森矩陣稀疏性結合邊緣化操作將優(yōu)化問題降階，加速求解過程。

海森矩陣稀疏性是由BA 問題中相機與路標點觀測形式導致的，如圖3所示。

圖3 BA問題的矩陣稀疏性

基于信息矩陣結構特性，常在求解式（4）過程中結合舒爾消元進行邊緣化操作，即求出xp的邊緣分布，將求(?xp，?xl)的問題轉(zhuǎn)化為先固定?xl，求出?xp，再求出?xl的過程：

最后回代得到?xl：

2.3 基于雅克比域零空間邊緣化

如上節(jié)所述，基于舒爾消元的邊緣化依賴于超大維度海森矩陣的顯式構建，若想在避免構建海森矩陣前提下將優(yōu)化問題進行降階，可直接在式（2）優(yōu)化問題中將路標點進行邊緣化，即利用QR 分解將Jl進行左零空間投影：

式中：Q為正交矩陣；Q1和Q2分別構成了Jl列空間和零空間的基底；R1為上三角矩陣，表示將Jl旋轉(zhuǎn)到列空間Q1中的實際信息。

如此一來狀態(tài)量?xl在零空間Q2中變化不會影響整體殘差，利用此特性，可將式（2）優(yōu)化問題經(jīng)正交投影變換后拆成兩個子問題求解：

由于R1可逆，當給定?xp增量時，總能使得第一項為零，即

則式（13）優(yōu)化項可簡化為

若要求解式（15），將其展開后對?xp求導即可得到邊緣化降階后僅含?xp的增量方程，對該增量方程求解后將?xp反代回式（14）即可得到式（2）優(yōu)化問題在該次迭代中的結果。

相比基于舒爾消元的邊緣化方法，該方法在避免構建超大維度海森矩陣H(x)前提下就將優(yōu)化問題進行邊緣化降階，轉(zhuǎn)化為先求出?xp、再求出?xl的過程。

2.4 等價性證明及數(shù)值穩(wěn)定性分析

2.4.1 等價性證明

由于Q1和Q2為正交矩陣，即滿足：

將舒爾消元的式（9）及式（10）利用式（12）進行左零空間投影，結合式（16）進行化簡可得：

同時，將利用零空間邊緣化降階后的式（15）最小二乘問題展開：

對?xp求導并令其導數(shù)為零后可得基于零空間邊緣化降階后的增量方程：

可以發(fā)現(xiàn)，兩種邊緣化方式求得降階后的增量方程?xp=-完全等價，至此，證明了利用兩種邊緣化方法將式（2）優(yōu)化問題降階后的等價性。隨后，僅須證明兩種邊緣化方式將?xp回代求得?xl完全等價即可。

同樣，將基于舒爾消元的回代式（11）方程進行左零空間投影：

該式和式（14）完全等價，證畢。

2.4.2 數(shù)值穩(wěn)定性分析

如前所述，基于舒爾消元的邊緣化方法雖然可以對優(yōu)化問題進行降階，但不可避免需要顯式構建海森矩陣，本文提出的基于雅克比域零空間邊緣化方法在避免構建海森矩陣的前提下就能將優(yōu)化問題降階，從而提升求解效率。

同時注意到，邊緣化的本質(zhì)是在信息矩陣的維度生成帶有xp邊緣分布的增量方程，從數(shù)值分析的角度上，在海森域與在雅克比域進行邊緣化操作的矩陣條件數(shù)是不同的，矩陣條件數(shù)含義即為求解過程中線性方程的抗擾程度。在實數(shù)域上推導上述公式無須考慮矩陣條件數(shù)，但對精度有限的計算機運算來說，數(shù)值誤差的出現(xiàn)不可避免。為降低數(shù)值誤差對增量影響程度，須降低迭代求解過程中的數(shù)值誤差，故現(xiàn)有求解器都是基于雙精度浮點數(shù)來實現(xiàn)的，從而保證求解過程中的數(shù)值穩(wěn)定性。對于BA問題，由于H(x)=J(x)TJ(x)，可以得出基于舒爾消元邊緣化矩陣條件數(shù)是基于零空間邊緣化矩陣條件數(shù)的平方關系，這直接導致線性增量方程抗噪和抗擾能力產(chǎn)生了巨大差別。

對于浮點數(shù)計算而言，雖然單精度浮點數(shù)比雙精度浮點數(shù)運算效率更高，但也會引入更多數(shù)值誤差，若想通過使用單精度浮點數(shù)加速運算效率，降低條件數(shù)是必不可少的前提。綜上所述，本文提出在非線性優(yōu)化框架的視覺SLAM 系統(tǒng)中使用雅克比域零空間邊緣化方法，可使得SLAM 系統(tǒng)在執(zhí)行后端優(yōu)化時在數(shù)值穩(wěn)定性上有顯著改善，從而支持后端模塊可使用單精度浮點數(shù)計算，進一步提升求解效率，甚至為其在一些算力有限、只支持32 位架構的計算平臺上運行提供了可能性，這也符合自動駕駛設備落地部署的低成本化目的。

2.5 優(yōu)化求解器實現(xiàn)

常見SLAM 系統(tǒng)在后端優(yōu)化時一般使用成熟第三方通用優(yōu)化庫（如g2o、ceres）實現(xiàn)BA 問題的求解，為評估使用雅克比零空間邊緣化求解BA 的效率，本文基于上述零空間邊緣化方法實現(xiàn)了列文伯格-馬夸爾特（Levenberg-Marquardt）［23］算法求解器，該求解器在細節(jié)流程上與g2o 實現(xiàn)的LM 算法對齊，阻尼項更新方式采用Nielsen 策略，對于阻尼項初始值，g2o實現(xiàn)的LM算法阻尼項初始化策略如下：

式中max(J(x)TJ(x))ii為降階前海森矩陣H(x)對角線最大值。

由于零空間邊緣化方法直接生成降階后的增量方程?xp=-，無法計算J(x)TJ(x)，故阻尼項初始值設為1e-4。此外，由于ORB-SLAM 基于共視關系維護局部地圖，為盡可能高效存儲BA過程中的矩陣信息，本文基于BASALT 實現(xiàn)的稠密路標相機索引塊（dense landmark block）［24］實現(xiàn)求解器，進一步提高內(nèi)存利用效率。該求解器算法流程如下：

For 每次迭代 do

（1）殘差e計算即式（1），Huber核函數(shù)映射

（2）狀態(tài)量線性化J(x)即式（6）和式（7）

（3）邊緣化降階即式（12）

（4）構建增量方程即式(20)

（5）增量求解?x即式（14），備份狀態(tài)量

（6）更新狀態(tài)量x⊕?x

（7）計算更新后的殘差e(x⊕?x)即式（1）

Ifρ>0 do

更新阻尼項（減?。?/p>

If ?x收斂do

跳出迭代，優(yōu)化結束

Else

更新阻尼項（增大）

狀態(tài)量回滾

END

3 試驗與結果分析

為評估本文提出的基于雅克比域零空間邊緣化算法性能，在KITTI 數(shù)據(jù)集和實車場景進行試驗，并與基準系統(tǒng)進行對比，基準選擇ORB-SLAM2 的單目版本在通用優(yōu)化庫g2o（基于舒爾消元的邊緣化）的統(tǒng)計結果，本文實現(xiàn)的后端流程與優(yōu)化策略和基準進行了對齊，從而保證試驗的準確性。同時，由于系統(tǒng)后端優(yōu)化算法的整體改變，導致許多因素耦合在一起，使得后端優(yōu)化模塊對系統(tǒng)精度與效率的影響僅能從實際場景的最終結果進行衡量。為此，本文設計了仿真試驗，在保證優(yōu)化器輸入數(shù)據(jù)完全相同前提下分析優(yōu)化過程的細節(jié)，從而佐證其對實際場景的結果影響。最后，設計了本文系統(tǒng)與ORBSLAM3的對比試驗。

試驗環(huán)境為Ubuntu 20.04，CPU 為i7-12700H，內(nèi)存為16 GB，由于基于零空間邊緣化方法條件數(shù)遠低于基準方法（數(shù)值穩(wěn)定性較好），使得優(yōu)化器支持單精度浮點數(shù)運算從而進一步提升求解速度，對比試驗分為單精度浮點數(shù)版本和雙精度浮點數(shù)版本，以下使用double 表示雙精度浮點數(shù)版本，float 表示單精度浮點數(shù)版本，SM（schur marginalization）表示基于舒爾消元的邊緣化方法，NM（nullspace marginalization）表示基于零空間投影的邊緣化方法。

3.1 試驗介紹

公開數(shù)據(jù)集選擇城市自動駕駛測試場景較為豐富的KITTI 數(shù)據(jù)集，KITTI 數(shù)據(jù)集是目前自動駕駛領域中應用范圍最廣的公開數(shù)據(jù)集之一，廣泛用于SLAM 算法驗證。本文選取KITTI odometry 數(shù)據(jù)集中多組序列進行試驗，包含城鎮(zhèn)、高速、郊區(qū)等多個場景，能夠有效驗證本文算法在不同場景中的處理速度和精度，由于KITTI 數(shù)據(jù)集01 和08 序列分別由于基準軌跡發(fā)散和真值不準確問題，不參與此次試驗。

仿真數(shù)據(jù)可視化后的效果圖如圖4 所示，其通過系統(tǒng)在KITTI 數(shù)據(jù)集上運行時統(tǒng)計生成，記錄了某次局部BA時待優(yōu)化的關鍵幀相機位姿、路標點位置以及觀測情況。在仿真試驗中，3 種優(yōu)化器的配置參數(shù)完全相同，迭代次數(shù)為20 次，在輸入數(shù)據(jù)完全相同的前提下，統(tǒng)計每次優(yōu)化迭代與殘差和耗時之間的關系，從而保證結果分析的有效性。

圖4 仿真數(shù)據(jù)可視化效果圖

實車場景利用如圖5（a）所示的試驗車采集，其上搭載了ZED-2i 視覺傳感器以及華測-410 組合慣導，錄制了一段包含長度為1 026 m、頻率為10 Hz、分辨率為540×960 的圖像數(shù)據(jù)，實車數(shù)據(jù)運行效果如圖5（b）所示，為隨機選取的雨天校園內(nèi)，包含上坡、減速帶、匯車等場景。

圖5 試驗平臺車及實車數(shù)據(jù)運行效果圖

本文采用絕對位姿誤差（absolute pose error，APE）作為評價指標，計算每一相機曝光時刻算法輸出的相機位姿與真值之間對齊后的絕對誤差，以該指標的平均值（mean）和均方根誤差（root mean squared error，RMSE）綜合評估算法性能，同時統(tǒng)計后端模塊中每幀關鍵幀進行局部BA時的耗時，評估后端模塊在處理速度上的效率提升。

3.2 結果分析

3 種優(yōu)化器在仿真數(shù)據(jù)上進行BA 優(yōu)化的結果統(tǒng)計如圖6 所示，左側為迭代次數(shù)與殘差的關系統(tǒng)計圖，右側為迭代次數(shù)與耗時的關系統(tǒng)計圖。

圖6 仿真數(shù)據(jù)優(yōu)化過程統(tǒng)計圖

從兩組仿真數(shù)據(jù)的迭代次數(shù)與殘差的統(tǒng)計圖可以看出，隨著迭代的不斷進行，整體殘差呈下降趨勢，由于零空間邊緣化算法與舒爾消元邊緣化算法在代數(shù)上的等價性，殘差的整體下降趨勢幾乎相同，但從圖6（a）可以看出，基準版本在優(yōu)化時存在著經(jīng)過一次迭代后殘差卻沒有下降的情況，因為此次迭代的狀態(tài)增量會導致整體殘差上升，根據(jù)2.5節(jié)所述的優(yōu)化器策略，需要將狀態(tài)量回滾并重新調(diào)整阻尼項，此種不穩(wěn)定迭代會導致多線程的SLAM在發(fā)生線程干預時系統(tǒng)誤差增大，而基于零空間邊緣化的優(yōu)化器在兩種精度浮點數(shù)的優(yōu)化結果上都呈穩(wěn)定下降且高度一致的態(tài)勢，這說明了由于更小的條件數(shù)帶來優(yōu)化穩(wěn)定性的提升。同時，從迭代次數(shù)與耗時的關系圖可以發(fā)現(xiàn)單精度版本的優(yōu)化器在每次迭代的處理速度上是最快的，而雙精度版本比基準版本略快。對于相同的輸入數(shù)據(jù)，單精度版本和雙精度版本在每次迭代的處理時間上幾乎相差一個定值，這說明改變浮點數(shù)精度所帶來的效率提升存在著一定上限，根據(jù)經(jīng)驗，實際效率的提高與具體BA問題的形式有關，如問題規(guī)模、觀測情況等。

KITTI 數(shù)據(jù)集與實車數(shù)據(jù)集試驗APE 結果如表1 所示，提供了基準版本、兩種精度的浮點數(shù)版本以及ORB-SLAM3的對比統(tǒng)計。

表1 KITTI及實車數(shù)據(jù)集試驗APE對比結果

從表1 可以看出，在定位精度方面，本文提出的基于零空間邊緣化方法相較舒爾消元邊緣化方法，APE均在同一量級。同為雙精度浮點數(shù)求解的基準方法與本文方法（g2o-baseline 和Ours-double）對比可發(fā)現(xiàn)，本文方法由于矩陣條件數(shù)降低帶來數(shù)值穩(wěn)定性提高的特性，可使基于本文方法實現(xiàn)的后端模塊在定位結果上取得更好的精度。在00 到10 序列中，基于零空間邊緣化的雙精度版本（Ours-double）定位誤差降低了10%～40%左右。單精度浮點版本相比于雙精度浮點版本，雖然定位精度有略微下降，但整體和基準版本持平，僅有02、07、09 及實車數(shù)據(jù)集的RMSE 比基準方法略低4%左右，精度下降的原因有可能是系統(tǒng)在轉(zhuǎn)換浮點數(shù)時發(fā)生數(shù)值截斷所導致。ORB-SLAM3 相比于ORB-SLAM2 在后端優(yōu)化時處理外點的策略有所改變，從對比試驗可以發(fā)現(xiàn)，兩者整體精度相當，但存在部分序列誤差上升現(xiàn)象，說明僅改變后端優(yōu)化策略不一定能在所有已經(jīng)成功的場景中帶來精度提升，側面反映了對于一個完善的視覺SLAM 系統(tǒng)需要嘗試從各個模塊的本質(zhì)進行改進從而提升系統(tǒng)性能。

上述對比版本在對應數(shù)據(jù)集上的后端優(yōu)化耗時統(tǒng)計如表2 所示。由于ORB-SLAM3 與ORBSLAM2 在本文的BA 問題中耗時幾乎相同，故不在表中進行統(tǒng)計?？梢园l(fā)現(xiàn)，基準版本、雙精度浮點版本與單精度浮點版本在耗時統(tǒng)計上符合仿真試驗的結果，整體耗時呈依次遞減趨勢。其中雙精度浮點數(shù)版本由于不需要顯式構造維度較大的海森矩陣，在處理速度上有7%左右的提升。而單精度版本由于浮點精度的降低帶來運算效率的進一步提高，將求解BA 問題的耗時縮短了20%左右，為后端模塊釋放了一定的算力壓力，相較于基于通用優(yōu)化庫實現(xiàn)的視覺SLAM，本文的工作展現(xiàn)出了更好的性能。

表2 KITTI及實車數(shù)據(jù)集后端優(yōu)化耗時統(tǒng)計 ms

圖7 展示了在KITTI 的 00 序列上詳細試驗對比結果。對比圖7（a）與圖7（b）可以看出，本文提出的算法在定位精度上要優(yōu)于基準算法，如同仿真試驗所展現(xiàn)的結果，雙精度版本和單精度版本在軌跡的一致性上表現(xiàn)較好，并沒有出現(xiàn)因引入單精度浮點數(shù)的數(shù)值計算誤差從而導致優(yōu)化結果發(fā)散的情況。圖8為該00序列的統(tǒng)計后端優(yōu)化中路標數(shù)量和優(yōu)化耗時關系圖，由于待優(yōu)化路標點數(shù)量遠遠大于待優(yōu)化相機數(shù)量，優(yōu)化問題維度基本取決于路標點數(shù)量，路標點數(shù)量可能在車輛發(fā)生轉(zhuǎn)向、地圖共用的情況出現(xiàn)時，導致局部關鍵幀數(shù)量增多，從而導致優(yōu)化問題規(guī)模增大，耗時增加。對比圖8（a）～圖8（c）可以發(fā)現(xiàn)，雖然本文提出的邊緣化算法和基于舒爾消元通用優(yōu)化庫算法（g2o）的優(yōu)化耗時都和路標點數(shù)量成正比，但整體效率占優(yōu)，尤其是單精度浮點版本的整體優(yōu)化效率提升較大。此外，注意到使用本文算法生成的關鍵幀數(shù)量會比基準算法稠密一些，在平均每關鍵幀優(yōu)化的路標點數(shù)量上也會略多，但雙精度版本的整體性能（精度、耗時）略好于基準算法，并且單精度版本在規(guī)模較大BA 問題（高峰值處）上由于數(shù)值位數(shù)降低引入的效率提升更大，降低了自動駕駛車輛在實際行駛過程中需要進行大規(guī)模BA 的算力負擔。

圖7 KITTI數(shù)據(jù)集00序列定位結果

圖8 KITTI數(shù)據(jù)集00序列后端優(yōu)化模塊耗時統(tǒng)計

在實車試驗中，如圖9 所示，由于陰雨天氣導致相機部分模糊及校園內(nèi)動態(tài)物體的存在，關鍵幀插入較為密集，從而導致局部地圖待優(yōu)化路標點數(shù)量有一定上升，如圖10 所示，平均優(yōu)化路標點數(shù)量達到了2 300 左右。在定位結果方面，如圖11 所示，雙精度浮點版本APE 的RMSE 比基準版本提升了28%，單精度浮點版本APE 基本和基準版本持平，但處理速度提升了19%。通過上述試驗分析，可以有效證明本文工作的雙精度浮點版本系統(tǒng)比基準版本處理速度略快的同時精度更高，單精度浮點版本的系統(tǒng)可在精度沒有顯著下降的情況下提升處理速度。

圖9 實車場景展示

圖10 實車數(shù)據(jù)集定位結果

圖11 實車數(shù)據(jù)集后端優(yōu)化模塊耗時統(tǒng)計

4 結論

為提升基于非線性優(yōu)化的視覺SLAM 在后端優(yōu)化模塊處理速度，本文提出一種基于雅克比域零空間邊緣化的視覺SLAM 方法，該方法通過在后端優(yōu)化模塊執(zhí)行BA時將雅克比進行左零空間投影，從而達到無須顯式構建大維度海森矩陣就能將優(yōu)化問題邊緣化降階效果。在代數(shù)上推導了基于雅克比域零空間邊緣化公式，及其與基于舒爾消元邊緣化方法的等價性，同時在數(shù)值分析角度上表明其相較基于舒爾消元的邊緣化方法具有更好的數(shù)值穩(wěn)定性，這為自動駕駛車輛在實際行駛過程中需要進行大規(guī)模BA 提供支持，甚至為其在一些算力有限、只支持32位架構的計算平臺上運行提供了可能性。最后，基于該邊緣化方法實現(xiàn)了LM 算法優(yōu)化求解器并與ORB-SLAM 系統(tǒng)進行融合，并在自動駕駛公開數(shù)據(jù)集和實車上進行試驗，結果表明本文工作相較于基于成熟第三方優(yōu)化庫的后端優(yōu)化模塊具有更好的精度和處理速度。

未來將嘗試在嵌入式平臺上基于此邊緣化方法實現(xiàn)多傳感器融合版本的求解器工作，提升系統(tǒng)魯棒性，并嘗試實現(xiàn)增量式BA，進一步提升后端效率。