基于改進模糊PID的叉車式AGV路徑跟蹤控制

李 航，廖映華，黃 波

（四川輕化工大學機械工程學院，四川 宜賓 644000）

引 言

隨著物流行業、智能工廠、柔性制造車間的智能設備的快速發展[1-2]，近年來，叉車式自動導引車（Auto Guided Vehicle，AGV）在生產車間發揮著重要的作用，尤其面對改造的倉儲空間，能夠靈活運作，減少運行成本，提高了企業的運行效率[3]。路徑跟蹤是AGV 運動控制的關鍵技術，通過控制運動速度和運動方向實現偏差的減小[4-5]。目前國內外在路徑跟蹤問題上，有較多學者對其開展了研究。文獻[6]提出了一種利用模糊控制器調節進行路徑跟蹤的方法，為實現更好的跟蹤效果采用模型預測控制器進行跟蹤，并結合車身的特點進行約束，實現基于PSO（Particle Swarm Optimization，粒子群）優化的模型預測控制器在不同工況情況下進行路徑跟蹤。文獻[7]針對如何快速消除距離和角度誤差的問題，提出了基于改進等速趨近律的滑模控制跟蹤算法，利用反演法處理非線性系統的控制策略得到滑模切換函數，采用連續函數取代符號函數得到控制律式，實現差速AGV 穩定的路徑跟蹤效果。文獻[8]針對AGV 停靠位置精度不高的問題，利用Data matrix二維碼中的位置信息進行導航定位，為適應不同路況條件，采用二步糾偏法在跟蹤過程中進行位置糾偏，最終AGV 路徑跟蹤的實現過程是先采用二步糾偏算法進行位置糾偏，再采用積分分離型比例積分微分（Proportion Integration Differentiation，PID）實現了角度糾偏。文獻[9]根據紅外傳感器獲取AGV 車身距離引導線的偏差距離，利用動態偏差實現前視距離的判斷，提出了基于改進型純跟蹤算法，解決不同初始狀態的路徑跟蹤問題。文獻[10]提出了基于改進免疫算法尋優的P比例模糊控制結構對AGV跟蹤路徑進行偏差修正，實現跟蹤誤差的減小。文獻[11]提出了針對具有動態平衡約束的運動模型，采用基于模型預測控制跟蹤算法實現路徑跟蹤，解決復雜動力學的路徑跟蹤問題。文獻[12-13]將叉車式AGV 運動學模型轉化為位姿誤差量方程。文獻[14]提出了利用Lyapunov 直接法構造漸進穩定函數，設計路徑跟蹤控制律來實現路徑跟蹤。

上述文獻中，控制方法在路徑跟蹤控制過程中有較好的跟蹤效果，但是針對改造空間中出現的特殊跟蹤路徑以及載荷和速度等因素的影響，路徑跟蹤目前沒有考慮到對跟蹤的穩定性、安全性和精度的影響。基于此，本文根據單舵輪AGV 的結構特點，針對不同載荷對彎道路徑的跟蹤采用改進型模糊PID 組合控制算法進行控制，該算法能解決彎道路徑跟蹤的不穩定和精度較低的問題，同時利用實驗樣機進行負載運行，驗證該方法在彎道路徑跟蹤的有效性。

1 運動學模型分析

1.1 叉車式AGV運動學模型

本文研究對象為叉車式AGV，主要由1 個單舵輪、2個從動輪、2個萬向輪構成運動系統，其中單舵輪負責驅動和轉向，萬向輪起支撐作用從而提高運動中的穩定性[15]。叉車式AGV 的簡化模型如圖1所示。X-B-Y為全局坐標系，x-o-y是以叉車自身運動點為原點的局部坐標系。圖1 中，L1為單舵輪與定向輪軸中心點的直線距離，L2為定向輪軸中心距離，Lr為轉向半徑，δ為單舵輪的轉向角度，α為叉車的轉彎角度，θ為航向偏角。

