雙舵輪自動導引車軌跡追蹤控制算法

齊嘉暉， 吳耀華， 汪 威

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

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

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

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

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

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

1 AGV轉彎運動學模型

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

1.1 AGV轉彎運動模型分析

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

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

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

雙舵輪AGV的運動系統由4個萬向輪和兩個驅動舵輪組成,前后兩個舵輪分別安裝在車體前進方向軸線上。AGV的轉彎和直線運動主要通過舵輪控制實現,可以按照預定的路線行進。

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

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

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

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

即驅動輪行進速度Vf恒定的前提下,車體轉角2θf與其行進時間t1成正比。根據轉彎軌跡三角關系,可知:

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

1.2 雙舵輪AGV定位算法

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

質心坐標C=(X0,Y0)可以表示為

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

質心在全局坐標系下的線速度狀態量為

即線速度vc可表示為

其航向角α為

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

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

假設車輪與地面純滾動接觸,即AGV在平面運動時沒有滑動,可以給出系統運動學模型如下：

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

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

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

為規劃AGV跟蹤路徑,設立運行參考軌跡,參考軌跡任意點均滿足狀態空間表達式,即

即

結合狀態空間表達式,可得:

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

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

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

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

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

2.2 軌跡跟蹤控制器模型

離散化誤差模型在系統k時刻增量式表達式為

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

則運動學模型的狀態空間表達式為

式(2)中：

(22)

這里假設有限預測時域為p,在時域[τ,τ+p]內對系統狀態進行預測。控制時域為c,在時域[τ,τ+c]內生成系統控制序列。所以預測時域內系統的預測方程為

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

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

在系統預測方程中,需要建立準確的目標函數求解控制增量ΔU(k),從而得到控制時域[τ,τ+c]內合適的控制量序列,這里建立目標函數為

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

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

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

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

將預測方程式(23)代入目標函數式(26),經過整理可得:

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

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

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

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

當系統進入下一采樣周期后,系統重新計算控制輸入增量,循環迭代實現雙舵輪AGV軌跡跟蹤控制。

3 仿真與結果分析

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

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

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

umin≤u≤umax(31)

控制增量約束為

Δumin≤Δu≤Δumax(32)

其中:

車輛初始位置:

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

表1 仿真試驗默認參數值Table 1 Simulation test default parameters value

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

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

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

3.1.1 控制時域權重矩陣對系統影響分析

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

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

3.1.2 預測時域對系統影響分析

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

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

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

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

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

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

對比仿真實驗結果如圖6所示。

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

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

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

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

4 結論

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