雙舵輪自動導(dǎo)引車軌跡追蹤控制算法

齊嘉暉， 吳耀華， 汪 威

(山東大學(xué)控制科學(xué)與工程學(xué)院,濟南 250061)

自動導(dǎo)引車(automated guided vehicle,AGV)是集成傳感器技術(shù)、運動控制等技術(shù)于一體的綜合控制系統(tǒng),已廣泛應(yīng)用于物資運輸、工業(yè)生產(chǎn)等諸多領(lǐng)域,成為智慧物流、智能制造和柔性生產(chǎn)中的重要設(shè)備。

自動導(dǎo)引車按照驅(qū)動方式分類,可以分為差速驅(qū)動導(dǎo)引車和舵輪驅(qū)動導(dǎo)引車。而舵輪式自動導(dǎo)引車由于驅(qū)動輪數(shù)量不同,常分為單舵輪自動導(dǎo)引車、雙舵輪自動導(dǎo)引車與多舵輪自動導(dǎo)引車。其中雙舵輪AGV由于車體運動靈活且應(yīng)用場景豐富,值得重點研究。

能夠精準(zhǔn)、高效地完成軌跡跟蹤任務(wù)控制是AGV系統(tǒng)的關(guān)鍵性能指標(biāo),也是AGV系統(tǒng)的研究熱點之一。中外學(xué)者對AGV軌跡跟蹤問題的研究較多,且多采用橫向或縱向糾偏控制實現(xiàn)AGV軌跡跟蹤。針對差速自動導(dǎo)引車,羅哉等[1]提出了一種基于最優(yōu)偏差路徑的模糊PID(proportional-integral-derivative,比例-積分-微分)糾偏算法,可以實現(xiàn)跟蹤精度為3.2 mm,橫向偏差在5 mm范圍內(nèi)的糾正,但位姿偏差的獲取方式影響了系統(tǒng)魯棒性；鄭炳坤等[2]提出了一種基于模糊自整定PD(proportional-derivative，比例-微分)調(diào)節(jié)器來動態(tài)跟蹤AGV軌跡,可實時監(jiān)控車載的運行狀態(tài)、跟蹤控制信息,但在轉(zhuǎn)彎時需要較大幅度降低速度；郭景華等[3]提出一種基于遺傳算法的無人車橫向模糊控制策略,但是實時性不高,且需要經(jīng)過大量訓(xùn)練。針對單舵輪自動導(dǎo)引車,Ibari等[4]基于李雅普諾夫穩(wěn)定性理論,設(shè)計了實現(xiàn)單舵輪AGV軌跡追蹤的反演控制器,保證了跟蹤誤差穩(wěn)定性；熊中剛等[5]提出基于免疫模糊PID的智能路徑控制方法,實現(xiàn)了小型農(nóng)機在復(fù)雜環(huán)境下,直線與曲線的軌跡跟蹤。針對雙舵輪自動導(dǎo)引車,Hemami[6]分析前后舵輪轉(zhuǎn)角間關(guān)系,提出了低速行駛下車輛軌跡追蹤方案；Yuan等[7]基于反演法設(shè)計了一種針對雙舵輪半掛牽引車的軌跡追蹤控制器,控制前后舵輪沿著統(tǒng)一軌跡行駛,提升了車輛的機動性。以上模型均包含較多參數(shù),且參數(shù)值選取方法僅針對特定場景,難以實現(xiàn)推廣。在快速路徑跟蹤時,AGV可能出現(xiàn)調(diào)整不及時而大幅走偏。針對四輪全向自動導(dǎo)引車,張業(yè)等[8]提出一種結(jié)合主動懸掛的四輪轉(zhuǎn)向集成控制系統(tǒng),運用模型預(yù)測控制算法,以車體各項性能指標(biāo)為控制目標(biāo),滿足極限工況下車體四輪轉(zhuǎn)向的穩(wěn)定性要求。Pandu等[9]提出了一種基于反步法的軌跡跟蹤控制器來跟蹤給定的軌跡。Nguyen等[10]設(shè)計一種滑動模式動態(tài)控制器,使自動導(dǎo)引車的速度收斂于速度控制輸入,實現(xiàn)AGV勻速跟蹤軌跡。

