室內(nèi)機(jī)器人自主導(dǎo)航的實(shí)現(xiàn)

陳龍 劉娟秀

摘要：為使機(jī)器人完成對(duì)室內(nèi)環(huán)境地圖的建立，并對(duì)自身進(jìn)行實(shí)時(shí)定位，實(shí)現(xiàn)自主導(dǎo)航、自主行走的目的，選用電源模塊、驅(qū)動(dòng)模塊、主控模塊等硬件設(shè)備搭建一個(gè)機(jī)器人運(yùn)動(dòng)平臺(tái)，并采用激光雷達(dá)作為該平臺(tái)距離傳感器。在同時(shí)定位與地圖構(gòu)建數(shù)學(xué)模型的基礎(chǔ)上，采用擴(kuò)展卡爾曼濾波算法對(duì)該數(shù)學(xué)模型進(jìn)行優(yōu)化求解，得到模型最優(yōu)解。在機(jī)器人操作系統(tǒng)ROS上對(duì)機(jī)器人進(jìn)行編程控制，實(shí)現(xiàn)機(jī)器人實(shí)時(shí)定位和建圖、自主導(dǎo)航功能。

關(guān)鍵詞：室內(nèi)機(jī)器人;概率模型;EFK-SLAM;定位與建圖;自主導(dǎo)航

中圖分類(lèi)號(hào)：TP242 ? ? ?文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：1009-3044（2020）24-0001-03

Abstract： In order to make the robot complete the establishment of the indoor environment map， and to be positioned in real-time， realize autonomous navigation and autonomous walking， power module， drive module， main control module， and other hardware equipment is used to build a robot motion platform in this paper. Laser radar is selected as the distance sensor of the platform. The Slam （simultaneous localization and mapping） mathematical model and the EKF （Extended Kalman filter） algorithm are used to solve the mathematical model. The optimal solution of the model is obtained， and the robot is programmed and controlled on the ROS （robot operating system）. Realize the robot real-time localization， mapping， and autonomous navigation functions.

Key words： indoor robot; probability model; EFK-SLAM; localization and mapping; autonomous navigation

1 背景

隨著經(jīng)濟(jì)的發(fā)展和社會(huì)的進(jìn)步，人們對(duì)高質(zhì)量生活的需求也逐漸提高。室內(nèi)機(jī)器人成為近幾年發(fā)展迅速的一類(lèi)機(jī)器人，在醫(yī)療、文娛、服務(wù)、安保行業(yè)的應(yīng)用也日益廣泛。作為當(dāng)前代替人工的優(yōu)秀解決方法，有著巨大的發(fā)展前景。為使室內(nèi)機(jī)器人更加智能化，能夠在更少人工干預(yù)的條件下，自主去完成設(shè)定的任務(wù)。本文設(shè)計(jì)一款室內(nèi)機(jī)器人，使用激光雷達(dá)采集的距離數(shù)據(jù)，通過(guò)優(yōu)化過(guò)后的SLAM模型進(jìn)行運(yùn)算，可以使機(jī)器人知道自身在室內(nèi)環(huán)境的實(shí)時(shí)位置，在此基礎(chǔ)上完成對(duì)標(biāo)定位置點(diǎn)之間的自主導(dǎo)航。

2系統(tǒng)設(shè)計(jì)

2.1總體設(shè)計(jì)

室內(nèi)機(jī)器人的總體可以分為上位機(jī)和下位機(jī)兩大部分，兩個(gè)部分獨(dú)立運(yùn)行實(shí)現(xiàn)各自功能，并通過(guò)總線方式進(jìn)行數(shù)據(jù)的交互，其原理框圖如圖1所示。

上位機(jī)可以由微型計(jì)算機(jī)充當(dāng)，也可以直接使用筆記本電腦，相當(dāng)于系統(tǒng)的大腦，對(duì)大數(shù)據(jù)量的信息進(jìn)行處理，即對(duì)來(lái)自下位機(jī)的數(shù)據(jù)進(jìn)行處理，得到運(yùn)動(dòng)平臺(tái)的位置信息，依靠處理過(guò)后的數(shù)據(jù)進(jìn)行實(shí)時(shí)定位和建圖，并對(duì)機(jī)器人的運(yùn)動(dòng)做出決策。將控制命令發(fā)送給下位機(jī)，下位機(jī)控制運(yùn)動(dòng)平臺(tái)的動(dòng)作，完成計(jì)劃的任務(wù)。在運(yùn)動(dòng)平臺(tái)中，下位機(jī)起到了兩個(gè)作用：控制平臺(tái)的運(yùn)動(dòng)和傳感器信息的收集，將這些信息進(jìn)行簡(jiǎn)單的處理過(guò)后發(fā)送給上位機(jī)，根據(jù)上位機(jī)發(fā)送的指令對(duì)平臺(tái)的運(yùn)動(dòng)進(jìn)行控制。激光雷達(dá)安裝在運(yùn)動(dòng)平臺(tái)的前端，根據(jù)運(yùn)動(dòng)平臺(tái)的移動(dòng)得到位置的距離信息。

