徐曉蘇，游穎輝

（1. 微慣性儀表與先進導(dǎo)航技術(shù)教育部重點實驗室，南京 210096；2. 東南大學(xué)儀器科學(xué)與工程學(xué)院，南京 210096）

移動機器人在服務(wù)、工業(yè)自動化等領(lǐng)域具有廣泛應(yīng)用[1]。視覺同步定位與建圖（Simultaneous Localization and Mapping, SLAM）是一種基于圖像的相機運動估計方法，在移動機器人的導(dǎo)航與控制中起到至關(guān)重要的作用，尤其在無外部信號作為參考時。在近二十年里，國內(nèi)外在視覺SLAM領(lǐng)域有了較多研究，包括基于穩(wěn)定特征的特征點法[2-4]、基于像素灰度的直接法[5,6]、同時參照灰度和特征的半直接法[7]、通過迭代最近點法（Iterative Closest Point, ICP）跟蹤相機的方法[8]等。特征點法對于光照變化較為魯棒，但存在計算描述子耗時較長、無法充分利用環(huán)境信息等問題。直接法可以充分利用環(huán)境信息且不依賴環(huán)境紋理，但無法有效應(yīng)對光照變化的環(huán)境[9]。半直接法可以避免灰度變化的影響，但仍然受制于圖像信息的準(zhǔn)確度?；谶吘墝?zhǔn)的視覺SLAM方法類似于半直接法，提取圖像中的邊緣特征，通過邊緣對準(zhǔn)實現(xiàn)運動估計，能夠充分利用環(huán)境信息，并對光照變化有一定穩(wěn)定性[10]，可以克服目前主流方法的缺點。

基于邊緣對準(zhǔn)的視覺定位方法可以追溯到Eade E等人的研究[11]，該方法首次實現(xiàn)了基于粒子濾波的利用邊緣特征的視覺里程計。基于優(yōu)化的方法則發(fā)端于Tarrio J J等人于2015年公開的一種基于邊緣特征的單目視覺里程計，該研究首次說明了邊緣對準(zhǔn)是一個高效的可并行的操作[12]。Zhou Y等人于2019年提出的基于Canny檢測子的視覺里程計Canny-VO[13]，該方法提出了邊緣配準(zhǔn)中常用的歐式域距離變換的兩種替代方法：近似最近鄰域和定向最近鄰域，并將邊緣的旋轉(zhuǎn)加入到優(yōu)化中。文獻[14]提出的實時魯棒的邊緣對準(zhǔn)SLAM（RESLAM）利用隨機蕨實現(xiàn)了回環(huán)檢測。目前的多數(shù)基于邊緣對準(zhǔn)的方法都未考慮圖像光度變化對于邊緣提取的影響，更未在設(shè)計算法時設(shè)計離線或在線光度標(biāo)定的環(huán)節(jié)。在光度標(biāo)定的相關(guān)研究中，文獻[6]通過對數(shù)據(jù)集的圖像曝光時間、相機的Gamma響應(yīng)函數(shù)以及詳細(xì)的鏡頭畸變因子進行了離線光度標(biāo)定來提高視覺里程計性能，Paul Bergmann等人在2018年提出了一種對自動曝光的視頻流進行光度校正的方法[15]，并和文獻[6]提出的算法進行組合，有效提升了定位精度和建圖效果。

針對圖像光度變化導(dǎo)致不同圖像提取的邊緣特征分布差異較大的問題。本文對文獻[14]所提出的基于RGB-D相機的算法進行了優(yōu)化改進，集成了在線光度標(biāo)定模塊，提升了進行邊緣提取時的圖像質(zhì)量，在數(shù)據(jù)集上驗證了在算法中集成在線光度標(biāo)定對于定位精度有一定提升；在此基礎(chǔ)上，對殘差項的殘差權(quán)重函數(shù)進行設(shè)計，提升了系統(tǒng)多次運行的穩(wěn)定性。

1 算法整體框架

