林連秀,葉 蕓,姚劍敏,郭太良
(福州大學(xué) 物理與信息工程學(xué)院, 福建 福州 350002)
基于ORB-SLAM的移動機(jī)器人嵌入式實(shí)現(xiàn)與優(yōu)化*
林連秀,葉 蕓,姚劍敏,郭太良
(福州大學(xué) 物理與信息工程學(xué)院, 福建 福州 350002)
ORB-SLAM(基于ORB特征識別的同時定位與地圖構(gòu)建系統(tǒng))的源代碼無法在嵌入式開發(fā)板運(yùn)行,其構(gòu)建的點(diǎn)云圖太稀疏無法滿足移動機(jī)器人路徑規(guī)劃要求。針對這個問題,文章提出將ORB-SLAM進(jìn)行改進(jìn)與優(yōu)化,移植到嵌入式開發(fā)板完成SLAM過程。首先,刪除原PC端Linux系統(tǒng)下的軌跡、點(diǎn)云圖、一些依賴庫,保留并改進(jìn)src和include文件夾下大部分C++代碼;其次,在嵌入式平臺以JNI調(diào)用方式調(diào)用改進(jìn)后的C++代碼,增加OpenCV、g2o、DBoW2、Eigen等依賴庫;最后根據(jù)處理后的關(guān)鍵幀連接繪制柵格地圖,完成實(shí)時軌跡顯示和地圖構(gòu)建。實(shí)驗(yàn)結(jié)果表明,通過移植實(shí)現(xiàn)了在嵌入式開發(fā)板進(jìn)行SLAM過程,硬件配置要求和成本大大降低,所構(gòu)建的柵格地圖占存量更小且更直觀反映實(shí)際環(huán)境的布局情況,地圖誤差控制在±0.5 m的較高精度范圍內(nèi),較大程度地提高了SLAM性能。
ORB-SLAM;嵌入式;移植;柵格地圖
同時定位與地圖構(gòu)建(SLAM)技術(shù)是移動機(jī)器人在未知環(huán)境中,通過對環(huán)境的觀測,遞增地構(gòu)建環(huán)境地圖,并通過構(gòu)建的環(huán)境地圖實(shí)現(xiàn)對機(jī)器人的定位,它是實(shí)現(xiàn)機(jī)器人自主導(dǎo)航和定位的關(guān)鍵技術(shù)。目前實(shí)現(xiàn)SLAM的算法很多,其中ORB-SLAM以追蹤效果好、地圖精度高、定位穩(wěn)定性好等優(yōu)勢脫穎而出,成為研究熱點(diǎn)。
ORB-SLAM算法由Raúl M A等人于2015年發(fā)表在IEEE Transactions on Robotics[1],且在PC的Linux系統(tǒng)上開放了開源ORB-SLAM系統(tǒng)。采用該ORB-SLAM開源算法,在PC Linux系統(tǒng)上較好實(shí)現(xiàn)了SLAM過程,但等同于PC的硬件配置需求成本高,對移動機(jī)器人普通嵌入式開發(fā)板不適用[2]。ORB-SLAM算法主要由追蹤、地圖構(gòu)建、閉環(huán)控制3個線程組成,其中地圖構(gòu)建部分平均處理每關(guān)鍵幀約385 ms,相對比較耗時,具備較大優(yōu)化空間[3]。采用ORB-SLAM開源算法所構(gòu)建的地圖是用點(diǎn)云圖方式顯示,點(diǎn)云很稀疏,不容易看出所構(gòu)建的室內(nèi)結(jié)構(gòu)[4]。對移動機(jī)器人地圖要求上,柵格地圖表示法優(yōu)于點(diǎn)云圖表示法,一方面可以大大節(jié)省內(nèi)存空間,另一方面也便于后續(xù)路徑規(guī)劃過程中進(jìn)行碰撞檢測和空間可行性分析[5]。從硬件開發(fā)成本考慮,如若用PC進(jìn)行移動機(jī)器人開發(fā),顯然只用到PC很小部分功能,造成功能浪費(fèi)、成本偏高,而嵌入式開發(fā)板有著低功耗、低成本、適當(dāng)?shù)腃PU運(yùn)算效能與友善的軟件接口等優(yōu)勢[6]。
針對以上ORB-SLAM源代碼只能運(yùn)行在PC而無法在嵌入式開發(fā)板上運(yùn)行,且其構(gòu)建的點(diǎn)云圖太稀疏無法滿足移動機(jī)器人路徑規(guī)劃要求的問題,本文對此進(jìn)行了較大改進(jìn),保留ORB-SLAM部分源碼并做出修改,移植到嵌入式開發(fā)平臺,修改地圖構(gòu)建中的耗時部分,增加基于柵格地圖實(shí)時軌跡顯示部分。移植到普通嵌入式開發(fā)板后的ORB-SLAM,不僅大大降低了對硬件平臺的要求,還完成了基于柵格地圖的實(shí)時軌跡顯示,使地圖能清晰了然地反映實(shí)際環(huán)境的布局情況,且所建地圖控制在較高精度范圍內(nèi)。
1.1 總設(shè)計流程的改進(jìn)
本文將原PC端Linux系統(tǒng)下的ORB-SLAM進(jìn)行了移植和改進(jìn),最終在Android平臺完成了基于柵格地圖的軌跡繪制即完成了實(shí)時地圖構(gòu)建,改進(jìn)后的總設(shè)計流程如圖1所示。
在上述流程圖模塊中,本文主要修改的部分是:修改所保留的ORB-SLAM原src和include文件夾下的C++代碼;在LOCAL MAPPING部分中進(jìn)行了大部分刪減,只做優(yōu)化部分;刪除了原實(shí)時點(diǎn)云及軌跡顯示部分,在嵌入式平臺添加GRID MAP部分即基于柵格地圖的實(shí)時軌跡顯示部分。
1.2 ORB-SLAM源碼的修改
ORB-SLAM算法的實(shí)現(xiàn)主要在src和include文件夾下,完成追蹤、地圖構(gòu)建、閉環(huán)控制和重定位等任務(wù)。本文對源碼所作的修改工作主要有:刪除MapDrawer.cc和Viewer.cc代碼即刪除軌跡、點(diǎn)云圖顯示部分的代碼;刪除Linux下依賴復(fù)雜的ROS系統(tǒng)部分的代碼;更換不同系統(tǒng)下的OpenCV、g2o、DBoW2、Eigen等庫;修改輸入接口,以便直接連接攝像頭在線使用;注釋掉部分代碼,包含保存當(dāng)前幀圖片、點(diǎn)云的代碼等。
1.3 LOCAL MAPPING部分的改進(jìn)
LOCAL MAPPING線程用于處理新的幀,更新優(yōu)化它們本地鄰近的幀。它對關(guān)鍵幀作確定新的地圖云點(diǎn)、更新局部區(qū)域、局部偏差調(diào)整、去除冗余關(guān)鍵幀等工作。LOCAL MAPPING在整個系統(tǒng)中耗時相對較長,其平均處理每關(guān)鍵幀要用時約385 ms,而像TRACKING平均處理每幀只要約30 ms??紤]到一方面LOCAL MAPPING相對耗時,一方面這部分作刪減后精度不會受到很大影響,這為移植到嵌入式平臺提供了一個可行之處。所以本文對LOCAL MAPPING部分中的Triangulate ORB、Update Local Area、Prune Redundant KeyFrame進(jìn)行了刪減,只做優(yōu)化部分,主要部分的設(shè)計如圖2所示。
1.4 GRID MAP部分的設(shè)計
機(jī)器人地圖包括幾何地圖、點(diǎn)云地圖、拓?fù)涞貓D和柵格地圖等,其中原ORB-SLAM采用點(diǎn)云地圖來表示,其所構(gòu)建的點(diǎn)云圖中點(diǎn)云很稀疏,完全不能看出任何結(jié)構(gòu)。而柵格地圖將環(huán)境分解成一系列具有二值信息的網(wǎng)格單元,對環(huán)境描述直觀清晰,適當(dāng)調(diào)整柵格的大小既可以降低存儲消耗,又保留環(huán)境的空間結(jié)構(gòu),同時也便于后續(xù)路徑規(guī)劃過程中進(jìn)行碰撞檢測和空間可行性分析,所以本文去除點(diǎn)云圖表示法而改用柵格地圖表示法。本文采用的柵格地圖用黑格表示障礙物,白格表示無障礙物可通過區(qū)。GRID MAP主要部分的設(shè)計如圖3所示。
由設(shè)計圖3可知,本文設(shè)計的顯示端顯示三個部分:一是USE_CAMERA_INPUT,顯示攝像頭采集到的當(dāng)前圖像,未經(jīng)過任何處理;二是DRAW_FRAME,對當(dāng)前圖像幀進(jìn)行3D ORB特征點(diǎn)的提取與匹配,并在窗口顯示特征點(diǎn)追蹤情況;三是DRAW_GRID_MAP,繪制最終處理后的所有KeyFrames,追蹤當(dāng)前KeyFrames和CameraCenter。采用柵格地圖表示法將各KeyFrames用線連起來,繞室內(nèi)全覆蓋運(yùn)行即可完成最終室內(nèi)地圖的繪制,地圖以矢量形式存儲供移動機(jī)器人合理規(guī)劃路徑和重定位之用。
2.1 實(shí)驗(yàn)硬件配置對比
實(shí)驗(yàn)硬件配置為PC和自制的移動機(jī)器人模型,其中移動機(jī)器人模型主要用到的模塊有基于Android平臺的開發(fā)板、單目攝像頭、電機(jī),PC和開發(fā)板參數(shù)如表1所示。PC系統(tǒng)擁有很強(qiáng)的運(yùn)算能力,但通常只用到小部分,倘若用于機(jī)器人開發(fā)而配置一個PC,不僅眾多未開發(fā)資源被浪費(fèi),且其在購買成本、功耗性、便攜性、集成性方面都具有較大劣勢。而嵌入式開發(fā)板具有低功耗、低成本、適當(dāng)?shù)腃PU運(yùn)算效能與友善的軟件接口等優(yōu)點(diǎn),被稱之為低價計算機(jī)。對比表1參數(shù),將ORB-SLAM從PC移植到嵌入式開發(fā)板上實(shí)現(xiàn),則大大降低了對硬件配置的要求,成本、功耗也大大降低,縮小了機(jī)器人體積,方便攜帶。
實(shí)驗(yàn)選用的單目攝像頭為小蟻攝像頭,采用 “張正友”標(biāo)定法原理,標(biāo)定模板采用包含12×12的正方塊的棋盤格,每個棋盤格大小為30 mm×30 mm,在MATLAB下運(yùn)行程序載入20張不同角度、像素為640×480的棋盤格圖片進(jìn)行相機(jī)標(biāo)定,標(biāo)定結(jié)果如表2所示。
2.2 移植后的ORB-SLAM運(yùn)行結(jié)果分析
本文建立的柵格地圖為640×960像素的地圖,占用柵格大小為30,機(jī)器人實(shí)際寬度為0.15 m,如圖4所示,白色部分為移動機(jī)器人繞室內(nèi)一圈的軌跡,完整柵格地圖即為移動機(jī)器人繞室內(nèi)全覆蓋移動,最終白色部分表示可通過區(qū),黑色部分表示障礙物區(qū)。由表3可知,地圖軌跡點(diǎn)與實(shí)際標(biāo)定點(diǎn)的誤差最大為0.539 m,其軌跡誤差一般能控制在±0.5 m的較小范圍內(nèi),約為地圖尺寸的0.78%,相比原PC Linux系統(tǒng)實(shí)現(xiàn)的軌跡誤差約為地圖尺寸的1%,誤差并未擴(kuò)大反而有所縮小,即定位精度得到提高。
2.3 原基于點(diǎn)云圖的ORB-SLAM與基于柵格地圖的ORB-SLAM的地圖對比
圖5為修改前所構(gòu)建的點(diǎn)云圖,圖中軌跡由相機(jī)位姿估計點(diǎn)構(gòu)成,左下圖為相機(jī)實(shí)時采集的圖像并對特征點(diǎn)進(jìn)行提取與匹配。圖6為修改后所構(gòu)建的柵格地圖,左上圖為相機(jī)采集的原圖,左下圖為特征點(diǎn)提取與匹配圖,右圖為柵格地圖。首先從占存量對比分析,移動機(jī)器人按同樣路線繞室內(nèi)一圈構(gòu)建一張1 280×960的地圖,原Linux系統(tǒng)運(yùn)行的ORB-SLAM所建的點(diǎn)云圖占存量約為4 940 KB,而移植后的柵格地圖占存約98 KB,相差大約50倍的耗存量,可見使用柵格地圖表示法在節(jié)約存儲空間上具有非常大的優(yōu)勢。再從直觀性對比分析,原Linux系統(tǒng)運(yùn)行的ORB-SLAM所建的點(diǎn)云圖非常稀疏,只含有稀疏的地圖特征點(diǎn)來構(gòu)成軌跡,看不出實(shí)際空間結(jié)構(gòu)與布局,這稀疏的地圖對于機(jī)器人下一步的應(yīng)用會造成很大的困難。而基于柵格地圖繪制的ORB-SLAM地圖,每個柵格可直接映射到現(xiàn)實(shí)環(huán)境的某個區(qū)域,白格表示已經(jīng)運(yùn)行的可通過區(qū),黑格表示障礙物區(qū),機(jī)器人繞室內(nèi)全覆蓋一次即可直觀地得到布局情況,其所存儲的障礙信息非常便于移動機(jī)器人進(jìn)行動態(tài)避障以及路徑規(guī)劃。
2.4 移植前后運(yùn)行指標(biāo)對比
先對原ORB-SLAM與移植后ORB-SLAM代碼運(yùn)行情況進(jìn)行對比分析,本文移植過程中在同一臺PC上安裝了WIN7+Ubuntu雙系統(tǒng)用于開發(fā)ORB-SLAM,將ORB-SLAM代碼在同一臺PC上運(yùn)行,原Linux系統(tǒng)CPU占用率高達(dá)約180%,移植到WIN7后CPU占用率約48%,可知原ORB-SLAM因?yàn)橐蕾囈恍㎜inux系統(tǒng)自帶庫使運(yùn)行速度得到限制。再對LOCAL MAPPING部分運(yùn)行速率進(jìn)行對比,原ORB-SLAM的LOCAL MAPPING部分主要包含KeyFrame Insertion、Map Point Cullin、Map Point Creation、Local BA、KeyFrame Culling,總體運(yùn)行時間約為464.27 ms。優(yōu)化后,通過減小與字典的匹配次數(shù),減小關(guān)鍵幀重新計算的次數(shù),將總體運(yùn)行時間提高到約280.50 ms,刪減ORB-SLAM的LOCAL MAPPING部分速率得到40%的提升,較大程度地簡化了移植工作。
本文提出并實(shí)現(xiàn)了將ORB-SLAM通過改進(jìn)與優(yōu)化,最終移植到嵌入式開發(fā)板完成SLAM過程,CPU占用率和運(yùn)行速率得到提升,大大降低了硬件成本和配置要求。針對原點(diǎn)云圖太稀疏無法滿足移動機(jī)器人路徑規(guī)劃要求的問題,改用柵格地圖表示法,從而使地圖占存量縮小了約98%,且更直觀地反映了實(shí)際環(huán)境的布局情況。通過移植,最終的軌跡誤差一般能控制在±0.5 m的較小范圍內(nèi),相比原軌跡誤差得到了縮小。這種改進(jìn)技術(shù)不只適用于移動機(jī)器人,同樣也適用于像VR/AR那樣需要用到嵌入式開發(fā)板來完成定位的技術(shù)領(lǐng)域。
[5] 張彪,曹其新,王雯珊. 使用三維柵格地圖的移動機(jī)器人路徑規(guī)劃[J]. 西安交通大學(xué)學(xué)報,2013,47(10): 57-61.
[6] 金麗科技.讓嵌入式Linux變身為低價計算機(jī)(上)[J]. 電子與電腦,2007,14(5):61-64.
Embedded implementation and optimization of mobile robot based on ORB-SLAM
Lin Lianxiu,Ye Yun,Yao Jianmin,Guo Tailiang
(College of Physics and Information Engineering, Fuzhou University, Fuzhou 350002, China)
The source code of ORB-SLAM (Simultaneous localization and mapping system based on the ORiented Brief feature recognition) can’t be run on the embedded development board, and construction of the point cloud is too sparse to meet the requirements of mobile robot path planning.To solve this problem, the ORB-SLAM was improved and optimized, and then transplanted to the SLAM process in the embedded development board. Firsty, the trajectory, point cloud, and some dependency database in original Linux system under the PC were deleted. Then the most of the C++ code in src and include folder were retained and improved. Subsequently, the improved C++ code were called by JNI method in the embedded platform, and OpenCV, g2o, DBoW2, Eigen, and other dependent libraries were increased. Finally, according to the key frame connected to the grid map, the real-time track display and map building was completed. The results showed that the SLAM process, hardware configuration requirements and cost of the hardware configuration were greatly reduced by transplanting the embedded development board, and the construction of the grid map of the stock was smaller and more intuitive to reflect the actual layout of the environment. The construction chart error was controlled in the high precision of ±0.5 m similarly, which greatly improved the performance of SLAM.
ORB-SLAM; embedded; transplantation; grid map
福建省自然科學(xué)基金(2014J01236);福建省科技重大專項(xiàng)(2014HZ0003-1);福建省資助省屬高校專項(xiàng)課題(JK2014003).
TP242
A
10.19358/j.issn.1674- 7720.2017.05.016
林連秀,葉蕓,姚劍敏,等.基于ORB-SLAM的移動機(jī)器人嵌入式實(shí)現(xiàn)與優(yōu)化[J].微型機(jī)與應(yīng)用,2017,36(5):50-53.
2016-11-08)
林連秀(1990-),女,碩士研究生,主要研究方向:嵌入式系統(tǒng)。
葉蕓(1977-),女,研究員,主要研究方向:光電顯示材料與器件。
姚劍敏(1978-),男,副研究員,主要研究方向:電路與系統(tǒng)。