基于ROS的移動(dòng)機(jī)器人混合路徑規(guī)劃研究

摘 要：為有效提高機(jī)器人的路徑規(guī)劃能力，設(shè)計(jì)了一款基于ROS移動(dòng)機(jī)器人的混合路徑規(guī)劃系統(tǒng)。首先，針對(duì)室內(nèi)定位和地圖構(gòu)建問(wèn)題，研究了基于2D激光雷達(dá)的SLAM系統(tǒng)以提高定位精度和地圖構(gòu)建效率；其次，將改進(jìn)A*算法和改進(jìn)DWA算法相結(jié)合，形成了一種混合路徑規(guī)劃算法，該算法可以應(yīng)對(duì)不同的環(huán)境，如無(wú)障礙物、狹窄通道、U型障礙物和復(fù)雜環(huán)境等。實(shí)驗(yàn)結(jié)果表明，所設(shè)計(jì)的混合路徑規(guī)劃算法能成功將移動(dòng)機(jī)器人導(dǎo)航到目標(biāo)點(diǎn)，在各種環(huán)境下表現(xiàn)良好，且尋路時(shí)間短，適應(yīng)性強(qiáng)。這種混合路徑規(guī)劃算法提高了移動(dòng)機(jī)器人的自主導(dǎo)航性能和工作效率，為實(shí)現(xiàn)機(jī)器人的自主導(dǎo)航能力提供了有效支持。

關(guān)鍵詞：ROS移動(dòng)機(jī)器人；A*算法；DWA算法；混合路徑規(guī)劃；自主導(dǎo)航；2D激光雷達(dá)

中圖分類號(hào)：TP39 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：2095-1302（2024）10-0-05

0 引 言

在機(jī)器人研究與應(yīng)用領(lǐng)域，定位與路徑規(guī)劃是實(shí)現(xiàn)機(jī)器人自主導(dǎo)航的關(guān)鍵[1]。通過(guò)準(zhǔn)確定位和規(guī)劃最優(yōu)路徑，機(jī)器人可以高效完成各種任務(wù)，如物料搬運(yùn)、勘測(cè)、拍攝等。近年來(lái)，隨著ROS機(jī)器人操作系統(tǒng)的廣泛應(yīng)用，研究者們開始使用ROS進(jìn)行機(jī)器人定位與路徑規(guī)劃研究。然而，目前仍存在許多挑戰(zhàn)，如復(fù)雜環(huán)境下定位準(zhǔn)確性不高、路徑規(guī)劃的效率低下與安全性差等問(wèn)題。因此，有必要深入研究ROS機(jī)器人定位與路徑規(guī)劃，以提高機(jī)器人導(dǎo)航的性能和可靠性。

在機(jī)器人定位方面，傳統(tǒng)方法包括里程計(jì)定位、激光雷達(dá)定位和視覺定位[2]等。其中，里程計(jì)定位通過(guò)計(jì)算每輪的轉(zhuǎn)動(dòng)距離來(lái)估計(jì)機(jī)器人的位姿，然而累積誤差會(huì)逐漸增大，影響定位的準(zhǔn)確性。激光雷達(dá)定位利用掃描獲得的地圖信息進(jìn)行機(jī)器人位置估計(jì)，但面臨數(shù)據(jù)配準(zhǔn)和實(shí)時(shí)性等挑戰(zhàn)。視覺定位則通過(guò)攝像機(jī)采集場(chǎng)景信息進(jìn)行定位，但受光照條件和特征提取等因素的影響。在路徑規(guī)劃方面，常見算法包括Dijkstra算法[3]、A*算法[4]和RRT[5]算法。Dijkstra算法適用于靜態(tài)環(huán)境且不考慮機(jī)器人的動(dòng)態(tài)性能，A*算法則在搜索過(guò)程中引入啟發(fā)式函數(shù)以提高搜索效率。而RRT算法基于概率方法隨機(jī)構(gòu)造樹狀結(jié)構(gòu)，可以快速找到可行路徑。

