基于數(shù)據(jù)融合的SLAM系統(tǒng)研究與路徑規(guī)劃實(shí)現(xiàn)

1、    基于數(shù)據(jù)融合的slam系統(tǒng)研究與路徑規(guī)劃實(shí)現(xiàn)    賴秋玲禹素萍丁紳一摘要：移動(dòng)機(jī)器人同時(shí)定位與建圖（slam- simultaneous locating and mapping）與路徑規(guī)劃是實(shí)現(xiàn)智能化機(jī)器人的前提。該系統(tǒng)首先通過hcr（home care robot）機(jī)器人搭載激光雷達(dá)和rgb-d傳感器，在ros的運(yùn)行環(huán)境下，分別實(shí)現(xiàn)基于orb-slam2和rgb-d slam算法框架的同時(shí)定位與建圖，獲取三維點(diǎn)云圖。然后將三維點(diǎn)云圖與激光雷達(dá)生成的二維平面圖進(jìn)行數(shù)據(jù)融合，得到含有較豐富環(huán)境信息的環(huán)境二維柵格地圖，并利用a*算法路徑搜索算法，

2、對(duì)機(jī)器人進(jìn)行路徑規(guī)劃。實(shí)驗(yàn)結(jié)果表明融合了環(huán)境三維點(diǎn)云信息的地圖，更適合用于路徑規(guī)劃。關(guān)鍵詞：同時(shí)定位與建圖；rgb-d傳感器；激光雷達(dá)；路徑規(guī)劃：tp242 ：a ：1009-3044（2017）33-0024-03abstract： mobile robot simultaneous locating and mapping （slam） and path planning are the prerequisites for realizing intelligent robots. in this system， the hcr （home care robot） robot was eq

3、uipped with lidar and an rgb-d sensor. in the ros operating environment， the slam based on the orb-slam2 and rgb-d slam algorithms were implemented respectively and three - dimensional point cloud map was generated. then， the three - dimensional point cloud map was combined with the two - dimensiona

4、l map generated by the lidar to get a grid map with the rich environment information. finally， the a * algorithm path search algorithm was applied to plan the path for the robot. experiment results show that the grid map which integrates the three-dimensional cloud information is more suitable for p

5、ath planning.key words： slam； rgb-d sensor； lidar； path planning1 概述同時(shí)定位與建圖（simultaneous localization and mappingslam）是移動(dòng)機(jī)器人領(lǐng)域的熱門研究課題1。文獻(xiàn)2已對(duì)現(xiàn)有的slam算法做了較全面的總結(jié)。slam的發(fā)展主要分為兩個(gè)階段：第一階段為經(jīng)典階段（1986-2004），在這個(gè)階段學(xué)者們?yōu)閷?shí)現(xiàn)slam提出了基于貝葉斯的濾波器方法，包括卡爾曼濾波（kalman filtering）、擴(kuò)展卡爾曼濾波（extended kalman filtering）3、rao-blackwell

6、ized粒子濾波4等。第二階段為算法分析階段（2004-2016），該階段主要對(duì)slam的基本性質(zhì)進(jìn)行研究，包括可觀察性、收斂性和一致性。在此時(shí)期，人們理解了稀疏性5對(duì)高效地實(shí)現(xiàn)slam的關(guān)鍵作用，并開源了較多的slam庫，例如rgb-d slam和orb-slam。orb-slam26是目前較優(yōu)秀的稀疏性開源slam框架，它優(yōu)化了orb-slam7，支持標(biāo)定后的rgb-d相機(jī)。隨著傳感器技術(shù)的發(fā)展，rgb-d相機(jī)、激光雷達(dá)等傳感器簡(jiǎn)化了slam的實(shí)現(xiàn)難度，而傳感器信息融合8也不斷地被應(yīng)用到機(jī)器視覺領(lǐng)域。因?yàn)榧す饫走_(dá)僅能檢測(cè)到一定高度的平面信息，所以靠單一的激光雷達(dá)，并不能得到更全面的環(huán)境信息

