基于MPC的無人駕駛拖拉機軌跡跟蹤控制

嚴國軍,賁能軍 , 顧建華 , 楊 彥

(1. 鹽城工業職業技術學院 江蘇 鹽城 224000; 2. 東南大學,江蘇 南京 211189；3. 南京理工大學，江蘇， 南京 210094)

0 引 言

農用車輛自動化作業能夠使農業勞動者從沉重的體力勞動中解放出來，其中農用車輛自動化行走是實現自動化作業的基礎。近階段針對農用車輛自動行走的研究逐漸成為各國研究重點[1-2]。在早期研究中，一些學者在園藝設施中嘗試將一些引導線埋藏在地下，以引導車輛沿著固定的軌道進行作業，但這種方法在實施過程中具有很大的局限性。隨著科技不斷進步，攝像頭和衛星定位系統得到廣泛應用，促使農用車輛在自由軌跡上行駛的研究越來越多[2-6]。總的來看，農業機械自動駕駛的研究方向可以分為車輛行駛軌跡規劃和車輛軌跡跟蹤控制兩個方面。

車輛軌跡跟蹤控制的實質是車輛能夠根據給定的行駛路徑完成自主轉向控制，并使跟蹤誤差最小，即使在行駛過程中遇到一些干擾，也能夠保證穩定行駛。軌跡跟蹤控制是自動駕駛車輛體系中最基礎的一環，它的性能好壞直接影響到整個自動駕駛車輛是否能夠完成給定的駕駛任務[7]。車輛在給定行駛軌跡時，需能穩定且迅速地跟蹤給定行駛軌跡，這是對自動駕駛車輛最基本的要求。軌跡跟蹤的重點就是設計出軌跡跟蹤控制器，使智能駕駛車輛能夠跟隨既定行駛軌跡駕駛，行駛路徑可以通過車載ECU采集各種探測器信號并經ECU計算得出。筆者針對無人駕駛拖拉機在給定軌跡情況下的循跡效果開展研究。

1 車輛運動學模型建立

動力學模型和運動學模型是車輛模型建立過程中最常用的兩種模型，車輛的動力學模型是根據車輛在行駛中的受力情況，利用牛頓第二定律建立的；車輛的運動學模型是從幾何學角度來研究車輛的運動規律，包括車輛所位于的位置坐標、航向角、速度等。在車輛的路徑跟蹤研究中使用運動學模型，可以滿足行駛過程中的運動學約束。拖拉機速度一般在0.5～3.0m/s，屬于低速行駛的運動車輛，可以不考慮拖拉機在行駛過程中側滑、側傾等穩定性控制的動力學問題，因此采用運動學模型設計的軌跡跟蹤控制器具有可靠的控制性能[8]。

在后軸中心(Xr，Yr)處，后軸中心速度vr為：

(1)

前后軸的運動學約束方程為：

(2)

圖1 拖拉機運動學模型Fig. 1 Tractor kinematics model

由式(1)和(2)可得：

(3)

根據前后輪的幾何關系可得：

(4)

由式(4)可得橫擺角速度ω為：

(5)

同時，由ω和車速vr可得到轉向半徑R和前輪偏角δf。

(6)

由上式可以得到拖拉機的運動學模型為

(7)

2 模型預測控制

模型預測控制(Model Predictive Control MPC)算法是20世紀70年代末，美國、法國等工業領域中最早出現的一種計算機優化控制算法，模型預測控制在實現過程中都是基于預測模型、滾動優化和反饋矯正的控制策略[9-10]。這些方法不僅具有較強的魯棒性和較好的控制效果，而且對控制系統模型的精確度要求較低。基于模型預測控制的優勢，其被大量應用到過程控制中，現已經在機械工程和化工生產中得到成熟運用。

線性時變模型預測控制算法是基于線性時變的模型控制算法，其與非線性模型預測控制相比，具有計算較為簡單、計算速度快、實時性好的優點[11-15]。對車輛的軌跡跟蹤控制而言，實時性是最重要的考慮因素，因此筆者采取線性時變的模型預測控制算法。

考慮離散系統的線性化模型：

x(k+1)=Ak,tx(k)+Bk,tu(k)

(8)

x(k)和u(k)是在第k采樣時刻系統的狀態量和控制量，設定：

(9)

可以得到一個新的狀態空間表達式：

(10)

假設：

(11)

系統預測時域為Np，控制時域為Nc，則預測時域內的狀態量和系統輸出量可用下式表示：

(12)

未來時刻輸出可以由如下矩陣形式表達：

Y(t)=ψtξ(t|t)+ΘtΔu(t)

(13)

由此，在預測時域內的狀態變量和輸出變量都可以通過對系統當前狀態量ξ(t|t)和控制時域內的控制量Δu(t)計算得到，從而實現模型預測控制算法中的預測功能。

在實際過程中，系統的控制增量是不知道的，需要通過對合適的優化目標求解來得到控制時域內的控制序列，文獻[16]給出的目標函數如下：

j-1)Ru(k+j-1)

(14)

此形式的優化目標函數可以將其轉化為帶有線性或非線性的二次規劃，這一典型數學優化問題來解決。

在完成每個控制周期求解后，可以得到控制時域內的一系列控制輸入增量：

(15)

根據模型預測控制的基本原理，控制序列中第一個元素作為實際模型預測控制的輸入增量作用于系統中：

(16)

系統執行這一控制量，并根據當前狀態量預測下一周期新的控制增量序列，如此的反復循環，滾動優化，直到系統完成整個控制過程。

