夏韻凱 張 征 蔡穎杰 戚鴻斌 姚富林 周圣云

(*浙江工業(yè)大學(xué)機(jī)械工程學(xué)院 杭州310023)

(**深圳怡豐機(jī)器人科技有限公司 深圳518116)

0 引言

隨著國民生活水平的提高,城市的交通問題日益突出,特別是人員密集的大城市,停車難成為一種社會問題,一定程度上影響了社會的發(fā)展。近年來,自動導(dǎo)引車(automated guided vehicle,AGV)得到了快速發(fā)展,在自動化工廠與物流行業(yè)得到了廣泛應(yīng)用[1]。目前停車AGV 作為智能停車庫的組成部分,越來越受到社會的廣泛關(guān)注[2]。

定位、軌跡跟蹤與運(yùn)動控制是AGV 的核心技術(shù)[3-5]。軌跡跟蹤是指移動機(jī)器人從已知的初始位姿狀態(tài)開始,通過運(yùn)動控制,使其能以預(yù)設(shè)的速度沿著已規(guī)劃的一條幾何路徑行駛。常見的軌跡跟蹤方法可分為預(yù)瞄式和非預(yù)瞄式[6-8]。預(yù)瞄式路徑跟隨通過比較機(jī)器人當(dāng)前位姿與期望路徑上預(yù)瞄點(diǎn)坐標(biāo)的幾何關(guān)系,對機(jī)器人轉(zhuǎn)向系統(tǒng)進(jìn)行控制從而使得AGV 能沿著規(guī)劃的路徑行駛。非預(yù)瞄式控制方法主要根據(jù)機(jī)器人的當(dāng)前位姿狀態(tài)與理論的坐標(biāo)點(diǎn)的偏差進(jìn)行控制,從而使得移動機(jī)器人沿著規(guī)劃的路徑行駛。常見的非預(yù)瞄式控制方法主要有比例積分微分(progortional integral derivative,PID)控制、前饋-反饋控制[9]和模型預(yù)測控制[10-12]等。PID 是工業(yè)上常用的線性控制器,控制方法簡單,但其控制參數(shù)需要經(jīng)過大量實(shí)驗(yàn)試湊法獲得,而且不同機(jī)器人的PID 參數(shù)還有所區(qū)別,不適宜工業(yè)機(jī)器人的大量部署。前饋-反饋控制通過移動機(jī)器人運(yùn)動參考路徑曲率變化的干擾量進(jìn)行運(yùn)動控制量的補(bǔ)償,但是該方法不適合在高速運(yùn)動下的機(jī)器人,因?yàn)殡S著速度的提高,機(jī)器人將無法跟蹤彎曲的道路[13]。純追蹤[14]是基于預(yù)瞄式系統(tǒng)控制,由于其穩(wěn)定的性能,常用于無人駕駛汽車的軌跡控制。

目前的軌跡跟蹤算法都是針對無人駕駛汽車或簡單運(yùn)動形式的移動機(jī)器人,無法直接應(yīng)用于全向運(yùn)動的雙舵輪停車AGV 軌跡跟蹤。因此本文基于純追蹤預(yù)瞄式系統(tǒng)控制思想,提出了基于偏差的純追蹤雙舵輪停車AGV 全向運(yùn)動軌跡跟蹤。本文的主要工作內(nèi)容包括以下四方面。

(1) 針對雙舵輪停車AGV 建立了對應(yīng)的運(yùn)動模型,設(shè)計(jì)了雙舵輪AGV 的全向運(yùn)動控制律。

(2) 在純追蹤算法的基礎(chǔ)上提出了基于偏差的純追蹤雙舵輪AGV 全向運(yùn)動軌跡跟蹤算法。

(3) 通過Python 對雙舵輪停車AGV 進(jìn)行數(shù)學(xué)建模及仿真,成功驗(yàn)證了算法的可行性。

(4) 通過實(shí)車軌跡跟蹤實(shí)驗(yàn),驗(yàn)證了本文所提出控制模型與軌跡跟蹤算法的有效性。

1 雙舵輪停車AGV 的運(yùn)動控制

