基于自適應慣導輔助特征匹配的視覺SLAM 算法

賈曉雪，趙冬青，張樂添，肖國銳，徐 青

（信息工程大學 地理空間信息研究所，河南 鄭州 450001）

1 引言

視覺同步定位與建圖技術（Simultaneous Localization and Mapping，SLAM）同時包含定位與建圖兩個問題，提高SLAM 系統(tǒng)的定位精度是構建精確稠密三維點云地圖的前提。視覺里程計作為SLAM 的關鍵部件，其主要任務是根據(jù)相鄰圖像的匹配信息估計相機運動，其中對圖像進行特征匹配是最關鍵環(huán)節(jié)之一，匹配的準確度直接影響視覺里程計的精度［1-3］。常用的特征匹配算法有暴力匹配算法和快速最近鄰匹配算法［4-5］。然而，當相機運動過快時，圖像模糊、場景動態(tài)、光線變化和圖像特征本身的局部特性，使特征誤匹配廣泛存在，并成為制約視覺里程計性能提升的一大瓶頸［6-7］。

為解決圖像特征誤匹配率高的問題，國內(nèi)外很多學者提出基于慣性測量單元（Inertial Measurement Unit，IMU）輔助的視覺SLAM 系統(tǒng)［8-9］。文獻［10］提出了一種仿射尺度不變特征變換（Affine Scale-invariant Feature Transform，ASIFT）特征匹配方法，利用IMU 提供的運動信息虛構相機視圖并估計本質(zhì)矩陣，以提高特征匹配的穩(wěn)健性。文獻［11］利用圖像的對極約束建立了前后兩幀圖像像素所在極線的數(shù)學關系，并結合IMU 預測相機位姿獲取特征點所在極線，有效地剔除誤匹配的特征點。文獻［12-14］提出了一種全局搜索和局部搜索交替操作并自動切換的特征匹配算法，利用相機投影模型和IMU預積分結果預測特征點可能出現(xiàn)的區(qū)域，并對局部固定區(qū)域進行搜索匹配。同樣地，文獻［15］提出的視覺慣性SLAM 系統(tǒng)ORB-SLAM3 在特征匹配階段也引入了慣導約束，利用IMU 預測相機位姿，將雙目相機左右匹配或三角化得到的地圖點投影至圖像，預測像素位置。

然而IMU 測量值本身存在誤差，在慣導輔助圖像特征匹配時，通常將全圖像的特征匹配轉(zhuǎn)換為以預測位置為原點，某一固定值為半徑的局部固定區(qū)域特征匹配。為了適配不同精度IMU，普遍做法是設置一個較大的搜索半徑值，以避免漏掉特征點，這在IMU 精度較高時易造成計算資源浪費，增加圖像特征的誤匹配率，使SLAM系統(tǒng)的位姿估計精度降低。為此，本文提出了一種自適應的慣導輔助視覺特征匹配方法，以慣性測量單元誤差為基礎，根據(jù)誤差傳播定律自動調(diào)整圖像特征匹配搜索半徑，不僅能提升特征點匹配的準確度，還能提高雙目視覺慣性SLAM 的定位精度，并構建更高精度的稠密地圖。

2 慣導輔助圖像特征匹配

為提高視覺慣性里程中圖像特征匹配的準確度，ORB-SLAM3 利用IMU 預積分預測相機位姿，將圖像預處理獲取的地圖點三維坐標投影至當前幀，預測地圖點在當前幀圖像的像素位置，從而將全圖像特征匹配轉(zhuǎn)換為局部區(qū)域的特征匹配，具體流程如圖1 所示。

圖1 慣導輔助圖像特征匹配流程Fig.1 Flow chart of IMU aided image feature matching

2.1 雙目圖像預處理

雙目相機一般由左眼和右眼兩個水平放置的相機組成，經(jīng)過校正的左右相機只在x軸上有位移，所以地圖點在左右相機的像素坐標也只在x軸（對應圖像的u軸）上有差異。通過計算雙目圖像對應像素點的視差，可以估計出像素的深度。記地圖點P的左側(cè)像素坐標為pl，右側(cè)像素坐標為pr，幾何關系如圖2 所示。根據(jù)相似三角形原理可得地圖點在相機系下的深度值z為：

圖2 雙目相機的成像模型Fig.2 Imaging model of stereo camera

其中：f為相機的焦距，b為雙目相機的基線。

將特征點在相機坐標系下的深度值z代入針孔相機投影模型，可得地圖點在相機系下的坐標Pc為：

其中：(u，v)是地圖點P在左目相機圖像平面坐標系下的像素坐標，cx和cy是相機內(nèi)參。

2.2 IMU 預積分

