張 濤，陳 浩，閆 捷，李 瑤

（1. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096；2. 東南大學(xué) 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；3. 北京電子工程總體研究所，北京 100854）

隨著科技的進(jìn)步以及人們需求的增加，移動(dòng)機(jī)器人等智能機(jī)器在人類社會(huì)越來越常見，尤其是近年來，無人機(jī)等微型機(jī)器人廣泛應(yīng)用于搜索救援、邊境偵查、工業(yè)廠房檢查等[1]領(lǐng)域。但由于其載重的有限性以及電池的持久性限制，所攜帶的傳感器必須具有重量輕、功耗小、高可靠性以及易配置的特點(diǎn)。視覺里程計(jì)（Visual Odometry,VO）以其導(dǎo)航高度自主、低成本及便捷等特點(diǎn)，在無人機(jī)等自主機(jī)器人導(dǎo)航領(lǐng)域具有重要的作用。

視覺里程計(jì)通過研究圖像幀與幀間的變換關(guān)系，估計(jì)機(jī)器人相對(duì)周邊環(huán)境的位置與姿態(tài)信息[2]，其主要采用兩種計(jì)算方式[3]：直接法和特征點(diǎn)法。直接法利用圖像的亮度信息，通過最小化光度誤差來估計(jì)機(jī)器人的位姿信息。直接法省去了計(jì)算特征點(diǎn)和描述子的時(shí)間，但是直接法對(duì)單個(gè)像素沒有區(qū)分度，且在光照變化時(shí)會(huì)破壞灰度值不變的假設(shè)，使算法失敗。

特征點(diǎn)法可以分為兩步：1）檢測(cè)圖像特征點(diǎn)并建立描述子，采用極線約束等方法進(jìn)行特征點(diǎn)匹配；2）利用第一步匹配結(jié)果，由三角測(cè)量建立局部地圖，解算最小化重投影誤差得到位姿信息。

相比直接法，特征點(diǎn)法運(yùn)行穩(wěn)定，是視覺里程計(jì)的主流方法。

目前基于特征點(diǎn)法的視覺里程計(jì)存在以下問題：

1）在特征匹配階段樣本容量大、匹配準(zhǔn)確率低，導(dǎo)致視覺里程計(jì)位姿估計(jì)精度下降。張?jiān)粕鶾4]利用隨機(jī)采樣一致性方法（RANSAC）對(duì)匹配算法進(jìn)行改進(jìn)，提升了匹配精度；李小紅[5]等采用PROSAC（Progressive Sample Consensus）算法來消除誤匹配點(diǎn)，得到比RANSAC方法更好的魯棒性和計(jì)算效率；Wu Yue等[6]采用迭代最近點(diǎn)法增加正確匹配點(diǎn)對(duì)以改進(jìn)RANSAC算法，但是此方法增加了樣本容量，以致算法效率降低；徐曉蘇等[7]運(yùn)用聚類抽樣方法以降低錯(cuò)誤匹配率；

2）視覺里程計(jì)僅通過相鄰兩幀進(jìn)行位姿估計(jì)，必然會(huì)產(chǎn)生漂移誤差，此外相機(jī)分辨率也會(huì)帶來特征點(diǎn)的漂移，這些都會(huì)降低視覺里程計(jì)的精度。Bellavia F等[8]提出 SSLAM 算法，該算法通過優(yōu)化關(guān)鍵幀的選取來縮小空間點(diǎn)的確定范圍，提高空間點(diǎn)的確定精度；程傳奇等[9]采用非線性優(yōu)化方法并結(jié)合三維地圖點(diǎn)以削弱累計(jì)誤差；Badino H等[10]提出MFI算法，當(dāng)特征點(diǎn)漂移到一定距離后，采用計(jì)算得到的預(yù)測(cè)特征點(diǎn)代替原特征點(diǎn)來減小漂移誤差，但是該算法需要時(shí)刻計(jì)算特征點(diǎn)的預(yù)測(cè)值，計(jì)算量大，耗時(shí)久。

