面向速度自適應(yīng)控制的AGV路徑跟蹤方法研究

何杰明,戴國(guó)志,余 璇

(廣東煙草惠州市有限責(zé)任公司,廣東 惠州 516003)

近年來(lái),隨著智能倉(cāng)儲(chǔ)的興起,叉車(chē)無(wú)人化、智能化改造成為了發(fā)展趨勢(shì)和研究熱點(diǎn)[1- 2]。叉車(chē)自動(dòng)導(dǎo)向車(chē)(automated guided vehicle,AGV)作為現(xiàn)代物流先進(jìn)智能裝備的典型代表之一,能夠較好地解決倉(cāng)儲(chǔ)環(huán)境中物料流量大、運(yùn)送環(huán)節(jié)多、生產(chǎn)過(guò)程自動(dòng)化/柔性化程度需求高等難題[3]。當(dāng)前,在叉車(chē)AGV技術(shù)研究中,路徑跟蹤控制一直以來(lái)都是研究的重點(diǎn)和關(guān)鍵?，F(xiàn)有的路徑跟蹤控制方法雖然在常規(guī)環(huán)境下具有較好的跟蹤效果,但針對(duì)具有窄道、彎道等特殊場(chǎng)景的復(fù)雜倉(cāng)儲(chǔ)環(huán)境,其路徑跟蹤效果并不理想,并且叉車(chē)AGV在上述場(chǎng)景中的控制穩(wěn)定性和精度優(yōu)化程度也相對(duì)不足[4]。

AGV路徑跟蹤要求AGV在規(guī)定時(shí)間到達(dá)某一預(yù)設(shè)好的參考路徑點(diǎn),常見(jiàn)的路徑跟蹤方法有純跟蹤算法[5]、斯坦利算法[6]、線(xiàn)性二次型控制器(linear quadratic regulator,LQR)算法[7]等。其中,純跟蹤算法是基于幾何的跟蹤方法,通過(guò)調(diào)整叉車(chē)AGV驅(qū)動(dòng)輪轉(zhuǎn)角,使叉車(chē)AGV質(zhì)心沿著經(jīng)過(guò)目標(biāo)點(diǎn)的圓弧行駛,達(dá)到跟蹤期望路徑的目的。冀同濤等[8]針對(duì)純跟蹤算法在非固定曲率下路徑跟蹤誤差較大的問(wèn)題,提出了一種基于前饋和反饋相結(jié)合的路徑跟蹤算法,在改善路徑跟蹤效果的同時(shí),保證了車(chē)輛的穩(wěn)定性。陳龍等[9]提出了一種利用模糊控制方法改進(jìn)的純跟蹤模型,實(shí)現(xiàn)了純跟蹤模型前視距離的動(dòng)態(tài)調(diào)整,以適應(yīng)復(fù)雜的泊車(chē)路徑。斯坦利算法是一種基于橫向跟蹤誤差的非線(xiàn)性反饋函數(shù),能實(shí)現(xiàn)橫向跟蹤誤差指數(shù)收斂于零,并根據(jù)叉車(chē)AGV位姿與期望路徑的幾何位置關(guān)系,獲得叉車(chē)方向盤(pán)轉(zhuǎn)角控制量。王亞平[10]提出了一種基于斯坦利控制理論的自適應(yīng)橫向控制系統(tǒng),該控制系統(tǒng)可實(shí)現(xiàn)對(duì)不同車(chē)速與不同曲率軌跡的跟蹤。靳欣宇等[11]在斯坦利轉(zhuǎn)向幾何預(yù)瞄模型的基礎(chǔ)上,提出了一種自適應(yīng)的最優(yōu)預(yù)瞄時(shí)間改進(jìn)算法,實(shí)現(xiàn)了無(wú)人車(chē)在復(fù)雜開(kāi)放道路上的無(wú)人駕駛。LQR算法的控制對(duì)象是以狀態(tài)空間形式給出的線(xiàn)性系統(tǒng),目標(biāo)函數(shù)為對(duì)象狀態(tài)和控制輸入的二次型函數(shù),基于LQR算法可得到狀態(tài)線(xiàn)性反饋的最優(yōu)控制規(guī)律,易于構(gòu)成閉環(huán)最優(yōu)控制。劉曉東[12]針對(duì)LQR算法參數(shù)確定困難的問(wèn)題,引入了同步擾動(dòng)隨機(jī)逼近(simultaneous perturbation stochastic approximation,SPSA)算法對(duì)LQR參數(shù)進(jìn)行尋優(yōu),并設(shè)計(jì)了基于SPSA-LQR的橫向控制器,改善了路徑跟蹤性能和能量消耗。

