基于ROS與融合算法的AGV路徑規劃研究

馮浩然,吳瑞明,傅陽,陳凱旋,程強,李陽

(浙江科技學院機械與能源工程學院，浙江杭州 310023)

0 前言

隨著人工智能的應用和發展，傳統生產運輸方式將逐步被淘汰，自動引導車(Automated Guided Vehicle，AGV)的應用與開發得到了越來越多的關注，并應用于各個領域。AGV在智能制造業中占有極其重要的地位，是現代智能倉儲和物流系統的關鍵組成部分。AGV具有自動化程度高、柔性化程度高以及可靠性強等優點，作為一種用于物流運輸的移動機器人，它已廣泛應用于物流倉儲及煙草等行業，而其路徑規劃問題也已成為當下研究的熱點。

AGV已廣泛應用于各種工程、運輸和物流等領域，而路徑規劃在AGV的運動控制中極為關鍵，且路徑規劃算法的效率直接影響移動機器人的尋路效率。路徑規劃可以分為全局和局部規劃，取決于環境和目標的性質。針對靜態環境，目前已經有許多算法能夠解決全局路徑規劃問題，如動態窗口算法、A算法、D算法、蟻群算法、遺傳算法等。遺傳算法在尋找全局最優解的過程中，是一個隨機搜索的過程，當節點增多時，搜索時間會變得較長。而A算法是一種啟發式搜索算法，也是目前應用最廣泛的算法之一，實現容易，搜索效率高，具有一定方向性。傳統的A算法也存在轉折點過多、節點冗余、避障安全性考慮不充分等問題。傳統A算法傾向于關注最短路徑，通常產生一條接近障礙物的路徑。 而在實際應用中，AGV可能會偏離計劃軌跡，這是由傳感器測量誤差和車輪的滑移引起的。選用動態窗口法進行路徑規劃雖然可以使AGV具有優良的避障安全性，路徑平滑，但很容易陷入局部最優，無法以最短距離到達目標點。針對上述問題，對傳統A算法和動態窗口法進行改進并融合。利用A算法進行全局路徑搜索后，去除多余節點和改進安全避障距離；結合改進動態窗口法完成局部路徑規劃，為AGV規劃出一條避障安全性好、平滑度高、具有實時性的最優路徑。

1 改進A*算法

1.1 傳統A*算法

A算法因簡單高效而被廣泛應用。它是一種典型的啟發式搜索算法，也是一種圖形搜索算法，通過評價函數來確定搜索方向，并將啟發式信息用于評價函數。這不僅利于找到最優路徑，而且可減少搜索時間。該算法從起始節點開始構建路徑，不斷將當前節點的周圍柵格中心點加入OpenList，且不斷選取OpenList列表中值最小的點作為當前節點，直到當前節點的周圍節點出現目標節點或者OpenList為空時停止。該算法公式為

(,)=(,)+(,)

(1)

距離函數(,)一般取歐氏距離或曼哈頓距離，為提高算法準確率，文中采用歐氏距離：

(2)

其中：、分別為起點的坐標與坐標；、分別為目標點的坐標與坐標。

1.2 設置OpenList訪問時間閾值

在傳統A算法中，當當前節點擴展子節點時，每次都會擴展所有候選節點。這種方式在復雜的環境中可能會存在無效搜索，影響算法運行效率。因此，在傳統A算法基礎上加以改進，避免陷入局部最優解，以真正規劃全局路徑，并提高搜索能力。具體改進方法：每次檢測OpenList中的第一個插入節點，判斷它是否在經過一定時間閾值后被擴展，如果沒有，則該節點將被視為最高優先級擴展。

1.3 Floyd算法優化

Floyd算法指采用動態規劃的思想來搜索最短路徑，用于求解兩點之間的最優距離問題。采用Floyd算法和A算法相結合的改進策略，可以減小路徑長度，滿足應用需求。Floyd算法原理如圖1所示。

圖1 Floyd算法原理示意

設(,)為點與點之間的距離，(,)為點到點的路線，而在圖1中，如果與之間存在障礙，則設(,)=∞，與之間無法直接連接通行。

設點是點與點之間的計劃節點，如果:

(,)+(,)=(,)

(3)

則：

(,)=(,)+(,)

(4)

(,)=→→

(5)

設點是點與點之間的計劃節點，如果：

(,)+(,)<(,)+(,)

(6)

則：

(,)=(,)+(,)

(7)

(,)=→→

(8)

刪除點，優化路徑為從點到點再到點。采用Floyd算法優化A算法規劃的路徑，可以去除冗余點，進一步優化路徑。

