面向?qū)嶋H工況的AGV局部路徑規(guī)劃及跟蹤研究

吳 斌，張 新，VIALOVA Varvara

南京工業(yè)大學(xué) 經(jīng)濟(jì)與管理學(xué)院，南京 211816

隨著“工業(yè)4.0”“中國(guó)制造2025”的發(fā)展，各種智能設(shè)備在制造領(lǐng)域應(yīng)用越來(lái)越廣泛。叉車型AGV作為自動(dòng)搬運(yùn)工具，在生產(chǎn)系統(tǒng)、物流系統(tǒng)中可以輕松搬運(yùn)各種物料，實(shí)現(xiàn)物料搬運(yùn)的自動(dòng)化，提升物流效率，降低物流成本。但AGV 雖能實(shí)現(xiàn)托盤的自主拾取，其工作效率和拾取精度卻會(huì)因行走路徑的不同存在較大差距。優(yōu)良的路徑能使得AGV 行走過(guò)程中不易抖動(dòng)、花費(fèi)時(shí)間短且易于跟蹤。而差的路徑雖然能使AGV到達(dá)指定目標(biāo)點(diǎn)，但可能會(huì)使AGV在行走過(guò)程中抖動(dòng)嚴(yán)重、頻繁大幅度轉(zhuǎn)彎、行走時(shí)間長(zhǎng)、跟蹤效果較差，并容易引起叉車堵塞、貨物墜落等問題。因此，對(duì)叉車型AGV的規(guī)劃路徑深入研究對(duì)提升AGV 運(yùn)行效率、精度和安全性意義重大。

AGV路徑規(guī)劃問題可以分為全局路徑規(guī)劃與局部路徑規(guī)劃。全局路徑問題以已知全局環(huán)境出發(fā)，在起點(diǎn)與終點(diǎn)間規(guī)劃出符合目標(biāo)函數(shù)最優(yōu)的無(wú)碰撞路徑，目前已取得豐富的研究成果，常用算法包括A*算法[1-2]、蟻群算法[3]等。但這些研究方法注重考慮全局環(huán)境約束，缺少對(duì)于局部環(huán)境不確定性和路徑跟蹤可行性的考慮，而局部路徑規(guī)劃能很好地解決上述問題。AGV的局部路徑規(guī)劃重點(diǎn)關(guān)注AGV 當(dāng)前所處局部環(huán)境的信息，在AGV當(dāng)前位置與目標(biāo)位置間規(guī)劃出一條能滿足運(yùn)動(dòng)約束及局部環(huán)境約束的曲線路徑，主要研究方法有人工勢(shì)場(chǎng)法[4-5]、神經(jīng)網(wǎng)絡(luò)算法[6-7]、動(dòng)態(tài)窗口法[8-9]及參數(shù)曲線擬合法[10-14]等。由于AGV進(jìn)行托盤拾取作業(yè)時(shí)，起點(diǎn)位姿及托盤位姿存在不確定性，屬于未知的局部環(huán)境信息，因此屬于局部路徑規(guī)劃問題。

多數(shù)AGV的局部路徑規(guī)劃問題實(shí)際就是局部環(huán)境避障問題，即對(duì)于檢測(cè)到的意外障礙物，如何規(guī)劃局部的路徑進(jìn)行避障。而解決叉車型AGV托盤拾取的局部路徑規(guī)劃問題時(shí)，由于貨叉與托盤對(duì)接精度要求高，相較于避障，其更注重起始點(diǎn)和終止點(diǎn)的位姿約束及路徑跟蹤精度。托盤拾取路徑規(guī)劃相關(guān)的文獻(xiàn)較少。呂恩利等[15]提出了基于B 樣條曲線的叉車托盤拾取路徑規(guī)劃方法，綜合考慮叉車運(yùn)動(dòng)約束和托盤對(duì)接精度等約束，以路徑曲率最小化為優(yōu)化目標(biāo)求解出了托盤拾取路徑。文獻(xiàn)[16]及文獻(xiàn)[17]同樣采用了參數(shù)曲線擬合法成功解決了路徑規(guī)劃中的起止點(diǎn)位姿約束的問題。因此，參數(shù)曲線擬合法適合作為托盤拾取路徑規(guī)劃的基本方法。但是，現(xiàn)有文獻(xiàn)的研究成果尚無(wú)法完全應(yīng)用于實(shí)際工況下的AGV 托盤拾取作業(yè)場(chǎng)景中。文獻(xiàn)[15]以最小化最大曲率為優(yōu)化目標(biāo)，但實(shí)際工況中為了提高AGV工作效率，相較于最小化曲率，規(guī)劃路徑時(shí)更多以最小化路徑長(zhǎng)度為優(yōu)化目標(biāo)。在起止點(diǎn)間位姿差距較大的情況下，以最小化最大曲率生成的路徑過(guò)長(zhǎng)，較大地增加了AGV 的行駛時(shí)間。文獻(xiàn)[16]以路徑最短為優(yōu)化目標(biāo)設(shè)計(jì)AGV的路徑，但其應(yīng)用場(chǎng)景不為托盤拾取，對(duì)于位姿誤差要求低，且沒有使用跟蹤算法進(jìn)行實(shí)際路徑跟蹤驗(yàn)證，因此路徑無(wú)法直接應(yīng)用于托盤拾取場(chǎng)景中。文獻(xiàn)[17]以路徑最短作為優(yōu)化目標(biāo)，依據(jù)B 樣條曲線生成路徑，并進(jìn)行了跟蹤驗(yàn)證。但其研究針對(duì)拖拉機(jī)的軌跡跟蹤，相較于AGV的托盤拾取，對(duì)終點(diǎn)位姿誤差的要求同樣較低，因此其路徑不能完全被托盤拾取場(chǎng)景所應(yīng)用。