由上述文獻(xiàn)可知,純跟蹤算法在具有跟蹤誤差和非連續(xù)路徑的場(chǎng)景中,抗干擾能力較強(qiáng),但對(duì)速度要求較高的場(chǎng)合,其跟蹤精度較差;斯坦利算法在中低速或路徑曲率連續(xù)的場(chǎng)景中,其路徑跟蹤平衡性表現(xiàn)較好,但并沒(méi)有考慮前視距離,時(shí)常會(huì)發(fā)生轉(zhuǎn)向過(guò)度的情況;而LQR算法雖然具有優(yōu)異的閉環(huán)最優(yōu)控制性能,但是其參數(shù)調(diào)節(jié)復(fù)雜,不僅需要AGV的模型參數(shù),而且還要調(diào)節(jié)LQR目標(biāo)函數(shù)以獲得較好的跟蹤效果,同時(shí)對(duì)外界抗干擾效果的魯棒性較差。綜上所述,為解決當(dāng)前倉(cāng)儲(chǔ)環(huán)境下叉車(chē)AGV彎道路徑跟蹤控制精度不足、抗干擾能力弱等問(wèn)題,本文結(jié)合叉車(chē)AGV的運(yùn)動(dòng)特點(diǎn),考慮融合純跟蹤算法和斯坦利算法的優(yōu)點(diǎn),選用PID速度控制器,提出了一種新的面向速度自適應(yīng)控制的AGV路徑跟蹤算法,以改善叉車(chē)AGV路徑跟蹤效果。

1 速度自適應(yīng)控制算法描述

PID是一種閉環(huán)控制算法,包括比例(proportional)、積分(integral)、微分(derivative)3種控制方法。通過(guò)這3種方法的組合可有效糾正被控制對(duì)象的偏差,使其達(dá)到一個(gè)穩(wěn)定的狀態(tài)。

純跟蹤算法基于幾何關(guān)系和阿克曼轉(zhuǎn)向定理計(jì)算叉車(chē)AGV驅(qū)動(dòng)輪的轉(zhuǎn)角,在具有跟蹤誤差和非連續(xù)路徑的場(chǎng)景中抗干擾能力強(qiáng),低速時(shí)跟蹤精度良好,但在前視距離較大時(shí),會(huì)發(fā)生轉(zhuǎn)向不足、轉(zhuǎn)向回調(diào)速度慢、轉(zhuǎn)彎內(nèi)切等現(xiàn)象,且在高速時(shí)跟蹤精度會(huì)變差。純跟蹤算法模型圖如圖1所示,由其計(jì)算出的驅(qū)動(dòng)輪轉(zhuǎn)角δ1為:

圖1 純跟蹤算法模型圖

(1)

式中:L為叉車(chē)AGV支撐輪與驅(qū)動(dòng)輪之間的軸距,R為給定轉(zhuǎn)角下支撐輪遵循的圓半徑,k為道路曲率,ld為前視距離,α為預(yù)瞄點(diǎn)與叉車(chē)AGV前進(jìn)方向的夾角。

斯坦利算法的核心思想是利用驅(qū)動(dòng)輪中心的路徑跟蹤偏差量對(duì)方向盤(pán)控制量進(jìn)行計(jì)算,在中低速或路徑曲率連續(xù)時(shí)效果較好,但時(shí)常會(huì)發(fā)生轉(zhuǎn)向過(guò)度的情況。斯坦利算法模型圖如圖2所示,由其計(jì)算出的驅(qū)動(dòng)輪轉(zhuǎn)角δ2為:

圖2 斯坦利算法模型圖

(2)

式中:δφ為航向誤差引起的轉(zhuǎn)角,在不考慮橫向誤差時(shí),δφ=φe,φe為叉車(chē)AGV航向與最近路徑點(diǎn)切線(xiàn)方向之間的夾角;δe為橫向誤差引起的轉(zhuǎn)角;p為增益系數(shù);e為叉車(chē)AGV當(dāng)前姿態(tài)與目標(biāo)點(diǎn)之間的橫向誤差;v為當(dāng)前瞬時(shí)車(chē)速。

