羅冬梅，魯玉軍，張振濤

（浙江理工大學(xué)機械工程學(xué)院，浙江杭州 310018）

0 引言

壓鑄機裝配是中小型壓鑄機制造過程中的一個重要環(huán)節(jié)，裝配流程主要分為機架部裝、電氣部裝、柱架部裝、油路部裝、鈑金部裝和打料部裝共6 步，根據(jù)物料配送計劃由自動導(dǎo)引小車（Automated Guided Vehicle，AGV）從暫存區(qū)配送至裝配工位。在整個配送環(huán)境中，密集區(qū)域與稀疏區(qū)域并存。其中，暫存區(qū)內(nèi)部及工位之間的可行區(qū)域大，單個裝配工位的零部件擺放密集。AGV 配送的零部件多數(shù)體積大、重量重，為防止零部件在配送過程中出現(xiàn)傾斜或損壞等情況，在密集區(qū)域及轉(zhuǎn)彎處對AGV 行駛的平穩(wěn)性要求非常高。

路徑規(guī)劃是實現(xiàn)AGV 自主導(dǎo)航的重要組成部分，合理的路徑規(guī)劃對于提高配送效率和安全性至關(guān)重要［1-2］。目前針對某一環(huán)境的路徑規(guī)劃算法通常分為兩類：①基于已知環(huán)境模型的全局路徑規(guī)劃算法，包括A*算法、Dijstra 算法和遺傳算法；②基于傳感器信息的局部避障算法，包括DWA（Dynamic Window Approach）算法、TEB 算法和人工勢場法［3-6］。鑒于A*算法利用啟發(fā)式評價函數(shù)保證搜索方向與目標點一致，避免了對地圖中所有點進行評價，相比于Dijstra 算法和遺傳算法，具有快速實現(xiàn)最短路徑規(guī)劃的優(yōu)勢，因此本文以A*算法作為全局路徑算法?？紤]到整個配送環(huán)境中密集區(qū)域與稀疏區(qū)域并存，人工勢場法在密集區(qū)域中路徑不平滑，且易陷入局部最優(yōu)解，TEB 算法在出現(xiàn)動態(tài)障礙物或速度控制量不平滑時，路徑規(guī)劃變化頻率較高，令A(yù)GV 存在震蕩和碰撞風(fēng)險［7］，而DWA 算法綜合考慮了AGV 的運動模型與環(huán)境因素，更加符合真實AGV的運行需求，因此本文將DWA 算法作為局部路徑算法［8］。

1 相關(guān)工作

目前針對A*算法的研究集中于對其規(guī)劃的路徑存在大量拐點、路徑不平滑而展開［9-11］。其中，勞彩蓮等［12］增加了對關(guān)鍵節(jié)點的提取，對前后點運動方向進行判斷，將與前節(jié)點運動方向一致的節(jié)點視為冗余點；陳藝文等［13］將轉(zhuǎn)折點作為局部目標點，利用混合A*進行二次規(guī)劃；陳嬌等［14］直接對節(jié)點之間的連線與障礙物的位置關(guān)系進行判斷，識別冗余節(jié)點，去除冗余路徑。針對DWA 算法的研究主要集中于AGV 小車在障礙物密集環(huán)境中成功率不高或容易出現(xiàn)繞行而展開。王永雄等［15］和吳宇等［16］利用移動機器人自帶的傳感器獲取的環(huán)境信息結(jié)合自適應(yīng)思想，實現(xiàn)DWA 評價函數(shù)的權(quán)重自適應(yīng)調(diào)節(jié)；陳志明［17］通過設(shè)置機器人與目的地的距離增量為DWA 算法評價函數(shù)，加快了機器人朝目標點前進。

以上文獻針對A*算法的改進集中于對已有路徑節(jié)點的篩選，而缺少對轉(zhuǎn)角較大處節(jié)點的優(yōu)化，因此本文設(shè)計雙層優(yōu)化A*算法，在去除冗余節(jié)點的基礎(chǔ)上，調(diào)整轉(zhuǎn)角大小，以提高路徑平滑度。改進后的A*算法更加符合AGV小車的運動模型，并提出轉(zhuǎn)向成本函數(shù)來調(diào)整DWA 算法的評價函數(shù)，以確保小車的運動更加貼合A*算法規(guī)劃的路徑，避免不必要的轉(zhuǎn)彎，最終為AGV 在壓鑄機裝配車間提供高質(zhì)量的可通行路徑。