2.2上位機(jī)設(shè)計(jì)

本設(shè)計(jì)將使用安裝在上位機(jī)中的ROS（robot operating system）操作系統(tǒng)，該系統(tǒng)的特點(diǎn)是利用節(jié)點(diǎn)發(fā)布話題，通過(guò)訂閱相同話題實(shí)現(xiàn)節(jié)點(diǎn)間通信和數(shù)據(jù)交換。本設(shè)計(jì)自定義5個(gè)ROS節(jié)點(diǎn)：抽象節(jié)點(diǎn)，激光節(jié)點(diǎn)，建圖節(jié)點(diǎn)，遙控節(jié)點(diǎn)以及導(dǎo)航節(jié)點(diǎn)。其中，抽象節(jié)點(diǎn)負(fù)責(zé)上位機(jī)與主控板的數(shù)據(jù)的讀寫(xiě)以及接收編碼器數(shù)據(jù)。激光節(jié)點(diǎn)負(fù)責(zé)接收激光雷達(dá)測(cè)量數(shù)據(jù)。建圖節(jié)點(diǎn)是根據(jù)激光雷達(dá)的數(shù)據(jù)，運(yùn)用SLAM算法模型得到機(jī)器人實(shí)時(shí)位置信息，并繪制成為地圖。遙控節(jié)點(diǎn)實(shí)現(xiàn)鍵盤(pán)控制運(yùn)動(dòng)平臺(tái)移動(dòng)。導(dǎo)航節(jié)點(diǎn)是根據(jù)已有地圖，控制機(jī)器人完成自主導(dǎo)航任務(wù)。ROS控制系統(tǒng)主要的流程圖如圖2所示，其中虛線框位上位機(jī)的部分。

2.3下位機(jī)設(shè)計(jì)

下位機(jī)部分主要由主控模塊，電源模塊，電機(jī)驅(qū)動(dòng)模塊構(gòu)成。主控模塊是采用意法半導(dǎo)體stm32 f407vgt6芯片組成的最小系統(tǒng)，該芯片的最高工作頻率可以達(dá)到168MHz，可以對(duì)一些小數(shù)據(jù)量的信息進(jìn)行處理。電源模塊有多路開(kāi)關(guān)和多種電源轉(zhuǎn)換功能，可以為整個(gè)運(yùn)動(dòng)平臺(tái)的各種設(shè)備提供電力支持。電機(jī)驅(qū)動(dòng)模塊中使用兩塊BTS7970芯片，可以驅(qū)動(dòng)一個(gè)電機(jī)，保證足夠大的驅(qū)動(dòng)能力。

3 SLAM基本原理與程序設(shè)計(jì)

3.1 SLAM的基本原理

同時(shí)定位與建圖（simultaneous localisation and mapping，SLAM）[1]是指：如果將可以移動(dòng)的機(jī)器人放入一個(gè)未知而陌生的環(huán)境中，機(jī)器人如何通過(guò)對(duì)于環(huán)境的掃描在通過(guò)一定的處理得到與這個(gè)環(huán)境相一致的地圖，并且能夠知道自己在地圖之中的位置。

如圖3所示，一個(gè)機(jī)器人在室內(nèi)環(huán)境中移動(dòng)，它通過(guò)激光雷達(dá)得到一系列的標(biāo)志點(diǎn)和自己的相對(duì)位置的距離信息。

通過(guò)這些信息對(duì)移動(dòng)軌跡與所有的標(biāo)志點(diǎn)都是實(shí)時(shí)進(jìn)行估計(jì)的，構(gòu)成一個(gè)兩步的遞歸預(yù)測(cè)矯正算法模型。模型中包含有預(yù)測(cè)和矯正兩個(gè)過(guò)程，將預(yù)測(cè)得到的標(biāo)志點(diǎn)進(jìn)行矯正，不斷地縮小估計(jì)標(biāo)志點(diǎn)和真是標(biāo)志點(diǎn)之間的誤差。隨著觀測(cè)次數(shù)的增加，標(biāo)志點(diǎn)之間相關(guān)程度不斷增加，標(biāo)志點(diǎn)相對(duì)位置信息也不斷地變得準(zhǔn)確。