研究表明,在純跟蹤算法中加入斯坦利算法,可以很好地彌補(bǔ)單純使用純跟蹤算法導(dǎo)致的叉車(chē)AGV轉(zhuǎn)向不足、驅(qū)動(dòng)輪轉(zhuǎn)向回調(diào)速度慢、在高速階段路徑跟蹤精度不高等問(wèn)題。因此本文將兩種算法相結(jié)合,設(shè)計(jì)了一種面向速度自適應(yīng)的AGV路徑跟蹤算法,改進(jìn)后的算法可根據(jù)路徑曲率自主設(shè)定叉車(chē)AGV運(yùn)行速度,同時(shí)還可根據(jù)路徑曲率自動(dòng)切換比例因子,合理選取前視距離,精準(zhǔn)計(jì)算驅(qū)動(dòng)輪轉(zhuǎn)角,使叉車(chē)AGV既能在貨架密集、彎道集中、車(chē)速低的情況下實(shí)現(xiàn)精確跟蹤,又能在貨架疏散的直線(xiàn)路段、車(chē)速較高的情況下實(shí)現(xiàn)準(zhǔn)確路徑跟隨。融合兩種算法模型后形成的叉車(chē)AGV驅(qū)動(dòng)輪轉(zhuǎn)角δ計(jì)算公式如下:

δ=k1δ1+k2δ2

(3)

式中:k1和k2為比例因子,需根據(jù)路徑曲率來(lái)確定,且滿(mǎn)足k1+k2=1,若路徑曲率為0,即跟蹤路徑為直線(xiàn)時(shí),k1取0.50～0.80;若路徑曲率不為0,即跟蹤路徑為曲線(xiàn)時(shí),k1取0.30～0.50。在本文中,直線(xiàn)路段選取k1=0.75,k2=0.25;曲線(xiàn)路徑選取k1=0.50,k2=0.50。

最終設(shè)計(jì)的叉車(chē)AGV速度自適應(yīng)控制算法工作流程如圖3所示。

圖3 叉車(chē)AGV速度自適應(yīng)控制算法工作流程

2 仿真結(jié)果及分析

本文在MATLAB環(huán)境下對(duì)叉車(chē)AGV進(jìn)行路徑跟蹤仿真實(shí)驗(yàn),模擬叉車(chē)AGV的實(shí)時(shí)位姿,計(jì)算叉車(chē)AGV的驅(qū)動(dòng)輪轉(zhuǎn)角,并進(jìn)行運(yùn)動(dòng)控制。其中,驅(qū)動(dòng)輪轉(zhuǎn)角δ與車(chē)速v為模型的輸入量,叉車(chē)AGV位置信息及航向信息為模型的輸出量。

1)傾斜直線(xiàn)跟蹤仿真實(shí)驗(yàn)。

給定期望路徑為傾斜直線(xiàn),直線(xiàn)方程為y=2x+2,x∈[0,30],目標(biāo)終點(diǎn)位姿為(30, 62, 1.11),叉車(chē)AGV空載目標(biāo)速度為2.5 m/s,初速度為0 m/s,初始位姿為(0, 1, 0),驅(qū)動(dòng)輪初始轉(zhuǎn)向角為1.25 rad,仿真結(jié)果如圖4所示。

圖4 叉車(chē)AGV傾斜直線(xiàn)跟蹤效果及其驅(qū)動(dòng)輪轉(zhuǎn)角和航向角

從圖4可以看出,叉車(chē)AGV從初始位姿向x軸方向移動(dòng)1 m后可跟蹤到期望路徑,響應(yīng)速度較快。驅(qū)動(dòng)輪轉(zhuǎn)角調(diào)整至最大角度后逐步收斂至0 rad,航向角隨轉(zhuǎn)角變化快速收斂至期望航向角。整體上看,叉車(chē)AGV跟蹤直線(xiàn)誤差逐漸收斂,無(wú)超調(diào),跟蹤效果較好。

2)圓周軌跡跟蹤實(shí)驗(yàn)。