本文基于機器人軟件平臺（Robot Operating System, ROS）實現(xiàn)集成在線光度標(biāo)定的邊緣對準(zhǔn)視覺里程計，其整體流程如圖1所示。該方法分為光度標(biāo)定節(jié)點以及包含前端跟蹤線程、局部建圖線程、全局建圖線程的里程計節(jié)點。通過訂閱傳感器節(jié)點或者數(shù)據(jù)集發(fā)布的話題獲取圖像，獲取的圖像首先被讀入光度標(biāo)定節(jié)點。在光度標(biāo)定節(jié)點，對圖像進行提取角點、角點跟蹤、估計參數(shù)等一系列操作，再通過發(fā)布話題的方式將校正后的圖像發(fā)布。里程計節(jié)點訂閱經(jīng)過光度標(biāo)定后的圖像消息，在跟蹤線程對圖像幀先進行提取圖像邊緣的操作，然后通過距離變換構(gòu)建殘差方程，迭代求解殘差方程實現(xiàn)邊緣對準(zhǔn)，進而實現(xiàn)對相機運動的跟蹤，然后進行關(guān)鍵幀選擇。在局部線程內(nèi)則對跟蹤線程選擇的關(guān)鍵幀進行回環(huán)編碼的計算，構(gòu)建非線性優(yōu)化滑動窗口，對滑動窗口中的變量進行局部優(yōu)化，對關(guān)鍵幀的位姿進行優(yōu)化，優(yōu)化完成后根據(jù)設(shè)置的條件執(zhí)行邊緣化策略以維持滑窗中的變量維數(shù)在一定數(shù)量；編碼后的關(guān)鍵幀被存入關(guān)鍵幀數(shù)據(jù)庫，用于進行回環(huán)檢測、回環(huán)驗證等環(huán)節(jié)，最后利用位姿圖構(gòu)建全局優(yōu)化對位姿做進一步優(yōu)化。

圖1 算法整體流程圖Fig.1 The overall flow chart of the algorithm

2 光度標(biāo)定

光度標(biāo)定節(jié)點通過提取角點建立圖像之間的數(shù)據(jù)關(guān)系實現(xiàn)跟蹤，進而完成對曝光時間、漸暈等參數(shù)的估計，再對圖像做校正并發(fā)布圖像消息話題。

2.1 圖像光度建模

場景點被光源照亮?xí)r，將光線反射到空間中，反射的總光量稱為該三維點的輻射L。如果移動觀察者接收到該點的輻射與觀察者的觀察角度無關(guān)，則稱該場景點的輻射表現(xiàn)為蘭伯特反射。場景點的輻射被相機的感光元件接收，記感光元件上位置x接收到的輻射照度為I(x)。理想情況下，不同空間位置具有相同輻射的場景點會產(chǎn)生相同的傳感器輻射照度。然而，對于大多數(shù)相機而言，采集到的圖像像素強度從圖像中心向圖像邊界下降。這種現(xiàn)象被稱為漸暈效應(yīng)，一般是由于光線被透鏡部分阻塞或者由于透鏡的幾何形狀引起的。則感光元件上的輻射照度I(x)可以通過I(x) =V(x)L表示，V: Ω→ [ 0,1]為漸暈因子，與圖像感光元件中的位置x?Ω相關(guān)。由于感光元件的輻射照度在曝光時間窗口內(nèi)隨時間累積，假設(shè)在時間窗口內(nèi)的傳感器感光元件的輻射照度恒定，對于曝光時間e內(nèi)累積的輻射照度為Iacc(x) =eI(x) 。f: ?→ [ 0 ,255]為相機響應(yīng)函數(shù)（Camera Response Function, CRF），從而場景點輻射L與圖像輸出的像素強度O之間的對應(yīng)關(guān)系可以表示為：

使用經(jīng)驗響應(yīng)模型（Empiric model of Response,EMoR）對CRF進行建模，并利用基函數(shù)hk(x)組合得到如下的全局響應(yīng)函數(shù)：