在軌跡跟蹤控制方面,目前常見的控制器模型主要有PID控制器、前饋-反饋控制器、線性二次型調(diào)節(jié)器[11]及模型預(yù)測控制器等。其中,模型預(yù)測控制(model predictive control, MPC)是通過搭建系統(tǒng)模型,預(yù)測系統(tǒng)未來狀態(tài)量和輸出量,對控制量進行滾動優(yōu)化,選擇最佳控制行為的算法[12],主要應(yīng)用于非線性高耦合的復(fù)雜系統(tǒng)控制中。致力于解決更長時間跨度的軌跡跟蹤問題,Felipe等[13]提出一種具有非完整約束的AGV控制方案,采用模型預(yù)測控制直接處理約束。將誤差模型連續(xù)線性化后,使用二次規(guī)劃求模型預(yù)測控制。Pacheco等[14]對基于MPC的軌跡跟蹤控制器與基于PID的軌跡跟蹤控制器進行對比,結(jié)果顯示在保證響應(yīng)速度的基礎(chǔ)上,MPC控制器具有更好的跟蹤效果。

迄今為止,對雙舵輪AGV的軌跡跟蹤研究還不夠深入。雙舵輪AGV按底盤結(jié)構(gòu)分類,主要分為兩種,分別為舵輪中線布置結(jié)構(gòu)與舵輪對角布置結(jié)構(gòu)。其中,雙舵輪驅(qū)動、激光導(dǎo)引,底盤采用舵輪中線布置的六輪AGV,具有操控性強、性價比高的優(yōu)點,但對其軌跡跟蹤控制研究甚少。雙舵輪自動導(dǎo)引車的糾偏過程與車體結(jié)構(gòu)、糾偏算法、控制系統(tǒng)設(shè)計等有密切的聯(lián)系。現(xiàn)以舵輪中線布置雙舵輪AGV為研究對象,研究其軌跡跟蹤控制。其底盤由前后并置的兩臺舵輪機構(gòu)和分列四角的4只從動萬向輪組成。在建立其運動學(xué)模型的基礎(chǔ)上,求得AGV最小轉(zhuǎn)向半徑,并重點分析該AGV轉(zhuǎn)向優(yōu)勢。然后設(shè)計基于模型預(yù)測的雙舵輪AGV糾偏控制器,結(jié)合控制系統(tǒng)設(shè)計,采用MATLAB軟件仿真AGV糾偏控制過程,驗證糾偏算法的實時性和魯棒性。

1 AGV轉(zhuǎn)彎運動學(xué)模型

為衡量單舵輪AGV與雙舵輪AGV在轉(zhuǎn)彎性能上的具體差異,首先建立兩者的轉(zhuǎn)彎運動學(xué)模型,在此基礎(chǔ)上,分別計算轉(zhuǎn)彎半徑進行比較。在建立運動學(xué)模型時,均假設(shè)AGV車體結(jié)構(gòu)為剛體,即符合剛體平動原理。同時,忽略AGV車身懸架對車體質(zhì)心影響。

1.1 AGV轉(zhuǎn)彎運動模型分析

單舵輪AGV由前輪單獨提供驅(qū)動力,后輪提供平衡支撐。其中,運動方向和速度控制由舵輪控制的前輪來完成。車體結(jié)構(gòu)示意圖如圖1(a)所示。

圖1 單舵輪AGV示意圖Fig.1 Single steering wheel AGV’s schematic

圖1中,Of為前舵輪中心點,Oc為后輪連線中心點。轉(zhuǎn)彎過程如圖1(b)所示,車體以C點為圓心做半徑為Rc的轉(zhuǎn)彎運動。設(shè)后輪中心點與前舵輪的距離OcOf長為Lc。則在三角形OcCOf中,有:

雙舵輪AGV的運動系統(tǒng)由4個萬向輪和兩個驅(qū)動舵輪組成,前后兩個舵輪分別安裝在車體前進方向軸線上。AGV的轉(zhuǎn)彎和直線運動主要通過舵輪控制實現(xiàn),可以按照預(yù)定的路線行進。

