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

（1.汽車車身先進(jìn)設(shè)計(jì)制造國家重點(diǎn)實(shí)驗(yàn)室，湖南大學(xué)機(jī)械與運(yùn)載工程學(xué)院，長沙 410082；2.山東省科學(xué)技術(shù)情報研究院，濟(jì)南 250101；3.湖南大學(xué)無錫智能控制研究院，無錫 214115）

前言

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

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

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

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

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

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

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

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

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

1 系統(tǒng)架構(gòu)

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

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

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

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

2 基于零空間邊緣化的BA

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

2.1 BA問題構(gòu)建

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

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

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

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

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

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

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

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

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

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

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

海森矩陣稀疏性是由BA 問題中相機(jī)與路標(biāo)點(diǎn)觀測形式導(dǎo)致的，如圖3所示。

圖3 BA問題的矩陣稀疏性

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

最后回代得到?xl：

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

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

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

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

由于R1可逆，當(dāng)給定?xp增量時，總能使得第一項(xiàng)為零，即

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

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

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

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

2.4.1 等價性證明

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

For 每次迭代 do

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

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

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

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

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

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

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

Ifρ>0 do

更新阻尼項(xiàng)（減?。?/p>

If ?x收斂do

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

Else

更新阻尼項(xiàng)（增大）

狀態(tài)量回滾

END

3 試驗(yàn)與結(jié)果分析

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

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

3.1 試驗(yàn)介紹

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

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

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

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

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

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

3.2 結(jié)果分析

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

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

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

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

表1 KITTI及實(shí)車數(shù)據(jù)集試驗(yàn)APE對比結(jié)果

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

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

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

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

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

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

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

圖9 實(shí)車場景展示

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

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

4 結(jié)論

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

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