多ROS移動小車同步定位構(gòu)圖與自主避障方法研究與實(shí)現(xiàn)

摘" 要：單個ROS移動小車已無法完成多類型場景的快速同步定位與構(gòu)圖，為此需要多個ROS機(jī)器人的協(xié)同定位、構(gòu)圖與自主避障。選擇了麥克納姆輪的全向移動機(jī)器人作為單個ROS小車，開展了Gmapping、Hector、Karto和Cartographer四類同步定位與建圖方法研究和效果對比分析，完成了多個ROS小車的同步定位與構(gòu)圖實(shí)驗，實(shí)驗結(jié)果表明基于Gmapping方法精度最高，選擇該方法實(shí)現(xiàn)定位與構(gòu)圖，在獲得環(huán)境地圖基礎(chǔ)上設(shè)計了基于領(lǐng)航—跟隨方法的多個ROS小車協(xié)同避障，并分別進(jìn)行了有障礙物和無障礙物的自主避障實(shí)驗，結(jié)果表明了自主避障策略的有效性。

關(guān)鍵詞：同步定位與構(gòu)圖；ROS小車；SLAM；Gmapping；自主避障

中圖分類號：TP242" 文獻(xiàn)標(biāo)識碼：A" 文章編號：2096-4706（2024）10-0183-06

Research and Implementation of Synchronous Localization and Mapping and Autonomous Obstacle Avoidance Method for Multiple ROS Mobile Vehicles

HE Rui1， LI Ji2， HAN Mingwei2， HU Bin2， SA Yina2， WANG Mengyu2

（1.School of Information Science and Technology， North China University of Technology， Beijing" 100144， China;

2.Brunel London School North China University of Technology， Beijing" 100144， China;）

Abstract： A single ROS mobile vehicle is no longer able to complete the rapid and synchronous positioning and mapping of multi-type scenes. For this reason， it is necessary to carry out the collaborative localization and mapping and autonomous obstacle avoidance of multiple ROS robots. An omnidirectional mobile robot with McNamee wheel is selected as a single ROS vehicle， and four types of synchronous localization and mapping methods and effect research and comparisons of Gmapping， Hector， Karto and Cartographer are given， and the synchronous localization and mapping experiments of multiple ROS vehicles are completed. The experiment results show that the accuracy of the Gmapping-based method is the highest. This paper selects the method to realize the localization and mapping， designs the multiple ROS vehicles for cooperative obstacle avoidance based on the pilot-follower method on the basis of the obtained environment maps. The autonomous obstacle avoidance experiments with and without obstacles are carried out respectively， and the results verify the effectiveness of the autonomous obstacle avoidance strategy.

Keywords： synchronous localization and mapping; ROS vehicle; SLAM; Gmapping; autonomous obstacle avoidance

0" 引" 言

近年來，伴隨機(jī)器人技術(shù)的發(fā)展和復(fù)雜任務(wù)的迫切需求，某些復(fù)雜任務(wù)無法僅僅依靠單機(jī)器人完成，目前的發(fā)展趨勢由單機(jī)器人自主完成簡單任務(wù)向多機(jī)器人高效協(xié)同方向發(fā)展[1]。多機(jī)器人系統(tǒng)已經(jīng)引起了大量學(xué)者密切關(guān)注，陳衛(wèi)東等人使用四臺Pionneer 2機(jī)器人搭建了多機(jī)器人分布式系統(tǒng)，實(shí)現(xiàn)了編隊控制與物料搬運(yùn)[2]；潘無為使用12臺水下滑翔機(jī)對南海地區(qū)進(jìn)行協(xié)同觀察[3]；京東物流也建立了全球首個無人倉庫，配合機(jī)器人協(xié)同控制大大提高了貨物自動分揀的效率，降低了人工成本[4]。但是大多數(shù)研究還處于試驗階段，距離多機(jī)器人編隊的普及還面臨著許多挑戰(zhàn)。

同步定位與構(gòu)圖（Simultaneous Localization and Mapping， SLAM）是指移動機(jī)器人在無環(huán)境先驗信息的條件下機(jī)器人在運(yùn)動過程中能夠通過傳感器檢測周圍環(huán)境信息來確定自身位姿，同時構(gòu)建周圍環(huán)境的增量式地圖[5]。SLAM常用的傳感器為激光雷達(dá)，使用基于圖優(yōu)化的2D平面激光SLAM算法實(shí)現(xiàn)ROS小車建圖。

