多ROS移動小車同步定位構圖與自主避障方法研究與實現

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

關鍵詞：同步定位與構圖；ROS小車；SLAM；Gmapping；自主避障

中圖分類號：TP242" 文獻標識碼：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" 引" 言

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

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

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

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

1" 移動小車運動學模型

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

（1）

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

2" SLAM方法

2.1" SLAM理論框架

機器人小車的自主導航是指給小車下達目標位置指令，小車在避開障礙物的前提下進行運動到目標點位的最佳路徑軌跡選擇。實現自主導航的核心是機器人的定位和路徑規劃。在該功能的框架中有兩個功能包：move_base功能包和amcl功能包，如圖1所示。

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

2.2" 多機器人編隊

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

在編隊過程中，領航者基于地圖中的環境信息，通過軌跡優化等方法可以優化出路徑最短的路徑，同時根據領航者當前的位置和預期位置計算得出位置誤差反饋控制調整領航者的位置，同時將目標位置信息發送給跟隨者，跟隨者根據接收到的目標位置信息進行移動。實驗中增加了通信模塊，能夠將領航者的位姿信息以及運動軌跡通過話題節點發布給跟隨者，跟隨者通過訂閱接收信息并根據相互之間的距離形成編隊，借助多機器人之間的通信實現編隊控制。在一個編隊系統中，只能有一個最高級別的領航者。該方法的優點是僅需領航者的位置和姿態信息，領航者和跟隨者之間的相對位置差關系，即可實現協同編隊。協同編隊的結構和運動軌跡由領航者控制，不受跟隨者的限制。同時該方法未考慮跟隨者避障區域范圍的信息未傳遞給領航者，容易造成領航者成功避障，某一個或多個跟隨者避障失敗的情況。

3" 實驗驗證

3.1" 單機器人室內建圖比較

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

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

以Gmapping算法為例探究小車在未知環境中建圖的過程。首先使用SSH完成遠程PC端與小車的通信連接，確認網絡配置成功后開始地圖創建。輸入指令打開Gmapping算法節點，啟動rviz仿真環境，獲得初始時刻激光雷達采集的點云數據，如圖4所示。

通過開啟鍵盤遙控節點，在實驗場地內ROS機器人順時針方向運動。建圖結果如圖5所示，該圖表明激光雷達獲得環境點云數據隨時間積累，不斷構成了實驗場景的環境信息。觀察整個建圖過程可以發現當ROS機器人離黃色小板凳較遠時并無法探測出其信息，所以在地圖上并不能顯示出小板凳的存在，但當ROS機器人行駛到如圖5（c）所示的工作區位置時，距離小板凳的距離較近，可以掃描出它的存在。當小車繞環境一周回到初始位置，實現了回環。環境信息建圖完成，與實際環境信息對比可以發現，建圖結果與實際場景相符合，建圖精度較高。

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

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

根據多方面比較，Gmapping算法計算量小，雖然沒有回環檢測但在小范圍的室內建圖能得到不錯的效果，最終選擇Gmapping算法進行實驗環境的地圖構建，并使用該算法建圖的結果圖6（a）作為后續多小車協同編隊避障實驗的基礎。

3.2" 多機器人導航與避障實驗

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

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

給領航者設置一個目標點于工作區與陽臺連接處，小車以0.3 m/s的速度進行無障礙定點導航。實驗結果如表2所示。ROS小車協同完成了導航任務。

圖8呈現了整體避障過程，三輛小車保持三角形通信結構。首先在機器人視場內設置一個目標點，圖8（a）中箭頭所示方向為全局的路徑方向，三輛機器人小車始終以排列成三角形運動，從圖8（b）表明，當領航者發現障礙物時，同時未發現可行空間時，領航者對可行路徑進行規劃，即從左側行駛避開障礙物，并繼續向目標點前進，此時領航者將位置信息和規劃的路徑信息發送給跟隨者，使得三輛小車能夠保持原隊形避開障礙物，到達目標點位，如圖8（c）所示。

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

由實驗結果可以看出，跟隨者1處于外側，可以和領航者順利完成避障，到達目標點。但處于內側的跟隨者2由于PID控制擁有的滯后性，在跟隨領航者軌跡運動時不能及時避開障礙物，導致兩次實驗撞擊到了擋板，未能完成避障。且在整體避障中領航者定位精度不一致，擁有一定的跟蹤誤差，會導致小車編隊變形。

4" 結" 論

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

參考文獻：

[1] 任立敏.未知環境下多移動機器人編隊控制及協作搬運策略研究 [D].哈爾濱：哈爾濱工業大學，2013.

[2] 陳衛東，席裕庚，顧冬雷，等.一個面向復雜任務的多機器人分布式協調系統 [J].控制理論與應用，2002（4）：505-510.

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

[4] 靳士超.基于麥克納姆輪的全向智能移動機器人導航系統研究 [D].蘇州：蘇州大學，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—），男，漢族，重慶人，本科在讀，研究方向：大數據與SLAM；韓銘偉（2003—），女，漢族，山東德州人，本科在讀，研究方向：大數據與SLAM；胡斌（2003—），男，漢族，福建莆田人，本科在讀，研究方向：大數據；薩逸娜（2002—），女，蒙古族，北京人，本科在讀，研究方向：大數據；王孟瑜（2003—），女，漢族，北京人，本科在讀，研究方向：大數據。