綜上所述，本文研究目的在于設(shè)計(jì)一套路徑規(guī)劃及路徑跟蹤算法，使得即使在起止點(diǎn)間初始偏移距離和航向角差值較大的情況下也能規(guī)劃出長(zhǎng)度盡可能短、托盤對(duì)接精度盡可能高的托盤拾取路徑，并使得路徑能被精確地跟蹤。本文的創(chuàng)新點(diǎn)在于：對(duì)于路徑規(guī)劃問題，設(shè)計(jì)滿足實(shí)際工況條件的約束集及路徑評(píng)價(jià)函數(shù)，運(yùn)用粒子群算法進(jìn)行路徑求解；對(duì)于路徑跟蹤問題，選取合適的跟蹤算法并進(jìn)行改良優(yōu)化，提高跟蹤精度。

1 問題描述

AGV 在拾取托盤時(shí)有兩種行駛方式，即正向行駛和反向行駛。如圖1、圖2 所示。當(dāng)AGV 正向行駛時(shí)，無(wú)須調(diào)整貨叉方位便可直接接近托盤，但其缺點(diǎn)在于，當(dāng)橫向偏移距離較小時(shí)，AGV 極有可能因?yàn)槠渥钚∞D(zhuǎn)彎半徑及始末航向角約束使得其無(wú)法順利到達(dá)目標(biāo)點(diǎn)。圖1中，由于D較小，且AGV在起點(diǎn)處與垂直方向存在角度差β，因此叉車無(wú)法到達(dá)P1點(diǎn)。當(dāng)AGV反向行駛時(shí)，以圖2 為例，先從起點(diǎn)A行至B點(diǎn)處，再?gòu)腂點(diǎn)行至目標(biāo)點(diǎn)P1。其路徑可分為兩段，一段為調(diào)整貨叉方向的緩沖路徑，一段為托盤拾取路徑。反向行駛的好處在于，當(dāng)托盤位于距離AGV 很近的P1點(diǎn)時(shí)，不會(huì)發(fā)生因航向角及最小轉(zhuǎn)彎半徑的約束使得AGV無(wú)法抵達(dá)目標(biāo)點(diǎn)的現(xiàn)象。因此，AGV 的兩種行駛方式都有其存在的必要。

圖1 AGV正向行駛示意圖Fig.1 Diagram of AGV’s driving forward

圖2 AGV反向行駛示意圖Fig.2 Diagram of AGV’s driving backward

但是實(shí)際工況中，無(wú)論選擇何種行駛方式驅(qū)動(dòng)AGV，在缺乏正確規(guī)劃及跟蹤算法的情況下，托盤拾取作業(yè)過(guò)程中總存在如下主要問題：路徑拐點(diǎn)附近AGV車身擺動(dòng)幅度較大；AGV 在行駛過(guò)程中車身抖動(dòng)較為嚴(yán)重；AGV雖能接近托盤目標(biāo)點(diǎn)，但其與托盤間的橫向誤差及朝向角誤差較大，導(dǎo)致貨叉與托盤孔不能很好地對(duì)接。以上問題嚴(yán)重影響了AGV托盤拾取工作的精度及效率，使得AGV 托盤拾取作業(yè)的自主性無(wú)法得到實(shí)現(xiàn)。因此，為解決以上問題，必須為AGV設(shè)計(jì)正確的路徑規(guī)劃算法，同時(shí)對(duì)現(xiàn)有的跟蹤算法進(jìn)行改進(jìn)。

綜上所述，為解決現(xiàn)存路徑問題，本文基于叉車型AGV 的兩種行駛方式，提出相應(yīng)的路徑規(guī)劃和跟蹤控制方法，并通過(guò)不同工況環(huán)境的仿真實(shí)驗(yàn)驗(yàn)證算法的可行性和優(yōu)越性。

2 AGV運(yùn)動(dòng)及跟蹤模型

為方便后續(xù)路徑規(guī)劃及跟蹤算法的設(shè)計(jì)，以正向行駛為例，建立AGV 的運(yùn)動(dòng)及跟蹤模型。以后輪為驅(qū)動(dòng)輪，前輪為承重輪，建立AGV運(yùn)動(dòng)及跟蹤模型如圖3所示。

圖3 AGV運(yùn)動(dòng)及跟蹤模型圖Fig.3 Motion and tracking model of AGV

其中，1 為AGV 驅(qū)動(dòng)輪，2 為承重輪，3 為等效后車輪，4為等效前車輪。設(shè)AGV的等效前車輪能隨路徑軌跡從點(diǎn)B運(yùn)動(dòng)至點(diǎn)C，則模型中變量的定義如表1所示。

表1 運(yùn)動(dòng)及跟蹤模型的變量說(shuō)明Table 1 Variable description of motion and tracking model

則根據(jù)以上變量可推出運(yùn)動(dòng)學(xué)模型公式如下：

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

3.1 B樣條曲線

B樣條曲線因其優(yōu)秀的性質(zhì)[18-19]被廣泛應(yīng)用于局部路徑規(guī)劃。B樣條曲線方程如下所示：

其中，參數(shù)u取值范圍為[0，1]，xi為曲線控制點(diǎn)，Ni,p(u)表示第i個(gè)p次B樣條基函數(shù)。B樣條曲線實(shí)際由數(shù)段貝塞爾曲線構(gòu)成，其相較于貝塞爾曲線的優(yōu)點(diǎn)在于B樣條曲線支持局部的修改，且其曲線拼接處能輕松滿足幾何連續(xù)條件。三次均勻B樣條曲線表達(dá)式如下所示：