3 控制結果分析

由于MPC控制目標是拖拉機，考慮其在低速行駛過程中軌跡跟蹤控制，以式(7)運動學非線性模型為預測模型，對其線性化處理，可以得到線性時變模型(17)，T為采樣時間。在控制過程中，根據給定時刻預測模型得到拖拉機的航向角，后軸中心位置與系統實際拖拉機狀態的偏差，計算得到此時刻的模型輸入，重復這一過程使拖拉機快速穩定的跟蹤上目標軌跡。

ξ(k+1)=Akξ(k)+Bku(k)

(17)

其中

3.1 正弦曲線工況

正弦曲線工況是檢測車輛操縱性能的行駛工況。在行駛過程中車輛需要不斷的輸入前輪轉角，以期望對給定軌跡有較好的跟蹤效果。在不斷轉向過程中可能會出現車輛狀態發生變化或其他情況的發生，這對控制器的控制性能具有很高的要求。

給定拖拉機車速為10m/s，仿真結果如圖2。由仿真結果可以看出，車輛在實際行駛過程中能夠很好的跟蹤給定的行駛路線。由圖2(c)和圖2(d)可知最大行駛距離偏差為-0.025 m，最大航向角偏差為0.01 rad，由于拖拉機給定初始狀態的航向角與軌跡參考值存在初始誤差，因此兩個偏差都是非常小的。在圖2(b)中可以看到，開始后控制器使誤差逐漸變小，后來趨于穩定狀態，說明MPC控制器對于雙移線工況跟蹤效果較好。

3.2 自適應MPC控制器

對于實際應用來說，如何提高控制器的速度，滿足MPC控制器對實時性的要求至關重要。研究發現，對于MPC控制器，當降低預測時域Np時，雖然控制器對軌跡的跟蹤精度有所下降(特別在曲率較大地方，跟蹤精度下降更為明顯)但是系統的計算速度可以得到提升[17]。計算速度和控制器的控制效果是相互矛盾的，如何在提升計算速度同時得到較好的控制效果是值得研究的問題。為了解決這個問題，筆者設計了一種自適應MPC控制器，預測時域Np能夠根據車輛跟蹤軌跡的曲率K的變化自動調節，以達到又快又好的完成控制目標。預測時域Np的大小將按照下式規律。

Np=Round(a·|Kx|+b)

(18)

式中，Np是預測時域，a為曲率增益系數，Kx為道路曲率，b為預測時域基本值，計算中a取為1 000，b取為10。

圖2 正弦曲線工況仿真結果Fig. 2 Simulation results for sinusoidal driving conditions

圖3 雙移線工況仿真結果Fig. 3 Simulation results for double-shift line driving conditions

為了驗證自適應MPC控制器的實際效果，選取雙移線工況進行仿真實驗。雙移線工況指的是車輛在行駛過程中遇到前方障礙物變道后再返回到原來車道的工況，在現實生活中較為常見。車輛在換道過程中，可能會因變化的道路條件或其他情況的發生導致輪胎力飽和引起轉向不足或過度轉向，這都會對給定行駛軌跡的跟蹤效果造成相應影響。

拖拉機速度設定為10 m/s，選取MPC控制器和自適應MPC控制器對跟蹤效果進行對比分析，其中MPC控制器的預測時域Np選擇為30、10，其余控制器參數保持不變。數值仿真結果如圖3，仿真曲線圖3(a)和圖3(b)可以看出三種控制器都能夠保證位移和航向角跟隨上參考值的變化。圖3(c)和圖3(d)給出了三種控制器航向角和橫向距離誤差，可以看出當控制時域為30時，控制效果最好，證明了增大控制時域可以提高控制效果，但從圖3(f)可以看出與控制時域為10相比，其計算耗時也相對增加。對于設計的自適應MPC控制器，航向角偏差和距離偏差與控制時域為10相比，控制效果得到了明顯提升，計算時間消耗也能夠有所降低。由仿真結果可以看出，在使用自適應MPC對車輛進行控制時，車輛在實際行駛過程中能夠很好的跟蹤給定的行駛路線。同時由圖3(c)和圖3(d)可知，最大行駛距離偏差為-0.03 m，最大航向角偏差為0.015 rad，兩個偏差都是非常小的，計算速度也得到了提升，說明自適應MPC控制器對于雙移線工況跟蹤效果較好。因此，自適應MPC控制器能夠提升控制系統的控制效果，同時提高控制系統的計算速度，提升控制系統的實時性。在硬件條件較低，又希望得到很好的控制實時性的情況下，可以采用此方法。

4 結 論

基于MPC對無人駕駛拖拉機的軌跡跟蹤進行相關研究，通過對建立的拖拉機運動學非線性模型進行線性化處理，得到所需的線性時變系統，運用線性時變模型預測控制算法對行駛軌跡進行控制。對雙移線工況和正弦曲線工況進行仿真分析，結果表明該算法能夠較好的對給定軌跡進行跟蹤，具有較好的動態跟蹤性能。為提高控制器的計算速度，筆者設計了一種自適應MPC控制器，預測時域Np根據車輛跟蹤軌跡的曲率K的變化自動調節。仿真結果表明，利用自適應MPC控制器，車輛最大行駛距離偏差為-0.03 m，最大航向角偏差為0.015 rad，同時降低了控制器的計算時間，提升控制系統的實時性，達到較好的軌跡跟蹤目的。