7、，而rgb-d相機(jī)則具有較大的視角，可以獲取更豐富的環(huán)境信息。因此該系統(tǒng)利用rgb-d相機(jī)，構(gòu)建基于orb-slam2和rgb-d slam算法框架的slam系統(tǒng)，對(duì)比兩種經(jīng)典slam算法，將較好的三維點(diǎn)云圖與激光雷達(dá)平面圖進(jìn)行環(huán)境信息融合，并在此融合地圖上實(shí)現(xiàn)機(jī)器人路徑規(guī)劃。2 軟硬件平臺(tái)與系統(tǒng)架構(gòu)該系統(tǒng)的軟件部分使用ros（robot operating system）系統(tǒng)9，機(jī)器人為hcr（home care robot）機(jī)器人。電腦配置為intel core i7處理器，操作系統(tǒng)為ubuntu14.04。軟件系統(tǒng)結(jié)構(gòu)如圖1所示。軟件系統(tǒng)結(jié)構(gòu)的主要分為以下三部分：（1） 解算機(jī)器人位姿：

8、由rgb-d相機(jī)采集環(huán)境的彩色和深度信息，經(jīng)由orb-slam2算法或者rgb-d slam算法，解算出機(jī)器人的當(dāng)前位姿xmymm。（2） 獲得環(huán)境地圖：通過orb-slam2算法或者rgb-d slam算法，拼接融合關(guān)鍵幀生成環(huán)境三維點(diǎn)云圖，通過ros的第三方庫octomap10得到三維網(wǎng)格地圖。octomap用八叉樹來存儲(chǔ)環(huán)境信息，遍歷八叉樹，將概率大于0.5的全部方塊投影得到二維平面地圖。然后將此平面地圖與激光雷達(dá)生成的地圖進(jìn)行或操作，得到融合后的二維柵格地圖。（3） 實(shí)現(xiàn)路徑規(guī)劃：將解算出的當(dāng)前位姿和機(jī)器人模型導(dǎo)入到生成的二維地圖中，將當(dāng)前位姿作為起始點(diǎn)，確定一個(gè)目標(biāo)點(diǎn)，通過a*算法，

9、搜索出一條路徑，并生成機(jī)器人控制命令。endprint2.1 基于orb-slam2和rgb-d slam算法框架的實(shí)時(shí)定位與建圖隨著傳感器技術(shù)不斷地發(fā)展，各類slam算法框架也應(yīng)運(yùn)而生。orb-slam2和rgb-d slam是近幾年較受關(guān)注的slam算法框架。orb-slam2是目前較優(yōu)秀的slam算法系統(tǒng)框架，如圖2所示，該系統(tǒng)主要由三個(gè)平行線程組成。（1） 相機(jī)位姿跟蹤線程：首先對(duì)輸入幀進(jìn)行預(yù)處理。然后相機(jī)位姿跟蹤，若不插入關(guān)鍵幀則不啟動(dòng)局部地圖線程。局部地圖跟蹤是將當(dāng)前幀與局部地圖進(jìn)行匹配，利用最小化投影誤差來優(yōu)化位姿。最后依據(jù)是否很長(zhǎng)時(shí)間沒有插入關(guān)鍵幀或者跟蹤失敗等因素來判斷是否生

10、成關(guān)鍵幀。（2） 局部地圖線程：該線程主要是負(fù)責(zé)處理新生成的關(guān)鍵幀，剔除地圖中新添加的質(zhì)量不高的地圖點(diǎn)。而后進(jìn)行地圖點(diǎn)的融合并進(jìn)行局部ba（bundle adjustment）11優(yōu)化，最后將那些地圖點(diǎn)超過90%以上能被其他至少3個(gè)關(guān)鍵幀共同檢測(cè)到的關(guān)鍵幀進(jìn)行剔除，以減少重復(fù)率，節(jié)省存儲(chǔ)空間。（3） 閉環(huán)檢測(cè)及矯正線程：利用基于dbow212的場(chǎng)景識(shí)別進(jìn)行閉環(huán)檢測(cè)，而后通過優(yōu)化位姿圖來實(shí)現(xiàn)閉環(huán)矯正。在閉環(huán)檢測(cè)及矯正線程結(jié)束之后，觸發(fā)第四個(gè)線程進(jìn)行全局的ba優(yōu)化，并更新地圖。該算法利用orb13特征點(diǎn)完成跟蹤，建圖，以及場(chǎng)景識(shí)別的任務(wù)。這些orb特征點(diǎn)對(duì)于旋轉(zhuǎn)和尺度變換有較強(qiáng)魯棒性，不受光線變換