則三次均勻B樣條曲線的一階、二階導(dǎo)數(shù)如下所示：

3.2 面向?qū)嶋H工況約束的托盤拾取路徑規(guī)劃

3.2.1 起止點(diǎn)位姿約束

設(shè)托盤拾取路徑的起點(diǎn)坐標(biāo)為Ps(x1,y1)，航向角為θs。終點(diǎn)坐標(biāo)為Pd(x2,y2)，航向角為θd，為了使得路徑曲線能夠滿足托盤拾取起始點(diǎn)和目標(biāo)點(diǎn)的航向角約束，同時(shí)減少曲線參數(shù)求解量，曲線的6個(gè)控制點(diǎn)坐標(biāo)求法分別如下[20]：

其中，L1和L2為待確定的路徑參數(shù)，由優(yōu)化算法求得。

3.2.2 曲率連續(xù)性約束

若路徑曲率不連續(xù)，會(huì)使得AGV 在行駛過(guò)程中出現(xiàn)車身頻繁抖動(dòng)、輪胎磨損過(guò)度的現(xiàn)象。由曲率公式（3）可知，欲使得路徑曲率連續(xù)，需保證樣條曲線二階可導(dǎo)。由公式（12）可知，三次均勻B樣條曲線二階導(dǎo)數(shù)連續(xù)，可以使得求解得到的曲率擁有連續(xù)的性質(zhì)。另外，B樣條曲線在曲線拼接處也能使得曲率連續(xù)，即規(guī)劃路徑擁有C2 連續(xù)的性質(zhì)，所以三次均勻B 樣條曲線能夠滿足規(guī)劃路徑的曲率連續(xù)性約束。

3.2.3 AGV運(yùn)動(dòng)性能及工作環(huán)境約束

為使得AGV在實(shí)際橫向偏移距離及航向角差距較大的場(chǎng)景下行至終點(diǎn)的位姿仍能保證貨叉順利與托盤對(duì)接，同時(shí)符合自身運(yùn)動(dòng)性能約束，以AGV自身局部坐標(biāo)系為參考方位，給出如下約束規(guī)劃集：

其中，(as,bs)為路徑起點(diǎn)坐標(biāo)，(ad,bd)為終點(diǎn)坐標(biāo)。?為曲線的曲率集合，r0為AGV 的最小轉(zhuǎn)彎半徑，yd為與目標(biāo)托盤所在貨架相對(duì)的貨架縱坐標(biāo)值，yr為生成路徑坐標(biāo)點(diǎn)的縱坐標(biāo)集，δ為AGV 車輪的轉(zhuǎn)向角度，θmax為AGV 車輪所允許的最大轉(zhuǎn)向角度。k1及k2都為恒定常數(shù)，其取值分別由實(shí)際工作場(chǎng)景環(huán)境確定。自上而下，式（a）保證路徑曲率不超過(guò)AGV最小轉(zhuǎn)彎半徑的倒數(shù)，式（b）與式（c）保證AGV在貨架間行駛時(shí)，路徑不與貨架產(chǎn)生碰撞，式（d）保證AGV車輪轉(zhuǎn)角不超過(guò)最大轉(zhuǎn)角。當(dāng)路徑取值點(diǎn)數(shù)較少時(shí)，式（e）使得起始點(diǎn)附近的路段較為平緩，防止AGV 在路徑跟蹤一開始就產(chǎn)生較大的抖動(dòng)。

3.2.4 多目標(biāo)路徑評(píng)價(jià)函數(shù)

若簡(jiǎn)單將最短路徑設(shè)置為優(yōu)化目標(biāo)，則路徑能到達(dá)理想情況下的最優(yōu)，但其首末端可能由于曲率過(guò)大，實(shí)際工況中AGV 無(wú)法準(zhǔn)確運(yùn)行，因此必須考慮多目標(biāo)優(yōu)化情況。多目標(biāo)優(yōu)化問題一般以帕累托最優(yōu)解集形式給出，但是考慮實(shí)際工況下AGV 運(yùn)行的實(shí)時(shí)性和方便性要求，需要從給出的帕累托最優(yōu)解集中，通過(guò)設(shè)置權(quán)重的方法選定合適的單一解。因此直接對(duì)多目標(biāo)評(píng)價(jià)函數(shù)設(shè)置權(quán)重，將其轉(zhuǎn)為方便處理的單目標(biāo)評(píng)價(jià)函數(shù)。假設(shè)路徑共由n個(gè)點(diǎn)擬合構(gòu)成，則關(guān)于生成路徑的目標(biāo)評(píng)價(jià)函數(shù)如下：