為提高移動(dòng)機(jī)器人的導(dǎo)航和路徑規(guī)劃能力，本文設(shè)計(jì)了基于ROS的2D激光雷達(dá)SLAM系統(tǒng)獲取周圍的環(huán)境信息以及加速度信息等，使用Gampping算法來(lái)實(shí)現(xiàn)機(jī)器人的自主定位和導(dǎo)航。為減少搜索時(shí)間，本文對(duì)A*算法和DWA（Dynamic Window Approach）算法進(jìn)行了優(yōu)化與改進(jìn)。

1 自主導(dǎo)航設(shè)計(jì)

1.1 AMCL定位系統(tǒng)

精準(zhǔn)定位對(duì)于移動(dòng)機(jī)器人導(dǎo)航的精確性至關(guān)重要。然而，在移動(dòng)過(guò)程中，機(jī)器人可能會(huì)出現(xiàn)打滑等問(wèn)題。在進(jìn)行移動(dòng)機(jī)器人導(dǎo)航和避障實(shí)驗(yàn)之前，需要測(cè)試機(jī)器人的定位能力，即準(zhǔn)確確定機(jī)器人在地圖中的位置（世界坐標(biāo)系）。一種常用的定位算法是蒙特卡羅定位（AMCL）方法，它基于粒子濾波器原理，用于估計(jì)機(jī)器人在自主導(dǎo)航過(guò)程中的方向和位置。蒙特卡羅定位算法利用粒子來(lái)代表機(jī)器人可能的狀態(tài)，并通過(guò)逐步更新粒子的方式逼近機(jī)器人的真實(shí)位置。每個(gè)粒子會(huì)與周圍環(huán)境中的障礙物信息進(jìn)行關(guān)聯(lián)評(píng)估，從而對(duì)機(jī)器人的方向和位置進(jìn)行估計(jì)。通過(guò)蒙特卡羅定位算法，機(jī)器人能夠在實(shí)際運(yùn)行過(guò)程中更準(zhǔn)確地感知和定位自身位置，為后續(xù)的導(dǎo)航和避障奠定基礎(chǔ)。

在啟動(dòng)導(dǎo)航時(shí)，AMCL會(huì)根據(jù)所配置的參數(shù)情況初始化其粒子濾波器。通過(guò)調(diào)整參數(shù)來(lái)控制定位的準(zhǔn)確性和計(jì)算效率。通過(guò)調(diào)整AMCL的參數(shù)和使用Rviz中的“2D Pose Estimate”按鈕來(lái)初始化機(jī)器人的位姿，可以提高導(dǎo)航定位的準(zhǔn)確性和計(jì)算效率，從而更好地滿足導(dǎo)航需求[6]。

1.2 基于2D激光雷達(dá)的SLAM系統(tǒng)

根據(jù)傳感器的分類，SLAM的實(shí)現(xiàn)方法[7]主要分為兩類：視覺SLAM和激光SLAM。本文采用激光雷達(dá)作為感知設(shè)備，以解決視覺SLAM中計(jì)算復(fù)雜度高、占用CPU和內(nèi)存空間大的問(wèn)題。相對(duì)于視覺信息，激光雷達(dá)通過(guò)發(fā)射激光和接收其反射數(shù)據(jù)來(lái)感知周圍環(huán)境，并進(jìn)行地圖構(gòu)建和位置估計(jì)。與攝像頭相比，激光雷達(dá)的工作不受環(huán)境和光照影響，具有更強(qiáng)的魯棒性。

激光雷達(dá)通過(guò)測(cè)量物體表面的距離和反射數(shù)據(jù)來(lái)構(gòu)建環(huán)境地圖，可以提供更精確的三維點(diǎn)云數(shù)據(jù)。激光雷達(dá)生成的數(shù)據(jù)量相對(duì)較小，處理速度較快，并且無(wú)需復(fù)雜的計(jì)算過(guò)程。此外，激光雷達(dá)適用于室外環(huán)境，光照影響較弱，不受黑暗條件的限制。因此，激光雷達(dá)在實(shí)時(shí)SLAM應(yīng)用中具有很大的優(yōu)勢(shì)。