其中ck?R，f0(x)為通過原理成分分析（Principle Component Analysis, PCA）獲得的平均響應(yīng)。本文假設(shè)漸暈的分布符合徑向漸暈?zāi)Ｐ?，即衰減因子在圖像中心是對稱的，進一步地假設(shè)漸暈的中心和圖像的中心一致，則漸暈因子可以用一個六階多項式進行擬合。

其中R(x)是圖像點x相對于圖像中心的歸一化半徑。

2.2 參數(shù)估計

對于給定的圖像F，式(1)中的各項參數(shù)僅輸出的像素強度O已知，因此進行光度標(biāo)定還需選擇一定數(shù)量的場景點通過重投影建立圖像幀之間的聯(lián)系。通過提取Shi-Thomais角點，并采用具有自適應(yīng)增益的Kanade-Lucas-Tomasi（KLT）光流跟蹤算法來獲取點對之間的聯(lián)系。同時為了更好地校正漸暈，需要提取的角點均勻分布在圖像上，因此將圖像分成一系列小塊再提取角點。同時為了增加觀測量，在跟蹤的角點周圍的像素塊也被用于計算重投影殘差。

記輸入的圖像幀集合為Fp，跟蹤的場景點集合為P，對于在Fp中可視的場景點p?P，通過跟蹤點對可以建立如式(4)所示的殘差方程[15]。

通常需要較多的圖像來求解優(yōu)化式(4)中的所有參數(shù)，曝光時間可以通過圖像相鄰幀之間的時差來估計，而漸暈?zāi)Ｐ秃晚憫?yīng)模型則需要多個圖像幀來估計。因此在本文算法中，將需要估計的參數(shù)進行分離估計。每新進一幀圖像，先用當(dāng)前估計的漸暈和響應(yīng)函數(shù)校正圖像，再估計曝光時間。因此式(4)可改寫為[15]：

其中，M為當(dāng)前圖像幀集合的大小，Pi記為在第i幀可視的場景點，f-1是逆響應(yīng)函數(shù)。對于固定的場景點的輻射照度pL，式(5)僅曝光時間是未知量，求解時間隨變量維度線性增長，可以快速求解。全部參數(shù)的優(yōu)化估計通過在后端優(yōu)化過程中對式(4)求解得到。在本文中對圖像進行光度標(biāo)定的效果如圖2所示，圖像質(zhì)量得到了明顯改善，提取的邊緣特征分布更加均勻。

圖2 光度標(biāo)定前后的圖像Fig.2 Images before and after photometric calibration

3 基于邊緣對準(zhǔn)的視覺SLAM

視覺里程計通過提取經(jīng)過光度標(biāo)定的圖像中的邊緣，并利用距離變換進行邊緣對準(zhǔn)，實現(xiàn)對相機運動的估計，并在后端構(gòu)建滑動窗口對位姿再進行優(yōu)化。

3.1 運動估計

對于在t時刻接收到的圖像It和深度圖像Zt，假設(shè)已完成了對準(zhǔn)和時間同步，也即對于圖像It上的像素u=(x,y)，在Zt中的對應(yīng)深度z=Zt(u)。

定義像素點u在三維空間對應(yīng)的場景點為P,則從像素點u計算三維點P的逆投影函數(shù)如下：

其中，fx、fy、cx、cy為相機的內(nèi)參，通過對相機標(biāo)定獲得。對應(yīng)地，將三維點P投影至像素坐標(biāo)系的投影函數(shù)具有如下的形式：

式中，ξji為圖像幀F(xiàn)i到Fj的相對變換，對應(yīng)的位姿變換矩陣為Tji。

在每幀圖像中，通過對齊當(dāng)前幀F(xiàn)j與關(guān)鍵幀F(xiàn)i對應(yīng)的邊緣檢測結(jié)果來估計兩幀之間的相對運動ξji，在對齊過程中，將關(guān)鍵幀F(xiàn)i中的具有有效深度的所有像素點εi重投影至當(dāng)前幀F(xiàn)j，并與當(dāng)前幀檢測到的最近的邊緣像素對齊。在本文中通過在歐氏距離域上計算距離變換來預(yù)先計算圖像中每個像素到有效邊緣像素的距離，從而得到重投影的邊緣像素距離變換殘差：