本文針對(duì)誤匹配消除問題，采用在特征檢測(cè)階段選取壽命長(zhǎng)的特征點(diǎn)、匹配階段進(jìn)行環(huán)形匹配以及利用優(yōu)質(zhì)匹配點(diǎn)對(duì)等方法選取優(yōu)質(zhì)特征點(diǎn)以降低RANSAC算法樣本容量、提升正確匹配點(diǎn)對(duì)比例。針對(duì)特征點(diǎn)漂移與累積誤差問題，提出一種基于雙向重投影的位姿估計(jì)算法，解算下一幀圖像中的最小化重投影誤差得到初步的位姿信息，并將此位姿用于反向重投影，解算當(dāng)前幀中最小化重投影誤差以提升位姿估計(jì)精度。

1 基本原理

1.1 坐標(biāo)系約定

在相機(jī)成像中，小孔成像模型運(yùn)用最廣泛。在此模型下，可以采用線性方程組來描述物體的空間坐標(biāo)和圖像坐標(biāo)之間的關(guān)系。圖1展示了模型中的坐標(biāo)系關(guān)系，其中，為世界坐標(biāo)系下的三維空間點(diǎn)，m為在圖像平面的投影點(diǎn)，為焦距，即光心到圖像平面的距離。

圖1 坐標(biāo)系關(guān)系圖Fig.1 Coordinate system

各坐標(biāo)系的定義如下：

像素坐標(biāo)系——像素坐標(biāo)系與圖像坐標(biāo)系基本一致，只有兩處區(qū)別，一是將坐標(biāo)原點(diǎn)移至圖像平面的左上角頂點(diǎn)，二是像素是此坐標(biāo)系下的基本單位。像素坐標(biāo)系相對(duì)于圖像坐標(biāo)系的平移量為，即像素坐標(biāo)系沿x軸正方向平移，沿y軸正方向平移，即可得到圖像坐標(biāo)系。

1.2 雙目相機(jī)模型

圖2(a) 雙目相機(jī)的成像模型Fig.2(a) Imaging model of binocular camera

圖2(b) 雙目相機(jī)幾何模型Fig.2(b) Geometric model of binocular camera

2 算法流程

本文以R. Mur-Artal等人的ORB-SLAM2算法[11]為基礎(chǔ)，對(duì)其中的位姿估計(jì)部分進(jìn)行改進(jìn)，以期提升位姿估計(jì)精度。改進(jìn)分為三個(gè)階段：1）特征點(diǎn)檢測(cè)階段；2）特征點(diǎn)匹配與誤匹配消除階段；3）重投影階段。

整體設(shè)計(jì)流程如圖3所示，具體包括以下步驟：

Step 1：獲取算法所需雙目圖像（本文采用KITTI數(shù)據(jù)集），在ORB-SLAM2特征點(diǎn)提取的基礎(chǔ)上，記錄所提取特征點(diǎn)的壽命，并建立對(duì)應(yīng)的描述子；

Step 2：對(duì)所得的特征點(diǎn)進(jìn)行環(huán)形匹配，選取優(yōu)質(zhì)匹配點(diǎn)對(duì)，并采用RANSAC算法進(jìn)行誤匹配消除；

Step 3：建立局部地圖，并對(duì)空間點(diǎn)進(jìn)行投影，解算最小重投影誤差：

Step 4：更新姿態(tài)矩陣與空間點(diǎn)，進(jìn)行反向投影，解算此時(shí)最小重投影誤差，解算結(jié)果求逆，可得姿態(tài)矩陣，完成位姿估計(jì)。

圖3 本文算法流程圖Fig.3 Flow chart of the algorithm

3 算法實(shí)現(xiàn)

3.1 ORB特征點(diǎn)提取