1.4 安全避障路徑的改進

AGV運動過程中可能會與障礙物發生碰撞，對此進行算法優化。若相鄰節點和當前節點的父節點關于當前節點的方向相同，則減小(,)值，反之則增加(,)值。在擴展子節點的過程中，優先擴展水平方向上的節點，隨后判斷水平方向上是否存在障礙物，如果存在，則不擴展與障礙物相鄰的對角線節點；如果不存在，則擴展對角線方向節點。安全避障路徑生成策略的流程如圖2所示。

圖2 避障優化流程

2 動態窗口法

在倉儲物流環境中，室內大多數物體按照一定設計放置，其位置已知，但存在一些移動物體如工人，因此很難獲取完整環境信息。為避免撞傷人，保證AGV的安全性，在全局路徑規劃的基礎上，采用動態窗口法進行局部路徑規劃。

由于動態窗口法具有符合AGV的運動特征、靈活性強等優點，動態窗口法成為動態環境下局部避障的主要算法。在AGV運動過程中，動態窗口算法實時預測AGV的速度與狀態空間，獲取其多組預測時間內的可行運動軌跡，再根據設計的評價函數選取最優運動軌跡的線速度與角速度，完成路徑規劃，從而獲得一條更安全且平滑的最優局部路徑。

2.1 AGV運動模型

動態窗口算法將AGV的位置控制轉換為速度控制。在利用速度模式對AGV運動軌跡進行預測時，首先需要對機器人的運動模型進行分析，通過對AGV的速度進行空間采樣，模擬出其運動軌跡。AGV的線速度和角速度的變化能夠反映出AGV的運動狀態，(,)代表AGV在世界坐標系下的平移速度與角速度，表征其軌跡。通過評價函數在所有可行軌跡里選取最佳軌跡，在采樣周期Δ內，假設AGV作勻速直線運動，則其運動模型為

(9)

2.2 AGV速度采樣

在速度搜索空間中,根據AGV及其環境所得到的一定范圍內的無窮多組(,)對采樣速度范圍進行約束，其限制因素可分為3種：AGV本體速度約束、加速度約束以及障礙物約束。

AGV速度約束:

={(,)|∈[,],∈[,]}

(10)

AGV加減速度約束。動態窗口下的采樣周期內，由于AGV加速度受電機力矩限制，AGV受到該限制所帶來的最大、最小速度限制：

={(,)|∈[-Δ,+Δ],∈[-Δ,+Δ]}

(11)

其中：和分別為當前時刻的線速度和角速度；和分別為當前時刻的最大速度和最大加速度；和分別為當前時刻的最小速度和最大減速度。

AGV制動約束。考慮到AGV安全性，需要保證AGV不與障礙物發生碰撞，則在局部路徑規劃時的最大減速度條件下，其速度約束為

(12)

式中：(,)是(,)對應軌跡中離障礙物的最近距離。

2.3 AGV評價函數

對于速度空間內若干組可行的采樣速度，設計使AGV盡可能規避障礙物及最短時間內到達指定點的評

價函數，以選取最優路徑軌跡。設計的評價函數為

(,)=(,)+(,)+(,)

(13)

其中：(,)為當前速度方向與目標位置之間的偏差角的評價函數；(,)為速度軌跡中與障礙物之間的最短距離；(,)為當前軌跡速度大小的評價函數;、、分別為各項的加權系數。

3 融合算法

利用傳統動態窗口算法進行規劃時，如果沒有預先進行全局路徑規劃而只有指定位置方向的指引，則在障礙物較多情況下，運動過程中容易陷入局部最優，導致規劃軌跡無法達到全局最優。而A算法可以得到全局最優解，故將兩者融合進行路徑規劃，充分發揮兩算法的優點，實現路徑長度、平滑度和安全性的全方面優化，提升動態規劃路徑的全局最優性。

3.1 環境模型描述

環境建模采用柵格法，如圖3所示，利用單位柵格對地圖進行劃分，形成正方形柵格地圖，并根據實際環境將AGV可自由移動區域定為白色柵格，障礙區域定為黑色柵格，如圖3所示。

圖3 環境建模柵格圖

3.2 仿真實驗

為驗證所提出的融合算法在復雜環境中進行路徑規劃的有效性，進行仿真對比驗證。為充分體現對比效果，設AGV的最大線速度為1 m/s，最大角速度為20.0 rad/s，最大線加速度為0.2 m/s，最大角加速度為50.0 rad/s，線速度為0.01 m/s，角速度為1 rad/s，周期為3.5 s，各算法中設定的AGV運行參數相同。實驗環境為運行內存為16 GB的64位WIN10操作系統，實驗平臺為MATLAB2019a。

