周紹磊，吳修振，劉 剛，張 嶸，徐海剛

（1. 海軍航空工程學(xué)院 控制工程系，煙臺 264001；2. 清華大學(xué) 精密儀器系，北京100084；3. 北京自動化控制設(shè)備研究所，北京 100074）

一種單目視覺ORB-SLAM/INS組合導(dǎo)航方法

周紹磊1，吳修振1，劉 剛1，張 嶸2，徐海剛3

（1. 海軍航空工程學(xué)院 控制工程系，煙臺 264001；2. 清華大學(xué) 精密儀器系，北京100084；3. 北京自動化控制設(shè)備研究所，北京 100074）

針對慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)在衛(wèi)星導(dǎo)航失效時(shí)無法使用的問題，提出了單目視覺ORB-SLAM/INS組合導(dǎo)航方法，用于擴(kuò)展組合導(dǎo)航系統(tǒng)在強(qiáng)干擾環(huán)境和室內(nèi)環(huán)境的應(yīng)用范圍。該算法分為兩個(gè)階段：初始化階段，當(dāng)ORB-SLAM形成閉環(huán)時(shí)設(shè)計(jì)算法在線估計(jì)單目視覺ORB-SLAM算法的尺度因子；導(dǎo)航階段，ORB-SLAM系統(tǒng)輸出的位置信息經(jīng)過尺度變換后作為觀測量進(jìn)行卡爾曼濾波，估計(jì)INS導(dǎo)航系統(tǒng)的誤差狀態(tài)量從而修正慣導(dǎo)系統(tǒng)的誤差。設(shè)計(jì)了硬件和軟件平臺對提出的組合導(dǎo)航方法進(jìn)行試驗(yàn)驗(yàn)證。跑車實(shí)驗(yàn)結(jié)果表明：所設(shè)計(jì)的 ORB-SLAM/INS組合導(dǎo)航系統(tǒng)具有較高的定位精度，導(dǎo)航時(shí)間6 min定位誤差為1.162 m，且不隨時(shí)間漂移，具有很強(qiáng)的應(yīng)用價(jià)值。

ORB-SLAM；INS；尺度因子；卡爾曼濾波；組合導(dǎo)航

隨著社會需求的增加和科學(xué)技術(shù)的進(jìn)步，移動機(jī)器人越來越多地融入日常的生活中，提高了生活效率，節(jié)約了生活成本，正在創(chuàng)造越來越多的社會價(jià)值。在移動機(jī)器人的技術(shù)中，首要解決的是機(jī)器人的定位問題，這是機(jī)器人完成特定任務(wù)的基礎(chǔ)。

對于室外機(jī)器人（如四旋翼無人機(jī)），定位主要依靠衛(wèi)星導(dǎo)航系統(tǒng)和慣性導(dǎo)航系統(tǒng)；對于室內(nèi)機(jī)器人，定位主要依靠光學(xué)運(yùn)動捕捉系統(tǒng)（如 Vicon）或者利用激光或者相機(jī)進(jìn)行實(shí)時(shí)定位和構(gòu)圖（Simultaneous Localization and Mapping, SLAM）。衛(wèi)星導(dǎo)航屬于有源定位方法，當(dāng)信號受到干擾時(shí)定位系統(tǒng)無法正常工作，光學(xué)運(yùn)動捕捉系統(tǒng)雖然定位精度高，但成本昂貴且環(huán)境適應(yīng)性差。慣性導(dǎo)航和視覺SLAM屬于無源定位方法，慣導(dǎo)誤差會隨著時(shí)間積累并發(fā)散，視覺SLAM由于具備閉環(huán)修正技術(shù)因此定位精度較高。單純地依賴一種定位方法很難取得理想的定位效果，本文的著眼點(diǎn)在于將慣性導(dǎo)航和單目視覺SLAM兩種無源定位方法相互融合，取長補(bǔ)短，構(gòu)建具有較高定位精度和魯棒性的無源定位系統(tǒng)。