研究對象為圖2(a)所示的雙舵輪AGV。AGV車體四角各為一個從動輪,沿中線前后并置兩輪為獨立驅(qū)動的驅(qū)動輪。由于車體運動主要是通過動態(tài)調(diào)整前后舵輪來實現(xiàn)的,因此,在不影響運動學(xué)建模分析的情況下[15],將AGV的運動模型簡化為前后驅(qū)動輪的運動模型。

圖2 雙舵輪AGV示意圖Fig.2 Double-steering wheel AGV schematic

雙舵輪AGV的結(jié)構(gòu)示意圖如圖2(a)所示,每臺舵輪驅(qū)動帶動一個驅(qū)動輪。通過調(diào)節(jié)前后舵輪舵角,AGV可以完成直線路徑與曲線路徑的軌跡跟蹤任務(wù)。

建立平面直角坐標(biāo)系{XOY},Of、Ob分別為前后驅(qū)動輪與地面接觸點。軸距為L。考慮AGV車身處于轉(zhuǎn)彎圓弧軌跡時,由于圓弧軌跡各處曲率相同,因此行進過程中前后驅(qū)動輪轉(zhuǎn)角相同。設(shè)前驅(qū)動輪與后驅(qū)動輪行進方向與機器人體坐標(biāo)系{X′CY′}X軸夾角為θf、θb,Vf、Vb為前后輪行進速度,轉(zhuǎn)彎半徑為R。從后驅(qū)動輪進入轉(zhuǎn)彎圓弧開始計時,經(jīng)過t1時間,前輪走過的軌跡弧長為2Rθf。則有:

即驅(qū)動輪行進速度Vf恒定的前提下,車體轉(zhuǎn)角2θf與其行進時間t1成正比。根據(jù)轉(zhuǎn)彎軌跡三角關(guān)系,可知:

結(jié)合式(1)和式(3)可知,在航向角θf滿足θf∈(0,90°]時,始終有R

1.2 雙舵輪AGV定位算法

如圖2(b)所示,設(shè)車體幾何中心(X0,Y0)為前舵輪中點Of和后舵輪中點Ob的連線中點,在負(fù)載均勻的情況下,認(rèn)為幾何中心(X0,Y0)即為AGV車體質(zhì)心,其初始值為(X0c,Y0c)。θf、θb分別為AGV前后輪轉(zhuǎn)角角度，Vf、Vb為前后輪線速度。設(shè)O點為小車轉(zhuǎn)彎時瞬時運動軌跡圓心,則Rf、Rb為前后輪轉(zhuǎn)彎過程中的軌跡半徑。則其前后舵輪的位置坐標(biāo)可表示為

質(zhì)心坐標(biāo)C=(X0,Y0)可以表示為

(X0,Y0)即為AGV車體位置坐標(biāo)。

質(zhì)心在全局坐標(biāo)系下的線速度狀態(tài)量為

即線速度vc可表示為

其航向角α為

考慮剛體運動特性,車體繞O點運動時,前后舵輪中心點及車體質(zhì)心C運動的在地面平動的角速度一致。車體運動過程中,結(jié)合舵輪運動速度及航向角,根據(jù)正弦定理得其角速度為

2 基于模型預(yù)測控制的雙舵輪AGV糾偏控制

假設(shè)車輪與地面純滾動接觸,即AGV在平面運動時沒有滑動,可以給出系統(tǒng)運動學(xué)模型如下：

進一步,雙舵輪自動導(dǎo)引車運動學(xué)方程如式(11)所示:

由于自動導(dǎo)引車系統(tǒng)為非線性系統(tǒng),對于非線性系統(tǒng),求解最優(yōu)控制的問題最終被轉(zhuǎn)化為求解Hamilton-Jacobi-Bellman方程[16]。考慮非線性約束的存在,直接求解方程來獲得精確解析解難以實現(xiàn)。由于可以將最優(yōu)控制問題的求解轉(zhuǎn)化為實時數(shù)值解有約束優(yōu)化問題,大大降低了計算復(fù)雜度,提高預(yù)測控制算法計算速度,滿足控制器實時性要求,因而采用MPC處理約束系統(tǒng)最優(yōu)控制是一種可行的辦法。

