ROS2 環(huán)境下移動機器人導航及路徑規(guī)劃研究

江 航，邱勝海，孫小肖，王 清，郭子鵬，程澤鵬

（南京工程學院 機械工程學院，江蘇 南京 211167）

0 引 言

隨著新一代信息技術的發(fā)展，移動機器人相關技術得到迅速發(fā)展。目前在制造領域，大多數(shù)物料運輸搬運機器人是自動導引小車（Automated Guided Vehicle, AGV）形式，其自身安裝有感應電磁與光線等信息傳感器，能夠沿著預定軌道自主行駛。隨著物聯(lián)網(wǎng)的普及應用及智能制造與數(shù)字孿生的推廣應用，傳統(tǒng)的AGV 小車已經(jīng)逐步被更智能、更自由、更安全的AMR 移動機器人取代。

ROS 系統(tǒng)以其開源性、靈活性佳在機器人領域廣受關注，針對AMR 移動機器人，ROS 系統(tǒng)提供了導航所需的SLAM建圖算法與路徑規(guī)劃算法，能夠通過算法進行靈活自主的行駛和避障，增加了其機動性與安全性。目前國內(nèi)外學者對智能制造、數(shù)字孿生及ROS 的機器人應用研究理論較多，在實際應用場合缺少規(guī)范及成熟的技術，如：S.Gatesichapakorn[1]等、P. Phueakthong[2]等、F. Martín[3]等在ROS 中利用2D 激光雷達、RGB-D 相機、PCL 獲得三維深度點云用于障礙物檢測，將2D 激光雷達和RGB-D 相機互補，增強了移動機器人導航功能；陶飛[4]等人針對新一代信息技術發(fā)展的背景，提出數(shù)字孿生的概念、系統(tǒng)組成、運行機制、特點、關鍵技術等，探討了CPS 與信息交互和共融理論及實現(xiàn)方法；施佳宏[5]等人提出面向生產(chǎn)線仿真的數(shù)字孿生邏輯模型構建方法，并通過生產(chǎn)線實例仿真驗證了建模方法的有效性；曹遠沖[6]等人提出了一種基于數(shù)字孿生的復雜產(chǎn)品離散裝配車間動態(tài)調(diào)度方法，構建了數(shù)字孿生的調(diào)度框架和調(diào)度模型，構建調(diào)度原型并驗證了該方法的有效性和可行性；褚俊嫻[7]以石化行業(yè)某包裝車間為研究對象，對運輸任務與AGV 之間的匹配策略和AGV 的行走路徑問題進行探討，建立AGV調(diào)度優(yōu)化模型，并結合Pareto 理論與改進粒子算法求解調(diào)度問題，同時根據(jù)車間電子地圖，應用改進A*算法解決單個AGV 的行走路徑問題，為車間實現(xiàn)物流智能化提供重要的理論依據(jù)和技術支撐；溫淑慧[8]等設計了基于ROS 的移動機器人自主建圖與路徑規(guī)劃系統(tǒng)，利用基于頭尾雙向搜索的A*算法進行全局路徑規(guī)劃，采用DWA 算法完成局部避障工作及自主導航任務；王東霞[9]開發(fā)了移動機器人導航系統(tǒng)，實現(xiàn)了機器人的定位、繪圖、導航等功能。綜上所述，國內(nèi)外學者對ROS 移動機器人進行了廣泛研究，包括ROS1 系統(tǒng)的定位與導航等，但對ROS2 系統(tǒng)的研究卻較少。ROS2系統(tǒng)增加了對多機器人系統(tǒng)的支持，同時在ROS1 的基礎上支持MCU 等嵌入式微控制器，實現(xiàn)了跨平臺運行。因此，基于ROS2 系統(tǒng)，對移動機器人導航及路徑規(guī)劃系統(tǒng)進行設計與開發(fā)，并通過自研的移動機器人進行現(xiàn)場測試，滿足了系統(tǒng)的功能及性能要求。

1 AMR 導航與路徑規(guī)劃系統(tǒng)總體設計

AMR 導航與路徑規(guī)劃系統(tǒng)總體框架采用2 層分布式架構，從下往上分別是硬件層和軟件層，如圖1 所示。

圖1 AMR 系統(tǒng)總體框架

系統(tǒng)2 層構成描述如下所示：