目前典型的建圖算法有Gmapping、Hector、Karto和Cartographer。Gmapping是基于粒子濾波算法的一種建圖方法，特點(diǎn)是將定位與建圖的過程分離，先通過粒子濾波算法定位，再利用里程計信息得到先驗位姿并不斷矯正里程計誤差形成地圖[6]。該算法對激光雷達(dá)頻率要求較低，但是嚴(yán)重依賴?yán)锍逃嬊覜]有回環(huán)檢測，小范圍建圖精度較高。Hector使用高斯牛頓方法解決scan-matching問題，無須里程計，只根據(jù)激光雷達(dá)即可構(gòu)建地圖[7]。該算法估計了激光點(diǎn)在地圖的表示和占據(jù)網(wǎng)格的概率，加入慣性測量系統(tǒng)并利用卡爾曼濾波算法進(jìn)行濾波以避免在建圖中出現(xiàn)局部最小而非全局最優(yōu)。但該算法要求激光雷達(dá)有更高的頻率且同樣沒有回環(huán)檢測，需要將機(jī)器人小車控制在較低速度時建圖效果才會理想。Karto算法是基于圖優(yōu)化方法進(jìn)行建圖，采用系數(shù)點(diǎn)調(diào)整法[8]，通過掃匹配和閉環(huán)檢測，先求得位姿后建立地圖。隨著地圖擴(kuò)大所需內(nèi)存增加，相比與其他方法在大環(huán)境下制圖更讓有優(yōu)勢，但會影響時效性。Cartographer算法也是基于圖優(yōu)化框架，支持多傳感器融合建圖，通過處理激光雷達(dá)、IMU、里程計等傳感器的信息數(shù)據(jù)進(jìn)行地圖構(gòu)建。該算法的主要理論是通過閉環(huán)檢測來消除構(gòu)圖過程中產(chǎn)生的積累誤差[9]，所以使用低成本的激光雷達(dá)也能構(gòu)建出效果不錯的地圖。但是在提高地圖準(zhǔn)確性的同時需要占用更多內(nèi)存，計算量較大。

多機(jī)器人協(xié)同避障是指在編隊控制算法下多個機(jī)器人在未知環(huán)境下進(jìn)行交互，并保持特定的幾何隊形到達(dá)目標(biāo)區(qū)域，同時能夠躲避運(yùn)動至目標(biāo)位置的過程中出現(xiàn)的障礙物[10]。典型的機(jī)器人編隊控制方法有領(lǐng)航跟隨法、人工勢場法、虛構(gòu)結(jié)構(gòu)法等。本文以塔克創(chuàng)新出品的小車平臺為研究對象，通過增加通信模塊建立多機(jī)器人間通信，使用SLAM技術(shù)和PID算法探究領(lǐng)航跟隨模式下多機(jī)器人小車室內(nèi)整體避障效果。

1" 移動小車運(yùn)動學(xué)模型

麥克納姆輪的所有輪子均為主動輪，可以實(shí)現(xiàn)全方位移動。在ROS系統(tǒng)中坐標(biāo)系遵循右手定則，即以機(jī)器人底部的幾何中心建立機(jī)器人局部坐標(biāo)系，對于小車來說X軸正方向為小車的正前方，Y軸正方向為小車的左方，Z軸正方向為小車逆時針旋轉(zhuǎn)方向。由于目標(biāo)物在移動的機(jī)器人坐標(biāo)系和真實(shí)坐標(biāo)系中的坐標(biāo)不相等，所以需要進(jìn)行坐標(biāo)變換。文獻(xiàn)[11]從四輪驅(qū)動的全向移動機(jī)器人平臺的運(yùn)動矢量出發(fā)，將機(jī)器人小車視為剛體在剛體平面運(yùn)動，推導(dǎo)出了機(jī)器人小車各機(jī)械輪的運(yùn)動學(xué)逆解速度模型為：

（1）

其中， 表示某個輪子的速度，、 表示小車沿X軸、Y軸運(yùn)動速度，ω表示yaw軸自轉(zhuǎn)的角速度，a、b表示小車底盤質(zhì)心與車輪周之間的半徑。

