視覺導引AGV軌跡跟蹤控制及仿真驗證

張如灝，崔 岸，張 睿，劉立源

（吉林大學汽車仿真與控制國家重點實驗室，吉林 長春 130025）

1 引言

AGV（Automated Guided Vehicle-自動導引小車）是一種裝有獨立尋址系統和非接觸導向裝置的輪式移動機器人，在工廠、物流、軍事、航空等領域得到了越來越多的應用［1］。它包含導引、控制、驅動、供電等系統，其中導引和控制系統是AGV的熱點研究。

在導引領域，國內外學者對導引方式進行了研究。發現磁性導引定位準確性較低；激光導引裝置較為復雜，成本較高；視覺導引具有導引精度高、性價比好等特點，具有良好的發展前景［2］。文獻［3］采取攝像機前傾安裝方式進行多分支路徑識別研究，該方式適用于單方向前進，而雙向型AGV 具有更好的柔性，可提高AGV路徑規劃的調度效率。文獻［4］提出一種基于曲率角的分類方法，對直線、圓弧轉彎和非圓弧轉彎路徑模型，實現導引路徑的實時測量。文獻［5］通過綜合平均斜率差及拐點分辨指數對非圓弧路徑進行分段擬合，提高了AGV的測量精度。

在控制領域，國內外學者主要集中在最優控制、PID控制、模糊控制、神經網絡控制等方面的研究。目前大多數AGV采用PID控制策略，PID控制方法具有算法簡單、魯棒性較好的特點，但在一些復雜場景中，PID 控制器參數的全局整定較為困難［6］。文獻［7］基于線性二次型的最優控制器，把控制對象簡化為線性系統，但對控制模型精準程度要求較高。文獻［8］采用T-S模糊神經網絡模型，設計了一種模糊神經網絡控制器，進行AGV軌跡跟蹤控制。

針對雙向雙驅動的視覺尋跡AGV，根據其后兩驅動輪差速的特性，建立運動系統模型，基于直線、圓弧、非圓弧三種路徑模型，采用最小均方差法和內切圓弧修正法分別獲得AGV在直線、圓弧和非圓弧三種路徑運行過程中的距離偏差和角度偏差，最后設計模糊PID控制器，進行軌跡跟蹤控制及仿真驗證，評價軌跡跟蹤控制系統的穩定性和有效性。

2 AGV運動系統模型

AGV采用后輪差速驅動的四輪結構，前輪為自由輪，后輪為差速驅動輪。對AGV運動模型做如下假設：車體為剛性，全向輪做純滾動，忽略車輪寬度與空氣阻力，如圖1所示。參考行駛方向設定為X軸方向，O點為驅動軸軸心，L為軸距。行駛時，驅動軸軸心與X軸距離偏差為d，單位為m，與X軸角度偏差為α，α即參考軌跡與當前行駛方向的夾角。設逆時針偏轉角度為正，單位為rad。VL、VR、VO分別代表左輪速度、右輪速度和小車的行駛速度。單位為m∕s。

圖1 AGV運動模型Fig.1 AGV Motion Model

小車運動半徑，如式（1）所示，單位為m。

在t時刻，小車的角速度ω，如式（2）所示，單位為rad∕s。

在t時刻，瞬時速度為：

瞬時驅動軸軸心O點在水平方向和豎直方向的兩個速度分量：

對式（2）和式（4）積分得：

式中：α0—在初始時刻小車行駛方向與參考軌跡的角度偏差；x0和y0—初始時刻驅動軸軸心O的坐標；x，y—任意時刻的坐標。初始時刻α0、x0、y0的數值都是0。其中x0，y0，x，y單位均為m。

根據設第i個采樣時刻，驅動輪軸心Oi的位置坐標為（xi，yi）（其中i=0，1，2，…，m，…，n）：

運動一段時間Δt之后，會出現距離偏差Δd和角度偏差Δα，此時小車運動的偏差公式為：

將式（7）求導得：

兩個驅動輪的速度差ΔV=VL-VR，驅動軸軸心O點的速度VO=（VL+VR）∕2，根據式（7）和式（8）得到小車的狀態方程為：

式（9）表示兩個驅動輪的速度差ΔV與距離偏差d和角度偏差α之間的關系。該式將ΔV作為輸入變量，將d和α作為狀態變量，即通過驅動輪的速度差控制AGV位置偏差。

以兩驅動輪的電壓信號作為輸入量，以速度信號作為狀態量，以角度偏差α和距離偏差d作為輸出量，建立AGV控制系統框圖，如圖2所示。并由此得到整個系統的狀態方程，如式（10）所示。

圖2 AGV控制系統結構框圖Fig.2 Block Diagram of AGV Control System

設x=［ΔV α d］T為系統的狀態變量，ΔU為兩個差速輪的驅動電壓差，單位為V，b為電機的動態特性參數，得到整個系統的狀態方程，如式（11）所示。

3 路徑模型及偏差計算