11、的影響并且提取速度較快，可以滿足實(shí)時(shí)性的需求。而rgb-d slam14主要由前端與后端兩部分組成，如圖3所示，前端部分處理深度相機(jī)獲取的環(huán)境信息，解算出環(huán)境信息的空間幾何位置，后端用g2o庫將前端部分得到的機(jī)器人位姿進(jìn)行優(yōu)化。最終得到環(huán)境三維點(diǎn)云圖和機(jī)器人位姿。2.2 基于a*算法的路徑規(guī)劃該系統(tǒng)采用a*算法來對(duì)機(jī)器人進(jìn)行路徑規(guī)劃。a*算法15是一種啟發(fā)式的算法，它被廣泛地用于解決各類搜索問題，例如機(jī)器人路徑規(guī)劃，網(wǎng)絡(luò)游戲以及uav（unmanned aerial vehicles）避障。a*算法的基本思想是結(jié)合dijkstra算法所用的離起始點(diǎn)最近的頂點(diǎn)信息和greedy best-fir

12、st search算法采用的離目標(biāo)點(diǎn)最近的頂點(diǎn)信息。通常用公式（1）來說明a*算法，g（n）表示從起始點(diǎn)到任意頂點(diǎn)n的實(shí)際消耗，即走的路徑長(zhǎng)度，h（n）表示從頂點(diǎn)n到目標(biāo)點(diǎn)的啟發(fā)式預(yù)測(cè)消耗。在每一次循環(huán)中，算法找到一個(gè)n使得f（n）最小，從而搜索到目標(biāo)點(diǎn)的最小消耗，即最短路徑。fn=gn+h（n） （1）3 實(shí)驗(yàn)結(jié)果與分析3.1 室內(nèi)orb-slam2，rgb-d slam與激光雷達(dá)建圖結(jié)果通過orb-slam2算法框架和rgb-d slam算法，實(shí)驗(yàn)結(jié)果如圖4所示，（a）為實(shí)驗(yàn)場(chǎng)景圖，（b） 激光雷達(dá)生成的二維平面圖，（c）為orb-slam2生成的三維點(diǎn)云圖， （d）為rgb-d slam

13、生成的三維點(diǎn)云圖。從實(shí)驗(yàn)結(jié)果看出，orb-slam2算法框架的閉環(huán)檢測(cè)效果優(yōu)于rgb-d slam算法。rgb-d slam算法通過計(jì)算關(guān)鍵幀中的特征點(diǎn)描述子來進(jìn)行回環(huán)檢測(cè)，而orb-slam2是通過基于dbow2的場(chǎng)景識(shí)別來進(jìn)行閉環(huán)檢測(cè)，而且具有閉環(huán)修正模塊，較好地修正了地圖。在實(shí)驗(yàn)過程中發(fā)現(xiàn)，orb-slam2算法的實(shí)時(shí)性也較rgb-d slam好。orb-slam2算法采用的是多線程方式，而rgb-d slam是單線程，在同樣的硬件設(shè)備條件下，orb-slam2運(yùn)行速度較快，可以滿足實(shí)際應(yīng)用中對(duì)實(shí)時(shí)性的要求。orb-slam2算法的基于dbow2的場(chǎng)景識(shí)別模型具有全局重定位能力，在跟蹤丟

14、失之后，可以進(jìn)行重新定位，魯棒性更優(yōu)。<其中左上角（a）圖為場(chǎng)景圖；右上角（b）圖激光雷達(dá)平面圖；左下角（c）圖為orb-slam2生成的三維點(diǎn)云圖；右下角（d）圖為rgb-slam生成的三維點(diǎn)云圖。從以上的實(shí)驗(yàn)結(jié)果圖分析，該系統(tǒng)選用orb-slam2生成的三維點(diǎn)云圖經(jīng)過octomap處理，得到如圖5所示的三維網(wǎng)格環(huán)境地圖。octomap使用八叉樹的數(shù)據(jù)結(jié)構(gòu)存儲(chǔ)地圖，首先遍歷八叉樹節(jié)點(diǎn)，刪除值小于0.5的節(jié)點(diǎn)（大于0.5表示該區(qū)域被占概率較大），然后將其投影得到二維平面地圖，為了便于使用a*算法，將障礙物方格化，地圖中標(biāo)示1的為障礙物，0為可移動(dòng)空間（灰色區(qū)域），最后將投影得到的平面圖與