特征點(diǎn)由關(guān)鍵點(diǎn)和描述子兩部分組成。ORB算法是在2011年國際計(jì)算機(jī)視覺大會(huì)（ICCV）上提出的一種新型方法[12]。ORB算法中的關(guān)鍵點(diǎn)提取采用FAST關(guān)鍵點(diǎn)[13]檢測(cè)算法。FAST是一種角點(diǎn)，主要檢測(cè)局部像素灰度變化明顯的地方，速度非?？?。在以候選像素點(diǎn)為圓心、半徑為3像素的圓周內(nèi)，有N個(gè)像素點(diǎn)與候選像素點(diǎn)的亮度差別明顯，則該候選像素點(diǎn)即為FAST關(guān)鍵點(diǎn)。設(shè)為候選像素點(diǎn)，且

其中，I(o)為候選特征點(diǎn)的灰度值，I(x)為候選點(diǎn)周圍圓周上一點(diǎn)的灰度值，ω為以o 為圓心的半徑為 3個(gè)像素的圓周區(qū)域，εd為符合條件的點(diǎn)數(shù)閾值，通常為 9、11、12，N 為符合界定條件的像素個(gè)數(shù)。如果N大于閾值，則認(rèn)為o 是一個(gè)特征點(diǎn)。

FAST關(guān)鍵點(diǎn)檢測(cè)計(jì)算簡(jiǎn)單、速度快，但FAST特征點(diǎn)不具有方向性和尺度信息，因此ORB通過構(gòu)建圖像金字塔與采用灰度質(zhì)心法實(shí)現(xiàn)了尺度不變性和旋轉(zhuǎn)不變性。所謂質(zhì)心是指以圖像塊灰度值作為權(quán)重的中心，考慮特征點(diǎn)o 的一個(gè)鄰域B，定義圖像塊的矩為

其中，p,q ∈(0,1)，r 表示圖像塊B 的半徑，I(x,y)表示圖像塊(x,y)位置處的灰度值，并利用矩來定義特征點(diǎn)的質(zhì)心：

至此，F(xiàn)AST關(guān)鍵點(diǎn)具有了尺度不變性和旋轉(zhuǎn)不變性，大大提升了其健壯性。

在提取關(guān)鍵點(diǎn)后，ORB算法利用改進(jìn)的 BRIEF特征向量來構(gòu)造特征點(diǎn)描述子。對(duì)特征點(diǎn)o 的一個(gè)S×S鄰域D(其中，S一般為31)，定義一個(gè)二進(jìn)制測(cè)試準(zhǔn)則τ：

其中，q(x)為點(diǎn)x 處的灰度值。在鄰域D 選擇n 個(gè)點(diǎn)對(duì)進(jìn)行二進(jìn)制測(cè)試，BRIEF描述子得到一個(gè)n 維二進(jìn)制碼：

由于BRIEF不具有旋轉(zhuǎn)不變性，因此利用式(8)求得的特征點(diǎn)的方向夾角θ。假設(shè)生成特征點(diǎn)描述符的n 個(gè)測(cè)試點(diǎn)對(duì)為(xi,yi)，由式(10)可以定義一個(gè)2×n的矩陣：

對(duì)各測(cè)試點(diǎn)對(duì)，利用θ形成的旋轉(zhuǎn)矩陣Rθ，構(gòu)造經(jīng)過旋轉(zhuǎn)之后的匹配點(diǎn)的位置Sθ：

在新的點(diǎn)集位置上構(gòu)建二進(jìn)制串描述符，此時(shí)描述子已具有旋轉(zhuǎn)不變性。至此ORB特征點(diǎn)提取結(jié)束，下一步將進(jìn)行特征點(diǎn)匹配與誤匹配消除。

3.2 特征點(diǎn)匹配與誤匹配消除

傳統(tǒng)的 RANSAC方法將所有的匹配結(jié)果作為樣本，樣本容量大，含有誤匹配較多，正確匹配比例低，效率低下。因此本文從減少誤匹配數(shù)，降低樣本容量方面改進(jìn)RANSAC算法，具體步驟如下：

