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

嚴(yán)國軍,賁能軍 , 顧建華 , 楊 彥

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

0 引 言

農(nóng)用車輛自動(dòng)化作業(yè)能夠使農(nóng)業(yè)勞動(dòng)者從沉重的體力勞動(dòng)中解放出來，其中農(nóng)用車輛自動(dòng)化行走是實(shí)現(xiàn)自動(dòng)化作業(yè)的基礎(chǔ)。近階段針對(duì)農(nóng)用車輛自動(dòng)行走的研究逐漸成為各國研究重點(diǎn)[1-2]。在早期研究中，一些學(xué)者在園藝設(shè)施中嘗試將一些引導(dǎo)線埋藏在地下，以引導(dǎo)車輛沿著固定的軌道進(jìn)行作業(yè)，但這種方法在實(shí)施過程中具有很大的局限性。隨著科技不斷進(jìn)步，攝像頭和衛(wèi)星定位系統(tǒng)得到廣泛應(yīng)用，促使農(nóng)用車輛在自由軌跡上行駛的研究越來越多[2-6]?？偟膩砜?，農(nóng)業(yè)機(jī)械自動(dòng)駕駛的研究方向可以分為車輛行駛軌跡規(guī)劃和車輛軌跡跟蹤控制兩個(gè)方面。

車輛軌跡跟蹤控制的實(shí)質(zhì)是車輛能夠根據(jù)給定的行駛路徑完成自主轉(zhuǎn)向控制，并使跟蹤誤差最小，即使在行駛過程中遇到一些干擾，也能夠保證穩(wěn)定行駛。軌跡跟蹤控制是自動(dòng)駕駛車輛體系中最基礎(chǔ)的一環(huán)，它的性能好壞直接影響到整個(gè)自動(dòng)駕駛車輛是否能夠完成給定的駕駛?cè)蝿?wù)[7]。車輛在給定行駛軌跡時(shí)，需能穩(wěn)定且迅速地跟蹤給定行駛軌跡，這是對(duì)自動(dòng)駕駛車輛最基本的要求。軌跡跟蹤的重點(diǎn)就是設(shè)計(jì)出軌跡跟蹤控制器，使智能駕駛車輛能夠跟隨既定行駛軌跡駕駛，行駛路徑可以通過車載ECU采集各種探測(cè)器信號(hào)并經(jīng)ECU計(jì)算得出。筆者針對(duì)無人駕駛拖拉機(jī)在給定軌跡情況下的循跡效果開展研究。

1 車輛運(yùn)動(dòng)學(xué)模型建立

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

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

(1)

前后軸的運(yùn)動(dòng)學(xué)約束方程為：

(2)

圖1 拖拉機(jī)運(yùn)動(dòng)學(xué)模型Fig. 1 Tractor kinematics model

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

(3)

根據(jù)前后輪的幾何關(guān)系可得：

(4)

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

(5)

同時(shí)，由ω和車速vr可得到轉(zhuǎn)向半徑R和前輪偏角δf。

(6)

由上式可以得到拖拉機(jī)的運(yùn)動(dòng)學(xué)模型為

(7)

2 模型預(yù)測(cè)控制

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

線性時(shí)變模型預(yù)測(cè)控制算法是基于線性時(shí)變的模型控制算法，其與非線性模型預(yù)測(cè)控制相比，具有計(jì)算較為簡(jiǎn)單、計(jì)算速度快、實(shí)時(shí)性好的優(yōu)點(diǎn)[11-15]。對(duì)車輛的軌跡跟蹤控制而言，實(shí)時(shí)性是最重要的考慮因素，因此筆者采取線性時(shí)變的模型預(yù)測(cè)控制算法。

考慮離散系統(tǒng)的線性化模型：

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

(8)

x(k)和u(k)是在第k采樣時(shí)刻系統(tǒng)的狀態(tài)量和控制量，設(shè)定：

(9)

可以得到一個(gè)新的狀態(tài)空間表達(dá)式：

(10)

假設(shè)：

(11)

系統(tǒng)預(yù)測(cè)時(shí)域?yàn)镹p，控制時(shí)域?yàn)镹c，則預(yù)測(cè)時(shí)域內(nèi)的狀態(tài)量和系統(tǒng)輸出量可用下式表示：

(12)

未來時(shí)刻輸出可以由如下矩陣形式表達(dá)：

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

(13)

由此，在預(yù)測(cè)時(shí)域內(nèi)的狀態(tài)變量和輸出變量都可以通過對(duì)系統(tǒng)當(dāng)前狀態(tài)量ξ(t|t)和控制時(shí)域內(nèi)的控制量Δu(t)計(jì)算得到，從而實(shí)現(xiàn)模型預(yù)測(cè)控制算法中的預(yù)測(cè)功能。

在實(shí)際過程中，系統(tǒng)的控制增量是不知道的，需要通過對(duì)合適的優(yōu)化目標(biāo)求解來得到控制時(shí)域內(nèi)的控制序列，文獻(xiàn)[16]給出的目標(biāo)函數(shù)如下：

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

(14)

此形式的優(yōu)化目標(biāo)函數(shù)可以將其轉(zhuǎn)化為帶有線性或非線性的二次規(guī)劃，這一典型數(shù)學(xué)優(yōu)化問題來解決。

在完成每個(gè)控制周期求解后，可以得到控制時(shí)域內(nèi)的一系列控制輸入增量：

(15)

根據(jù)模型預(yù)測(cè)控制的基本原理，控制序列中第一個(gè)元素作為實(shí)際模型預(yù)測(cè)控制的輸入增量作用于系統(tǒng)中：

(16)

系統(tǒng)執(zhí)行這一控制量，并根據(jù)當(dāng)前狀態(tài)量預(yù)測(cè)下一周期新的控制增量序列，如此的反復(fù)循環(huán)，滾動(dòng)優(yōu)化，直到系統(tǒng)完成整個(gè)控制過程。

3 控制結(jié)果分析

由于MPC控制目標(biāo)是拖拉機(jī)，考慮其在低速行駛過程中軌跡跟蹤控制，以式(7)運(yùn)動(dòng)學(xué)非線性模型為預(yù)測(cè)模型，對(duì)其線性化處理，可以得到線性時(shí)變模型(17)，T為采樣時(shí)間。在控制過程中，根據(jù)給定時(shí)刻預(yù)測(cè)模型得到拖拉機(jī)的航向角，后軸中心位置與系統(tǒng)實(shí)際拖拉機(jī)狀態(tài)的偏差，計(jì)算得到此時(shí)刻的模型輸入，重復(fù)這一過程使拖拉機(jī)快速穩(wěn)定的跟蹤上目標(biāo)軌跡。

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

(17)

其中

3.1 正弦曲線工況

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

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

3.2 自適應(yīng)MPC控制器

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

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

(18)

式中，Np是預(yù)測(cè)時(shí)域，a為曲率增益系數(shù)，Kx為道路曲率，b為預(yù)測(cè)時(shí)域基本值，計(jì)算中a取為1 000，b取為10。

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

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

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

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

4 結(jié) 論

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