2" SLAM方法

2.1" SLAM理論框架

機(jī)器人小車的自主導(dǎo)航是指給小車下達(dá)目標(biāo)位置指令，小車在避開障礙物的前提下進(jìn)行運(yùn)動到目標(biāo)點(diǎn)位的最佳路徑軌跡選擇。實(shí)現(xiàn)自主導(dǎo)航的核心是機(jī)器人的定位和路徑規(guī)劃。在該功能的框架中有兩個功能包：move_base功能包和amcl功能包，如圖1所示。

move_base功能包提供導(dǎo)航的主要運(yùn)行與接口交互，實(shí)現(xiàn)了最優(yōu)路徑規(guī)劃，主要由兩大規(guī)劃器組成。一是全局路徑規(guī)劃（global_planner），根據(jù)給定的目標(biāo)位置和全局地圖進(jìn)行總體路徑的規(guī)劃。二是本地實(shí)時規(guī)劃（local_planner），在實(shí)際情況中機(jī)器人小車針對地圖信息和周圍隨時可能出現(xiàn)的障礙物使用TEB（Timed Elastic Band）算法探索躲避，通過“是否會撞擊障礙物”“所需要時間”等評價標(biāo)準(zhǔn)選取最優(yōu)路徑。為了保證導(dǎo)航點(diǎn)準(zhǔn)確性，小車還需要進(jìn)行精準(zhǔn)的定位，由自適應(yīng)蒙特卡洛定位方法實(shí)現(xiàn)，根據(jù)粒子濾波的不斷迭代收斂推算出小車的精準(zhǔn)定位。

2.2" 多機(jī)器人編隊

如圖2所示，本文使用領(lǐng)航跟隨法進(jìn)行多小車的協(xié)同控制與整體避障，通過領(lǐng)航者計算障礙物間距判斷車隊是否能按照期望隊形通過，若小于可行間距則進(jìn)行整體避障[12]。

在編隊過程中，領(lǐng)航者基于地圖中的環(huán)境信息，通過軌跡優(yōu)化等方法可以優(yōu)化出路徑最短的路徑，同時根據(jù)領(lǐng)航者當(dāng)前的位置和預(yù)期位置計算得出位置誤差反饋控制調(diào)整領(lǐng)航者的位置，同時將目標(biāo)位置信息發(fā)送給跟隨者，跟隨者根據(jù)接收到的目標(biāo)位置信息進(jìn)行移動。實(shí)驗中增加了通信模塊，能夠?qū)㈩I(lǐng)航者的位姿信息以及運(yùn)動軌跡通過話題節(jié)點(diǎn)發(fā)布給跟隨者，跟隨者通過訂閱接收信息并根據(jù)相互之間的距離形成編隊，借助多機(jī)器人之間的通信實(shí)現(xiàn)編隊控制。在一個編隊系統(tǒng)中，只能有一個最高級別的領(lǐng)航者。該方法的優(yōu)點(diǎn)是僅需領(lǐng)航者的位置和姿態(tài)信息，領(lǐng)航者和跟隨者之間的相對位置差關(guān)系，即可實(shí)現(xiàn)協(xié)同編隊。協(xié)同編隊的結(jié)構(gòu)和運(yùn)動軌跡由領(lǐng)航者控制，不受跟隨者的限制。同時該方法未考慮跟隨者避障區(qū)域范圍的信息未傳遞給領(lǐng)航者，容易造成領(lǐng)航者成功避障，某一個或多個跟隨者避障失敗的情況。

3" 實(shí)驗驗證

3.1" 單機(jī)器人室內(nèi)建圖比較

SLAM技術(shù)是機(jī)器人小車路徑規(guī)劃的前提，而單機(jī)器人小車構(gòu)建未知環(huán)境的地圖是一切實(shí)驗的基礎(chǔ)，是實(shí)現(xiàn)多ROS機(jī)器人小車高效快速實(shí)現(xiàn)協(xié)同編隊的基礎(chǔ)。在獲得位置信息后，移動小車可依據(jù)自身位置與目標(biāo)位置的位置差優(yōu)化最優(yōu)路徑，實(shí)現(xiàn)協(xié)同導(dǎo)航與避障。在多小車編隊中往往是以領(lǐng)航者所規(guī)劃出的最優(yōu)路徑作為整個隊形的整體路徑。Gmapping、Hector、Karto和Cartographer四種SLAM激光雷達(dá)算法實(shí)際建圖中存在較大的性能差異，為了評估實(shí)際建圖的效果和有效性，開展單機(jī)器人建圖性能分析與對比。