圖1 叉車式AGV運動學示意圖

叉車式AGV 的運動點為O點，在全局坐標點的位姿坐標為( )x0,y0,θ0，單舵輪的實際驅動速度為v，由此可得到如下關系式：

其中：ωr為期望角速度，vr為期望速度，v為實際驅動速度，ω為實際角速度，xe為縱向誤差，ye為橫向誤差，θe為航向角誤差。

叉車式AGV實現路徑跟蹤通過改進型模糊PID組合控制算法，減小實際坐標點與期望坐標點位置偏差，并且逐漸趨于穩定；曲線跟蹤受到不同載荷的影響和離心力的作用，需要通過叉車式AGV 的運動控制，保證曲線路徑跟蹤的精度和安全。

1.2 運動學分析

叉車式AGV 在工程環境中運行，載荷是影響運動控制精度的重要因素[16]。傳統路徑跟蹤方法采用固定速度進行轉彎路徑曲線的跟蹤，但這種方法應對復雜環境時，降低了AGV 的運行效率和安全。由于載荷的影響，若載荷大則運動慣性大，車身受到離心力的作用大，貨物與車身之間的靜摩擦力大小決定車身的安全性，同時不穩定性也影響跟蹤的精度。

圖2（a）及圖2（b）所示分別為叉車式AGV 的運動力學及轉彎力學分析示意圖。其中，F為驅動力，F22為后輪受到的反壓力，F21為前輪受到的反壓力，G1和G2分別為載荷質量和車身重量，f1和f2分別為定向輪和驅動輪的摩擦力，f為載荷與叉車式AGV之間的摩擦力，R為單舵輪的轉彎半徑。

圖2 叉車式AGV力學分析示意圖

根據運動力學分析可得載荷與運動速度之間的關系，運動控制過程中根據載荷大小可得速度的最大值。其中，單舵輪的材料為聚氨酯；工程環境地面采用混凝土地面，摩擦系數μ的范圍為0.6～0.63，負載情況下的靜摩擦力大小為：

叉車式AGV 在曲線運動中，載荷質量m1與車身質量m2受到離心力為：

將式（4）與式（3）結合可得叉車式AGV 安全運行的最大速度為：

2 彎道路徑分析

叉車式AGV 的期望跟蹤曲線路徑主要針對直角轉彎路徑的情況，所以將進彎點和出彎點設置為曲線路徑的固定點。Bezier 曲線是不規則曲線[17]，需要在起點和終點之間構建插值多項式的混合函數，通常由個定點定義一個n次多項式，在給定空間個點的位置矢量，則Bezier 參數曲線上各點坐標的插值公式如下：

其中：Pi為構成Bezier 曲線的特征多邊形的位置矢量，Bi,n(t)為n次Bezier 的基函數。Bi,n(t)由式（7）獲得：

Bezier 曲線中已知兩固定點，同時得到第三點即可確定曲線，曲線生成基本原理如圖3所示。

圖3 Bezier曲線原理圖

本文采用t= 1/3 處的點為例進行說明[18]，其中圖3 中，點M1為虛線段PM靠近M點的1/3處，點N1為虛線段PN靠近P點的1/3處，將點M1和點N1連接形成虛線段M1N1,其中點P1 為虛線段M1N1靠近點M1的1/3 處，此時P1 為t=1/3 時構成Bezier 曲線上的點，隨著變量t的變化即可得到Bezier曲線。

通過調整P點的位置變化得到不同的Bezier 曲線路徑，為保證叉車式AGV 運動安全距離，P點選擇較為關鍵。在全局坐標系X-B-Y中，M(Mx,My)和坐標已知，令，可得：

線段MN與橫坐標X軸之間的夾角αMN為：

可得P點坐標為：