1.1 雙舵輪運(yùn)動模型

雙舵輪停車AGV 因?yàn)槠潋?qū)動形式,除了普通的前進(jìn)、后退、轉(zhuǎn)彎、自旋運(yùn)動外,還可以實(shí)現(xiàn)平移、橫移,甚至組合運(yùn)動以適應(yīng)不同路徑要求,環(huán)境適應(yīng)性更強(qiáng)。

雙舵輪停車的運(yùn)動學(xué)模型示意圖如圖1 所示,X-Y為局部坐標(biāo)系,兩舵輪之間的軸距為2L;α、α1和α2分別為AGV 的速度方向角、舵輪1 的方向角和舵輪2 的方向角(逆時針為正);v、v1和v2分別為AGV 的速度、舵輪1 的速度和舵輪2 的速度;S1(L,0),S2(-L,0) 分別是舵輪1 與舵輪2 在局部坐標(biāo)系下的坐標(biāo)。需要根據(jù)舵輪的運(yùn)動參數(shù)求得AGV運(yùn)動參考點(diǎn)O的速度v與角速度ω。

因此AGV 的運(yùn)動學(xué)模型可表示為

其中vx、vy為運(yùn)動參考點(diǎn)速度v的正交分解的速度。

1.2 雙舵輪停車AGV 的運(yùn)動控制

常見的移動機(jī)器人控制律為[v,ω]T,即通過控制機(jī)器人的線速度與角速度,從而實(shí)現(xiàn)機(jī)器人的運(yùn)動控制,但其并不適用于本文的雙舵輪停車AGV 的運(yùn)動控制。因此需要通過純追蹤算法獲取能夠控制雙舵輪運(yùn)行的控制律。本文將控制律設(shè)計(jì)為[v,α,ω]T,其中v為參考點(diǎn)的速度大小,α為速度方向角,ω為車體旋轉(zhuǎn)角速度。通過此控制律,可以求解AGV 中各個舵輪的轉(zhuǎn)角和速度大小,從而實(shí)現(xiàn)AGV的全向運(yùn)動控制。下面是已知控制律[v,α,ω]T的情況下,求解AGV 兩個舵輪的轉(zhuǎn)角與速度。

從圖1 可知,所示O′為剛體的旋轉(zhuǎn)中心,根據(jù)剛體運(yùn)動的速度投影法[15]可得:

圖1 雙舵輪局部坐標(biāo)系運(yùn)動示意圖

其中,vx為參考點(diǎn)O處速度v在局部坐標(biāo)系下正交分解得到的X軸方向分速度,v1x與v2x分別為兩舵輪速度v1和v2基于AGV 局部坐標(biāo)系正交分解得到的X軸方向的分速度。

由式(1)與式(3)建立方程組,可得:

其中,vy為參考點(diǎn)O處速度v在局部坐標(biāo)系下正交分解得到的Y軸方向分速度,v1y與v2y分別為兩舵輪速度v1和v2基于AGV 局部坐標(biāo)系正交分解得到的Y軸方向的分速度。

因此AGV 2 個舵輪的速度與角速度可表示為

2 基于偏差的純追蹤路徑跟蹤

2.1 常規(guī)的純追蹤算法

純追蹤由于其簡單穩(wěn)定的控制算法,廣泛應(yīng)用于無人駕駛汽車與移動機(jī)器人中[16]。純追蹤是根據(jù)車輛與目標(biāo)路徑的幾何關(guān)系,對車輛進(jìn)行橫向運(yùn)動控制,從而實(shí)現(xiàn)軌跡跟蹤。

將純追蹤算法應(yīng)用于叉車AGV 的運(yùn)動控制時,以叉車從動輪中心O1為切點(diǎn),車輛縱向車身為切線,通過控制前輪偏角θ,使車輛可以沿著一條經(jīng)過預(yù)瞄點(diǎn)的圓弧行駛,叉車AGV 運(yùn)動過程的路徑跟隨過程如圖2 所示。其中,O為AGV 的運(yùn)動圓周中心,O1為AGV 叉車從動輪中心,O2為AGV 預(yù)瞄的參考點(diǎn),wb為舵輪到參考點(diǎn)的距離,ld為預(yù)瞄點(diǎn)與當(dāng)前參考點(diǎn)的距離,Δα為叉車與預(yù)瞄點(diǎn)的夾角,R1為叉車的運(yùn)動半徑。