本文使用基于RBPF（Rao-Blackwellized Particle Filter）濾波算法的Gmapping算法進(jìn)行定位與建圖[8]。Gmapping算法通過(guò)改進(jìn)粒子采樣和重采樣過(guò)程，有效減少了所需的粒子數(shù)量，同時(shí)提高了算法的計(jì)算效率。傳統(tǒng)的RBPF算法可能需要大量的粒子進(jìn)行重復(fù)采樣，為解決粒子退化和多樣性降低的問(wèn)題，Gmapping算法采用了改進(jìn)提議分布和選擇性重采樣方法。通過(guò)改進(jìn)提議分布，可以更準(zhǔn)確地估計(jì)機(jī)器人的位置和姿態(tài)，從而減少了所需的粒子數(shù)量。同時(shí)，設(shè)置合適的閾值，只有當(dāng)采樣粒子的數(shù)量不足時(shí)才進(jìn)行重采樣。這些改進(jìn)使得Gmapping算法能夠在構(gòu)建地圖時(shí)具有較小的計(jì)算量，并能夠高效實(shí)現(xiàn)SLAM任務(wù)。通過(guò)優(yōu)化粒子采樣和重采樣過(guò)程，Gmapping算法能夠更好地應(yīng)對(duì)粒子退化和多樣性降低問(wèn)題，提供更可靠和準(zhǔn)確的地圖構(gòu)建和位置估計(jì)結(jié)果。

1.3 混合路徑規(guī)劃算法

1.3.1 改進(jìn)A*算法

改進(jìn)啟發(fā)函數(shù)h（n）可以改進(jìn)A*算法，能夠有效搜索最優(yōu)路徑。不同的啟發(fā)函數(shù)能夠在不同的領(lǐng)域展現(xiàn)更好的性能，呈現(xiàn)更好的效果。選擇適當(dāng)?shù)膯l(fā)函數(shù)是整個(gè)算法的關(guān)鍵，它需要基于問(wèn)題的特性進(jìn)行權(quán)衡和決策，以獲得最佳路徑解。

在傳統(tǒng)的A*算法路徑列表中一般會(huì)存在冗余點(diǎn)，生成的路徑也非最優(yōu)路徑。為解決A*算法在移動(dòng)機(jī)器人運(yùn)動(dòng)控制過(guò)程中出現(xiàn)的多余拐彎和路徑不平滑問(wèn)題，需逐個(gè)判斷第N個(gè)節(jié)點(diǎn)和初始點(diǎn)之間是否存在障礙物，并在第N個(gè)節(jié)點(diǎn)和初始點(diǎn)之間無(wú)障礙物的情況下，再判斷初始點(diǎn)和第N+1個(gè)節(jié)點(diǎn)之間是否存在障礙物。若無(wú)障礙物，則剔除冗余節(jié)點(diǎn)N；若初始點(diǎn)和第N個(gè)節(jié)點(diǎn)之間無(wú)障礙物，但是第N+1個(gè)節(jié)點(diǎn)和初始點(diǎn)之間存在障礙物，則第N個(gè)節(jié)點(diǎn)被標(biāo)記為新起始點(diǎn)。使用二分法逐步縮小搜索范圍，判斷節(jié)點(diǎn)和初始點(diǎn)之間是否存在障礙物，直到達(dá)到最小柵格單元。該方法能夠提高路徑規(guī)劃算法的效率，確保機(jī)器人能夠安全繞過(guò)障礙物并成功到達(dá)目標(biāo)點(diǎn)。算法節(jié)點(diǎn)剔除示意圖如圖1所示。

1.3.2 優(yōu)化局部路徑規(guī)劃算法