面對復雜的工程環境情況時，對于P點位置的選擇需要考慮周圍環境存在不可改變的障礙物的情形。圖4 所示為障礙物對應4 種不同位置情況下的曲線路徑。本文的研究背景是在已知工況環境中，進行轉彎曲線路徑的規劃設計。根據曲線路徑可知，障礙物位置決定曲線路徑變化，當障礙物存在于曲線的凹側時，曲線路徑不變，此刻障礙物對跟蹤路徑影響最小。

圖4 曲線路徑

3 路徑跟蹤控制器設計

3.1 模糊PID控制器

PID 控制系統將橫向誤差作為驅動控制變量的輸入，不依賴于精確的數學模型，而且設計簡單，魯棒性高[19-20]。PID 控制系統的控制輸出隨著橫向誤差e(t)的變化進行調節，通過調整比例、積分、微分參數獲得運動控制量u(t)，實現對執行機構的控制，其中基于PID控制的運動控制量為：

模糊控制是通過建立模糊控制器的結構，選擇輸入變量，定義輸入輸出模糊集，再定義隸屬度函數以及建立模糊規則。如圖5 所示，模糊PID 控制主要是根據輸入誤差變量以及誤差的變化來調整比例參數Kp、積分參數Ki、微分參數Kd，將調整后的Kp、Ki、Kd，經過PID 控制器輸出控制量，實現執行機構的跟蹤控制。

圖5 模糊PID控制

3.2 改進型模糊PID組合控制器

模糊PID 組合控制器是根據切換函數來實現對控制參數的選擇[21]。改進型模糊PID 組合控制主要是根據不同的誤差大小采用組合式的控制算法，當橫向誤差較大時采用模糊控制確保快速性和抑制超調控制，當橫向誤差較小時采用PID 控制可以消除穩態誤差。兩種控制方式最終控制輸出為：

式中：upid為PID 控制輸出，ufuzzy為模糊控制輸出，β為輸出強度控制系數，最終控制輸出u采用加權平均計算的方法。

模糊切換的隸屬函數如圖6 所示。圖6 中，橫坐標e為控制誤差，m為最大控制輸出最小誤差值，n為最小控制輸出的最大誤差值，調整圖中m、n的值即可調整兩種控制器的強度分量。

圖6 模糊切換隸屬度函數

本文是以橫向誤差為控制輸入，通過改進型PID 模糊復合控制器控制輸出而實現對叉車式AGV路徑跟蹤控制。由于考慮到量化因子及比例因子對系統的影響以及相互制約的關系，控制決策的制定依據變論域的思想，當控制誤差e較小時，系統則會接近穩態，若適當減小Ke，可擴大論域，減弱控制作用，避免發生超調；當控制誤差較大時，可以適當增加Ke，進而縮小論域，增強控制作用，消除誤差。

根據控制策略，并結合優化改進算法，從而制定了改進型模糊PID 復合控制器，實現兩者的結合，利用位移誤差大小，通過模糊切換實現控制器的輸出，控制結果如圖7所示。

圖7 復合控制器結構圖

利用實驗測試以及PID參數調節規律，制定表1所示的模糊規則表。其中，cey為橫向誤差變化率、u為最終控制輸出，隸屬度函數采用全交疊的三角形隸屬函數；在論域上將其分為均勻相等的5 個模糊子集：負大（NB）、負小（NS）、零（Z）、正大（PB）、正小（PS）。

表1 模糊規則表

4 跟蹤路徑仿真實驗對比分析

4.1 仿真驗證

本文利用MATLAB 軟件平臺對基于改進型模糊PID 組合控制算法進行曲線路徑跟蹤仿真。初始位置即為AGV 的進彎點M坐標為（1.0,1.0），出彎點N坐標為（3.0,3.0），彎道路徑中P1 坐標為（1.5,3.5）、P2 坐標為（1.0,2.5）；不同載荷情況下運動速度參數設置范圍不同，參數設置見表2。

表2 載荷分布速度范圍