其中，Dj為當(dāng)前幀F(xiàn)j的距離變換，ui為關(guān)鍵幀F(xiàn)i中邊緣像素的位置。聯(lián)合參考關(guān)鍵幀中的所有具有有效深度的像素點的距離變換殘差，構(gòu)建優(yōu)化方程：

Huber權(quán)重系數(shù)的計算公式如下：

T-分布的計算公式如下：

式中，k、vσ、 均為根據(jù)傳感器誤差特性擬合得到的系數(shù)。

3.2 后端優(yōu)化

為了提高算法的精度，在局部建圖線程構(gòu)建滑動窗口對邊緣像素的深度、相機位姿和相機內(nèi)參進行聯(lián)合優(yōu)化。記滑動窗口中的圖像幀集合為W，將滑動窗口中的每一幀激活的邊緣像素點分別投影到其他幀上，類似于式(11)，得到：

式中，Ai為第i個關(guān)鍵幀中激活的殘差項集合，δr為誤差權(quán)重系數(shù)，r是包含優(yōu)化后的相機內(nèi)參C和逆深度參數(shù)ρi的殘差項：

其中，為利用優(yōu)化后的關(guān)鍵幀相對世界的變換矩陣ξi,ξj重投影至關(guān)鍵幀F(xiàn)j。通過Gauss-Newton優(yōu)化方法最小化式(11)，從而有：

式中，W??n×n為對角權(quán)重矩陣，r為殘差序列，J為r的Jacobian矩陣，對式(14)迭代求解得到優(yōu)化后的參數(shù)。

3.3 關(guān)鍵幀選擇及邊緣化

為了減少后端非線性優(yōu)化的計算量，在前端跟蹤結(jié)束后，對于當(dāng)前幀分別計算以下4個關(guān)鍵幀指標(biāo)。

1）平均平方光流Mfov用于衡量視場內(nèi)邊緣像素的整體光度變化。

式中u為邊緣像素坐標(biāo)，u′為重投影后的像素坐標(biāo)，n為邊緣像素的數(shù)量。

2）無旋轉(zhuǎn)的平均平方光流Mtran用于衡量因平移引起的光度變化。

式中ut為僅做平移變換后的像素坐標(biāo)。

3）無位移的平均平方光流Mrota用于衡量因旋轉(zhuǎn)引起的光度變化。

式中ur為僅做旋轉(zhuǎn)變換后的像素坐標(biāo)。

4）參考關(guān)鍵幀投影到當(dāng)前幀的像素點數(shù)量Nin和未投影到當(dāng)前幀的像素點數(shù)量Nout。

當(dāng)以上指標(biāo)符合式(20)(21)所示的條件之一時，將當(dāng)前幀作為新的關(guān)鍵幀。

滑動窗口內(nèi)的關(guān)鍵幀需要維持在一定的數(shù)量，因此需要采取邊緣化部分關(guān)鍵幀的策略，關(guān)鍵幀的邊緣化策略有兩種選擇方式。

1）該關(guān)鍵幀中的邊緣像素點在優(yōu)化過程中激活的數(shù)量少于設(shè)置的閾值θvis，則該關(guān)鍵幀被邊緣化。

2）若不存在符合上述條件的關(guān)鍵幀，則保留最新的兩幀關(guān)鍵幀，對其余的關(guān)鍵幀計算幀間的歐氏距離得分si，邊緣化部分關(guān)鍵幀使其在空間上分布均勻。

式中,di,j為兩個關(guān)鍵幀之間的歐氏距離。

3.4 回環(huán)檢測

回環(huán)檢測分為發(fā)現(xiàn)回環(huán)候選幀，利用回環(huán)候選幀的信息修正位姿估計，再驗證回環(huán)檢測是否正確。本文的回環(huán)檢測是基于隨機蕨詞袋設(shè)計的，其流程如下。