（1）硬件層：硬件層包括2D 激光雷達、IMU 以及輪轂伺服電機三種硬件。激光雷達是如今移動機器人領域最常見、最重要的傳感器之一，通過360°全方位掃描提供平面點云的信息，使用驅動程序啟動雷達工作；IMU 包含眾多運動傳感器，主要用來測量機器人角速度和加速度，本文中的IMU主要提供機器人的角度信息；輪轂伺服電機內(nèi)帶有編碼器，主要能夠提供輪轂轉速、位置等信息，控制器通過編碼器傳遞的脈沖來確定位置。

（2）軟件層：軟件層包含SLAM 建圖模塊和Navigation2導航模塊。建圖時需要激光雷達信息和機器人的位姿信息，其中，機器人位姿信息通過里程計與IMU 提供的數(shù)據(jù)融合之后得到。再使用SLAM Toolbox 包實現(xiàn)對周圍環(huán)境的二維地圖創(chuàng)建，保存后提供給導航使用。導航時，機器人啟動之后通過調(diào)用Navigation2 包加載建成二維地圖，通過Rviz2 可視化界面顯示二維地圖，確定機器人初始位姿，給出目標位置，調(diào)用路徑規(guī)劃算法或自定義優(yōu)化算法實現(xiàn)自主導航與路徑規(guī)劃。該層在ROS2-Galactic 開發(fā)環(huán)境中實現(xiàn)。

軟件層與硬件層之間通過Modbus 通信協(xié)議、MQTT 物聯(lián)網(wǎng)通信協(xié)議進行連接與信息傳遞。

2 多傳感器數(shù)據(jù)融合與地圖構建技術

AMR 機器人狀態(tài)信息包括位姿、行走距離、速度、旋轉角度、電量消耗等，由不同的傳感器采集，通過數(shù)據(jù)采集與控制系統(tǒng)對各類信息進行有效的數(shù)據(jù)融合，具體描述如下：

（1）2D 激光雷達：激光雷達分為節(jié)點的發(fā)布與訂閱兩部分，啟動后可以在Rviz2 中看到激光雷達掃描到的平面點云的信息，這些信息將會在建成的二維地圖中顯示出來。

（2）輪式里程計計數(shù)及計算：輪式里程計通過統(tǒng)計編碼脈沖在單位時間內(nèi)相對增加或減少的值轉化而來。每0.1 s讀取一次編碼器脈沖值，相鄰兩次讀數(shù)之差即為0.1 s 編碼器的增量值，依次推測出機器人行走的距離和速度，又由兩輪編碼器數(shù)值之差可以推算出機器人旋轉的角度和角速度，在此基礎上對小車進行里程計發(fā)布。里程計信息包括父坐標系、子坐標系、x坐標值、y坐標值、線速度、角速度等屬性值。

（3）IMU 數(shù)據(jù)：IMU 主要通過陀螺儀測算出機器人旋轉的角度值，其坐標系與里程計坐標系一致。

導航地圖構建主要采用軟件層的SLAM 建圖模塊完成，其過程為先掃描實際制造車間的三維地圖，再在編碼器里程計中設定坐標x，y以及偏航角速度vyaw為真值，IMU 中設定偏航角yaw 為真值，將這些參數(shù)整合，最后應用SLAM Toolbox 包建圖。完成的二維地圖如圖2（a）所示，實際車間地圖如圖2（b）所示。

圖2 車間布局地圖對照圖

3 移動機器人導航算法及實現(xiàn)

移動機器人導航系統(tǒng)基于ROS2 中搭載的Navigation2包實現(xiàn)。Navigation2 項目包繼承了ROS 中的Navigation 項目，其核心組件是ROS2 系統(tǒng)，主要目的是尋找一條安全的路徑控制機器人從A點移動到B點。使用Navigation2 包能夠實現(xiàn)動態(tài)路徑規(guī)劃、動態(tài)移動速度計算、躲避障礙物等功能。Navigation2 中提供的全局路徑規(guī)劃算法是傳統(tǒng)的Dijkstra 或A*算法，可以獲得最短路徑，但并沒有考慮能量的消耗，并不適用制造車間物流檢測AMR 的路徑規(guī)劃。本文提出一種考慮能量消耗的改進A*算法，綜合考慮AMR 與地面摩擦、載重等因素，建立能耗模型并進行優(yōu)化。

3.1 傳統(tǒng)A*算法

A*算法常用于靜態(tài)環(huán)境下最優(yōu)路徑的求解，其根據(jù)代價函數(shù)在靜態(tài)環(huán)境下選擇一條從起點到終點距離最優(yōu)的路徑。傳統(tǒng)A*算法的代價函數(shù)如式（1）所示：