Step 1：對(duì)左右兩幅圖像提取ORB特征，并記錄每個(gè)特征點(diǎn)的存活幀數(shù)，即特征點(diǎn)壽命。特征點(diǎn)的壽命越長(zhǎng)，其可靠性越高。采取網(wǎng)格法將圖像分成10n×10n的網(wǎng)格，對(duì)每個(gè)網(wǎng)格點(diǎn)內(nèi)的特征點(diǎn)，按壽命大小進(jìn)行排序，每個(gè)網(wǎng)格只保留前n個(gè)特征點(diǎn)，并使用其首次出現(xiàn)時(shí)的描述子。對(duì)同一個(gè)特征點(diǎn)使用相同的描述子可以減小漂移誤差。

Step 2：對(duì)特征點(diǎn)進(jìn)行環(huán)形匹配，如圖4所示。首先采用極線約束，對(duì)當(dāng)前一幀 Ii和下一幀圖像 Ij的特征點(diǎn)進(jìn)行左右圖像匹配，得到左右匹配點(diǎn)對(duì)然后利用最近鄰方法，求取最小漢明距離，對(duì)當(dāng)前幀 Ii和下一幀圖像Ij的特征點(diǎn)進(jìn)行前后幀間匹配，得到匹配點(diǎn)對(duì)當(dāng)四組匹配形成環(huán)形時(shí)，則視為有效匹配，否則匹配失敗。

圖4 環(huán)形匹配Fig.4 Circle match

Step 3：將所有匹配結(jié)果分為優(yōu)質(zhì)匹配和一般匹配，只在優(yōu)質(zhì)匹配中進(jìn)行 RANSAC算法，縮減了樣本量，提升了效率。優(yōu)質(zhì)匹配有兩個(gè)判定標(biāo)準(zhǔn)：1）壽命長(zhǎng)的特征點(diǎn)的匹配對(duì)；2）漢明距離小的匹配點(diǎn)對(duì)。

對(duì)同一張圖片進(jìn)行三種匹配實(shí)驗(yàn)：ORB原始匹配、經(jīng)過RANSAC后的匹配、經(jīng)過改進(jìn)RANSAC之后的匹配。匹配結(jié)果如圖5～7所示。

由圖5可以看出，原始的ORB特征點(diǎn)最多，但是其中誤匹配數(shù)量也是最多，若以此匹配結(jié)果進(jìn)行位姿估計(jì)，耗時(shí)久且誤差大；從圖6可以看出，經(jīng)過RANSAC算法去除誤匹配后，匹配點(diǎn)對(duì)數(shù)量大大減少，誤匹配數(shù)量也僅有少量殘留且剩余匹配點(diǎn)對(duì)較集中，不利于后期位姿估計(jì)的解算；圖7采用了本文的算法，可以看出正確匹配所占比例大大增加，并使正確匹配對(duì)分散開來，增強(qiáng)了算法的穩(wěn)定性，為后續(xù)的位姿估計(jì)奠定了良好的基礎(chǔ)。

圖5 ORB原始匹配Fig.5 ORB original matching

圖6 RANSAC匹配Fig.6 Matching by RANSAC

圖7 改進(jìn)的RANSAC匹配Fig.7 Matching by improved RANSAC

3.3 基于雙向重投影的位姿估計(jì)方法

在得到匹配點(diǎn)對(duì)并進(jìn)行誤匹配消除后，需要建立局部地圖并將當(dāng)前幀的空間點(diǎn)投影到下一幀圖像中，并計(jì)算投影點(diǎn)的最小重投影誤差。但是由于特征點(diǎn)的漂移會(huì)導(dǎo)致空間點(diǎn)的確定帶有較大誤差，在計(jì)算重投影誤差時(shí)誤差會(huì)累積，對(duì)精度造成較大影響。因此，本文提出一種基于雙向重投影的位姿估計(jì)方法，其流程如圖8所示。

圖8 二次重投影方法流程圖Fig.8 Flow chart of the twice reprojection method

其中，K為相機(jī)內(nèi)參矩陣，R為世界坐標(biāo)系到相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣，t為世界坐標(biāo)系到相機(jī)坐標(biāo)系的平移矩陣。

由于相機(jī)位姿未知以及觀測(cè)點(diǎn)有噪聲，由式(13)確定的投影點(diǎn)集存在誤差，即重投影誤差，因此構(gòu)建最小二乘問題，解算最優(yōu)的相機(jī)位姿，使得重投影誤差D(Pi,j)最?。?/p>