離散采樣機(jī)器人控制空間是指將機(jī)器人的連續(xù)控制空間離散成一組有限的點(diǎn)或狀態(tài)，然后根據(jù)機(jī)器人的當(dāng)前狀態(tài)來(lái)預(yù)測(cè)可能出現(xiàn)的情況。這種方法可以幫助機(jī)器人在復(fù)雜環(huán)境中進(jìn)行路徑規(guī)劃和運(yùn)動(dòng)控制。DWA算法實(shí)現(xiàn)了移動(dòng)機(jī)器人的實(shí)時(shí)路徑規(guī)劃和避障。通過(guò)使用一些度量標(biāo)準(zhǔn)，比如障礙物與目標(biāo)的距離、接近全局路徑的程度以及速度等，可以對(duì)每條可能的軌跡進(jìn)行評(píng)估。篩選不合適的路徑，避免與障礙物碰撞等。最終，從得分最高的運(yùn)動(dòng)軌跡中選擇最優(yōu)解，并將相應(yīng)的速度傳送給移動(dòng)平臺(tái)，指導(dǎo)機(jī)器人移動(dòng)。這種方法能夠提高機(jī)器人在復(fù)雜環(huán)境中的路徑規(guī)劃和控制能力，并且根據(jù)具體問(wèn)題和需求，可以選擇不同的度量標(biāo)準(zhǔn)和評(píng)估方法來(lái)適應(yīng)不同的場(chǎng)景和任務(wù)。這使得機(jī)器人能夠?qū)崟r(shí)適應(yīng)環(huán)境，并進(jìn)行路徑優(yōu)化和避障，以實(shí)現(xiàn)高效的移動(dòng)和導(dǎo)航。DWA算法流程如圖2所示。

當(dāng)移動(dòng)機(jī)器人面臨“U”型障礙物時(shí)，采用DWA算法仿真，機(jī)器人的路線圖顯示機(jī)器人在“U”型障礙物內(nèi)不斷轉(zhuǎn)動(dòng)，機(jī)器人朝著距離目標(biāo)點(diǎn)函數(shù)值較小的方向移動(dòng)，但始終無(wú)法逃離這個(gè)區(qū)域。這種情況在路徑規(guī)劃中被稱為局部極小值狀態(tài)，即機(jī)器人陷入了局部最優(yōu)解且無(wú)法找到更優(yōu)路徑。機(jī)器人陷入局部極小值示意圖如圖3所示。

本文采用沿墻走模式解決移動(dòng)機(jī)器人陷入局部極小值的問(wèn)題。機(jī)器人在“U”型障礙物內(nèi)進(jìn)行兩次或以上180°往返運(yùn)動(dòng)則判定為機(jī)器人陷入局部極小值狀態(tài)，機(jī)器人啟用沿墻走模式。機(jī)器人首先沿著“U”型障礙物的邊界逃離內(nèi)

部，直到找到合適的路徑退出沿墻走模式，并重新進(jìn)入正常的動(dòng)態(tài)窗口法模式。當(dāng)機(jī)器人與障礙物邊界過(guò)近時(shí)，引入β角度參數(shù)，調(diào)整速度和前進(jìn)方向來(lái)保持安全距離以避免碰撞；當(dāng)與障礙物邊界過(guò)遠(yuǎn)時(shí)，調(diào)整速度和前進(jìn)方向接近安全距離。通過(guò)沿墻走模式和控制β角度，機(jī)器人能夠更有效地逃離“U”型障礙物并繼續(xù)進(jìn)行路徑規(guī)劃。沿墻走示意圖如圖4所示。

2 移動(dòng)機(jī)器人混合路徑規(guī)劃環(huán)境實(shí)驗(yàn)

2.1 機(jī)器人硬件平臺(tái)搭建

機(jī)器人是基于ROS開發(fā)的阿克爾曼4輪機(jī)器人，使用了控制器、驅(qū)動(dòng)器和傳感器等硬件結(jié)構(gòu)?？刂葡到y(tǒng)基于樹莓派Jetson Nano和STM32控制板搭建。樹莓派是基于Linux系統(tǒng)的微型電腦，而STM32控制板用于發(fā)出電機(jī)驅(qū)動(dòng)信號(hào)和采集傳感器數(shù)據(jù)，如陀螺儀信息采集、電機(jī)控制、里程計(jì)信息采集。ROS主控與STM32之間的連接如圖5所示。

在機(jī)器人中用到很多控制器和外設(shè)，包括樹莓派、激光雷達(dá)、STM32控制器、電機(jī)、編碼器、雙路驅(qū)動(dòng)、藍(lán)牙、陀螺儀等，提供了串口和CAN接口方便用戶拓展控制。機(jī)器人控制器連接圖如圖6所示。

2.2 移動(dòng)機(jī)器人PC端和ROS環(huán)境配置