2.1 雙舵輪AGV軌跡跟蹤誤差模型

為規(guī)劃AGV跟蹤路徑,設(shè)立運行參考軌跡,參考軌跡任意點均滿足狀態(tài)空間表達式,即

即

結(jié)合狀態(tài)空間表達式,可得:

將式(13)與式(12)做差,可得:

式(15)中：x′=x-xr表示車體實際運動位置與參考位置的狀態(tài)誤差；u′=u-ur則是控制輸入量的誤差。式(16)為連續(xù)模型,通過使用前向差分近似x′,得到以下AGV運動線性時變離散誤差模型:

式(17)中：線性時變矩陣分別為

式(18)中：T為采樣周期；k為采樣時間。

非線性不完整系統(tǒng)是完全可控的,它可以在有限時間內(nèi),通過使用有限輸入,從任何初始狀態(tài)轉(zhuǎn)向任何最終狀態(tài)。很容易看出,當(dāng)AGV靜止時,關(guān)于靜止操作點的線性化是不可控制的。但是,只要控制輸入u不為零,這種線性化就變得可控[17-19]。這意味著用線性MPC跟蹤參考軌跡是可行的。

2.2 軌跡跟蹤控制器模型

離散化誤差模型在系統(tǒng)k時刻增量式表達式為

Δu(k)=u(k)-u(k-1) (20)

則運動學(xué)模型的狀態(tài)空間表達式為

式(2)中：

(22)

這里假設(shè)有限預(yù)測時域為p,在時域[τ,τ+p]內(nèi)對系統(tǒng)狀態(tài)進行預(yù)測。控制時域為c,在時域[τ,τ+c]內(nèi)生成系統(tǒng)控制序列。所以預(yù)測時域內(nèi)系統(tǒng)的預(yù)測方程為

Y(k)=φpψ(k)+QcΔU(k) (23)

式(23)中：Y(k)為系統(tǒng)輸出量；φp為狀態(tài)量參數(shù)；Qc為控制量參數(shù)；ΔU(k)為控制增量序列,用公式表達為

在系統(tǒng)預(yù)測方程中,需要建立準(zhǔn)確的目標(biāo)函數(shù)求解控制增量ΔU(k),從而得到控制時域[τ,τ+c]內(nèi)合適的控制量序列,這里建立目標(biāo)函數(shù)為

目標(biāo)函數(shù)既保證了模型精準(zhǔn)跟蹤目標(biāo)軌跡,又保證控制量符合AGV實際性能限制。式(25)中：Q為預(yù)測時域權(quán)重因子；R為控制時域權(quán)重因子；ρ為權(quán)重系數(shù)；γ為松弛變量。在目標(biāo)函數(shù)引入松弛變量是為了便于在更大的可行域內(nèi)尋求解。對松弛變量的選取不宜過大,否則模型即使具有較好的實時性,卻不能保證較高的精度[20]。

AGV實際運行中,考慮AGV行駛可行域、舵輪轉(zhuǎn)角和行進速度等現(xiàn)實約束條件,軌跡跟蹤控制器模型可描述為

2.3 軌跡跟蹤控制器模型算法求解

為獲取系統(tǒng)最優(yōu)控制序列,將上述最優(yōu)化軌跡跟蹤問題轉(zhuǎn)化為帶約束線性二次規(guī)劃(quadratic programming, QP)問題進行求解。其形式如下[16]:

將預(yù)測方程式(23)代入目標(biāo)函數(shù)式(26),經(jīng)過整理可得:

式(28)中：G為控制增量系數(shù)矩陣；Ek為預(yù)測時域[τ,τ+p]內(nèi)軌跡跟蹤誤差。針對帶有線性約束的二次實函數(shù),采用具有通用性的內(nèi)點法求解,求得[τ,τ+c]內(nèi)控制輸入增量為

ΔUt=[Δut,Δut+1,…,Δut+c-1]T(29)

將該控制序列中第一個元素作為實際輸入的控制量增量,通過計算得到控制量作用于實際系統(tǒng),即

u(t)=u(t-1)+Δut(30)

當(dāng)系統(tǒng)進入下一采樣周期后,系統(tǒng)重新計算控制輸入增量,循環(huán)迭代實現(xiàn)雙舵輪AGV軌跡跟蹤控制。