2 AGV路徑規(guī)劃全局算法

2.1 A*算法基本原理

A*算法結(jié)合給定的起點和終點信息，通過對可行節(jié)點的搜索與評價，求解最短路徑。在進行節(jié)點搜索的過程中，將確定為路徑的節(jié)點稱為父節(jié)點存儲于Closed 集合，將與父節(jié)點相鄰且可到達的節(jié)點（子節(jié)點）存儲于Open 集合，通過函數(shù)（fn）對子節(jié)點的成本值進行估計，將成本值最小的子節(jié)點看作新的父節(jié)點［18］，步驟如下：

其中，g（n）為起點到當(dāng)前子節(jié)點的實際成本值，h（n）為當(dāng)前子節(jié)點到終點的成本估計值。

Step 1：初始化Open 集合、Closed 集合，將起點P1添加至Open集合中。

Step 2：獲取與起點P1鄰近且可到達的節(jié)點作為子節(jié)點添加至Open 集合中，將起點作為父節(jié)點從Open 集合轉(zhuǎn)移至Closed 集合中。

Step 3：計算各個子節(jié)點的（fn）值，并將（fn）值最小的點作為新的父節(jié)點Pi，從Open集合轉(zhuǎn)移至Closed 集合中。

Step 4：檢查與新的父節(jié)點鄰近且可到達的節(jié)點，如果某相鄰點不在Open 集合中，則添加；如果已在Open 集合中，則計算該點對應(yīng)的g（n）。如果新的g（n）值更低，則更新父節(jié)點為當(dāng)前點并重新計算（fn），否則保持不變。

Step 5：重復(fù)Step 3、Step 4，直至Open 集合中出現(xiàn)目標點Pn，或Open集合為空。

2.2 改進A*算法

針對A*算法規(guī)劃得到的路徑存在大量拐點、路徑平滑度低，從而影響AGV 平穩(wěn)運行的問題，本文提出一種改進的A*算法，如圖1所示。

對A*算法規(guī)劃出的路徑節(jié)點集合｛P1，…，Pn｝，當(dāng)n＞2時，進行兩階段優(yōu)化處理：第一階段提取路徑節(jié)點并消除冗余節(jié)點，實現(xiàn)部分轉(zhuǎn)彎路徑變直；第二階段增加節(jié)點密度，調(diào)整轉(zhuǎn)角大的節(jié)點坐標，減小轉(zhuǎn)彎幅度。具體步驟如下：

第一階段：提取節(jié)點，去除冗余路徑。

Step 1：連接各節(jié)點。將節(jié)點Pi依次與集合｛Pi+1，…，Pm，…，Pn｝中的節(jié)點連接。

Step 2：判斷并消除冗余節(jié)點。當(dāng)節(jié)點Pi與節(jié)點Pm-1的連線之間不存在障礙物且與節(jié)點Pm的連線之間存在障礙物時，定義節(jié)點Pi與節(jié)點Pm-1之間的節(jié)點為冗余點，即集合｛Pi+1，…，Pm-2｝為冗余點，并將其刪除。

Step 3：以節(jié)點Pm-1為起點與集合｛Pm，…，Pn｝中的點依次作連接，判斷冗余節(jié)點，以此類推，當(dāng)i=n-1時停止。

Step 4：連接剩余節(jié)點，獲得新路徑。

第二階段：增加節(jié)點密度，調(diào)整轉(zhuǎn)角大的節(jié)點坐標，減小轉(zhuǎn)彎幅度。

首先，對新路徑的節(jié)點進行遍歷，當(dāng)兩節(jié)點之間的距離大于或等于設(shè)定值時（見式（2）），通過插入中點增加節(jié)點密度（見式（3））。

Fig.1 Flow of improved A*algorithm圖1 改進A*算法流程

然后，再次遍歷節(jié)點，利用式（4）計算連續(xù)3 點（C（cx，cy）、D（dx，dy）、E（ex，ey））之間的轉(zhuǎn)角，之后通過式（5）依次比較轉(zhuǎn)角與設(shè)定角θ的大小。對于轉(zhuǎn)角大的情況，利用式（6）調(diào)整中間節(jié)點的坐標，以獲得新路徑、減小轉(zhuǎn)彎幅度。