慣性測量單元是由三軸陀螺儀和三軸加速度計組成的，陀螺儀和加速度計分別以一定的頻率采集載體系(b)相對于慣性系(i)的三軸角速度ω和加速度a。為解決視覺慣性里程計中采樣頻率不一致和重復積分問題，文獻［16］首次提出了歐式空間中的預積分方法；隨后，文獻［17］在前者的基礎上提出了IMU 在流形面的預積分方法，得到了與初始狀態(tài)無關的相對運動增量，從而顯著減少了重復計算。

IMU 采樣頻率遠高于相機，在兩個連續(xù)圖像 幀i，j之間有多幀IMU 數(shù) 據(jù)，通過式（3）對IMU 量測值預積分可得到兩幀圖像的相對運動增量。

2.3 預測像素坐標

將當前圖像幀j時刻載體在世界系w的位姿轉(zhuǎn)換為世界系w相對j時刻載體系bj的位姿為：

利用視覺慣性里程計標定的相機系c與載體系b變換矩陣，預測當前圖像幀j時刻相機相對世界系w的位姿為

其中：Rcb和tcb分別為相機系c與載體系b的旋轉(zhuǎn)矩陣和平移向量，Rcw和tcw分別為利用IMU 預積分預測出的世界系w相對于j時刻相機系c的旋轉(zhuǎn)矩陣和平移向量。

當相機模型為針孔相機模型時，利用雙目相機模型獲取的地圖點在當前幀圖像上對應像素坐標為：

其中：fx，fy，cx，cy為相機的內(nèi)參數(shù)；Pw為地圖點P在世界系下的三維坐標；Pc=[Xc，Yc，Zc]T為 地圖點在當前幀相機系的坐標；(u，v)為地圖點對應像素坐標。

式（7）表明前一幀圖像像素點對應地圖點在當前幀圖像的位置。相機的內(nèi)參數(shù)可以通過相機標定獲取，因此只需通過IMU 量測值預測相機位姿便可確定地圖點在當前幀圖像的像素位置。但IMU 預積分預測相機位姿存在一定誤差，因此在圖像特征匹配時沒有直接匹配預測特征點，而是在以預測位置為原點，某一閾值為半徑的局部區(qū)域內(nèi)搜索特征點并匹配，以防出現(xiàn)無法匹配的情況。為適配不同精度的慣性測量單元，目前普遍的做法是設置一個較大的固定值作為搜索半徑，以避免漏掉特征點。但這種慣例在IMU 精度較高時易造成計算資源浪費，同時增加了圖像特征的誤匹配率，使SLAM 系統(tǒng)的位姿估計精度降低。

3 自適應慣導輔助特征匹配

針對慣導輔助圖像特征匹配方法中固定搜索半徑過大的問題，本文提出了一種搜索半徑自適應算法。根據(jù)誤差傳播定律和慣導輔助圖像特征匹配原理計算預測像素位置的中誤差，確定地圖點最有可能出現(xiàn)的區(qū)域半徑，將局部固定區(qū)域特征匹配轉(zhuǎn)化為局部自適應區(qū)域的特征匹配，確定自適應特征匹配搜索半徑的流程如圖3所示。

圖3 確定自適應特征匹配搜索半徑流程Fig.3 Flow chart of determining adaptive feature matching search radius

3.1 預積分誤差分析

由于預積分量與IMU 噪聲存在復雜的依賴關系，難以直接利用誤差傳播定律遞推出預積分的信息矩陣，因此可將IMU 噪聲從預積分量中分離出來，得到預積分測量噪聲與IMU 噪聲關系為：

三個預積分狀態(tài)量誤差表達式較為復雜，直接計算會給計算平臺帶來資源的浪費，經(jīng)推導與簡化可得預積分量誤差的遞推公式為：

3.2 自適應閾值的確定

由IMU 預積分誤差和誤差傳播定律可得圖像間相對位姿的協(xié)方差，利用慣性導航輔助特征匹配原理可將其傳遞至每個地圖點對應的像素，獲取預測像素位置的中誤差。由于特征匹配過程中的函數(shù)與位姿有關，因此誤差傳遞時從李代數(shù)的角度對函數(shù)進行線性化。

將IMU 預積分相對位姿誤差傳遞至當前圖像幀時刻載體相對世界系的位姿誤差過程中，需要對式（4）線性化，得到其關于相對姿態(tài)和位置的導數(shù)為：

則當前圖像幀時刻載體相對世界系位姿的協(xié)方差為：

同理，依據(jù)式（5）和（6）由當前圖像幀時刻載體系相對世界系位姿協(xié)方差可得相機相對世界系位姿的協(xié)方差為：