目標(biāo)評(píng)價(jià)函數(shù)z由三部分組成：路徑近似長(zhǎng)度、目標(biāo)點(diǎn)附近曲率絕對(duì)值的最大值和目標(biāo)點(diǎn)附近路徑曲率變化程度。其中，ω1至ω4為權(quán)重參數(shù)，Length為規(guī)劃路徑的近似長(zhǎng)度。第二部分中的?(s:n)表示靠近目標(biāo)點(diǎn)處的部分路徑曲率集合，第三部分中的?(j)-?(j-1)表示靠近目標(biāo)點(diǎn)處的部分路徑曲率變化值的集合。這兩部分作用為將接近終點(diǎn)處的路徑轉(zhuǎn)彎點(diǎn)提前，使得AGV在實(shí)際跟蹤中不會(huì)因終點(diǎn)附近曲率過(guò)大而導(dǎo)致跟蹤效果不佳。當(dāng)路徑擬合點(diǎn)數(shù)較少時(shí)，評(píng)價(jià)函數(shù)的第三部分可以使得路徑轉(zhuǎn)彎點(diǎn)提前。但當(dāng)擬合點(diǎn)數(shù)較多時(shí)，點(diǎn)間曲率變化值已經(jīng)較小，第三部分不再發(fā)揮有效作用，因此必須加入第二部分使得在此種情況下評(píng)價(jià)函數(shù)也能挑選出合適路徑。算法對(duì)于以上三部分的實(shí)際尋優(yōu)方式分別為：使得路徑長(zhǎng)度盡可能??；使得終點(diǎn)附近路徑曲率最大值的絕對(duì)值盡可能小；使得終點(diǎn)附近路徑曲率變化值的最大值的絕對(duì)值盡可能小。該三部分同為最小化函數(shù)值問題，因此可以通過(guò)取權(quán)重的方式將其轉(zhuǎn)換為單目標(biāo)優(yōu)化問題，從而得到單一的解。此外，由于函數(shù)中三部分的量綱有差距，為了使得三部分的變化對(duì)整體評(píng)價(jià)函數(shù)有較為平等的影響，一個(gè)簡(jiǎn)單的對(duì)策為將三部分的實(shí)際變化值乘以合適的系數(shù)，使得三部分的最終變化值在同一個(gè)量綱下。通過(guò)多次仿真實(shí)驗(yàn)得出，將第一部分的實(shí)際變化值縮小10 倍，第二部分的實(shí)際變化值保持不變，第三部分的實(shí)際變化值放大10 倍能取得較好的評(píng)價(jià)效果，因此設(shè)置參數(shù)k3及k4，取值分別為0.1及10，而對(duì)于公式中的權(quán)重值，則通過(guò)仿真實(shí)驗(yàn)進(jìn)行選取。綜上，整理出最終的目標(biāo)評(píng)價(jià)函數(shù)如公式（20）所示：

3.3 基于粒子群算法的局部路徑參數(shù)優(yōu)化

路徑曲線的形狀由其控制點(diǎn)決定。AGV正向行駛時(shí)，依據(jù)3.2.1小節(jié)規(guī)定的控制點(diǎn)確定方法，可以使得待優(yōu)化參數(shù)減少為2個(gè)，即只需確定L1與L2的值即可確定整條路徑。當(dāng)AGV 反向行駛時(shí)，其路徑求解方法如圖4所示。首先，以AGV當(dāng)前位置S為起點(diǎn)，以貨架中最接近起點(diǎn)的托盤位置為目標(biāo)點(diǎn)，起點(diǎn)航向角固定為0 rad，終點(diǎn)航向角取實(shí)際值代入優(yōu)化算法。若優(yōu)化算法以起點(diǎn)S坐標(biāo)、目標(biāo)點(diǎn)P坐標(biāo)及規(guī)定起止點(diǎn)航向角為基礎(chǔ)在迭代一定次數(shù)后無(wú)法求解出符合條件的路徑，則將起點(diǎn)橫坐標(biāo)往后平移相應(yīng)合適距離，再進(jìn)行尋優(yōu)，直到滿足要求，得到點(diǎn)S′。若S點(diǎn)無(wú)須迭代即可導(dǎo)出合格的路徑，則直接將S點(diǎn)作為S′。

圖4 AGV反向行駛路徑尋優(yōu)示意圖Fig.4 Path optimization of AGV’s driving backward

其中，設(shè)初始起點(diǎn)坐標(biāo)為S(a,b)，最接近AGV的托盤坐標(biāo)為P(c,d)，迭代后的新起始點(diǎn)坐標(biāo)為S′(m,n)。

確定了點(diǎn)S′后，若托盤P位于貨架最左端附近，則在x軸上尋一點(diǎn)H，使得其與點(diǎn)S′關(guān)于點(diǎn)G(c,0)對(duì)稱。則AGV 從起點(diǎn)S行駛至點(diǎn)H(2c-m±kd,n)即可進(jìn)入托盤拾取路徑。若托盤離AGV 較遠(yuǎn)，設(shè)此時(shí)托盤P′坐標(biāo)為(j,k)，則將點(diǎn)H坐標(biāo)修改為H′(j+c-m±kd,n)即可。kd為一常系數(shù)，代表一小段緩沖距離，取值由初始點(diǎn)相對(duì)貨架的方位確定。

綜上所述，反向行駛的AGV行走路徑以點(diǎn)H或H′為界限劃分為兩段。以目標(biāo)點(diǎn)P為例，則完整規(guī)劃路徑由曲線SH及曲線HP構(gòu)成。AGV先以等比例反饋控制法由S點(diǎn)行駛至H點(diǎn)，再以H點(diǎn)為起始點(diǎn)，托盤拾取點(diǎn)為目標(biāo)點(diǎn)，跟蹤規(guī)劃出的相應(yīng)B樣條曲線路徑到達(dá)終點(diǎn)。此種方式能解決AGV因最小轉(zhuǎn)彎半徑約束導(dǎo)致的無(wú)法抵達(dá)目標(biāo)點(diǎn)的問題，且路徑長(zhǎng)度接近最優(yōu)。

本文采用粒子群算法作為路徑參數(shù)求解的優(yōu)化算法，其慣性系數(shù)及速度迭代公式如下所示：

其中，ω為慣性系數(shù)，ωmax、ωmin分別為慣性系數(shù)最大值和最小值，k為算法當(dāng)前迭代代數(shù)，T為算法迭代總次數(shù)。vi(k)、xi(k)分別為第k次迭代時(shí)第i個(gè)粒子的速度和位置，c1、c2分別為個(gè)體認(rèn)知系數(shù)和社會(huì)認(rèn)知系數(shù)，r1、r2為0至1之間的隨機(jī)數(shù)。pibest、gbest分別為粒子局部最優(yōu)位置和全局最優(yōu)位置。粒子群算法的迭代流程如下：