式中：g(n)表示起點到節(jié)點n的實際距離代價；h(n)表示節(jié)點n到目標點的預估距離代價。距離代價采用歐式距離計算，如式（2）所示：

式中：(xn,yn)、(xstart,ystart)、(xgoal,ygoal)分別代表的是節(jié)點n、起點start、終點goal 的坐標。

3.2 改進的A*算法

改進的A*算法在傳統(tǒng)A*算法基礎上增加了能耗最省約束條件，需要建立移動機器人能耗方程。AMR 能耗主要包括電子設備工作消耗的能量Eequipment和運動過程中所消耗的能量Emotion，因此AMR 能耗方程可表示為：

式中：pmotion為運動過程中的功率耗損；pequipment為機器人電子設備的功率耗損。

移動機器人在運動過程中克服牽引阻力所損耗的能量為：

式中：W為運動過程的總功；Ftraction為運動過程的總牽引力；Ff、Fair、Fg為運動過程中的摩擦力、空氣阻力以及重力斜面水平分力；L為移動距離；移動機器人在運動過程中始終保持勻速行駛且速度低。因此，由式（3）、式（4）可知機器人在運動過程中的總能耗Erobot：

式中：m為機器人總質量；g 為重力加速度；μ為摩擦系數(shù)；φ為坡度；s為移動距離；k為比例系數(shù)。

為了表征復雜環(huán)境下機器人的移動距離和能量消耗，Mejri 等[10]構建了基于距離的鄰接矩陣AD與基于能量的鄰接矩陣AE。以能量矩陣AE(i,j)為改進A*算法的搜索準則：

式中：μi,j為位置i到位置j的平均摩擦系數(shù)?？紤]現(xiàn)實坡度對機器人移動的能耗影響，采用水平地面的鄰接矩陣Aφ(i,j)，其表達式如式（7）所示：

式中：Aφ(i,j)表示位置i到位置j的坡度大小以及坡度狀態(tài)，相鄰位置i與j數(shù)值差的絕對值代表對應區(qū)域間的坡度，正負表示移動機器人爬坡與下坡兩種狀態(tài)。結合式（1）、式（6）、式（7）可得基于能耗成本的路徑搜索新準則：

3.3 AMR 導航系統(tǒng)的實現(xiàn)步驟

（1）配置TF 坐標變換，實現(xiàn)地圖到里程計、里程計到底盤坐標系和底盤坐標系到各傳感器坐標系的轉換。

（2）設置里程計與傳感器，該任務在建圖時已經(jīng)配置完成，此時啟動相應節(jié)點即可。

（3）設定機器人的地面投影footprint，在Nav2 導航算法包中，將params.yaml 文件中footprint 的值修改為實際機器人地面投影大小的值。

（4）應用C++語言實現(xiàn)改進型A*算法，并將其命名為SeaStar。Nav2 已包含多個算法，例如Dijkstra、A*及混合A*算法。測試AMR 為二輪差速機器人，為了能在ROS2 中應用SeaStar 算法，只需在Nav2 的規(guī)劃服務器中添加該算法，重新編譯后便可調(diào)用SeaStar 算法。

（5）實體機器人進行導航測試，啟動機器人節(jié)點，即啟動雷達、IMU、里程計發(fā)布節(jié)點，在nav2_car 工作空間中運行所保存的地圖map.yaml。使用系統(tǒng)中的“2D Pose Estimate”功能，系統(tǒng)估算出機器人的實際位置，啟動導航。使用“Nav2 Goal”功能，在地圖上指定一個目標位置，AMR 便能夠通過SeaStar 算法規(guī)劃出一條能耗最小路徑，通過計算AMR 電池能量消耗數(shù)值，與傳統(tǒng)A*算法進行比較，改進SeaStar 算法節(jié)省約14.6%的能耗。

4 結 語

對AMR 導航系統(tǒng)和路徑規(guī)劃算法進行研究，通過參考ROS2 系統(tǒng)中提供的Slam Toolbox 包和Navigation2 包設計基于ROS2 的機器人導航系統(tǒng)。為使AMR 在運動過程中能耗最小，提出一種改進A*算法并在ROS2 中實現(xiàn)。對自研的AMR 進行試驗，結果表明，該系統(tǒng)能夠實現(xiàn)快速建圖與最小能耗路徑規(guī)劃，為低碳制造提供了可行的途徑。

注：本文通訊作者為邱勝海。