圖3給出了實(shí)驗環(huán)境場景，該實(shí)驗環(huán)境位于某個樓梯間的過渡區(qū)，整體呈長方形，分為自主搭建的工作區(qū)和陽臺，兩片區(qū)域之間可連通并由分隔墻隔開。工作區(qū)中設(shè)立一張較大的桌子、椅子和黃色小板凳作為障礙物，陽臺沒有障礙物，可行區(qū)域較大，可驗證小車在空曠和復(fù)雜環(huán)境中的建圖與導(dǎo)航效果。

以Gmapping算法為例探究小車在未知環(huán)境中建圖的過程。首先使用SSH完成遠(yuǎn)程PC端與小車的通信連接，確認(rèn)網(wǎng)絡(luò)配置成功后開始地圖創(chuàng)建。輸入指令打開Gmapping算法節(jié)點(diǎn)，啟動rviz仿真環(huán)境，獲得初始時刻激光雷達(dá)采集的點(diǎn)云數(shù)據(jù)，如圖4所示。

通過開啟鍵盤遙控節(jié)點(diǎn)，在實(shí)驗場地內(nèi)ROS機(jī)器人順時針方向運(yùn)動。建圖結(jié)果如圖5所示，該圖表明激光雷達(dá)獲得環(huán)境點(diǎn)云數(shù)據(jù)隨時間積累，不斷構(gòu)成了實(shí)驗場景的環(huán)境信息。觀察整個建圖過程可以發(fā)現(xiàn)當(dāng)ROS機(jī)器人離黃色小板凳較遠(yuǎn)時并無法探測出其信息，所以在地圖上并不能顯示出小板凳的存在，但當(dāng)ROS機(jī)器人行駛到如圖5（c）所示的工作區(qū)位置時，距離小板凳的距離較近，可以掃描出它的存在。當(dāng)小車?yán)@環(huán)境一周回到初始位置，實(shí)現(xiàn)了回環(huán)。環(huán)境信息建圖完成，與實(shí)際環(huán)境信息對比可以發(fā)現(xiàn)，建圖結(jié)果與實(shí)際場景相符合，建圖精度較高。

四種方法最終構(gòu)建的地圖效果如圖6所示。其中，白色區(qū)域表示可通行區(qū)域，灰色區(qū)域表示未知區(qū)域，黑色區(qū)域表示障礙物或邊界。當(dāng)小車完成一次順時針閉環(huán)運(yùn)動后，由圖6（a）和圖6（c）可以看出使用Gmapping算法和Karto算法可以構(gòu)建出質(zhì)量較高、較為完整的實(shí)驗環(huán)境地圖，但相比之下Gmapping算法所用時間更短、所占的內(nèi)存更少，可以在較小環(huán)境中擁有更高的建圖效率。由圖6（b）可以看出Hector算法雖然不許要里程計信息，但是由于沒有回環(huán)檢測以及對雷達(dá)頻率的要求，即使將小車運(yùn)動速度降低至0.2 m/s也會產(chǎn)生地圖錯位，導(dǎo)致建圖質(zhì)量較低。而由圖6（d）可以看出Cartographer算法的計算量較大，在小車順時針運(yùn)動一圈后較其他算法而言地圖中仍存在較多灰色未知區(qū)域，不能準(zhǔn)確反饋障礙物信息，建圖質(zhì)量較差。

根據(jù)理論分析以及實(shí)驗分析，得出四種建圖方法的建圖效果對比，如表1所示。

根據(jù)多方面比較，Gmapping算法計算量小，雖然沒有回環(huán)檢測但在小范圍的室內(nèi)建圖能得到不錯的效果，最終選擇Gmapping算法進(jìn)行實(shí)驗環(huán)境的地圖構(gòu)建，并使用該算法建圖的結(jié)果圖6（a）作為后續(xù)多小車協(xié)同編隊避障實(shí)驗的基礎(chǔ)。