（1）初始化由路徑參數(shù)L1及L2構(gòu)成的粒子種群矩陣，初始化粒子速度矩陣。

（2）根據(jù)初始化種群解，解出對(duì)應(yīng)曲線方程，并計(jì)算其路徑長(zhǎng)度及曲率集合。

（3）將路徑長(zhǎng)度和曲率集合代入公式（20），求解出初始適應(yīng)度值，記錄最小適應(yīng)度值及其粒子的位置。

（4）判斷是否滿足迭代終止條件。是則退出循環(huán)，否則繼續(xù)迭代。

（5）解出粒子種群矩陣所對(duì)應(yīng)的曲線方程，依據(jù)公式（20）計(jì)算出適應(yīng)度值，記錄最小適應(yīng)度值及其粒子的位置。

（6）對(duì)局部最優(yōu)值及全局最優(yōu)值進(jìn)行比較和更新。

（7）依據(jù)公式（21）進(jìn)行粒子速度及位置的更新，得到新的種群矩陣。

（8）令迭代次數(shù)增1，返回步驟（4）。

4 路徑跟蹤控制算法

跟蹤算法的目的在于依照規(guī)劃的路徑和跟蹤對(duì)象的各類參數(shù)，根據(jù)一定步驟計(jì)算得出規(guī)定周期內(nèi)跟蹤對(duì)象的控制量，包括速度、車輪轉(zhuǎn)角等[21-25]。純跟蹤法在叉車型AGV 行走速度較慢的情況下，其跟蹤效果較為理想。但是，純跟蹤法的缺陷在于其無(wú)法對(duì)航向角偏差實(shí)行有效控制，因此可能會(huì)導(dǎo)致AGV 實(shí)際停止的位姿與目標(biāo)位姿間存在不可忽視的誤差。另外，預(yù)瞄距離取值也非常影響純跟蹤法的跟蹤效果。因此需對(duì)純跟蹤算法進(jìn)行相應(yīng)的改進(jìn)和補(bǔ)充，以形成完整的、精度更高的跟蹤算法。

4.1 前視距離的改進(jìn)

原始純跟蹤法中的前視距離公式大多由如下表達(dá)式?jīng)Q定：

其中，Ld表示預(yù)瞄距離，kv為速度系數(shù)，v為當(dāng)前車輛行駛速度，L0為預(yù)瞄距離下限值。由于本文研究AGV的行駛速度較小，因此對(duì)于固定的L0,Ld的變化范圍較小。若Ld的取值較大，則AGV在跟蹤路徑時(shí)將不易產(chǎn)生抖動(dòng)，跟蹤更為自然，但缺點(diǎn)是在一些曲率較大的路徑轉(zhuǎn)角，會(huì)出現(xiàn)轉(zhuǎn)向不足的現(xiàn)象從而產(chǎn)生危險(xiǎn)。若Ld取值較小，則能保證可以較為安全地通過(guò)曲率較大的彎道，但在整體路徑跟蹤上抖動(dòng)較大，不利于跟蹤。因此，必須設(shè)置動(dòng)態(tài)的Ld，使之能隨路徑的變化而變化。本文提出一種確定Ld的方式如下：

其中，index為AGV 參考路徑上距離AGV 當(dāng)前實(shí)際位置最近點(diǎn)的序號(hào)，curv(index)代表該點(diǎn)處的曲率，k5為一常系數(shù)。依照該方法確定預(yù)瞄距離，可以使得Ld隨路徑曲率的變化而動(dòng)態(tài)變化。

4.2 航向角誤差消除

考慮到AGV 通過(guò)純跟蹤法控制后，與目標(biāo)點(diǎn)間位姿的差距已經(jīng)變小，因此在使用純跟蹤法控制AGV 使得其與目標(biāo)點(diǎn)間位姿差距減小至一定程度后，可以采用更為直接的航向角控制法對(duì)AGV 實(shí)行控制，直到其位姿誤差能滿足精度要求?？刂品椒ㄈ缦拢?/p>

其中，θdest為目標(biāo)航向角，θreal為當(dāng)前AGV 實(shí)際航向角，k6為一常系數(shù)。位置誤差計(jì)算公式如下：

其中，dx與dy分別為AGV當(dāng)前位置橫、縱坐標(biāo)與目標(biāo)點(diǎn)位置橫、縱坐標(biāo)之差。

4.3 AGV控制量求解

因?yàn)榇朔NAGV 屬于四輪車結(jié)構(gòu)，因此等效轉(zhuǎn)向角需進(jìn)行換算轉(zhuǎn)變成AGV左右側(cè)車輪的轉(zhuǎn)角：

其中，δi與δj分別表示AGV的左側(cè)車輪與右側(cè)車輪轉(zhuǎn)角，L代表前后輪軸距，R代表轉(zhuǎn)向的等效轉(zhuǎn)彎半徑，d代表驅(qū)動(dòng)側(cè)車輪間的輪距。

綜上所述，本文路徑規(guī)劃及跟蹤控制算法流程如圖5 所示。AGV 首先通過(guò)激光傳感器定位得到起始點(diǎn)位姿，隨后獲取目標(biāo)托盤的位姿，依據(jù)橫向偏移距離選擇行駛方式，若正向行駛，則通過(guò)規(guī)劃算法求解出相應(yīng)路徑，最后采用改進(jìn)跟蹤控制算法進(jìn)行跟蹤。若反向行駛，則通過(guò)規(guī)劃算法求解出位姿調(diào)整路徑和托盤拾取路徑，合并生成完整的路徑，最后采用改進(jìn)的跟蹤控制算法進(jìn)行跟蹤。

