ROS2 環境下移動機器人導航及路徑規劃研究

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

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

0 引 言

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

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

1 AMR 導航與路徑規劃系統總體設計

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

圖1 AMR 系統總體框架

系統2 層構成描述如下所示：

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

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

軟件層與硬件層之間通過Modbus 通信協議、MQTT 物聯網通信協議進行連接與信息傳遞。

2 多傳感器數據融合與地圖構建技術

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

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

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

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

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

圖2 車間布局地圖對照圖

3 移動機器人導航算法及實現

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

3.1 傳統A*算法

A*算法常用于靜態環境下最優路徑的求解，其根據代價函數在靜態環境下選擇一條從起點到終點距離最優的路徑。傳統A*算法的代價函數如式（1）所示：

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

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

3.2 改進的A*算法

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

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

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

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

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

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

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

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

3.3 AMR 導航系統的實現步驟

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

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

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

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

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

4 結 語

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

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