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

（南京工程學(xué)院 機(jī)械工程學(xué)院，江蘇 南京 211167）

0 引 言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖2 車間布局地圖對照圖

3 移動機(jī)器人導(dǎo)航算法及實(shí)現(xiàn)

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

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

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

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

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

3.2 改進(jìn)的A*算法

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

式中：pmotion為運(yùn)動過程中的功率耗損；pequipment為機(jī)器人電子設(shè)備的功率耗損。

移動機(jī)器人在運(yùn)動過程中克服牽引阻力所損耗的能量為：

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

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

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

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

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

3.3 AMR 導(dǎo)航系統(tǒng)的實(shí)現(xiàn)步驟

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

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

（3）設(shè)定機(jī)器人的地面投影footprint，在Nav2 導(dǎo)航算法包中，將params.yaml 文件中footprint 的值修改為實(shí)際機(jī)器人地面投影大小的值。

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

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

4 結(jié) 語

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

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