圖5 叉車型AGV路徑規(guī)劃及跟蹤算法流程圖Fig.5 Flowchart of path planning and tracking

5 實(shí)驗(yàn)分析

5.1 問題背景

叉車型AGV 接到托盤拾取命令后，首先行駛至目標(biāo)托盤所在貨架位置，隨后通過(guò)激光傳感器識(shí)別和托盤識(shí)別技術(shù)獲取自身位姿及托盤位姿。最后依據(jù)托盤拾取局部路徑規(guī)劃算法，在起點(diǎn)和目標(biāo)點(diǎn)間規(guī)劃出一條最優(yōu)路徑并跟蹤。其工作場(chǎng)景圖如圖6所示。

圖6 AGV托盤拾取工作場(chǎng)景圖Fig.6 working scene of pallet picking

圖6 中，AGV 接到托盤拾取命令后，按照全局路徑由A點(diǎn)行至B點(diǎn)，隨后啟動(dòng)托盤拾取路徑規(guī)劃算法，在B與C間生成局部路徑，追蹤路徑直至到達(dá)目標(biāo)點(diǎn)C。

5.2 仿真參數(shù)設(shè)置

本文參照實(shí)際的工況環(huán)境對(duì)仿真參數(shù)進(jìn)行設(shè)置。坐標(biāo)系以圖6 中坐標(biāo)系為參照，設(shè)定沿x軸正方向?yàn)? rad，角度以逆時(shí)針方向增長(zhǎng)，則沿y軸正方向?yàn)棣?2 rad。其具體仿真數(shù)據(jù)如表2所示。其中，第一行至第四行為AGV 的重要運(yùn)動(dòng)及尺寸參數(shù)，第五行至第九行為AGV工作環(huán)境的設(shè)置。第十行至最后為規(guī)劃及跟蹤算法的重要參數(shù)。

表2 仿真參數(shù)設(shè)置表Table 2 Simulation parameters

5.3 路徑評(píng)價(jià)函數(shù)權(quán)重值選取

路徑評(píng)價(jià)函數(shù)的權(quán)重實(shí)際決定了終點(diǎn)附近路徑的彎曲程度，從而影響最終的跟蹤誤差。因此確定權(quán)重的思路為：根據(jù)托盤及貨叉的匹配要求，分別確定橫向?qū)诱`差及航向角誤差的一個(gè)最大允許值，再分別選取權(quán)重代入求解路徑，使用跟蹤算法進(jìn)行誤差檢驗(yàn)，以誤差值為權(quán)重選取的依據(jù)。通過(guò)初步仿真實(shí)驗(yàn)發(fā)現(xiàn)，當(dāng)ω1取值為0.9 左右時(shí)，此時(shí)無(wú)論ω3如何取值，跟蹤算法得到的最終橫向?qū)诱`差及航向角誤差都較大。而當(dāng)ω1取值0.8 左右及其以下時(shí)，跟蹤算法得到的最終航向角誤差都很小，且符合精度要求。因此航向角誤差已經(jīng)無(wú)法起到比較權(quán)重的作用，此時(shí)只能依靠橫向?qū)诱`差的對(duì)比來(lái)確定權(quán)重。因此，設(shè)定橫向?qū)诱`差為權(quán)重選取的主要依據(jù)。設(shè)定橫向?qū)诱`差最大允許值為G?？紤]到權(quán)重確定工作的方便性，以0.1 為間隔對(duì)權(quán)重進(jìn)行選取及測(cè)試。結(jié)合現(xiàn)場(chǎng)實(shí)際并與相關(guān)技術(shù)人員討論后，確定G值取0.024 m，即最終托盤孔與貨叉的橫向?qū)诱`差不大于0.024 m時(shí)，能保證托盤拾取工作的精準(zhǔn)度。為了使得跟蹤誤差在起止點(diǎn)間位姿差距較大的情況下也能滿足限制，仿真實(shí)驗(yàn)選取起點(diǎn)位姿(0 m,0 m,-π/6 rad)，終點(diǎn)位姿(9 m,6 m,7π/12 rad)，幾組仿真對(duì)照結(jié)果如表3所示。

表3 目標(biāo)函數(shù)權(quán)重實(shí)驗(yàn)表Table 3 Experiment of weight

由仿真數(shù)據(jù)可知，當(dāng)ω1減小，ω2增大時(shí)，終點(diǎn)橫向跟蹤誤差顯著減小。當(dāng)ω1及ω2保持不變，ω3減小，ω4增大時(shí)，終點(diǎn)橫向跟蹤誤差變大。當(dāng)ω3取0.9、0.8、0.7時(shí)，跟蹤誤差穩(wěn)定。直到ω3取到0.6 時(shí)，跟蹤誤差進(jìn)一步增大。結(jié)合G值和仿真數(shù)據(jù)，當(dāng)ω1、ω2、ω3、ω4分別取0.7、0.3、0.7、0.3時(shí)，使得終點(diǎn)橫向跟蹤誤差為0.022 7 m，能很好地符合托盤孔與貨叉的對(duì)接精度。則依據(jù)公式（20），取ωa為0.7，ωb為0.21，ωc為0.09。最終確定路徑的目標(biāo)評(píng)價(jià)函數(shù)Z如公式（27）所示：

5.4 改進(jìn)跟蹤算法的仿真對(duì)比

選取同樣規(guī)劃路線，為使得AGV 跟蹤路徑時(shí)振蕩較小，選取同一個(gè)較大的前視距離值為1.2 m，原跟蹤算法和本文跟蹤算法的理想跟蹤情況如圖7。

圖7 跟蹤算法對(duì)比圖Fig.7 Comparison of tracking algorithms