3.2" 多機(jī)器人導(dǎo)航與避障實(shí)驗

為了驗證多機(jī)器人編隊的有效性，將3輛機(jī)器人小車擺放至實(shí)際場景中，啟動小車，使其保持三角形隊結(jié)構(gòu)，領(lǐng)航者位于視場范圍前方，跟隨者位于后方。當(dāng)領(lǐng)航者檢測到障礙物時，如果障礙物寬度遠(yuǎn)大于小車隊形寬度，則可以直接編隊通過，不需要避障；如果障礙物寬度小于等于小車隊形寬度，則對移動小車的通信結(jié)構(gòu)路徑再優(yōu)化，實(shí)現(xiàn)協(xié)同避障，在避障過程中，多機(jī)器人通信結(jié)構(gòu)保持三角形。

首先需要在rviz可視化界面使用方框中的綠色箭頭“2D Pose Estimate”進(jìn)行小車初始位置的標(biāo)定與修正，當(dāng)領(lǐng)航小車的雷達(dá)信息與地圖的邊緣及障礙物完全重合時即為標(biāo)定成功。圖7為三輛小車進(jìn)行標(biāo)定后在實(shí)驗環(huán)境地圖中的真實(shí)位置。然后使用“2D Nav Goal”給小車發(fā)布導(dǎo)航信息，小車便可避開障礙物實(shí)現(xiàn)自動路徑規(guī)劃。

給領(lǐng)航者設(shè)置一個目標(biāo)點(diǎn)于工作區(qū)與陽臺連接處，小車以0.3 m/s的速度進(jìn)行無障礙定點(diǎn)導(dǎo)航。實(shí)驗結(jié)果如表2所示。ROS小車協(xié)同完成了導(dǎo)航任務(wù)。

圖8呈現(xiàn)了整體避障過程，三輛小車保持三角形通信結(jié)構(gòu)。首先在機(jī)器人視場內(nèi)設(shè)置一個目標(biāo)點(diǎn)，圖8（a）中箭頭所示方向為全局的路徑方向，三輛機(jī)器人小車始終以排列成三角形運(yùn)動，從圖8（b）表明，當(dāng)領(lǐng)航者發(fā)現(xiàn)障礙物時，同時未發(fā)現(xiàn)可行空間時，領(lǐng)航者對可行路徑進(jìn)行規(guī)劃，即從左側(cè)行駛避開障礙物，并繼續(xù)向目標(biāo)點(diǎn)前進(jìn)，此時領(lǐng)航者將位置信息和規(guī)劃的路徑信息發(fā)送給跟隨者，使得三輛小車能夠保持原隊形避開障礙物，到達(dá)目標(biāo)點(diǎn)位，如圖8（c）所示。

為驗證算法有效性，開展了5次自主避障實(shí)驗，結(jié)果如表3所示。

由實(shí)驗結(jié)果可以看出，跟隨者1處于外側(cè)，可以和領(lǐng)航者順利完成避障，到達(dá)目標(biāo)點(diǎn)。但處于內(nèi)側(cè)的跟隨者2由于PID控制擁有的滯后性，在跟隨領(lǐng)航者軌跡運(yùn)動時不能及時避開障礙物，導(dǎo)致兩次實(shí)驗撞擊到了擋板，未能完成避障。且在整體避障中領(lǐng)航者定位精度不一致，擁有一定的跟蹤誤差，會導(dǎo)致小車編隊變形。

4" 結(jié)" 論

本文以基于麥克納姆輪的全向移動ROS智能機(jī)器人小車為實(shí)驗平臺，通過實(shí)驗驗證了自主避障方法的有效性。首先對比了SLAM技術(shù)中四種激光雷達(dá)建圖算法的效果，結(jié)果表明Gmapping算法更適用于小范圍室內(nèi)建圖，并以建圖結(jié)果作為后續(xù)實(shí)驗的基礎(chǔ)。其次為小車之間增加了通信模塊，實(shí)現(xiàn)了多機(jī)器人小車間信息的實(shí)時傳輸，并采用領(lǐng)航跟隨模式對小車進(jìn)行協(xié)同控制編隊，實(shí)現(xiàn)了對多機(jī)器人小車整體避障效果的實(shí)驗探究。本文在避障實(shí)驗時考慮的障礙物情況較為簡單，未來將針對跟隨者的避障問題對領(lǐng)航跟隨方法進(jìn)行改進(jìn)，根據(jù)實(shí)際場景進(jìn)行隊形的自由切換，或選擇不同路徑到達(dá)目標(biāo)點(diǎn)，以靈活運(yùn)用于各種復(fù)雜的環(huán)境之中，拓展其工作范圍并提升其工作效率。