15、激光雷達(dá)平面圖進(jìn)行或操作，得到融合了環(huán)境三維點(diǎn)云信息和激光雷達(dá)信息的平面圖。3.2 機(jī)器人路徑規(guī)劃結(jié)果分別在激光雷達(dá)柵格地圖和融合三維環(huán)境點(diǎn)云信息的柵格地圖上，使用a*路徑搜索算法，對(duì)機(jī)器人路徑進(jìn)行規(guī)劃。圖中所繪路線即為a*算法結(jié)果，黑色方塊為終點(diǎn)。實(shí)驗(yàn)結(jié)果如圖6所示。由路徑規(guī)劃結(jié)果看出，在激光雷達(dá)地圖上進(jìn)行導(dǎo)航出現(xiàn)失誤，即規(guī)劃的線路中經(jīng)過了障礙物占有的區(qū)域，而在融合了點(diǎn)云信息的環(huán)境地圖上，規(guī)劃的路線正確。這在一定程度上說明融合了三維環(huán)境點(diǎn)云信息的激光雷達(dá)地圖比未融合信息的激光雷達(dá)地圖包含較豐富的環(huán)境信息，有助于對(duì)機(jī)器人路徑規(guī)劃的實(shí)現(xiàn)。4 結(jié)論該系統(tǒng)實(shí)現(xiàn)了室內(nèi)移動(dòng)機(jī)器人slam與路徑規(guī)劃，并比

16、較了兩種經(jīng)典slam算法框架即orb-slam2和rgb-d slam算法， 融合三維點(diǎn)云信息和激光雷達(dá)信息，實(shí)現(xiàn)機(jī)器人路徑規(guī)劃。實(shí)驗(yàn)結(jié)果顯示orb-slam2算法總體來說優(yōu)于rgb-d slam。融合了環(huán)境三維點(diǎn)云信息的激光雷達(dá)地圖，更有利于對(duì)機(jī)器人進(jìn)行路徑規(guī)劃。但該系統(tǒng)在以下幾個(gè)方面不足，首先，將三維網(wǎng)格地圖投影生成二維平面地圖時(shí)存在噪聲影響，會(huì)對(duì)障礙物判斷產(chǎn)生一定誤差。其次，當(dāng)運(yùn)行rgb-d slam算法的時(shí)候，光照對(duì)其影響較大，所以有一定的誤差產(chǎn)生。endprint參考文獻(xiàn)：1 李貴亞. 基于立體視覺的機(jī)器人slam算法研究d. 哈爾濱工業(yè)大學(xué)， 2015.2 cadena c， ca

17、rlone l， carrillo h， et al. simultaneous localization and mapping： present， future， and the robust-perception agej. 2016， 32（6）.3 時(shí)也， 吳懷宇， 徐文霞，等. 基于擴(kuò)展卡爾曼濾波器的移動(dòng)機(jī)器人slam研究j. 電子設(shè)計(jì)工程， 2012， 20（1）：104-106.4 羅元， 傅有力， 程鐵鳳. 基于改進(jìn)rao-blackwellized粒子濾波器的同時(shí)定位與地圖構(gòu)建j. 控制理論與應(yīng)用， 2015， 32（2）：267-272. （下轉(zhuǎn)第37頁）（上接第26頁）

18、5 bichucher v， walls j m， ozog p， et al. bathymetric factor graph slam with sparse point cloud alignmentc/ oceans. ieee， 2015：1-7.6 murartal r， tardos j d. orb-slam2： an open-source slam system for monocular， stereo and rgb-d camerasj. 2016.7 mur-artal r， montiel j m m， tardós j d. orb-slam： a

19、versatile and accurate monocular slam systemj. ieee transactions on robotics， 2015， 31（5）：1147-1163.8 belter d， labcki p， skrzypczynski p. estimating terrain elevation maps from sparse and uncertain multi-sensor datac/ ieee international conference on robotics and biomimetics. ieee， 2012：715-722.9 張建偉， 張立偉， 胡穎，等. 開源機(jī)器人操作系統(tǒng)rosm.北京：科學(xué)出版社，2012.10 王允華， 李建勝. 基于octomap的無人機(jī)室內(nèi)導(dǎo)航可飛行區(qū)域分析j.測(cè)繪與空間地理信息，2017（2）：95-97.11 zhao l， huang s， yan l， et al. large-scale monocular slam by local bundle adjustment