由圖7可知，對(duì)于同一個(gè)前視距離，（a）中AGV在起點(diǎn)和終點(diǎn)拐點(diǎn)處明顯存在轉(zhuǎn)向不足的情況，且行至終點(diǎn)處的車身航向角明顯與終點(diǎn)朝向角不一致，嚴(yán)重影響托盤拾取精確度。（b）中由于實(shí)現(xiàn)了預(yù)瞄距離隨路徑曲率及跟蹤速度的動(dòng)態(tài)變化，且添加了航向角控制法，使得AGV 在曲率較大的拐點(diǎn)處也能很好地進(jìn)行路徑跟蹤，同時(shí)到達(dá)終點(diǎn)處的車身朝向角與終點(diǎn)朝向角一致。因此，改進(jìn)的跟蹤算法能有效提高AGV實(shí)際路徑跟蹤的精度。

5.5 實(shí)際工況下規(guī)劃算法跟蹤對(duì)比分析

實(shí)際路徑跟蹤之所以會(huì)和規(guī)劃路徑存在偏差，是因?yàn)锳GV 自身硬件、控制系統(tǒng)及實(shí)際工作環(huán)境綜合導(dǎo)致的。在橫向偏移距離、航向角差值較小的情況下，則文獻(xiàn)[15]的路徑規(guī)劃算法效果較好。但若以路徑長(zhǎng)度最小為優(yōu)化目標(biāo)，則橫偏距及航向角差值越大，其規(guī)劃路徑的不穩(wěn)定性越大，導(dǎo)致跟蹤誤差變化也越大。令L1與L2為以路徑最短為單目標(biāo)優(yōu)化函數(shù)的原算法求解參數(shù)，與為本文算法求解參數(shù)。設(shè)置起點(diǎn)坐標(biāo)局部不變，通過(guò)改變終點(diǎn)坐標(biāo)及起止點(diǎn)朝向角考察仿真結(jié)果。對(duì)于正向行駛，為了加大仿真的場(chǎng)景規(guī)模以更好驗(yàn)證規(guī)劃算法的優(yōu)越性，設(shè)計(jì)正交實(shí)驗(yàn)進(jìn)行驗(yàn)證。將終點(diǎn)坐標(biāo)、起止點(diǎn)朝向角視為三個(gè)因素，以朝向角變化范圍為參照，統(tǒng)一將三因素設(shè)置為五水平。選用L25(56)正交表，場(chǎng)景按照限定范圍采取正交表設(shè)計(jì)仿真。若在某些仿真場(chǎng)景下無(wú)法求解出合適的路徑參數(shù)，則在表格中以“無(wú)解”體現(xiàn)。若能求解出路徑參數(shù)，則將求解結(jié)果填入表格中。最終規(guī)劃工況仿真數(shù)據(jù)及算法求解結(jié)果如表4所示。

表4 仿真場(chǎng)景參數(shù)及求解結(jié)果Table 4 Parameters of simulation scene and results of algorithm

由表4 可知，對(duì)于場(chǎng)景1、2、3 及6，兩種算法均無(wú)法規(guī)劃出合適的路徑，這是因?yàn)橐陨蠄?chǎng)景起始點(diǎn)和目標(biāo)點(diǎn)間的初始橫向偏移距離較小，而航向角差距較大，導(dǎo)致路徑的最大曲率無(wú)法滿足AGV 的最小轉(zhuǎn)彎半徑約束。對(duì)求解出的兩種路徑進(jìn)行跟蹤對(duì)比驗(yàn)證，其終點(diǎn)橫向偏移誤差及航向角誤差如圖8 及表5 所示。其中，將跟蹤算法取得的誤差值設(shè)為實(shí)際值的絕對(duì)值。由于表4中，場(chǎng)景1、2、3、6 無(wú)法求解出合適路徑，因此在表5 中不對(duì)以上場(chǎng)景進(jìn)行跟蹤測(cè)試。

表5 終點(diǎn)跟蹤誤差對(duì)比Table 5 Comparison of tracking error for end point

圖8 兩種算法終點(diǎn)處橫偏距對(duì)比Fig.8 Comparison of lateral error

圖8 中，場(chǎng)景7、10、14、15 下，原規(guī)劃算法與本文規(guī)劃算法的終點(diǎn)處橫偏距差距較小。對(duì)于場(chǎng)景10、14、15，這是因?yàn)槠鹬裹c(diǎn)間的航向角差距較小、行駛方向與起止點(diǎn)的連線方向相差較小所導(dǎo)致的。對(duì)于場(chǎng)景7，由于起止點(diǎn)的航向角位于不同象限導(dǎo)致AGV 存在調(diào)頭現(xiàn)象，同時(shí)初始橫偏距較小，導(dǎo)致兩種路徑終點(diǎn)附近曲率差距較小，因此終點(diǎn)橫偏距差距較小。除去以上特殊場(chǎng)景，余下的大量場(chǎng)景中，原算法與本文算法得到的最終橫偏距差距較大，且改進(jìn)的跟蹤算法使得AGV 跟蹤本文規(guī)劃算法得出的路徑時(shí)，能在終點(diǎn)處取得更小的航向角誤差。

選取表3 中的典型場(chǎng)景16 進(jìn)行規(guī)劃及跟蹤效果的詳細(xì)分析。其粒子群迭代曲線、跟蹤效果對(duì)比、航向角變化情況如圖9、圖10、圖11所示。

圖9 場(chǎng)景16粒子群算法迭代過(guò)程Fig.9 Iterative process of particle swarm optimization for scenario16

圖10 場(chǎng)景16規(guī)劃路徑跟蹤效果對(duì)比圖Fig.10 Comparison of tracking effect for scenario 16