3 仿真與結(jié)果分析

為驗證上述雙舵輪AGV模型預(yù)測控制器的有效性,基于MATLAB平臺,設(shè)計了軌跡跟蹤仿真實驗,具體流程如圖3所示。

圖3 雙舵輪AGV軌跡跟蹤系統(tǒng)仿真流程Fig.3 Simulation of double steering wheel AGV trajectory tracking system

在輪式自動導(dǎo)引車實際運行中,其控制量約束為

umin≤u≤umax(31)

控制增量約束為

Δumin≤Δu≤Δumax(32)

其中:

車輛初始位置:

仿真試驗中輪式機器人運動模型和MPC控制器的默認(rèn)參數(shù)如表1所示。

表1 仿真試驗?zāi)J(rèn)參數(shù)值Table 1 Simulation test default parameters value

由于輪式自動導(dǎo)引車在運動過程中,運動軌跡可分解為直線與圓弧兩種路徑。其中,車體行駛在弧線軌跡間穩(wěn)定跟蹤的能力是軌跡追蹤算法魯棒性的重要體現(xiàn)。為驗證雙舵輪AGV在模型預(yù)測控制算法下的軌跡跟蹤效果,分別選取圓形路徑和大曲率正弦曲線路徑作為目標(biāo)軌跡,進行跟蹤仿真實驗。首先分析系統(tǒng)控制量權(quán)重矩陣R以及采樣周期N對MPC控制器的追蹤效果的影響,以選取合理的模型參數(shù)。

3.1 模型參量對軌跡跟蹤影響分析

模型預(yù)測控制算法實時性和魯棒性受模型參數(shù)影響。實驗選取圓形路徑進行跟蹤仿真,研究控制時域權(quán)重和預(yù)測時域大小對模型軌跡跟蹤效果影響。

3.1.1 控制時域權(quán)重矩陣對系統(tǒng)影響分析

在目標(biāo)函數(shù)中,控制時域權(quán)重矩陣R影響系統(tǒng)控制量的平滑性,從而影響軌跡跟蹤的穩(wěn)定性。為目標(biāo)路徑設(shè)定輪式機器人線速度為0.4 m/s；角速度0.074 rad/s；預(yù)測時域p=5。令控制時域權(quán)重矩陣R=λI2×2,其中λ∈{0.05,0.1,1,5,10},分別進行對比實驗。圖4(a)顯示出不同控制時域權(quán)重作用下,控制器軌跡跟蹤效果均較為理想。其中,圖4(b)～圖4(f)為不同權(quán)重值時,質(zhì)心位置及航向角追蹤偏差。結(jié)果顯示,當(dāng)控制時域權(quán)重R=λI2×2較小時,軌跡跟蹤初始階段誤差較小,而運行中后期穩(wěn)態(tài)跟蹤誤差近似。控制時域權(quán)重矩陣對系統(tǒng)的影響主要體現(xiàn)在軌跡跟蹤初始階段,當(dāng)系統(tǒng)運行穩(wěn)定后影響較小。

圖4 不同控制時域權(quán)重矩陣軌跡跟蹤結(jié)果Fig.4 Tracking results of different control time domain weight matrix

3.1.2 預(yù)測時域?qū)ο到y(tǒng)影響分析

預(yù)測時域反應(yīng)系統(tǒng)對未來輸出的預(yù)測能力,預(yù)測時域長度顯著影響軌跡跟蹤效果。受限于硬件系統(tǒng)計算能力,預(yù)測時域較長時,模型求解時間過長,影響系統(tǒng)的快速動態(tài)性能。因此需要針對MPC軌跡跟蹤算法單獨優(yōu)化預(yù)測時域,通過系統(tǒng)仿真選出合適參數(shù),達到實時性與穩(wěn)定性的平衡。