路徑模型有三種基本形式：直線、圓弧、非圓弧路徑［4］，系統需要實時計算小車運動狀態的距離偏差和角度偏差，并將偏差值傳輸給運動控制系統，以實現有效的軌跡跟蹤控制。

3.1 直線路徑模型及偏差計算

直線路徑模型，如圖3 所示。路徑擬合中心線是一條直線（圖3中BD線段），其直線方程為：y=ɑx+b。

圖3 直線路徑模型Fig.3 Linear Path Model

假設誤差為ei，考慮誤差的直線方程為：

誤差ei的最小均方差為：

路徑擬合中心線的標準差是：

小車偏離導引路徑中心線的距離偏差d：

式中：A—圖像距離與真實距離的比例系數。

小車前進方向與導引路徑中心線角度偏差α：

其中，式（17）中ψ是小車導引路徑方向與圖象坐標系X軸的夾角，單位為rad。

3.2 圓弧路徑模型及偏差計算

當AGV在運行過程中轉彎路徑為圓弧時，設轉彎半徑為R，圓弧的中心點為圓心O1（xO，yO），圓弧路徑模型，如圖4所示。

圖4 圓弧路徑模型Fig.4 Arc Path Model

小車驅動軸軸心O和圓弧中心點O1的連線與圓弧路徑的交點為F，OF即當前時刻AGV的距離偏差d。設圓弧路徑模型對應的圓的方程為：

令z=-（x2+y2），假設誤差是ei，考慮誤差的圓弧方程為：

由最小均方誤差法得到最小均方差是：

由于樣本基數比較大，該條件容易滿足。

圓弧路徑模型對應圓的圓心坐標為：

轉彎半徑為：

擬合路徑中心點得到的路徑中心線的標準差是：

小車當前位置驅動軸軸心O與導引圓弧路徑的距離偏差d，小車行駛方向與過F點圓弧切線的夾角為角度偏差α，表達式，如式（25）所示。

3.3 非圓弧路徑模型及偏差計算

當提取的路徑中心線近似折線時，可以進行非圓弧轉彎軌跡跟蹤。設置非圓弧路徑模型，如圖5所示。轉彎角為γ，采用內切圓弧修正法［10］，對角度偏差進行補償。

圖5 非圓弧路徑模型Fig.5 Non-Circular Arc Path Model

設內切圓方程為（x-x0）2+（y-y0）2=r2，與折線路徑分別相切于F（x1，y1），G（x2，y2）兩點，EF、EG直線方程分別為：y=k1x+b1，y=k2x+b2拐點為E（xe，ye），臨界曲率為=1∕r，得到方程組求解參數為：

且方程滿足如下條件：

當非圓弧路徑轉化為內切圓路徑模型后，求取d和α的數學模型與圓弧路徑模型相同，其擬合偏差，距離偏差和角度偏差與式（24）和式（25）相同。

4 軌跡跟蹤控制

AGV在運行過程中，會受到各種信息的干擾，傳統PID控制器較難建立精準的數學模型，無法保證AGV軌跡跟蹤精確性，而模糊PID控制不需要建立精確的數學模型，因此設計模糊PID控制器，進行AGV軌跡跟蹤控制。

4.1 模糊PID控制器設計

將距離偏差和角度偏差整合在一起，得到誤差E。

式中：p—權重，p取0.5。

模糊PID控制器將誤差E和誤差變化率Ec作為輸入量，輸出量為三個參數增量ΔKp、ΔKI和ΔKD，分別和Kp、KI、KD相加之后的結果輸入增量型PID控制器，輸出左右差速驅動輪電機的電壓差ΔU，控制AGV系統的運動，模糊PID結構示意圖，如圖6所示。

圖6 模糊PID控制結構圖Fig.6 Fuzzy PID Control Structure Diagram

4.2 隸屬度函數的確定

采用正態分布隸屬度函數，克服運行過程中距離偏差和角度偏差的隨機性較強的問題。輸入量和輸出量的論域取在［-6，+6］，隸屬度函數分布，如圖7所示。

圖7 隸屬度函數分布圖Fig.7 Distribution Diagram of Membership Function

4.3 模糊規則的確定

根據輸入變量的7個模糊子集：負大，負中，負小，零，正小，正中，正大，記為｛NB，NM，NS，ZO，PS，PM，PB｝，模糊規則語言描述形式為IfAandBThenC，D，E。從系統的響應速度、穩態誤差和系統的穩定性等多方面因素考慮，對控制參數ΔKp、ΔKI、ΔKD進行調節考慮各參數之間的相互作用和影響，共制定49條模糊控制規則，如表1所示。采用重心法（式（29））進行去模糊化，從而得到一個明確的輸出值。

表1 ΔKp、ΔKI、ΔKD模糊控制規則Tab.1 ΔKp、ΔKI、ΔKD Fuzzy Control Rules

式中：U—面積中心對應的橫坐標；xi—對應離散變量的橫坐標；μN（xi）—xi處的隸屬度。

4.4 模糊PID驗證

在Simulink中將模糊PID控制器與PID控制器進行仿真實驗對比，如圖8、圖9所示。