單目視覺SLAM技術(shù)近年來發(fā)展迅速，由最初的基于濾波的方法逐漸發(fā)展到基于關(guān)鍵幀的方法，Strasdat等人[1]證明基于關(guān)鍵幀的方法比基于濾波的方法精度更高。基于關(guān)鍵幀的方法最具代表性的是Klein和Murray提出的PTAM算法[2]，第一次提出將跟蹤定位和構(gòu)圖分為兩個(gè)線程并行處理，檢測的特征點(diǎn)為FAST特征點(diǎn)。Pirker等人[3]提出了CD-SLAM算法，是一個(gè)包括閉環(huán)修正、重定位等比較完善的SLAM系統(tǒng)，但沒有涉及地圖的初始化方法。Song等人[4]提出的視覺里程計(jì)算法將ORB特征點(diǎn)（FAST+BRIEF）用于跟蹤和光束平差法（Bundle Adjustment，BA）優(yōu)化，但沒有全局重定位和閉環(huán)修正。Engel等人[5]提出了LSD-SLAM算法，能夠建立半密度地圖，相對稀疏特征點(diǎn)地圖更具有實(shí)用意義，直接使用像素密度進(jìn)行優(yōu)化而不是通過基于特征點(diǎn)的光束平差法，但此算法依然需要稀疏特征點(diǎn)進(jìn)行閉環(huán)檢測。R.Mur-Artal和J.D.Tardos等人[6-7]提出了 ORB-SLAM 算法，融合了PTAM算法的主要思想和Strasdat提出的閉環(huán)修正方法，整個(gè)過程均采用ORB特征點(diǎn)[8-9]進(jìn)行跟蹤、構(gòu)圖、重定位和閉環(huán)修正，與前面提出的SLAM方法相比，具有較高的定位精度和實(shí)時(shí)性。

單目視覺SLAM的優(yōu)點(diǎn)是成本低，實(shí)際應(yīng)用簡單方便，但由于單目相機(jī)無法獲得深度信息，這就導(dǎo)致單目視覺SLAM無法得到準(zhǔn)確的尺度信息，因此必須通過一定的方法估計(jì)出尺度因子，單目視覺SLAM系統(tǒng)輸出的位置信息經(jīng)過尺度變換后才能用于實(shí)際導(dǎo)航。

綜上所述，本文采用 R.Mur-Artal等人提出的ORB-SLAM算法，提出了單目視覺ORB-SLAM/INS組合導(dǎo)航方法。組合導(dǎo)航的過程分為兩個(gè)階段：

第一階段：初始化階段。通過規(guī)劃運(yùn)動航跡使ORB-SLAM系統(tǒng)形成閉環(huán)，用ORB-SLAM系統(tǒng)輸出的位置信息和差分 GPS輸出的準(zhǔn)確位置信息在線估計(jì)ORB-SLAM系統(tǒng)的尺度因子。

第二階段：導(dǎo)航階段。ORB-SLAM系統(tǒng)輸出的位置信息經(jīng)過尺度變換后作為觀測量進(jìn)行卡爾曼濾波，估計(jì)INS導(dǎo)航系統(tǒng)的誤差狀態(tài)量后，用于修正慣導(dǎo)系統(tǒng)的誤差。

1 單目視覺ORB-SLAM算法

單目視覺ORB-SLAM算法分為三個(gè)并行的線程：跟蹤、本地地圖構(gòu)建以及閉環(huán)控制，算法總體框架如圖1所示。設(shè)計(jì)該算法的具體實(shí)施過程如下：

圖1 單目視覺ORB-SLAM算法Fig.1 Monocular ORB-SLAM algorithm

1）通過張正友棋盤標(biāo)定法對攝像頭的畸變參數(shù)進(jìn)行標(biāo)定，用標(biāo)定好的參數(shù)對每一幀圖像進(jìn)行畸變修正，然后對圖像進(jìn)行高斯濾波，消除噪聲，為特征點(diǎn)提取做準(zhǔn)備。

2）提取圖像的ORB特征點(diǎn)[10]，即采用具有方向信息的多尺度FAST特征點(diǎn)，用32個(gè)字節(jié)256位比特?cái)?shù)的BRIEF作為描述符，即一個(gè)特征點(diǎn)用一個(gè)256位的0或1表示，這樣就構(gòu)成了256維空間的特征點(diǎn)空間。