本文通過MATLAB 仿真對改進前后A*算法獲得的規(guī)劃路徑進行對比，結(jié)果如圖2 所示。其中，采用A*算法規(guī)劃的路徑如圖2（a）所示，明顯存在較多折點；利用改進A*算法規(guī)劃的路徑如圖2（b）、2（c）所示，圖2（b）經(jīng)過第一階段的拉直處理，刪除了部分節(jié)點，路徑較圖2（a）更加平滑，圖2（c）為經(jīng)過第二階段處理后的最終路徑，進一步提高了路徑的平滑性，并縮短了路徑長度，即最大轉(zhuǎn)角從45°減小到30°，路徑長度由最初的8.66縮短到8.3。

3 AGV局部路徑規(guī)劃算法

3.1 動態(tài)窗口算法基本原理

DWA 算法將滿足約束的速度采樣空間定義為動態(tài)窗口，通過遍歷窗口中的線速度和角速度，結(jié)合運動模型推算出多條路徑，再利用評價函數(shù)對路徑進行評分，將得分最高的路徑作為最優(yōu)路徑［19-20］。

3.1.1 機器人運動模型

由于AGV 在單個采樣周期ρt 內(nèi)的運動距離短，運動軌跡被視為直線，通過式（7）計算AGV 在ρt 時段內(nèi)x、y方向的速度變化量，即可得到ρt 時刻之后的位置信息（xt+1，yt+1，θt+1）（見式（8））。

3.1.2 采樣空間

AGV 機器人在單個采樣周期內(nèi)的采樣空間應(yīng)同時滿足3 個窗口：根據(jù)AGV 速度和角速度限制，形成窗口Vs；根據(jù)障礙物的距離，形成滿足避障要求的窗口Va；根據(jù)電機力矩限制得到機器人在采樣期間實際能夠達到的速度，形成窗口Vd。

其中，vc、wc分別是AGV 當(dāng)前的線速度、角速度；va、wa分別是AGV 的正向最大線加速度、角加速度；vb、wb分別是反向最大線加速度、角加速度；dis（tv，w）是速度為（v，w）的軌跡離障礙物最近的距離。

3.1.3 評價函數(shù)

在得到AGV 小車的運行軌跡后，結(jié)合環(huán)境設(shè)置評價函數(shù)G（v，w），用于挑選出最優(yōu)軌跡，如式（13）所示：

Fig.2 Results of global path planning圖2 全局路徑規(guī)劃結(jié)果

其中，Pdist 為到最鄰近全局路徑點的距離；Gdist為到局部目標點的距離；Obs用于判斷軌跡與環(huán)境障礙物是否有重合，當(dāng)軌跡與障礙物重合時，返回負成本值，即軌跡無效，否則返回零成本值。

3.2 改進動態(tài)窗口評價函數(shù)

DWA 算法路徑規(guī)劃的成果主要取決于對評價函數(shù)的設(shè)定，在壓鑄機裝配車間中，對AGV 行駛的平穩(wěn)性要求很高。對此，本文引入全局方向Dir成本函數(shù)來衡量AGV 局部路徑與全局路徑在前進方向上存在的差異，確保局部路徑與全局路徑的前進方向盡可能一致，以防止運行過程中不必要的轉(zhuǎn)向。具體原理如圖3所示。

Fig.3 Global cost principle diagram圖3 全局方向成本原理圖

其中，θ1為全局路徑中提取的局部目標點切線方向與x方向的夾角，θ2為局部路徑規(guī)劃的軌跡端點切線方向與x方向的夾角，θ3為局部路徑規(guī)劃的軌跡中點切線方向與x方向的夾角。

4 實例概述

本文以K 公司壓鑄機裝配車間作為AGV 應(yīng)用環(huán)境，如圖4所示。

每個裝配工位有多輛物料車裝載零部件，由AGV 小車從暫存區(qū)1、2 配送至裝配區(qū)?？紤]到AGV 在執(zhí)行任務(wù)過程中需要轉(zhuǎn)彎，避障時的路徑平滑度是本文對路徑規(guī)劃算法評價的重要指標，柵格化的地圖更加符合需求，因此本文采用SLAM 創(chuàng)建柵格地圖，如圖5 所示。其中白色部分為可通行區(qū)域，黑色部分為不可通行區(qū)域，灰色部分為雷達無法探尋的未知部分［21］。