圖8 PID控制器仿真模型Fig.8 PID Controller Simulation Model

圖9 模糊PID控制器仿真模型Fig.9 Fuzzy PID Controller Simulation Model

仿真結果，如圖10所示。模糊PID控制器在3s左右時便可以將角度偏差α穩定在0，而PID控制器的振蕩現象非常明顯，且在3s左右時，其角度偏差α在0.15rad左右，直到5.5s左右，角度偏差α才穩定在0；模糊PID控制器在4s左右時便可以將距離偏差d穩定在0，而PID控制器的振蕩現象非常明顯，且在4s左右時，其距離偏差d在0.02m左右，直到6s左右，距離偏差d才穩定在0。

圖10 PID控制器和模糊PID控制器仿真對比Fig.10 Simulation Comparison Between PID Controller and Fuzzy PID Controller

可以看出，在偏差相同且較大的初始狀態下，所設計的模糊PID控制器能在較短的時間內穩定在偏差為0的狀態，且在接近小偏差時，能通過智能調節參數，避免超調和振蕩，穩定AGV的運動軌跡，因此證明設計的模糊PID控制器具有穩定性和有效性。

5 仿真驗證

將AGV模型、路徑模型導入到仿真軟件PreScan中，添加模糊PID控制器，驗證模糊PID控制器的軌跡跟蹤控制性能。

5.1 直線路徑回正實驗

給予小車距離偏差d為20cm，角度偏差α為0.4rad，通過控制差速驅動輪的電壓差來矯正角度偏差α。小車在恒速0.5m∕s的運動情況下，大約在5s時可以穩定地跟蹤到直線模型且沒有任何超調。仿真結果，如圖11所示。

圖11 直線路徑回正仿真實驗結果Fig.11 Simulation Results of Straight-Line Path Alignment

5.2 直線路徑回歸實驗

為了進一步測試模糊PID控制算法的回歸能力，設定小車初始位置在偏離路徑線較遠的位置，距離偏差d為1m，角度偏差α為0rad。仿真結果，如圖12所示。在回歸的過程中，角度偏差有一個先上升后下降的過程，即通過較大的轉角來更快的回歸到參考軌跡。因此小車在恒速0.5m∕s的運動情況下，大約在（7～8）s時可以穩定地跟蹤到直線模型且沒有任何超調。

圖12 直線路徑回歸仿真實驗結果Fig.12 Results of Linear Path Regression Simulation Experiment

5.3 直線+圓弧路徑實驗

直線+圓弧路徑中，在轉彎圓弧部分是整體道路中最為困難的部分，給予距離偏差d為20cm，角度偏差α為0.4rad，仿真結果，如圖13所示。小車行駛2m左右即可跟蹤參考路徑，而在后面的轉彎和直線部分，小車可以穩定跟蹤參考路徑且偏差很小。在1s左右距離偏差d和角度偏差α達到最大值，隨后在5s左右，偏差基本為0，至實驗結束偏差亦無較大波動。因此小車在恒速0.5m∕s的運動情況下，大約在5s時可以穩定地跟蹤到參考軌跡且沒有任何超調。

圖13 直線+圓弧路徑仿真實驗結果Fig.13 Simulation Experimental Results of Linear+Circular Path

5.4 非圓弧路徑實驗

為了進一步驗證模糊PID控制器的路徑跟蹤性能，增加了非圓弧路徑的實驗。在非圓弧的S型路徑中，給予距離偏差d為20cm，角度偏差α為0.4rad，小車恒速0.5m∕s，仿真結果，如圖14所示。初始大偏差下，小車迅速控制電機電壓差達到轉彎效果，小車行駛2m左右可以回正狀態，隨后的S型軌跡跟蹤中，仍然可以保持不錯的跟蹤效果。大約在4s時，距離偏差d為0，大約在6s時，角度偏差α為0。相比于5.3節的實驗，S型路徑更為復雜，故角度偏差α的修正需時更久，但后續的跟蹤偏差一直趨于穩定。

圖14 非圓弧路徑仿真實驗結果Fig.14 Simulation Experimental Results of Non-Circular Path

6 結論

（1）建立AGV運動系統模型，選取導引路徑的三種基本形式：直線、圓弧、非圓弧，采用最小均方差法分別獲得直線和圓弧路徑模型下AGV運行過程的距離偏差和角度偏差，采用內切圓弧修正法獲得非圓弧路徑模型下AGV運行過程的距離偏差和角度偏差。

（2）設計一種模糊PID 控制器，并與常規PID 控制器進行了對比，結果表明：該模糊PID控制器在進行角度偏差回正的時間提升了45%，進行距離偏差回正的時間提升了33%。證明該模糊PID控制器能夠智能調節參數，快速回正角度偏差和距離偏差。

（3）在PreScan軟件中應用模糊PID控制器進行四組仿真實驗，分別是直線路徑回正實驗、直線路徑回歸實驗、直線+圓弧路徑實驗和非圓弧路徑實驗，仿真結果表明，AGV能夠進行穩定和有效的軌跡跟蹤。