ROS的運(yùn)行主要依賴于Linux操作系統(tǒng)，其中與ROS兼容性較好的是Ubuntu系列。除Ubuntu外，還有其他一些操作系統(tǒng)也支持ROS，但穩(wěn)定性可能無(wú)法得到保證。

機(jī)器人系統(tǒng)中的上位機(jī)是一個(gè)計(jì)算資源有限的微型電腦。因此，需要在PC端安裝完整版本的ROS以運(yùn)行可視化軟件（如Rviz），并通過(guò)PC端對(duì)機(jī)器人端進(jìn)行ROS節(jié)點(diǎn)的訂閱和發(fā)布來(lái)控制機(jī)器人的上位機(jī)。兼容ROS的相關(guān)操作系統(tǒng)如圖7所示。在本文中，PC端安裝基于Ubuntu 16.04的ROS Kinetic版本?？紤]硬件兼容性，機(jī)器人端按照?qǐng)D8所示的配置流程安裝了基于Ubuntu 18.04的ROS Melodic版本。

2.3 PC端和ROS仿真環(huán)境測(cè)試

按照?qǐng)D9所示流程來(lái)配置、搭配Gazebo仿真環(huán)境，進(jìn)行地圖創(chuàng)建、路徑規(guī)劃、自主導(dǎo)航等仿真實(shí)驗(yàn)。在Gazebo仿真界面中添加深度相機(jī)、激光雷達(dá)等模塊完成機(jī)器人仿真配置。在Rviz界面中，顯示傳感器的深度信息或點(diǎn)云信息。在Rviz和Gazebo的基礎(chǔ)上對(duì)機(jī)器人進(jìn)行仿真，為接下來(lái)的地圖創(chuàng)建和導(dǎo)航實(shí)驗(yàn)奠定基礎(chǔ)[9-10]。

2.4 混合路徑規(guī)劃實(shí)驗(yàn)

A*算法在global_planner插件內(nèi)實(shí)現(xiàn)，用于全局路徑規(guī)劃；改進(jìn)的DWA算法在dwa_local_planner插件內(nèi)實(shí)現(xiàn)，用于局部避障和實(shí)時(shí)路徑規(guī)劃；通過(guò)move_base實(shí)現(xiàn)混合路徑規(guī)劃的全過(guò)程。

A*算法是一種高效的全局路徑規(guī)劃算法，它利用啟發(fā)性搜索和雙向搜索思想，能夠在復(fù)雜環(huán)境中快速找到最優(yōu)路徑。改進(jìn)的DWA算法將動(dòng)態(tài)窗口法和沿墻走模式相結(jié)合，實(shí)現(xiàn)了機(jī)器人的實(shí)時(shí)避障和路徑規(guī)劃。通過(guò)將這兩種算法融合，可以兼顧全局規(guī)劃的效率和局部避障的實(shí)時(shí)性。

混合路徑規(guī)劃過(guò)程由move_base管理和調(diào)度，它將全局路徑規(guī)劃和局部路徑規(guī)劃相結(jié)合，根據(jù)實(shí)時(shí)環(huán)境信息和機(jī)器人的狀態(tài)，動(dòng)態(tài)調(diào)整路徑規(guī)劃策略，使機(jī)器人能夠安全、高效地完成導(dǎo)航任務(wù)。通過(guò)將改進(jìn)A*算法和改進(jìn)的DWA算法相配合，可以提高路徑規(guī)劃的性能和魯棒性，適應(yīng)不同場(chǎng)景下的復(fù)雜導(dǎo)航需求。

通過(guò)4種地圖導(dǎo)航驗(yàn)證混合算法的可行性，分別是無(wú)障礙物環(huán)境、狹窄通道環(huán)境、U型障礙物環(huán)境和復(fù)雜環(huán)境（如圖10所示）?；旌纤惴〝?shù)據(jù)對(duì)比見表1所列。

根據(jù)表1的結(jié)果可以得出以下結(jié)論：

（1）本文的改進(jìn)A*算法和改進(jìn)DWA混合算法在無(wú)障礙物的地圖中與傳統(tǒng)的A*算法和傳統(tǒng)DWA混合算法生成的路徑長(zhǎng)度大致相當(dāng)，但本文的混合算法尋路時(shí)間更短；