當(dāng)機(jī)器人運(yùn)動(dòng)到了新的點(diǎn)上，重新觀測(cè)先前的標(biāo)志點(diǎn)時(shí)，機(jī)器人與標(biāo)志點(diǎn)的位置都會(huì)被更新。同時(shí)也會(huì)出現(xiàn)這一個(gè)位置的標(biāo)志點(diǎn)，這些新出現(xiàn)的標(biāo)志點(diǎn)可以立即與已知的地圖相關(guān)聯(lián)。最終，所有的標(biāo)志點(diǎn)形成了一個(gè)相互關(guān)聯(lián)的網(wǎng)絡(luò)，它們的精度隨著觀測(cè)次數(shù)的增加而增加，標(biāo)志點(diǎn)數(shù)量隨觀測(cè)次數(shù)的增加而增加。

圖4以條帶表示點(diǎn)與點(diǎn)之間的相關(guān)性。當(dāng)機(jī)器人在環(huán)境當(dāng)中來(lái)回運(yùn)動(dòng)的時(shí)候，這些條帶會(huì)變得越來(lái)越粗并將會(huì)影響整個(gè)網(wǎng)絡(luò)。

3.2同時(shí)定位與建圖的程序設(shè)計(jì)

SLAM算法模型可以在一定程度上幫助減小估計(jì)標(biāo)志點(diǎn)和真實(shí)標(biāo)志點(diǎn)之間的誤差，但是想要真正實(shí)現(xiàn)機(jī)器人的實(shí)時(shí)定位與建圖，還需要使用擴(kuò)展卡爾曼濾波（Extended Kalman filter，EKF）算法對(duì)該數(shù)學(xué)模型進(jìn)行優(yōu)化，去除不正確的標(biāo)志點(diǎn)，使模型具有線性，這些都利于提高定位與建圖的精度。

將上述過(guò)程通過(guò)程序不停地迭代，得到EKF-SLAM定位與建圖程序的框圖如圖5所示：

3.3自主導(dǎo)航的程序設(shè)計(jì)

在已經(jīng)獲得室內(nèi)地圖數(shù)據(jù)的基礎(chǔ)之上，對(duì)機(jī)器人進(jìn)行路徑規(guī)劃，在機(jī)器人行走過(guò)程中也不斷對(duì)地圖進(jìn)行實(shí)時(shí)比對(duì)和重新規(guī)劃，從而實(shí)現(xiàn)機(jī)器人的自主導(dǎo)航。得到機(jī)器人自主導(dǎo)航的程序框圖如圖6所示。

4系統(tǒng)調(diào)試

4.1同時(shí)定位與建圖的調(diào)試

首先，在新終端啟動(dòng)機(jī)器人抽象節(jié)點(diǎn)，通過(guò)該節(jié)點(diǎn)對(duì)機(jī)器人的狀態(tài)進(jìn)行初始化，并啟動(dòng)對(duì)機(jī)器人運(yùn)動(dòng)平臺(tái)系統(tǒng)的控制程序。接下來(lái)，在另一個(gè)新終端中啟動(dòng)雷達(dá)激光節(jié)點(diǎn)，激光雷達(dá)能正常工作，這時(shí)的激光雷達(dá)能夠輸出距離的數(shù)據(jù)。之后，再新開(kāi)一個(gè)終端，啟動(dòng)建圖節(jié)點(diǎn)，這個(gè)節(jié)點(diǎn)是為了運(yùn)行建圖的程序，調(diào)取空白建圖界面，讀取激光雷達(dá)的距離數(shù)據(jù)和里程數(shù)據(jù)，使得機(jī)器人實(shí)時(shí)的定位信息能夠被讀取到。最后，利用RVIZ對(duì)建圖過(guò)程進(jìn)行可視化。如果正常的話，可以得到如圖7所示的畫(huà)面。

利用無(wú)線鍵盤(pán)遙控機(jī)器人在準(zhǔn)備導(dǎo)航的環(huán)境中緩慢移動(dòng)，使RVIZ中顯示的地圖逐漸補(bǔ)全，直到將整個(gè)環(huán)境的地圖建好為止。當(dāng)?shù)貓D建好之后就要對(duì)地圖進(jìn)行保存。最終，實(shí)驗(yàn)室外面走廊的地圖如圖8所示。

4.2自主導(dǎo)航的調(diào)試

按照之前的方法，在兩個(gè)終端中啟動(dòng)抽象和激光節(jié)點(diǎn)，之后將先前建好的地圖導(dǎo)入導(dǎo)航節(jié)點(diǎn)中，啟動(dòng)導(dǎo)航節(jié)點(diǎn)。若導(dǎo)航節(jié)點(diǎn)正常啟動(dòng)之后，就可知道當(dāng)前機(jī)器人的狀態(tài)，并對(duì)導(dǎo)航狀態(tài)進(jìn)行初始化，最后得到機(jī)器人的里程數(shù)據(jù)，此時(shí)機(jī)器人可以開(kāi)始進(jìn)行導(dǎo)航。利用RVIZ對(duì)機(jī)器人初始位置標(biāo)定，之后為機(jī)器人指定導(dǎo)航的目標(biāo)點(diǎn)。得到自主導(dǎo)航畫(huà)面如圖9所示。