圖9 重投影誤差示意圖Fig.9 Schematic diagram of a reprojection error

根據(jù)此時(shí)的姿態(tài)矩陣，利用式(15)更新空間點(diǎn)Qn=(xn,yn,zn)：

其中，Qn-1是上一次得到的空間點(diǎn)。

如圖10所示，將更新后的空間點(diǎn)集 Qn投影至當(dāng)前幀，重新計(jì)算重投影誤差 D(Qj,i)：

圖10 雙向重投影示意圖Fig.10 Diagram of bi-directional reprojection

4 實(shí)驗(yàn)結(jié)果與分析

以視覺SLAM公開數(shù)據(jù)集KITTI[14]為測(cè)試樣本，KITTI數(shù)據(jù)集由德國卡爾斯魯厄理工學(xué)院和豐田美國技術(shù)研究院聯(lián)合創(chuàng)辦，是目前國際上最大的自動(dòng)駕駛場(chǎng)景下的計(jì)算機(jī)視覺算法評(píng)測(cè)數(shù)據(jù)集。該數(shù)據(jù)集是通過在城市地區(qū)、農(nóng)村地區(qū)以及高速公路上行駛而獲得的，每張圖像上最多可顯示15輛汽車和30名行人。

如圖11所示，數(shù)據(jù)采集平臺(tái)裝配有2個(gè)灰度攝像機(jī)、2個(gè)彩色攝像機(jī)、一個(gè)Velodyne 64線3D激光雷達(dá)、4個(gè)光學(xué)鏡頭以及1個(gè)GPS導(dǎo)航系統(tǒng)，其以10 Hz的頻率采樣了 22個(gè)雙目圖像序列（8位 png灰度圖像），前11個(gè)圖像序列含有真實(shí)數(shù)據(jù)值，適合用來測(cè)定算法。其中相機(jī)坐標(biāo)系定義為“右-下-前”，雷達(dá)坐標(biāo)系定義為“前-左-上”，GPS和 IMU坐標(biāo)系定義為“前-左-上”。數(shù)據(jù)集所給真值與本文所得結(jié)果均為相機(jī)坐標(biāo)系下數(shù)據(jù)。

本文的實(shí)驗(yàn)環(huán)境為L(zhǎng)enovo X240筆記本電腦（酷睿i7-4600CPU，主頻2.50GHZ，4G內(nèi)存），以Ubuntu16.04為實(shí)驗(yàn)平臺(tái)，選取KITTI數(shù)據(jù)集中的第1序列和第7序列。第1序列共有1104幀圖像，運(yùn)動(dòng)時(shí)間為114 s，運(yùn)行速度較快；第 7序列共有 1101幀圖像，運(yùn)行時(shí)間為 114 s，運(yùn)動(dòng)軌跡為閉環(huán)且慢速運(yùn)動(dòng)。分別采用ORBSLAM2方法[9]和本文算法建立視覺里程計(jì)模型，仿真實(shí)驗(yàn)結(jié)果如圖12～15所示，圖中的(x,y,z)為相機(jī)坐標(biāo)系下的坐標(biāo)。

圖11 實(shí)驗(yàn)數(shù)據(jù)采集平臺(tái)Fig.11 Experimental data acquisition platform

圖12顯示了兩種算法在序列1的相對(duì)位置和姿態(tài)誤差。從圖12中可以看出，兩種算法在各個(gè)方向上的位置誤差都是越來越大，符合視覺里程計(jì)模型。與ORBSLAM2相比，本文算法解算的位置和姿態(tài)角誤差都較小，累積誤差明顯較小。在X方向，ORB-SLAM2的最大誤差大約為19 m，而本文算法的最大誤差約為15 m，在Z方向（即車輛前進(jìn)方向），ORB-SLAM2的最大誤差大約為 25 m，而本文算法的最大誤差約為20 m，精度提升了大約20%，且本文算法的穩(wěn)定性優(yōu)于ORB-SLAM2。