3）利用詞袋技術(shù)[11]對特征點(diǎn)空間進(jìn)行分類，分為M×N層，通過漢明距離就可以把提取的ORB特征點(diǎn)在空間中進(jìn)行分類。

4）前后幀圖像中的特征點(diǎn)經(jīng)過分類就可以在不同的空間位置上進(jìn)行快速匹配，通過匹配特征點(diǎn)滿足的極線約束關(guān)系就可以建立前后幀圖像幾何關(guān)系，從而得到相機(jī)的實(shí)時(shí)位姿信息。

5）對每一幀圖像進(jìn)行上述操作，存儲關(guān)鍵點(diǎn)和關(guān)鍵幀，從而構(gòu)建環(huán)境地圖信息，對新增加的關(guān)鍵點(diǎn)和關(guān)鍵幀在已建立的地圖中搜索匹配實(shí)現(xiàn)重定位或者閉環(huán)檢測。在整個(gè)SLAM過程中，通過光束平差法不斷對誤差進(jìn)行修正，當(dāng)檢測到閉環(huán)時(shí)，對整個(gè)地圖進(jìn)行優(yōu)化，從而提高SLAM定位的精度。

綜上所述，單目視覺ORB-SLAM算法融合了近年來在視覺SLAM領(lǐng)域最新的研究成果，在特征點(diǎn)選擇、地圖跟蹤、閉環(huán)檢測、重定位、關(guān)鍵幀地圖點(diǎn)的取舍等方面都有比較全面的優(yōu)化和改進(jìn)，位姿估計(jì)精度高，魯棒性強(qiáng)，從而使得其與慣性導(dǎo)航系統(tǒng)進(jìn)行組合成為可能。但由于單目視覺本身不具有深度信息，尺度因子無法確定，因此尺度因子的估計(jì)問題是單目視覺ORB-SLAM/INS組合導(dǎo)航系統(tǒng)首要解決的問題。

2 單目視覺ORB-SLAM尺度因子的估計(jì)

根據(jù)文獻(xiàn)[7]，ORB-SLAM 輸出的位置信息與相機(jī)真實(shí)的位置信息（由差分GPS得到）滿足相似變換的關(guān)系，因此通過相似變換的求解可以估計(jì)單目視覺ORB-SLAM 尺度因子，此過程為單目視覺 ORBSLAM/INS組合導(dǎo)航的第一階段——初始化階段。

單目視覺ORB-SLAM系統(tǒng)的導(dǎo)航參考坐標(biāo)系為地圖初始化時(shí)的相機(jī)坐標(biāo)系 Ocxcyczc，慣性導(dǎo)航系統(tǒng)和差分GPS的導(dǎo)航坐標(biāo)系為東北天地理系 Otxtytzt，如圖2所示。

圖2 坐標(biāo)系定義Fig.2 Definition of coordinate system

當(dāng) ORB-SLAM 系統(tǒng)第一次形成閉環(huán)時(shí)，假設(shè)ORB-SLAM 系統(tǒng)輸出的相機(jī)位置為，對應(yīng)差分 GPS輸出真實(shí)的位置信息，理想情況下，根據(jù)相似變換的原理，則有

其中，R為坐標(biāo)系 Ocxcyczc到坐標(biāo)系 Otxtytzt的旋轉(zhuǎn)矩陣，t0平移向量。R和t0可通過組合導(dǎo)航系統(tǒng)的標(biāo)定參數(shù)直接獲得；s為ORB-SLAM系統(tǒng)的尺度因子，在初始化階段需要估計(jì)此參數(shù)的值。

由于系統(tǒng)測量誤差的存在，式(1)的左右兩邊是不等的，記殘差為

尺度因子s的估計(jì)問題轉(zhuǎn)化為求取最優(yōu)值s使得：

分別求取n個(gè)位置點(diǎn)的中心，記為

中心平移后的位置點(diǎn)記為

根據(jù)文獻(xiàn)[15]，滿足式(3)尺度因子s的最優(yōu)值為

