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

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

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

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

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

針對(duì)慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)在衛(wèi)星導(dǎo)航失效時(shí)無(wú)法使用的問(wèn)題，提出了單目視覺(jué)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ì)算法在線(xiàn)估計(jì)單目視覺(jué)ORB-SLAM算法的尺度因子；導(dǎo)航階段，ORB-SLAM系統(tǒng)輸出的位置信息經(jīng)過(guò)尺度變換后作為觀(guān)測(cè)量進(jìn)行卡爾曼濾波，估計(jì)INS導(dǎo)航系統(tǒng)的誤差狀態(tài)量從而修正慣導(dǎo)系統(tǒng)的誤差。設(shè)計(jì)了硬件和軟件平臺(tái)對(duì)提出的組合導(dǎo)航方法進(jìn)行試驗(yàn)驗(yàn)證。跑車(chē)實(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)航

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

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

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

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

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

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

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

1 單目視覺(jué)ORB-SLAM算法

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

圖1 單目視覺(jué)ORB-SLAM算法Fig.1 Monocular ORB-SLAM algorithm

1）通過(guò)張正友棋盤(pán)標(biāo)定法對(duì)攝像頭的畸變參數(shù)進(jìn)行標(biāo)定，用標(biāo)定好的參數(shù)對(duì)每一幀圖像進(jìn)行畸變修正，然后對(duì)圖像進(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]對(duì)特征點(diǎn)空間進(jìn)行分類(lèi)，分為M×N層，通過(guò)漢明距離就可以把提取的ORB特征點(diǎn)在空間中進(jìn)行分類(lèi)。

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

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

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

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

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

單目視覺(jué)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ī)位置為，對(duì)應(yīng)差分 GPS輸出真實(shí)的位置信息，理想情況下，根據(jù)相似變換的原理，則有

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

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

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

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

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

根據(jù)文獻(xiàn)[15]，滿(mǎn)足式(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)航階段，通過(guò)式(1)把 ORB-SLAM得到的位置信息轉(zhuǎn)化到慣導(dǎo)系統(tǒng)的地理導(dǎo)航系中作為觀(guān)測(cè)量估計(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è)分量值見(jiàn)文獻(xiàn)[14]。

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

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

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

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

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

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

1）攝像頭

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

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

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

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

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

3）差分GPS

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

4.2 試驗(yàn)過(guò)程

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

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

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

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

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

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

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

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

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

5 結(jié) 論

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

目前提出的組合導(dǎo)航算法處于淺組合階段，下一步將開(kāi)展單目視覺(jué) 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)動(dòng)估計(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)的重啟動(dò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)光輔助的慣性/視覺(jué)室內(nèi)導(dǎo)航三維環(huán)境重構(gòu)方法[J]. 中國(guó)慣性技術(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]張紅良, 郭鵬宇, 李壯. 一種視覺(jué)輔助的慣性導(dǎo)航系統(tǒng)動(dòng)基座初始對(duì)準(zhǔn)方法[J]. 中國(guó)慣性技術(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

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

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

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