圖12(a) 位置誤差（序列1）Fig.12(a) Position error of sequence 1

圖12(b) 姿態(tài)角誤差（序列1）Fig.12(b) Attitude angle error of sequence 1

圖13(a) 位置誤差（序列7）Fig.13(a) Position error of sequence 7

圖13(b) 姿態(tài)角誤差（序列7）Fig.13(b) Attitude angle error of sequence 7

圖13顯示了兩種算法在一個(gè)閉環(huán)運(yùn)動(dòng)中（序列7）的相對(duì)位置和姿態(tài)誤差。從圖13中可以看出，ORBSLAM2在X和Z方向的誤差均在1.2 m以內(nèi)，本文算法在X方向的誤差在1 m以內(nèi)，Z方向誤差在0.9 m以內(nèi)，精度提升大約20%。

圖14與圖15顯示了本文算法與 ORB-SLAM2以及真實(shí)值的運(yùn)動(dòng)軌跡。圖14的運(yùn)動(dòng)大部分近似于直線運(yùn)動(dòng)，誤差隨著運(yùn)動(dòng)越來越明顯。由圖14可以看出，本文算法與 ORB-SLAM2算法的誤差都是越來越大，即包含累積誤差，本文算法的誤差增長(zhǎng)幅度相對(duì)較小，整體誤差約是ORB-SLAM2誤差的80%。圖15中運(yùn)動(dòng)從原點(diǎn)(0,0)出發(fā)，逆時(shí)針進(jìn)行運(yùn)動(dòng)，最終回到原點(diǎn)。本文算法和 ORB-SLAM2都能得到較為理想的結(jié)果。與圖14相比，圖15所示運(yùn)動(dòng)因?yàn)樗俣容^慢以及閉環(huán)檢測(cè)等環(huán)節(jié)的存在，算法會(huì)對(duì)累積誤差進(jìn)行修正，因此圖15中行程算法優(yōu)化效果最好。從圖14軌跡可以看出，本文算法在直線行程中的優(yōu)化效果最好，定位精度提升最高 20%左右，優(yōu)于ORB-SLAM2的結(jié)果。

圖14(a) 解算軌跡（序列1）Fig.14(a) Trajectory of sequence 1

圖14(b) 部分放大（序列1）Fig.14(b) Partial enlargement of sequence 1

圖15(a) 解算軌跡（序列7）Fig.15(a) Trajectory of sequence 7

圖15(b) 部分放大（序列7）Fig.15(b) Partial enlargement of sequence 7

表 1列出了兩種算法在兩個(gè)序列中的均方根誤差（RMSE）。

表1 兩種算法的均方根誤差Tab.1 RMSE of the two algorithms

綜上所述，實(shí)驗(yàn)結(jié)果可以驗(yàn)證本文算法位姿估計(jì)的可靠性與正確性。

5 結(jié) 論

本文針對(duì)視覺里程計(jì)中的誤差累積問題，首先改進(jìn)了傳統(tǒng)的 RANSAC算法，通過特征點(diǎn)壽命長(zhǎng)短、優(yōu)質(zhì)匹配等方法選取優(yōu)質(zhì)特征點(diǎn)以縮減 RANSAC樣本容量，從而提高了 RANSAC算法的效率；然后提出一種基于雙向重投影的位姿估計(jì)算法，將初次解算最小化重投影誤差后的位姿估計(jì)結(jié)果應(yīng)用到反向投影中，提高了位姿估計(jì)的精度；最后，利用SLAM公開數(shù)據(jù)集KITTI進(jìn)行了仿真實(shí)驗(yàn)。

仿真結(jié)果表明，與 ORBSLAM2進(jìn)行對(duì)比，本文算法在精度和穩(wěn)定性上都有提升，驗(yàn)證了本文改進(jìn)算法的正確性，說明了本文的研究具有一定的價(jià)值。

為進(jìn)一步提高定位精度、抑制累積誤差，下一步將考慮與其他傳感器進(jìn)行組合導(dǎo)航[15]，開展組合模式的視覺里程計(jì)算法研究。