3 ORB-SLAM/INS組合導(dǎo)航算法設(shè)計(jì)

求得尺度因子最優(yōu)值s后，ORB-SLAM/INS組合導(dǎo)航系統(tǒng)就進(jìn)入導(dǎo)航階段，通過式(1)把 ORB-SLAM得到的位置信息轉(zhuǎn)化到慣導(dǎo)系統(tǒng)的地理導(dǎo)航系中作為觀測量估計(jì)慣導(dǎo)系統(tǒng)的誤差狀態(tài)量。下面設(shè)計(jì)卡爾曼濾波模型估計(jì)慣導(dǎo)系統(tǒng)的誤差[12-13]。

選取15維狀態(tài)量，分別為姿態(tài)誤差、速度誤差、位置誤差、陀螺零位誤差、加速度計(jì)零位誤差：

狀態(tài)轉(zhuǎn)移方程為：

其中，子矩陣Fij各個(gè)分量值見文獻(xiàn)[14]。

以O(shè)RB-SLAM視覺導(dǎo)航位置作為Kalman濾波的量測值，量測方程為

估計(jì)慣導(dǎo)系統(tǒng)誤差狀態(tài)后，使用閉環(huán)反饋方式對慣性器件誤差、慣性導(dǎo)航誤差進(jìn)行修正，約束慣導(dǎo)誤差發(fā)散。

4 試驗(yàn)驗(yàn)證與分析

4.1 試驗(yàn)平臺搭建

搭建試驗(yàn)平臺對設(shè)計(jì)的單目視覺/INS組合導(dǎo)航算法進(jìn)行跑車試驗(yàn)，試驗(yàn)平臺如圖3所示。

試驗(yàn)所用的主要硬件設(shè)備型號及參數(shù)如下：

1）攝像頭

IDS工業(yè)相機(jī)：UI-1240LE-C-6；分辨率：1280×1024。

圖3 試驗(yàn)平臺Fig.3 Experimental platform

定焦鏡頭：UH0620-10M；視場角：54.1H°×41.9V°。

2）慣導(dǎo)系統(tǒng)

中星測控慣性測量單元，CS-IMU-07。陀螺測量范圍：≥±200 (°)/s；陀螺零偏：≤0.2 (°)/s；陀螺零偏穩(wěn)定性：≤0.015 (°)/s；加表測量范圍：≥±20g；加表零偏：≤0.05g。

3）差分GPS

Novatel公司，F(xiàn)lexPak6，定位精度為5 cm

4.2 試驗(yàn)過程

試驗(yàn)地點(diǎn)：煙臺市體育公園；試驗(yàn)時(shí)間：2016年8月9日；

試驗(yàn)過程：在120 m×120 m的空曠場地范圍內(nèi)進(jìn)行閉環(huán)跑車試驗(yàn)（無大面積遮擋，保證 GPS信號有效），共跑3圈，共耗時(shí)333 s。試驗(yàn)過程中采集攝像頭、慣性系統(tǒng)以及差分GPS的數(shù)據(jù)。

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

假設(shè)此范圍內(nèi)地理系是固定的，以試驗(yàn)開始時(shí)的東北天地理坐標(biāo)系為參考坐標(biāo)系O-XYZ，差分GPS的定位數(shù)據(jù)為基準(zhǔn)參考值，經(jīng)緯度信息轉(zhuǎn)換到參考坐標(biāo)系中，單目視覺SLAM /INS組合導(dǎo)航的結(jié)果也轉(zhuǎn)換到此坐標(biāo)系中，在參考坐標(biāo)系O-XYZ進(jìn)行定位誤差分析與比較。

單目視覺ORB-SLAM的試驗(yàn)結(jié)果如圖4所示，其中：圖4(a)顯示了特征點(diǎn)檢測過程，綠色方框代表當(dāng)前幀匹配上的FAST特征點(diǎn)；圖4(b)代表單目ORB-SLAM地圖構(gòu)建和定位過程，黑色和紅色點(diǎn)代表存儲的地圖點(diǎn)，藍(lán)色表示存儲的關(guān)鍵幀，綠色代表了軌跡。