實驗中輪式機器人速度為0.4 m/s；角速度為0.074 rad/s,預(yù)測時域N滿足N∈{5,6,7,8},結(jié)果如圖5所示。在合理范圍內(nèi)增大預(yù)測時域,輪式自動導(dǎo)引車取得了更好的穩(wěn)態(tài)跟蹤效果。其中,圖5(a)反映了舵輪的初始階段線速度輸入量變化情況,增大N會顯著改善穩(wěn)態(tài)誤差,但較長的預(yù)測時域增大了系統(tǒng)超調(diào)量。圖5(b)反映了舵輪航向角輸入量變化情況,當(dāng)采樣周期小于7時,機器人舵輪轉(zhuǎn)角的角速度滿足機器人性能限制,否則,舵角變化超過系統(tǒng)最大承受角速度0.2 rad/s。

圖5 不同控制時域權(quán)重矩陣軌跡跟蹤結(jié)果Fig.5 Tracking results of different control time domain weight matrix

3.2 MPC控制器與PID控制器對比實驗分析

為驗證基于模型預(yù)測控制的軌跡跟蹤算法,設(shè)計對比仿真實驗,比較PID控制器[14]和MPC控制器應(yīng)用于雙舵輪自動導(dǎo)引車進行軌跡跟蹤效果。通過前述實驗分析MPC模型參數(shù)變化對軌跡跟蹤效果的影響,取采樣周期N=7,控制時域權(quán)重矩陣R=0.1I2×2。

評估車體轉(zhuǎn)向能力是衡量軌跡跟蹤算法穩(wěn)定性的常用方法。為此,將參考路徑設(shè)置為直線與大曲率正弦曲線結(jié)合的S形彎道。其中,正弦曲線軌跡滿足:

對比仿真實驗結(jié)果如圖6所示。

圖6 PID控制器與MPC控制器跟蹤效果對比Fig.6 Comparison between PID controller and MPC controller tracking effect performance

由圖6(a)、圖6(b)可知,針對曲率較大的圓弧,基于PID控制器的導(dǎo)引車跟蹤軌跡會發(fā)生較大幅度超調(diào),MPC控制器則表現(xiàn)出較為優(yōu)良的控制特性。當(dāng)運行時間處于33.7 s時,車體第一次到達曲率最大值處。此時,已有穩(wěn)定運行趨勢的PID控制器軌跡再次出現(xiàn)單向19 cm的較大超調(diào)量,而MPC控制器軌跡誤差自18 s后,始終維持在[-1,1.1]范圍內(nèi)。

由圖6(c)、圖6(d)可知,當(dāng)車體運動一段時間后,會迅速穩(wěn)定在一定范圍內(nèi)達到跟蹤指標(biāo)要求。使用PID控制器的雙舵輪AGV達到穩(wěn)態(tài)時間較長,且穩(wěn)態(tài)誤差較大。

通過上述實驗結(jié)果可知,在常規(guī)圓形路徑以及曲率較大的正弦路徑中,基于雙舵輪AGV的MPC軌跡跟蹤控制器均展現(xiàn)了良好的快速性與穩(wěn)定性。模型預(yù)測控制器在選取合理參數(shù)后,其軌跡跟蹤性能要優(yōu)于常規(guī)PID算法。

4 結(jié)論

以縱向軸線安裝的雙舵輪自動導(dǎo)引車為研究對象，為研究雙舵輪AGV轉(zhuǎn)向性能,建立了自動導(dǎo)引車轉(zhuǎn)向運動模型。同時,針對該雙舵輪自動導(dǎo)引車,設(shè)計了一種基于模型預(yù)測控制的軌跡跟蹤算法,使自動導(dǎo)引車快速穩(wěn)定完成軌跡追蹤任務(wù)。最后,通過MATLAB平臺仿真實驗驗證了模型預(yù)測控制軌跡跟蹤技術(shù)應(yīng)用于雙舵輪自動導(dǎo)引車的可行性和有效性。通過研究不同模型參數(shù)條件下控制器的實際跟蹤效果,優(yōu)化得到較為滿意的控制器參數(shù)。同時,通過仿真實驗將基于比例-積分-微分控制器的軌跡跟蹤路線與參數(shù)優(yōu)化后的模型預(yù)測控制器軌跡跟蹤路線進行對比,驗證了模型預(yù)測控制算法具有更高的穩(wěn)定性。本文的AGV控制器只考慮了均勻負(fù)載情況,下一步將考慮載荷和慣性的影響。