圖2 叉車純追蹤算法

根據(jù)幾何關(guān)系不難得到叉車純追蹤的控制量表達(dá)式:

因此只需設(shè)置合理的ld值就能控制叉車運(yùn)動。文獻(xiàn)[17]已說明,預(yù)瞄參數(shù)ld與車速間存在一定數(shù)學(xué)關(guān)系:

其中,v為車輛行駛速度,A、B、C均為常數(shù)項(xiàng)。其中A可以表示為

amax為車輛最大制動加速度,B表示車輛遇到異常情況時進(jìn)行反應(yīng)的車輛行駛距離,C表示為車輛最小轉(zhuǎn)彎半徑。

2.2 基于偏差的雙舵輪停車AGV 的軌跡跟蹤算法流程

叉車AGV 由于運(yùn)動形式的限制,其軌跡跟蹤時的航向角只能與運(yùn)行的路徑相切,無法實(shí)現(xiàn)全向運(yùn)動,而雙舵輪停車AGV 可以進(jìn)行全向驅(qū)動。因此需要設(shè)計(jì)能夠滿足雙舵輪停車AGV 全向運(yùn)動的軌跡跟蹤算法。

雙舵輪AGV 全向軌跡跟蹤算法的流程如圖3所示,算法的具體流程描述如下。

圖3 AGV 全向軌跡跟蹤算法流程

(1) 設(shè)置前視距離ld

圖2 中叉車AGV 中的控制量是舵輪轉(zhuǎn)角θ,而本文設(shè)計(jì)的雙舵輪AGV 的控制量為[v,α,ω]T,因此本文對純追蹤算法進(jìn)行了修改。常規(guī)曲線運(yùn)動控制原理如下文的圖5 所示。預(yù)瞄距離ld由式(12)確定,本文中A=0.25,B=0.1,C=350 mm。預(yù)瞄距離ld用于計(jì)算路徑上的預(yù)瞄點(diǎn)(G點(diǎn))。

(2) 獲取路徑上預(yù)瞄點(diǎn)信息

AGV 沿著設(shè)計(jì)的軌跡行走,通過前視距離與軌跡相交,得到預(yù)瞄點(diǎn)(G)。預(yù)瞄點(diǎn)的信息包括(x,y,v,C,T)。其中x與y為預(yù)瞄點(diǎn)位置信息;v為軌跡的設(shè)定速度;C為軌跡在預(yù)瞄點(diǎn)處的曲率;T為軌跡的類型。有兩類軌跡,其中normal 表示常規(guī)曲線,另一類型為橫移曲線。

(3) 判斷路徑類型并計(jì)算AGV 控制律

根據(jù)軌跡類型T獲取對應(yīng)的曲線類型,當(dāng)T為normal 時使用下文的式(13)計(jì)算對應(yīng)的控制律;當(dāng)T不為normal 時,使用下文的式(14)計(jì)算對應(yīng)的控制律。

(4) 解析控制律得到控制AGV 舵輪運(yùn)動的參數(shù)

當(dāng)?shù)玫綄?yīng)的控制律后,將控制律帶入式(7)與式(8)后即可得到控制AGV 舵輪運(yùn)動的參數(shù)(v1,α1)T、(v2,α2)T,實(shí)現(xiàn)雙舵輪AGV 的全向軌跡跟蹤。

為了更好地分析本文提出的軌跡跟蹤算法的運(yùn)行效果,對實(shí)際使用的運(yùn)動方式進(jìn)行分析。雙舵輪AGV 的主要運(yùn)動形式如圖4 所示,不同運(yùn)動形態(tài)下兩舵輪的角度與速度情況如表1 所示。本文列舉了5 種運(yùn)動形態(tài),分別為自旋運(yùn)動、直線前進(jìn)運(yùn)動、橫移直線運(yùn)動、常規(guī)曲線運(yùn)動與橫移曲線運(yùn)動。表1中的變量與圖1 中的變量相同。