叉車式AGV 在不同載荷情況下實現曲線路徑追蹤，利用改進型模糊PID 組合算法，實現初始位置橫向誤差較大的情況下可以快速趨近誤差允許范圍，在初始位置橫向誤差較小的情況下，則趨近于穩態逐漸減小誤差。在仿真實驗中主要對改進型的模糊PID 組合控制在不同載荷情況下進行對比分析，主要是針對P1 和P2 點分別進行路徑曲線的規劃并進行對比實驗分析，得到如圖8～11 所示效果圖。

根據仿真分析可知，在P1點和P2點處的位置，構建出彎道的期望路徑，并且在大、中、小載荷條件下，可得到不同載荷下的曲線路徑跟蹤精度對比。圖8 和圖10 表示不同位置點的路徑跟蹤仿真效果，大載荷下叉車式AGV 的趨近速度更快，姿態的變化較大，而小載荷情況下趨近速度較慢，姿態變化較小，跟蹤穩定性更高。圖9 和圖11 表示不同位置點路徑跟蹤誤差變化曲線，彎道路徑跟蹤誤差變化曲線在大載荷負重跟蹤過程中，開始階段誤差減小快，但穩定性不高，小載荷下，跟蹤開始階段的誤差變化小，姿態穩定性高，后期跟蹤誤差的變化小，波動幅度小。

圖8 P1點路徑跟蹤效果對比圖

圖9 P1點路徑跟蹤偏差變化曲線

圖10 P2點路徑跟蹤效果對比圖

圖11 P2點路徑跟蹤偏差變化曲線

4.2 實驗驗證

本文為驗證改進型模糊PID 組合控制器在彎道路徑跟蹤過程中的有效性，將仿真實驗算法和實驗平臺相結合進行驗證，搭建的平臺主要以卓一叉車為本體，倍福CX5130 為主控制器，定位導航采用SICK的NAV530激光雷達，實驗平臺如圖12所示。

圖12 叉車式AGV示意圖

本次實驗平臺的主要參數為：叉爪長度L為定向輪中心軸與單舵輪的中心軸之間的距離，其長度為1250 mm，單舵輪的輪寬為230 mm，叉車最大有效載荷為1500 kg；最大運行速度為2.70 m/s。根據工程環境和實驗平臺的特點，利用叉車式AGV 在大小載荷負載的條件下進行曲線路徑的跟蹤實驗得到如圖13和圖14所示的效果圖。

圖13中，實驗中采用的載荷分為大載荷和小載荷，其中大載荷條件下，其質量為1200 kg，PID 參數設置為：Kp=40，Ki=4，Kd=2.4；小載荷條件下，其質量為300 kg；PID參數設置為Kp=35，Ki=3.5，Kd=2。運動速度通過倍福控制器的HMI 監控界面觀測，得到大載荷負載條件下的速度在0.63～0.88 m/s范圍內變化；小載荷條件下的速度在1.54～1.70 m/s 范圍內變化，由表2 可知，叉車式AGV 在大、小載荷條件下，實際運動速度均在運動速度范圍內，所以能實現穩定的路徑跟蹤。

圖13 曲線路徑跟蹤實驗圖

圖14中，大載荷最大變化范圍比小載荷最大變化范圍大4.32 cm，其中大載荷趨近于穩定后的最大誤差為1.35 cm，小載荷趨近于穩定后的最大誤差為0.88 cm，所以小載荷的位姿調整范圍大；彎道的跟蹤精度變化則表現為開始階段的穩定性不高，跟蹤后期的誤差變化較小，穩定性高。

圖14 路徑跟蹤誤差分析

5 結束語

本文結合工程實際環境，針對彎道路徑上結構化障礙物，設計基于Bezeir曲線的轉彎期望路徑，根據不同載荷設置實驗速度范圍，利用改進型模糊PID 組合控制算法，實現彎道路徑跟蹤；大載荷下在彎道跟蹤的前階段誤差變化大，姿態變化大且不穩定，誤差降低速度較快；小載荷下，在彎道路徑跟蹤的開始階段誤差變化小，姿態變化平穩，誤差降低速度較慢。