參考文獻(xiàn)：

[1] 任立敏.未知環(huán)境下多移動機(jī)器人編隊控制及協(xié)作搬運(yùn)策略研究 [D].哈爾濱：哈爾濱工業(yè)大學(xué)，2013.

[2] 陳衛(wèi)東，席裕庚，顧冬雷，等.一個面向復(fù)雜任務(wù)的多機(jī)器人分布式協(xié)調(diào)系統(tǒng) [J].控制理論與應(yīng)用，2002（4）：505-510.

[3] 潘無為.分布式多水下機(jī)器人編隊控制方法研究 [D].哈爾濱：哈爾濱工程大學(xué)，2018.

[4] 靳士超.基于麥克納姆輪的全向智能移動機(jī)器人導(dǎo)航系統(tǒng)研究 [D].蘇州：蘇州大學(xué)，2018.

[5] GOBHINATH S，ANANDAPOORANI K，ANITHA K，et al. Simultaneous Localization and Mapping [SLAM] of Robotic Operating System for Mobile Robots [C]//2021 7th International Conference on Advanced Computing and Communication Systems （ICACCS）.Coimbatore：IEEE，2021：577-580.

[6] GRISETTI G，STACHNISS C，BURGARD W. Improved Techniques for Grid Mapping With Rao-Blackwellized Particle Filters [J]. IEEE Transactions on Robotics，2007，23（1）：34-46.

[7] KOHLBRECHER S，STRYK O V，MEYER J，et al. A Flexible and Scalable SLAM System With Full 3D Motion Estimation [C]//2011 IEEE International Symposium on Safety，Security，and Rescue Robotics.Kyoto：IEEE，2011：155-160.

[8] NT S K，GAWANDE M，VERMA H，et al. Mobile Robot Terrain Mapping for Path Planning using Karto Slam and Gmapping Technique [C]//2022 IEEE Global Conference on Computing， Power and Communication Technologies （GlobConPT）.New Delhi：IEEE，2022：1-4.

[9] HESS W，KOHLER D，RAPP H，et al. Real-Time Loop Closure in 2D LIDAR SLAM [C]//2016 IEEE International Conference on Robotics and Automation （ICRA）.Stockholm：IEEE，2016：1271-1278.

[10] OH K K，PARK M C，AHN H S. A Survey of Multi-Agent Formation Control [J].Automatica，2015，53：424-440.

[11] MAULANA E，MUSLIM M A，HENDRAYAWAN V. Inverse Kinematic Implementation of Four-Wheels Mecanum Drive Mobile Robot using Stepper Motors [C]//2015 International Seminar on Intelligent Technology and Its Applications （ISITIA）.Surabaya：IEEE，2015：51-56.

[12] SHAO J Y，XIE G M，YU J Z，et al. Leader-Following Formation Control of Multiple Mobile Robots [C]//Proceedings of the 2005 IEEE International Symposium on， Mediterrean Conference on Control and Automation Intelligent Control.Limassol：IEEE，2005：808-813.

作者簡介：何瑞（2002—），男，漢族，北京人，本科在讀，研究方向：SLAM；李繼（2002—），男，漢族，重慶人，本科在讀，研究方向：大數(shù)據(jù)與SLAM；韓銘偉（2003—），女，漢族，山東德州人，本科在讀，研究方向：大數(shù)據(jù)與SLAM；胡斌（2003—），男，漢族，福建莆田人，本科在讀，研究方向：大數(shù)據(jù)；薩逸娜（2002—），女，蒙古族，北京人，本科在讀，研究方向：大數(shù)據(jù)；王孟瑜（2003—），女，漢族，北京人，本科在讀，研究方向：大數(shù)據(jù)。