其中，機(jī)器人前方的紅色細(xì)線為規(guī)劃出來(lái)的全局路徑，如果在行經(jīng)過(guò)程中沒(méi)有障礙物機(jī)器人將沿著這條路徑行走，當(dāng)遇到障礙物時(shí)，機(jī)器人將向右邊移動(dòng)繞開(kāi)障礙物之后重新回到這條全局路徑。

5結(jié)束語(yǔ)

通過(guò)硬件設(shè)備的搭建，獲得本設(shè)計(jì)的運(yùn)動(dòng)平臺(tái)，之后通過(guò)對(duì)SLAM模型的理解和優(yōu)化得到機(jī)器人同時(shí)定位與建圖以及自主導(dǎo)航的控制原理。根據(jù)原理利用ROS操作系統(tǒng)，對(duì)機(jī)器人運(yùn)動(dòng)平臺(tái)進(jìn)行編程控制，最終對(duì)程序進(jìn)行調(diào)試，使得機(jī)器人實(shí)現(xiàn)同時(shí)定位與建圖以及自主導(dǎo)航的功能。為后續(xù)機(jī)器人實(shí)現(xiàn)未知環(huán)境下的移動(dòng)，為室外機(jī)器人、無(wú)人駕駛車(chē)及其他輪式移動(dòng)機(jī)器人的工程應(yīng)用提供理論基礎(chǔ)和技術(shù)依據(jù)。

參考文獻(xiàn)：

[1] 潘心冰.基于Kinect的移動(dòng)機(jī)器人SLAM技術(shù)研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2017.

[2] 潘琪.自主移動(dòng)機(jī)器人的路徑規(guī)劃算法研究[D].長(zhǎng)沙：湖南大學(xué)，2016.

[3] 張?zhí)?導(dǎo)盲機(jī)器人自主導(dǎo)航技術(shù)研究[D].鎮(zhèn)江：江蘇科技大學(xué)，2016.

[4] 楊俊駒.動(dòng)態(tài)環(huán)境下基于ROS的移動(dòng)機(jī)器人路徑規(guī)劃技術(shù)研究[D].蘇州：蘇州大學(xué)，2016.

[5] 徐亮.室內(nèi)清掃機(jī)器人的路徑規(guī)劃研究[D].鄭州：中原工學(xué)院， 2011.

[6] 胡正偉.清掃機(jī)器人定位及路徑規(guī)劃研究[D].秦皇島：燕山大學(xué)，2010.

[7] 何炳蔚，張立偉，張建偉.基于ROS的機(jī)器人理論與應(yīng)用[M].北京：科學(xué)出版社，2017： 101.

[8] 余冬冬.移動(dòng)機(jī)器人避障與軌跡規(guī)劃[D].杭州：浙江大學(xué)，2017.

[9] 唐燁.智能家居環(huán)境中基于移動(dòng)機(jī)器人的目標(biāo)搜索[D].杭州：浙江大學(xué)，2017.

[10] 高志發(fā)，武建新.基于ROS的移動(dòng)機(jī)器人的路徑規(guī)劃策略研究[J].機(jī)械研究與應(yīng)用，2017， 30（3）： 161-162.

[11] Estévez P， Cappelletto J，?lvarez F，et al. Coordinated navigation Using Dynamically Varying Velocity Fields[C]. 9th International Conference on Artificial Intelligence and Soft Computing. Zakopane， Poland. June 2008.

[12] Ogata K. Modern Control Engineering[M]. 5th ed. Prentice-Hall Pearson， 2010.

[13] Medina-Meléndez W ， FermínL，F(xiàn)ernéndezG，et al. Optical Flow Based Velocity Field Control[C]. Modalidad： 9th International Conference on Artificial Intelligence and Soft Computing. Zakopane， Poland， 2008（6）： 771-781.

[14] Certad N，Acuna R，Terrones ?，et al.Study and improvements in landmarks extraction in 2D range images based on an adaptive curvature estimation[C]//2012 VI Andean Region International Conference. 7-9 Nov. 2012， Cuenca， Ecuador. IEEE， 2012：95-98.

[15] Point Clouds.Official Web Site[EB/OL].[2019-12-26].http：//www.pointclouds.org.

【通聯(lián)編輯：謝媛媛】