利用相機的針孔投影模型將預測相機位姿協(xié)方差傳遞到每個地圖點對應像素，得到像素的協(xié)方差為：

自適應特征匹配搜索半徑與像素協(xié)方差的對應關系為：

其中：σ0為單位權中誤差；σu和σv分別為像素坐標p=(u，v)T的中誤差；m為特征匹配的搜索半徑，取像素坐標的極限誤差。

4 實驗結果與分析

為驗證所提方法的可行性和有效性，分別使用EuRoC 和TUM 兩個公共數(shù)據(jù)集中的不同場景序列進行實驗分析［18-19］。其中EuRoC 數(shù)據(jù)集由搭載MT9V034 攝像機和ADIS16448 慣性測量單元的微型飛行器采集，同時使用激光跟蹤儀或Vicon 動作捕捉系統(tǒng)實時捕捉飛行時位姿，作為飛行器的真實軌跡；TUM 數(shù)據(jù)集由搭載uEye 相機和BMI160 慣性測量單元的移動小車采集，真實軌跡由OptiTrack 動作捕捉系統(tǒng)提供。

本文選擇兩個公開數(shù)據(jù)集中MH01、V101、V201、Corridor1、Magistrale1、Room1 和Slides1這7 個代表性序列，每個序列都給出了距離、加速度計和陀螺儀的噪聲，如表1 所示。

表1 實驗所選序列Tab.1 Sequences chosen for the experiment

4.1 自適應特征匹配結果分析

首先，利用本文的自適應算法在不同圖像序列上進行測試，計算特征匹配搜索半徑，得到各序列自適應特征匹配搜索半徑累積分布函數(shù)，如圖4 所示。圖中F(x)表示搜索半徑小于等于x的比例。

圖4 各序列自適應特征匹配搜索半徑的累積分布函數(shù)Fig.4 Cumulative distribution function of search radius for adaptive feature matching of sequences

從圖中可以看出，各圖像序列依據(jù)本文算法獲得的特征匹配搜索半徑絕大部分小于ORBSLAM3 中的固定值，這是因為ORB-SLAM3 設定了一個較大的搜索半徑以適配不同規(guī)格的設備，避免IMU 精度過低時漏掉特征點，導致系統(tǒng)跟蹤失敗。而且不同圖像序列對應的特征匹配搜索半徑并不完全與IMU 精度相關，這是由于不同圖像序列使用的相機模型不同，EuRoC 采用的是針孔相機，而TUM 采用的是魚眼相機；另外，每個地圖點所處位置也不同，進而說明特征匹配搜索半徑應該設定為自適應閾值，綜合考慮相機模型、慣性測量單元噪聲和每個地圖點位置等因素自適應調(diào)節(jié)，以提高特征匹配的準確度。

然后利用改進前后的特征匹配算法分別計算各圖像序列特征匹配所需時間，得到平均每個特征點匹配所用時間。如圖5 所示，改進后算法在EuRoC 數(shù)據(jù)集對應的三個序列中特征匹配時間有所降低，而在TUM 數(shù)據(jù)集對應的序列中有所增加，這是由于自適應特征匹配算法在縮小特征匹配搜索半徑的同時，增加了函數(shù)線性化及誤差傳遞的計算工作量；TUM 數(shù)據(jù)集采用魚眼相機，相機投影模型較為復雜，占用了一些計算資源，但增加的時間屬于微秒級，在可以接受的范圍內(nèi)。

圖5 改進前后每個特征點匹配的平均耗時Fig.5 Average time-consuming of each feature point matching before and after improvement

4.2 局部地圖跟蹤結果分析

IMU 初始化后，ORB-SLAM3 跟蹤線程利用局部地圖點跟蹤建立圖像幀間的多視圖幾何關系，并通過最小化重投影誤差估計載體位姿。本文提出的自適應閾值算法直接作用于局部地圖跟蹤的特征匹配階段，因此可通過評估跟蹤線程位姿精度間接驗證本文特征匹配方法的有效性，同時使用不同數(shù)據(jù)集中不同實驗場景序列驗證本文算法的魯棒性。

以V101 序列為例，改進的跟蹤線程位姿估計算法與原算法定位軌跡如圖6 所示。選用的數(shù)據(jù)為該序列在10 次評測中均方根誤差最接近均值的兩組。可以看出，本文算法輸出的位姿精度有所提升，這說明本文提出的自適應算法能夠降低圖像特征的誤匹配率，因為基于特征點法視覺里程計精度主要取決于圖像特征點匹配的準確度。

圖6 改進前后V101 序列跟蹤線程定位軌跡Fig.6 V101 sequence tracking thread location before and after improvement