2）利用回環(huán)信息修正位姿估計。當(dāng)相似度得分大于設(shè)置的閾值θHam，則將當(dāng)前幀視為回環(huán)候選幀加入到回環(huán)處理過程，以關(guān)鍵幀相對世界坐標(biāo)系的位姿作為節(jié)點，滑動窗口關(guān)鍵幀之間的相對位姿作為邊，構(gòu)建如下的位姿圖優(yōu)化問題。圖3(a)展示了這一過程，其中箭頭方向表示將該幀投影到另一幀。

3）回環(huán)驗證。將由候選回環(huán)關(guān)鍵幀及其鄰近關(guān)鍵幀構(gòu)成的回環(huán)驗證關(guān)鍵幀集合V投影到當(dāng)前幀，計算邊緣投影重疊的數(shù)量，圖3(b)展示了這一過程。為了避免重復(fù)計算，對回環(huán)驗證關(guān)鍵幀集合中的關(guān)鍵幀先統(tǒng)計圖像中邊緣像素的出現(xiàn)次數(shù)。對于關(guān)鍵幀iF計算如下的計數(shù)地圖：

圖3 回環(huán)檢測示意圖Fig.3 Schematic diagram of loop detection

式中ξci為關(guān)鍵幀F(xiàn)i到當(dāng)前幀F(xiàn)cur的位姿變換，N為回環(huán)驗證關(guān)鍵幀V中關(guān)鍵幀數(shù)量，對于整個回環(huán)驗證關(guān)鍵幀集合V的計數(shù)地圖有：

將候選回環(huán)關(guān)鍵幀中具有有效深度的邊緣像素點投影至計數(shù)地圖，對于集合V中每一幀，生成大小為N+1的直方圖來計算邊緣重疊數(shù)量。如果結(jié)果符合式(28)，則通過驗證。

式中，wi為權(quán)重系數(shù)?；丨h(huán)驗證通過后更新位姿。

4 實驗結(jié)果與分析

采用TUM RGB-D數(shù)據(jù)集驗證本文所提出的算法。針對光度標(biāo)定、誤差權(quán)重系數(shù)、回環(huán)驗證的改進效果，并與ORB-SLAM3[4]進行對比。該數(shù)據(jù)集包含39個室內(nèi)環(huán)境的圖像序列，使用Kinect相機以30 Hz的頻率錄制的，包含格式為BGR8的RGB圖像、格式為32FC的深度圖像，軌跡真值是通過具有16個高速攝像頭的動作捕捉系統(tǒng)以100 Hz獲得的，是驗證單目或基于RGB-D相機的視覺里程計性能常用的數(shù)據(jù)集。

實驗平臺為具有16 GB運行內(nèi)存，AMD Ryzen7-4800U計算處理單元的個人電腦，操作系統(tǒng)為Ubuntu20.04.2LTS，ROS版本為noetic。將本文提出的兩種改進算法與RESLAM[14]、ORB-SLAM3[4]進行對比，驗證算法性能。兩種改進算法分別是集成光度在線標(biāo)定利用Huber魯棒核函數(shù)構(gòu)造殘差項系數(shù)的RESLAM（下文稱RESLAM_PMH）、集成光度標(biāo)定并且利用T分布構(gòu)造殘差項系數(shù)的RESLAM（下文稱RESLAM_PMT）。

本文采用的評價指標(biāo)為絕對軌跡誤差（Absolute Trajectory Error,ATE）平移部分的均方根誤差（Root Mean Squared Error,RMSE）。時間點i的絕對軌跡誤差計算公式如下：

式中，S是估計位姿Pi到位姿真值Qi的剛體運動變換矩陣。本文中主要通過對比各算法的絕對軌跡誤差的大小說明算法的精度高低。

為了分析集成光度標(biāo)定模塊后對算法性能的影響，并且為了避免隨機的數(shù)值波動對評估結(jié)果的影響，本文以數(shù)據(jù)集中的freg1/xyz序列采用RESLAM、RESLAM_PMH、RESLAM_PMT、ORB-SLAM3算法分別進行了20次實驗并求其絕對軌跡誤差的平均值，其結(jié)果如圖4所示。