圖4 單目視覺ORB-SLAM的試驗(yàn)結(jié)果Fig.4 Experiment result of monocular ORB-SLAM

單目視覺ORB-SLAM與基準(zhǔn)差分GPS的軌跡對比如圖5所示。純慣性導(dǎo)航的導(dǎo)航誤差如圖6所示。單目視覺ORB-SLAM的導(dǎo)航誤差如圖7所示。單目視覺/INS組合導(dǎo)航誤差如圖8所示。

圖5 ORB-SLAM與差分GPS的軌跡對比Fig.5 Trajectory comparison of ORB-SLAM and differential GPS

由圖5～8可知，純慣性導(dǎo)航結(jié)果隨時(shí)間發(fā)散，與純慣性導(dǎo)航相比，單目視覺ORB-SLAM/INS組合導(dǎo)航算法具有較高的定位精度。由數(shù)據(jù)統(tǒng)計(jì)分析可知，組合導(dǎo)航定位誤差為 1.162 m，因此，單目視覺ORB-SLAM/INS組合導(dǎo)航能夠?qū)崿F(xiàn)無GPS條件的導(dǎo)航與定位。

圖6 純慣性導(dǎo)航誤差Fig.6 Navigation error of pure INS

圖7 ORB-SLAM導(dǎo)航誤差Fig.7 Navigation error of ORB-SLAM

圖8 單目視覺ORB-SLAM/INS組合導(dǎo)航誤差Fig.8 Integrated navigation error of monocular ORB-SLAM/INS

5 結(jié) 論

本文把單目視覺 ORB-SLAM與慣性導(dǎo)航系統(tǒng)相結(jié)合，提出了單目視覺 ORB-SLAM/INS組合導(dǎo)航算法，并設(shè)計(jì)了試驗(yàn)平臺對所提出的算法進(jìn)行了驗(yàn)證。結(jié)果表明，所設(shè)計(jì)的 ORB-SLAM/INS組合導(dǎo)航系統(tǒng)是切實(shí)可行的，具有較高的定位精度。

目前提出的組合導(dǎo)航算法處于淺組合階段，下一步將開展單目視覺 ORB-SLAM/INS深組合導(dǎo)航算法研究，進(jìn)一步提高導(dǎo)航精度和魯棒性，主要體現(xiàn)在以下兩個(gè)方面：

1）ORB-SLAM系統(tǒng)進(jìn)行特征點(diǎn)的跟蹤時(shí)，利用慣性導(dǎo)航系統(tǒng)的姿態(tài)信息進(jìn)行特征點(diǎn)的運(yùn)動估計(jì)從而提高特征點(diǎn)匹配的速度和準(zhǔn)確度；

2）當(dāng)ORB-SLAM系統(tǒng)跟蹤失敗時(shí)，利用INS導(dǎo)航系統(tǒng)輸出的位置和姿態(tài)信息作為初始狀態(tài)進(jìn)行ORB-SLAM系統(tǒng)的重啟動，從而提高了ORB-SLAM系統(tǒng)的魯棒性。

（References）：

[1]Strasdat H, Montiel J M M, Davison A J. Visual SLAM:why filter[J]. Image and Vision Computing, 2012, 30(2):65-77.

[2]Klein G, Murray D. Parallel tracking and mapping for small AR workspaces[C]//2007 IEEE and ACM International Symposium on Mixed and Augmented Reality(ISMAR). 2007: 225-234.

[3]Pirker K, Ruther M, Bischof H. CD SLAM-continuous localization and mapping in a dynamic world[C]//2011 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). 2011: 3990-3997.

[4]Song S, Chandraker M, Guest C C. Parallel, real-time monocular visual odometry[C]//2013 IEEE International Conference on Robotics and Automation (ICRA). 2013:4698-4705.

[5]Engel J, Schops T, Cremers D. LSD-SLAM: Large-scale direct monocular SLAM[C]//2014 European Conference on Computer Vision (ECCV). 2014: 834–849.

[6]Mur-Artal R, Tardos J D. Fast relocalisation and loop closing in keyframe-based SLAM[C]//2014 IEEE International Conference on Robotics and Automation (ICRA).2014: 846-853.