表1 雙舵輪AGV 運(yùn)動分析

圖4 雙舵輪AGV 的運(yùn)動形式

2.3 常規(guī)曲線軌跡跟蹤

雙舵輪AGV進(jìn)行常規(guī)曲線運(yùn)動時,AGV的航向角與規(guī)劃路徑相切,如圖5 所示。通過幾何關(guān)系可知常規(guī)曲線軌跡的控制律為[v,α,ω]T。其中v為軌跡中設(shè)定的AGV 運(yùn)行速度,α為雙舵輪AGV的參考點(diǎn)相對全局路徑地圖速度方向,ω為AGV 的運(yùn)動角速度。α與ω通過式(13)可得。

圖5 常規(guī)曲線路徑跟蹤原理圖

為了更好地控制雙舵輪AGV 的運(yùn)動,本文引入了偏差量控制參數(shù)k_φ與k_ω?？刂频钠顬锳GV 當(dāng)前位姿與路徑上最近位姿的位置偏差與角度偏差,通過調(diào)節(jié)k_φ與k_ω就能更好控制雙舵輪AGV 的常規(guī)曲線運(yùn)動。

2.4 橫移曲線軌跡跟蹤

雙舵輪停車AGV 的優(yōu)勢便是可以進(jìn)行橫移運(yùn)動,使其能在工作環(huán)境中更靈活地運(yùn)動。在橫移運(yùn)動時,兩舵輪轉(zhuǎn)角α1=α2≠0,將速度v1=v2代入式(1)可得,此時車體的角速度為0,車體中心的速度等于舵輪的速度?；谄畹碾p舵輪停車AGV的純追蹤控制原理如圖6 所示。

圖6 橫移曲線路徑跟蹤原理圖

預(yù)瞄點(diǎn)確定與常規(guī)曲線運(yùn)動一樣,由ld控制,通過幾何關(guān)系得到的橫移曲線運(yùn)動軌跡跟蹤控制律為[v,α,ω]T。其中v為軌跡中設(shè)定的AGV 運(yùn)行速度,α與ω通過式(14)可得。

與常規(guī)曲線運(yùn)動控制相同,橫移曲線運(yùn)動也通過調(diào)節(jié)k_φ與k_ω來更好地控制雙舵輪AGV 的運(yùn)動。

3 仿真與實(shí)驗(yàn)

為了驗(yàn)證雙舵輪全向AGV 的路徑跟蹤算法的效果,對其進(jìn)行了數(shù)學(xué)建模,并使用Python 實(shí)現(xiàn)仿真。仿真了AGV 運(yùn)行過程中的大部分軌跡類型,軌跡類型包括自旋運(yùn)動、直線前進(jìn)軌跡、常規(guī)曲線、橫移曲線,組合路徑運(yùn)動的軌跡如圖7 所示。

仿真時,使用了PID 控制與基于偏差的純追蹤軌跡跟蹤控制實(shí)現(xiàn)雙舵輪AGV 的組合運(yùn)動路徑仿真。PID 控制仿真過程中AGV 舵輪轉(zhuǎn)角、舵輪速度、運(yùn)動參考點(diǎn)速度變化如圖8 所示;基于偏差的純追蹤軌跡跟蹤控制仿真過程中AGV 舵輪轉(zhuǎn)角、舵輪速度、運(yùn)動參考點(diǎn)速度變化如圖9 所示。