將改進前后ORB-SLAM3 系統(tǒng)跟蹤線程位姿估計結果分別與真實軌跡對比，并計算其絕對位姿誤差，對比結果如圖7 所示。可以看出，本文算法整體位姿精度都有所提升，其中系統(tǒng)剛開始的16 秒內(nèi)精度提升尤為明顯，這是因為初始時刻IMU 預測位姿精度不高，為避免漏掉特征點，原算法設置了一個較大的特征匹配搜索半徑。而本文自適應特征匹配方法通過誤差傳播定律獲取了每個地圖點對應的搜索半徑，使其搜索區(qū)域顯著減小，因此初始一段時間特征點匹配的準確率會有明顯提升，跟蹤線程位姿估計也會更接近真值。

圖7 改進前后V101 序列絕對位姿誤差Fig.7 Absolute position and pose errors of V101 sequence before and after improvement

表2 是改進前后ORB-SLAM3 幀間跟蹤位姿在7 個不同場景序列中的絕對位姿誤差對比結果，這里的數(shù)據(jù)為各序列10 次評測絕對位姿誤差均方根的平均值。由表2 可知，在測試的7 個不同場景序列中，改進后算法輸出的位姿精度平均提升38.09%，說明自適應算法能夠提高特征點匹配的準確度，而且在不同設備和不同場景的序列中精度均有所提升，具有較強的魯棒性。

表2 各序列跟蹤線程絕對位姿誤差的均方根Tab.2 Root-mean-square of absolute pose error of each sequence tracking thread

4.3 ORB-SLAM3 系統(tǒng)跟蹤結果分析

為驗證算法的可用性，本文對比了不同數(shù)據(jù)集中不同圖像序列利用改進前后ORB-SLAM3系統(tǒng)處理輸出位姿的絕對位姿誤差，如表3 所示，這里的數(shù)據(jù)為各序列10 次評測絕對位姿誤差均方根的平均值。

由表3 可知，在選用的圖像序列中，依據(jù)本文特征匹配方法改進后的ORB-SLAM3 系統(tǒng)的絕對軌跡誤差均優(yōu)于原算法，改進后系統(tǒng)整體位姿精度平均提高16.38%。證明改進后的ORBSLAM3 系統(tǒng)能夠在不同場景序列中良好運行，具有較強的魯棒性，同時定位精度也有所提升。這是因為ORB-SLAM3 系統(tǒng)后端采用非線性優(yōu)化法，即使最終輸出位姿經(jīng)歷了三次優(yōu)化，但其仍與給定的初始位姿具有較強的相關性。而這里的初始位姿正是由跟蹤線程提供的，因此本文提出的自適應閾值慣性導航輔助視覺SLAM 特征匹配方法直接提升了跟蹤線程位姿估計的精度，為SLAM 系統(tǒng)提供了較為準確的初值，進而使SLAM 系統(tǒng)整體定位精度提高。

表3 各序列整體位姿的絕對位姿誤差的均方根Tab.3 Root-mean-square of the absolute pose error of the global pose of each sequence

4.4 稠密地圖的構建

為驗證算法在地圖構建中的有效性，本文利用改進后ORB-SLAM3 系統(tǒng)估計的位姿和ELAS（Efficient Large Scale Stereo Matching）算法恢復出的深度數(shù)據(jù)進行三維點云拼接。以V101 序列為例，構建的稠密地圖，如圖8 所示。從圖中可以看出雖然還有些不規(guī)整的地方，但該算法能夠完整地表述物體輪廓，幾乎沒有重影，且周圍的離散點云也比較少，墻面、地面近乎在一個平面。這說明本文提出的自適應慣導輔助視覺里程計特征匹配方法不僅能夠提升視覺慣性SLAM 系統(tǒng)的定位精度，在稠密建圖方面也有較好的效果，能夠為三維地形重建提供新思路。

圖8 V101 序列的稠密地圖Fig.8 Dense map of the V101 sequence

5 結論

為解決ORB-SLAM3 中圖像特征匹配失誤率高的問題，本文提出了自適應閾值慣導輔助的圖像特征匹配算法。以慣性測量單元誤差為基礎，根據(jù)誤差傳播定律自動調(diào)整圖像特征匹配搜索半徑，從而利用不同精度的慣性測量數(shù)據(jù)高效輔助視覺SLAM 的特征匹配。實驗結果表明，該方法可有效縮小特征匹配的搜索半徑，為特征點提供區(qū)域約束，顯著提高圖像特征匹配的準確度，使跟蹤線程位姿精度提高約 38.09%，系統(tǒng)整體位姿精度提高約16.38%，并且可以構建更精確的稠密三維點云地圖。