[7]Mur-Artal R, Montiel J M M, Tardos J D. ORB-SLAM: a Versatile and Accurate Monocular SLAM System[J].IEEE transactions on Robotics, 2015, 1:16.

[8]Rosten E, Drummond T. Machine learning for high-speed corner detection[C]//2006 European Conference on Computer Vision. 2006: 430-443.

[9]Calonder M, Lepetit V, Strecha C, et al. BRIEF: Binary robust independent elementary features[C]//2010 European Conference on Computer Vision. 2010: 778-792.

[10]Rublee E, Rabaud V, Konolige K, et al. ORB: an efficient alternative to SIFT or SURF[C]//IEEE International Conference on Computer Vision (ICCV). Barcelona, Spain,2011: 2564-2571.

[11]Galvez-Lopez D, Tardos J D. Bags of binary words for fast place recognition in image sequences[J]. IEEE Transactions on Robotics, 2012, 28(5): 1188-1197.

[12]王云舒, 劉建業(yè), 曾慶化, 等. 結(jié)構(gòu)光輔助的慣性/視覺室內(nèi)導(dǎo)航三維環(huán)境重構(gòu)方法[J]. 中國慣性技術(shù)學(xué)報(bào),2016, 24(1): 53-58.Wang Yun-shu, Liu Jian-ye, Zeng Qing-hua, et al. 3D environment restructure method with structured light for indoor vision/inertial navigation[J]. Journal of Chinese Inertial Technology, 2016, 24(1): 53-58.

[13]張紅良, 郭鵬宇, 李壯. 一種視覺輔助的慣性導(dǎo)航系統(tǒng)動基座初始對準(zhǔn)方法[J]. 中國慣性技術(shù)學(xué)報(bào), 2014,22(4): 470-475.Zhang Hong-liang, Guo Peng-yu, Li Zhuang. Vision aided alignment method for inertial navigation system on moving base[J]. Journal of Chinese Inertial Technology,2014, 22(4): 470-475.

[14]劉剛. 基于Kalman跟蹤環(huán)路的GNSS/INS標(biāo)量深組合系統(tǒng)研究[D]. 北京: 清華大學(xué), 2015.Liu Gang. GNSS/INS scalar deeply integrated system research based on Kalman tracking loop[D]. Beijing:Tsinghua University, 2015.

Integrated navigation method of monocular ORB-SLAM/INS

ZHOU Shao-lei1, WU Xiu-zhen1, LIU Gang1, ZHANG Rong2, XU Hai-gang3
(1. Department of Control Engineering, Naval Aeronautical and Astronautical University, Yantai 264001, China;2. Department of Precision Instrument, Tsinghua University, Beijing 100084, China;3. Beijing Institute of Automatic Control Equipment, Beijing 100074, China)

In order to solve such problem that an integrated INS/GNSS navigation system stops working in satellite failure situation, a deeply-integrated navigation method of monocular ORB-SLAM/INS is proposed to extend the application range of the integrated navigation system in severe interference environment and indoor circumstances. The method can be divided into two phases: in initialization phase, an algorithm is designed to estimate the scale factor online when the ORB-SLAM forms a closed loop; in navigation phase,the error of INS is corrected after the error states of INS are estimated through a Kalman filter whose observations is ORB-SLAM positions. The hardware and software platforms are designed to test the proposed integrated navigation method. Field test results indicate that the position accuracy of the integrated navigation system is satisfactory, with 1.162 m mean error in 6 min and no drift with time, showing that the proposed method has a notable application value.

ORB-SLAM; INS; scale factor; Kalman filter; integrated navigation

U666.1

A

1005-6734(2016)05-0633-05

10.13695/j.cnki.12-1222/o3.2016.05.013

2016-07-09；

2016-09-28

國家自然科學(xué)基金（61004002）

周紹磊（1963—），男，教授，博士生導(dǎo)師，從事導(dǎo)航、制導(dǎo)與控制方面研究。

聯(lián) 系 人：吳修振（1988—），男，博士研究生，從事導(dǎo)航、制導(dǎo)與控制方面研究。E-mail: wxz_lucky@163.com