圖4 算法的20次運行結(jié)果Fig.4 The resultsof 20runsof thealgorithms

由圖4可知，基于特征點法的ORB-SLAM3的算法性能在具有較為豐富紋理的freg1_xyz的序列上的運行結(jié)果最佳，而RESLAM_PMH、RESLAM_PMT經(jīng)過光度標(biāo)定后相對RESLAM都有較大的提升。且RESLAM_PMT在freg1/xyz序列上相較RESLAM和RESLAM_PMH的多次運行結(jié)果精度更高，更穩(wěn)定。更進一步地展示實驗組中的其中這一組軌跡及其在x、y、z各軸的對比如圖5-6所示。

圖5 展示了三種算法估計的freg1/xyz軌跡，可以看出RESLAM_PMT估計的軌跡在序列的大部分段與真實軌跡較為接近，直觀地反映了該算法的改進對定位精度有所提升。圖6展示了各算法估計的軌跡與真實軌跡的x，y，z軸運動分量對比，可知RESLAM_PMT估計的運動軌跡的各軸分量與真實軌跡更為接近，在y軸和z軸上精度提升表現(xiàn)得更為明顯，圖中橫軸為以相對起始時刻的時間偏移量，起始時刻以科學(xué)計數(shù)的方式標(biāo)注在橫軸末尾。

圖5 freg1/xyz序列軌跡Fig.5 freg1/xyz sequence trajector y

圖6 x、y、z軸運動軌跡分量Fig.6 x, y,z axismotiontrajectory comp onents

對freg3/nostructure_texture_near_withloop序列進行半稠密重建，圖7展示了三種算法的建圖效果，序列中存在回環(huán)使得RESLAM和RESLAM_PMH的建圖中紅色方框內(nèi)重建的海報圖形有較大的重疊，而RESLAM_PMT則更為清晰。

圖7 三種算法的稀疏建圖Fig.7 The sparse mapping of three algorithms

在數(shù)據(jù)集中的多個序列上運行算法，其絕對軌跡誤差的均方根誤差如表1所示，其中的“/”表示只有部分軌跡，也即視覺定位失效時間較長。

從表1中可以看出ORB-SLAM3的算法性能在具有較多紋理的序列中保持著最佳的定位精度，但在freg3/structure_notexture_far序列中存在失效的情況。而本文所設(shè)計的RESLAM_PMH和RESLAM_PMT方法，在大多數(shù)序列上都優(yōu)于原有的RESLAM方法，平均軌跡均方根誤差分別由0.073 m提高到0.053 m和0.033 m，分別提高27.3%和55.3%。

表1 各算法絕對軌跡誤差均方根誤差(單位：m)Tab.1 Root mean square error of absolute trajectory error of each algorithm(Unit: m)

5 結(jié) 論

本文提出了一種集成在線光度標(biāo)定的邊緣對準(zhǔn)視覺SLAM方法。首先集成在線光度標(biāo)定設(shè)計了RESLAM_PMH算法，對圖像的漸暈效應(yīng)、曝光時間進行估計，并依此校正圖像像素的強度，提升圖像的質(zhì)量；并對位姿估計中殘差項的權(quán)重系數(shù)進行選擇，設(shè)計了RESLAM_PMT算法，提高系統(tǒng)對于較大殘差的抑制效果；綜合提升了算法的定位精度和建圖效果。

在數(shù)據(jù)集中的多個序列上進行多組對比實驗，驗證了集成在線光度標(biāo)定后，基于邊緣對準(zhǔn)的視覺SLAM算法定位精度和建圖效果均有所提升。同時與目前較為先進的基于特征點法的開源算法ORB-SLAM3進行對比，反映了基于邊緣對準(zhǔn)算法在精度上略有不足，但在較為特殊的弱紋理環(huán)境下相較特征點法具有明顯優(yōu)勢，這為后續(xù)的研究提供了較好的借鑒。