將改進的全局路徑規(guī)劃A*算法和改進的局部路徑規(guī)劃DWA 算法導(dǎo)入ROS 智能小車中，關(guān)鍵參數(shù)設(shè)置如下：l=0.05 m，θ=45，最大角速度為5 rad/s，最小角速度為0 rad/s，最大線速度為0.5 m/s，最小線速度為0 m/s，采樣周期為1.0 s，α=64，β=24，γ=0.5，λ=25。

全局路徑規(guī)劃結(jié)果如圖6 所示。其中，圖6（a）、圖6（b）為A*算法規(guī)劃的結(jié)果，圖6（c）、圖6（d）為改進A*算法規(guī)劃的結(jié)果，優(yōu)化后的全局路徑明顯比優(yōu)化前平滑。

Fig.4 Workshop environment圖4 車間環(huán)境

Fig.5 Grid map圖5 柵格地圖

局部路徑規(guī)劃結(jié)果如圖7所示，改進前的局部路徑與全局路徑方向并不一致，小車行駛過程中會出現(xiàn)繞行的情況。改進后的局部路徑與全局路徑更加貼合，通過小車的實際路徑圖可知，改進后的轉(zhuǎn)角幅度更小，如圖8所示。對應(yīng)的角速度變化曲線如圖9所示，縱坐標為AGV小車的角速度，橫坐標為小車啟動后的時間，改進后的角速度集中在0.00～0.25區(qū)間，存在較少突變情況。綜上所述，本文優(yōu)化后的路徑規(guī)劃算法使小車在運輸物料過程中能更平穩(wěn)地行駛。

將AGV 小車置于密集環(huán)境中，對改進前后的算法分別執(zhí)行50 次，通過的成功率如表1 所示，從表中可看出改進后算法的成功率遠高于改進前。

AGV 小車利用改進前的算法規(guī)劃路徑時，由于算法自身存在的不足，規(guī)劃的路徑存在較大折點，無法使AGV 小車正常通行，導(dǎo)致小車會出現(xiàn)原地打轉(zhuǎn)無法前進的情況，如圖10（a）所示。改進后的算法路徑規(guī)劃如圖10（b）所示，小車的轉(zhuǎn)角變化較小，在一定程度上避免了AGV 不必要轉(zhuǎn)向的同時，提高了路徑的平滑度，使小車更適用于障礙物密集環(huán)境。

Fig.6 Comparison of global path planning圖6 全局路徑規(guī)劃對比

Table 1 AGV success rate表1 AGV小車成功率

Fig.7 Comparison of local path planning圖7 局部路徑規(guī)劃對比

Fig.8 Comparison of actual path圖8 實際路徑對比

Fig.9 Angular velocity comparison圖9 角速度對比

5 結(jié)語

本文針對壓鑄機裝配車間，提出基于A*算法和DWA算法的AGV 小車路徑優(yōu)化算法，主要創(chuàng)新點如下：

（1）針對A*算法規(guī)劃的路徑存在大量拐點、不平滑等問題，通過識別冗余節(jié)點，去除冗余路徑，增加節(jié)點密度，調(diào)整轉(zhuǎn)彎大的節(jié)點坐標，提高了路徑平滑度。

（2）考慮AGV 運動約束，采用DWA 算法。通過對其評價函數(shù)進行設(shè)定，彌補了AGV 小車容易出現(xiàn)繞行的缺陷。

（3）在壓鑄機裝配車間環(huán)境中利用AGV 小車進行仿真實驗，結(jié)果表明，AGV 能夠平穩(wěn)、快捷地到達目的地。

同時，本文方法也存在一些不足，需要深入研究相關(guān)技術(shù)，以提高AGV 小車的平穩(wěn)性。今后將更全面地考慮影響AGV 小車路徑規(guī)劃的因素，提高AGV 小車在車間環(huán)境中的適用性。

Fig.10 Comparison of dense environment path planning圖10 密集環(huán)境路徑規(guī)劃對比