圖11 場(chǎng)景16規(guī)劃路徑航向角變化對(duì)比圖Fig.11 Comparison of heading changes for scenario 16

由圖9 知，在對(duì)場(chǎng)景16 進(jìn)行求解的過(guò)程中，當(dāng)算法迭代到40 次左右時(shí)，目標(biāo)函數(shù)能收斂至最佳值。實(shí)際對(duì)表4中的所有場(chǎng)景進(jìn)行優(yōu)化求解時(shí)，迭代曲線顯示算法總能在迭代至約40代或之前時(shí)收斂至最佳值。由于篇幅限制，因此文章僅對(duì)典型場(chǎng)景16 的算法收斂曲線進(jìn)行展示。

由圖10（a）可知，AGV 在跟蹤至第一個(gè)拐點(diǎn)附近時(shí)，由于曲率較大發(fā)生明顯偏移，之后偏移量收斂。而終點(diǎn)附近曲率再一次變大，使得AGV 再次發(fā)生明顯偏移，最終導(dǎo)致實(shí)際停留點(diǎn)與目標(biāo)點(diǎn)間的誤差較大。其中，橫向偏移距離為0.088 2 m，航向角誤差為4.3×10-3rad，托盤對(duì)接誤差較大。由圖10（b）可知，AGV在跟蹤本文算法規(guī)劃的路徑時(shí)，在第一個(gè)拐點(diǎn)附近同樣產(chǎn)生明顯的偏移，之后誤差收斂。但終點(diǎn)附近曲率較小，使得AGV能更為精準(zhǔn)地跟蹤終點(diǎn)附近的路徑。最終橫向偏移距離為0.020 1 m，航向角誤差趨向于0 rad，能很好地滿足托盤拾取的精度要求。由圖10（c）可知，文獻(xiàn)[15]規(guī)劃算法得到的路徑雖然能使得路徑全局曲率更小，但在起止點(diǎn)間初始位姿差距較大的情況下，會(huì)使得路徑長(zhǎng)度過(guò)大，因此增加了路徑的行走時(shí)間，降低了托盤拾取工作的效率。圖11 中，AGV 在跟蹤本文算法規(guī)劃出的路徑時(shí)，其航向角變化曲線基本位于跟蹤原算法規(guī)劃出的路徑的航向角曲線下方，能較好地提高AGV 的跟蹤穩(wěn)定性。

為驗(yàn)證AGV 反向行駛規(guī)劃方法的可行性，設(shè)計(jì)3個(gè)仿真場(chǎng)景如表6所示。其求解、最終跟蹤結(jié)果如表7、8所示。

表6 算法仿真場(chǎng)景參數(shù)Table 6 Parameters of simulation scene

表7 算法仿真求解表Table 7 Results of algorithm

表8 終點(diǎn)跟蹤誤差對(duì)比Table 8 Comparison of tracking error for end point

其中，選取典型場(chǎng)景1 進(jìn)行跟蹤分析，其跟蹤效果圖、航向角變化情況如圖12所示。

圖12 場(chǎng)景1規(guī)劃路徑及跟蹤數(shù)據(jù)圖Fig.12 Path planning and tracking data for scenario 1

由圖12（a）可知，AGV在行駛至曲率較大的一段路徑時(shí)，與規(guī)劃路徑出現(xiàn)明顯的偏移，隨后路徑曲率減小，跟蹤誤差收斂。誤差收斂后的跟蹤路徑與規(guī)劃路徑高度重合。圖12（b）中，托盤拾取路段航向角變化平滑自然，使得AGV跟蹤路徑的精度更高，能較好地滿足托盤拾取工作的精度約束。

綜合以上分析，本文基于文獻(xiàn)[16]、文獻(xiàn)[17]的規(guī)劃算法進(jìn)行了改進(jìn)，使得其能應(yīng)用于托盤拾取工作場(chǎng)景中，同時(shí)重新設(shè)計(jì)了跟蹤算法以提高AGV 路徑跟蹤的準(zhǔn)確度。相較于文獻(xiàn)[15]的托盤拾取路徑規(guī)劃算法，本文提出的規(guī)劃及跟蹤算法能保證即使在起止點(diǎn)間位姿差距較大的情況下，AGV 仍能與托盤孔進(jìn)行平穩(wěn)且精確的對(duì)接，同時(shí)使得規(guī)劃路徑盡可能短。因此本文算法能更好地應(yīng)用于實(shí)際工況下的托盤拾取作業(yè)場(chǎng)景中。

6 結(jié)束語(yǔ)

針對(duì)實(shí)際工況中叉車型AGV托盤拾取局部路徑規(guī)劃與跟蹤問題，以B樣條曲線為基礎(chǔ)，綜合考慮AGV運(yùn)動(dòng)約束、起止點(diǎn)位姿約束及場(chǎng)地限制等約束，設(shè)定路徑長(zhǎng)度最短為主要優(yōu)化目標(biāo)，設(shè)置不同的初始橫向偏移距離及航向角差值，建立實(shí)際工況下叉車型AGV 托盤拾取的多約束、最小化目標(biāo)代價(jià)函數(shù)的路徑規(guī)劃模型。以純跟蹤法為基礎(chǔ)，考慮前視距離和航向角誤差，建立融合純跟蹤法和航向角控制法的路徑跟蹤算法。路徑規(guī)劃及跟蹤控制算法仿真的結(jié)果表明，本文的路徑規(guī)劃算法能較好克服起止點(diǎn)間橫偏距及航向角差距較大導(dǎo)致的實(shí)際工況下叉車型AGV 托盤拾取精度不佳的問題，且跟蹤算法使得AGV能更精確地跟蹤規(guī)劃路徑。