在建立的柵格模型中，假設AGV路徑規劃中的起點坐標為(30,30) mm、終點坐標為(8,2) mm。在靜態環境中運行傳統A算法，結果如圖4所示。在靜態環境中運行改進A算法，如圖5所示，成功地完成了靜態全局路徑規劃。為驗證動態環境中的路徑規劃，在靜態環境運行結束的基礎上，設置黃色柵格的臨時動態障礙物。圖6所示為AGV局部動態環境下運行后的路徑規劃運行過程，圖7所示為AGV局部動態環境下運行后的結果。表1所示為3種算法性能指標對比。利用傳統A算法得到的路徑總長度為44.384 3 mm,用時0.535 31 s，而改進后的A算法路徑總長度為41.218 9 mm，用時0.290 545 s，相比傳統A算法，改進A算法轉折點和冗余路段更少，路徑更短，平滑性更好且經過障礙物時可保持一定距離，安全性更好。利用動態窗口算法可實現路徑的實時規劃，彌補A算法時效性差的缺點，可以實現規避已規劃路徑上動態障礙物的功能，得到一條平滑、安全的最優路徑。

圖4 傳統A*算法路徑規劃結果

圖5 靜態環境路徑規劃結果

圖6 動態避障路徑規劃過程

圖7 動態避障路徑規劃結果

表1 各算法性能參數比較

3.3 輸出運動參數

所提出的改進A算法與動態窗口法的融合算法可實時輸出AGV的控制參數，有利于AGV閉環控制。AGV各參數變化如圖8—圖10所示，當AGV在局部路徑中躲避障礙物時，可以看出線速度和角速度均減小了，且位姿發生對應的變化，驗證了算法的可行性。

圖8 AGV姿態變化曲線 圖9 AGV線速度變化曲線

圖10 AGV角速度變化曲線

4 實驗驗證

將改進后的融合算法應用到所研究的AGV上，AGV車體框架采用6061鋼材制作，車輪采用聚氨酯制作，AGV控制系統組成如圖11所示。本地PC與AGV上的樹莓派4B進行數據交互，樹莓派控制AGV在未知環境中通過思嵐A2激光雷達實時收集周圍環境信息，并實時構建地圖。通過WiFi，AGV能夠接收本地PC發布的控制指令，向控制單元的STM32F103單片機發送速度控制指令，實現對電機的驅動控制。驅動單元部分采用兩個中間驅動輪差動驅動，驅動電機為AGV提供主要的驅動力。驅動電機決定AGV的功率性能和負載能力，從性能方面應選擇中大24 V直流無刷電機作為AGV的驅動電機。

圖11 AGV控制系統組成

融合算法的驗證結合ROS機器人操作系統進行，通過激光雷達掃描實際環境并建圖，如圖12所示；再設定機器人作業起點與目標點。

圖12 構建的實際環境地圖

通過SLAM算法實時獲取機器人當前位置信息，采用優化后的算法規劃AGV當前位置至終點的最優路徑，如圖13所示。

圖13 AGV初始路徑規劃圖

在AGV運動過程中，通過激光雷達實時檢測動態環境變化，轉為障礙物信息發送給AGV，得到環境變化后的最優路徑，AGV按照最優路徑進行作業。實際環境中AGV運動時會產生略微滑移現象，轉角位置會有些許偏移，但是并不影響AGV的路徑規劃和避障，因此可忽略不計。實驗過程如圖14所示，首先AGV規劃好最優路徑，在遇到障礙物后實時更改路徑，實時選擇最優路徑。通過實驗驗證，改進算法后的AGV能有效避開所有障礙物，平穩到達目標點，具有良好的避障能力和安全性，且路徑平滑，滿足實際作業要求。

圖14 AGV路徑規劃過程

5 結語

為提高在復雜環境下AGV的運動規劃效率，并提高其安全合理的動態路徑規劃能力，提出了一種改進融合算法。通過改進A算法的啟發函數，去除多余節點和改進安全避障距離，有效克服了傳統算法轉折角過多而影響AGV的正常運行作業問題，提高了AGV的安全性。同時，采用動態窗口算法實現路徑的實時規劃，彌補了傳統A算法時效性差的缺點，實現規避已規劃路徑上的動態障礙物的功能，可規劃出一條具有實時性的最優路徑。通過仿真實驗，驗證了該方法切實可行，更符合實際應用。進一步地，可將它與機器視覺相結合，對復雜環境的AGV路徑規劃進行更深入的研究。