給定期望軌跡為圓周軌跡,圓周方程為(x-7)2+(y-13)2=196,x∈R。叉車(chē)AGV初速度為0 m/s,目標(biāo)速度為1.0 m/s,初始位姿為(-20, 25, 0),驅(qū)動(dòng)輪初始轉(zhuǎn)角為-1.00 rad,仿真結(jié)果如圖5所示。

圖5 叉車(chē)AGV圓周曲線(xiàn)跟蹤效果及其驅(qū)動(dòng)輪轉(zhuǎn)角和航向角

由圖5可知,叉車(chē)AGV驅(qū)動(dòng)輪首先需調(diào)整一個(gè)較大的角度以快速跟上期望路徑,隨后穩(wěn)定運(yùn)行,以較快的響應(yīng)速度將轉(zhuǎn)角逐步收斂至0 rad?？傮w來(lái)看,改進(jìn)后的速度自適應(yīng)控制算法對(duì)圓周軌跡跟蹤效果較好,符合叉車(chē)AGV圓周軌跡跟蹤要求。

3)本文算法與純跟蹤算法直線(xiàn)路徑跟蹤對(duì)比。

設(shè)給定的直線(xiàn)路徑方程為y=5,x∈R,目標(biāo)終點(diǎn)位姿為(50,5,0)。設(shè)定叉車(chē)AGV目標(biāo)速度為2.5 m/s,初速度為0 m/s,初始位姿為(0, 1, 0),仿真結(jié)果如圖6和圖7所示。

圖6 直線(xiàn)路段路徑跟蹤結(jié)果對(duì)比

圖7 直線(xiàn)路段驅(qū)動(dòng)輪轉(zhuǎn)角對(duì)比

由圖6和圖7可以看出,純跟蹤算法超調(diào)量較大,響應(yīng)速度慢且達(dá)到穩(wěn)定狀態(tài)耗時(shí)較長(zhǎng);而本文提出的速度自適應(yīng)路徑跟蹤算法,響應(yīng)速度快且達(dá)到穩(wěn)定狀態(tài)耗時(shí)短,跟蹤誤差小,驅(qū)動(dòng)輪轉(zhuǎn)角大,轉(zhuǎn)向能力強(qiáng),且對(duì)驅(qū)動(dòng)輪回調(diào)速度快,收斂速度快,明顯優(yōu)于純跟蹤算法。

4)本文算法與純跟蹤算法曲線(xiàn)路徑跟蹤對(duì)比。

設(shè)給定的曲線(xiàn)路徑為類(lèi)正弦函數(shù)曲線(xiàn),曲線(xiàn)方程為y=sin(x/5)×x/2,x∈[0,45],叉車(chē)AGV目標(biāo)速度為1.0 m/s,初速度為0 m/s,初始位姿為(0,3,0),仿真結(jié)果如圖8和圖9所示。

圖8 彎曲路段路徑跟蹤結(jié)果對(duì)比

圖9 彎曲路段驅(qū)動(dòng)輪轉(zhuǎn)角對(duì)比

由圖8和圖9可知,本文所提算法可以更快、更準(zhǔn)確地跟蹤到期望路徑,路徑跟蹤無(wú)偏差,在曲率變化明顯的地方,驅(qū)動(dòng)輪轉(zhuǎn)角更大且收斂速度更快;而純跟蹤算法在曲率變化明顯的地方有超調(diào),路徑跟蹤存在偏差。因此在曲線(xiàn)路徑跟蹤方面,本文所提算法也明顯優(yōu)于純跟蹤算法。

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

本文以純跟蹤算法和斯坦利算法為基礎(chǔ),選用PID速度控制器,提出了一種面向速度自適應(yīng)控制的AGV路徑跟蹤算法。該算法通過(guò)跟蹤路徑曲率確定叉車(chē)AGV車(chē)速,合理選擇比例因子,得到最終的驅(qū)動(dòng)輪轉(zhuǎn)角控制量。仿真結(jié)果表明,該算法彌補(bǔ)了純跟蹤算法轉(zhuǎn)向不足和高速階段跟蹤精度不精確等問(wèn)題,可有效提高倉(cāng)儲(chǔ)環(huán)境下叉車(chē)AGV的路徑跟蹤精度,為大型無(wú)人智能倉(cāng)儲(chǔ)的流暢運(yùn)轉(zhuǎn)提供了技術(shù)支撐。