圖7 中的不同路徑對應(yīng)圖9 中不同階段。組合路徑運(yùn)動過程中的控制量變化符合表1 中的運(yùn)動分析,即①階段為自旋運(yùn)動,兩舵輪轉(zhuǎn)角為α1=π/2、α2=-π/2,運(yùn)動參考點(diǎn)的速度為v=0;②、⑥、⑩階段為直線前進(jìn)運(yùn)動,兩舵輪轉(zhuǎn)角為α1=α2=0,運(yùn)動參考點(diǎn)的速度為v=v1=v2;③、④階段為常規(guī)曲線運(yùn)動,兩舵輪轉(zhuǎn)角為α1=-α2≠0,運(yùn)動參考點(diǎn)的速度為v=v1cosα1=v2cosα2;⑤、⑦、⑨為橫移曲線運(yùn)動,兩舵輪轉(zhuǎn)角為α1=α2≠0,運(yùn)動參考點(diǎn)的速度為v=v1=v2。圖8 的結(jié)果與圖9 相近,說明兩種控制均能控制雙舵輪AGV 實(shí)現(xiàn)全向軌跡跟蹤。圖10 是基于偏差的純追蹤軌跡跟蹤控制仿真過程中的距離偏差,表明仿真過程中的最大偏差約為45 mm,平均偏差約為10 mm。圖11是PID控制仿真過程中的距離偏差,表明仿真過程中的最大偏差約為46 mm,平均偏差約為10mm。兩者在仿真過程中結(jié)果相近,但是由于PID 算法需要大量調(diào)試參數(shù),在實(shí)際工程應(yīng)用中,本文提出的基于偏差的純追蹤軌跡跟蹤控制具有較強(qiáng)的適用性。

圖7 AGV 組合運(yùn)動路徑

圖8 PID 算法組合運(yùn)動中的控制量變化

圖9 基于偏差的純追蹤算法組合運(yùn)動中的控制量變化

圖10 基于偏差的純追蹤算法仿真距離偏差

圖11 基于PID 算法仿真距離偏差

為了驗(yàn)證本文提出的基于偏差的純追蹤軌跡跟蹤控制的實(shí)際運(yùn)行效果,進(jìn)行了實(shí)車驗(yàn)證。圖12 為本文研究的雙舵輪停車AGV,車身尺寸為5120 mm×1580 mm,AGV 前后舵輪位于AGV 中軸線上且舵輪連線的中點(diǎn)與AGV 車體中心重合,舵輪之間的間距為2250 mm。AGV 采用工業(yè)計(jì)算機(jī)進(jìn)行數(shù)據(jù)處理。計(jì)算機(jī)CPU 型號為Intel aton E3845,主頻1.91 Hz,搭配4 GB 內(nèi)存。實(shí)驗(yàn)中的運(yùn)行路徑如圖13 與圖14所示,分別測試了速度為500 mm/s 與800 mm/s 的軌跡跟蹤情況,并且每隔0.02 s 取一次數(shù)據(jù)。圖15與圖16為運(yùn)行過程中距離偏差曲線。

圖12 雙舵輪停車AGV

圖13 速度為500 mm/s 的軌跡跟蹤

圖14 速度為800 mm/s 的軌跡跟蹤

圖15 速度為500 mm/s 的距離偏差

圖16 速度為800 mm/s 的距離偏差

圖15 表明,當(dāng)AGV 的運(yùn)行速度為500 mm/s時,基于純追蹤的雙舵輪的偏差最大約為85 mm,平均偏差為30 mm 左右;基于偏差的純追蹤的最大偏差為50 mm,平均偏差為20 mm。圖16 表明,當(dāng)運(yùn)行速度為800 mm/s 時,純追蹤的最大偏差為100 mm,平均偏差為40 mm 左右;基于偏差的純追蹤的最大偏差為85 mm,平均偏差為25 mm。

綜上,本文提出的基于偏差的純追蹤雙舵輪停車AGV 全向運(yùn)動軌跡跟蹤,能有效提高雙舵輪AGV 的控制精度,實(shí)現(xiàn)全向軌跡跟蹤。

4 結(jié)論

本文提出了一種全向雙舵輪停車AGV 的運(yùn)動控制方法,并使用基于偏差的純追蹤算法進(jìn)行路徑跟蹤。仿真實(shí)驗(yàn)表明,該方法能夠正確描述雙舵輪AGV 的全向軌跡跟蹤。實(shí)驗(yàn)驗(yàn)證了基于偏差的純追蹤算法相較簡單的純追蹤算法其控制精度有較大提升,該方法能有效控制雙舵輪AGV 的全向運(yùn)動控制。