（2）本文的混合算法在復(fù)雜的障礙物環(huán)境中能夠成功導(dǎo)航移動(dòng)機(jī)器人到目標(biāo)點(diǎn)。與傳統(tǒng)的混合算法相比，本文的混合算法在尋路時(shí)間方面展現(xiàn)出更好的性能，尤其是在路徑長(zhǎng)度增加的情況下，本文的混合算法在尋路時(shí)間方面表現(xiàn)得更為突出。

3 結(jié) 語(yǔ)

本文基于對(duì)移動(dòng)機(jī)器人定位的研究，成功在PC上安裝了Ubuntu 16.04操作系統(tǒng)和ROS Kinetic版本，建立了ROS開發(fā)環(huán)境。通過(guò)Rviz和Gazebo等可視化和物理仿真工具，實(shí)現(xiàn)了機(jī)器人的功能性仿真，在局部室內(nèi)環(huán)境中進(jìn)行了地圖構(gòu)建和路徑規(guī)劃實(shí)驗(yàn)，為后續(xù)移動(dòng)機(jī)器人在真實(shí)環(huán)境中的應(yīng)用和開發(fā)打下了基礎(chǔ)。

在路徑規(guī)劃研究方面，本文對(duì)A*算法和DWA算法進(jìn)行了改進(jìn)。A*算法能夠高效規(guī)劃全局最佳路徑，而DWA算法能夠?qū)崿F(xiàn)移動(dòng)機(jī)器人在運(yùn)行過(guò)程中的局部動(dòng)態(tài)避障。通過(guò)對(duì)傳統(tǒng)A*算法和DWA算法的改進(jìn)，本文提供了有效的理論基礎(chǔ)，以支持移動(dòng)機(jī)器人在真實(shí)環(huán)境下的研發(fā)和應(yīng)用。

注：本文通訊作者為孫婷婷。

參考文獻(xiàn)

[1]段廣學(xué).室內(nèi)機(jī)器人復(fù)雜環(huán)境下的自主導(dǎo)航方法研究[D].沈陽(yáng)：沈陽(yáng)工業(yè)大學(xué)，2019.

[2]韓昊旻，張崇明，陳志紅，等.基于ROS和激光雷達(dá)的AGV導(dǎo)航系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)[J]. 電子測(cè)量技術(shù)，2018，41（8）：112-117.

[3]李文超.移動(dòng)機(jī)器人環(huán)境建模與路徑規(guī)劃方法研究[D].邯鄲：河北工程大學(xué)，2020.

[4]王輝.基于ROS的機(jī)器人路徑導(dǎo)航系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[D].沈陽(yáng)：中國(guó)科學(xué)院大學(xué)（中國(guó)科學(xué)院沈陽(yáng)計(jì)算技術(shù)研究所），2019.

[5]劉文之. 基于激光雷達(dá)的SLAM和路徑規(guī)劃算法研究與實(shí)現(xiàn)[D]. 哈爾濱：哈爾濱工業(yè)大學(xué)，2018.

[6]張益輝，王長(zhǎng)寧，孫玲.基于A*算法的機(jī)器人路徑規(guī)劃與避障研究[J]. 微型電腦應(yīng)用，2020，36（2）：120-123.

[7]潘虎. 復(fù)雜環(huán)境下的移動(dòng)機(jī)器人自主路徑規(guī)劃算法研究[D]. 大連：大連海事大學(xué)，2018.

[8]張海燕，林志賢，郭太良. 機(jī)器人避障路徑規(guī)劃優(yōu)化控制仿真[J]. 計(jì)算機(jī)仿真，2017，34（9）：325-330.

[9]趙曉，王錚，黃程侃，等. 基于改進(jìn)A*算法的移動(dòng)機(jī)器人路徑規(guī)劃[J]. 機(jī)器人，2018，40（6）：903-910.

[10]李恒，楊亮，曾碧，等.基于ROS的移動(dòng)機(jī)器人仿真實(shí)驗(yàn)平臺(tái)設(shè)計(jì)與實(shí)現(xiàn)[J].電子設(shè)計(jì)工程，2022，30（14）：53-57.