陸國(guó)強(qiáng),許建秋

(1.三江學(xué)院 電子信息工程學(xué)院，江蘇 南京 210012；2. 南京航空航天大學(xué) 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，江蘇 南京 211106)

路徑跟蹤控制作為拖拉機(jī)自主化作業(yè)的核心技術(shù)，可使拖拉機(jī)按照規(guī)劃路徑進(jìn)行作業(yè)，提高作業(yè)質(zhì)量和效率，降低駕駛員勞動(dòng)強(qiáng)度[1-3]。拖拉機(jī)作業(yè)過(guò)程中的速度和跟蹤路徑曲率是不斷變化的，有必要提高路徑跟蹤控制方法的自適應(yīng)性，保證其自主作業(yè)精度。

目前用于拖拉機(jī)的路徑跟蹤控制方法主要包括PID控制、純跟蹤控制、模糊控制、模型預(yù)測(cè)控制等[4-7]。PID 控制無(wú)需構(gòu)建車輛模型，易于實(shí)現(xiàn)，但參數(shù)整定較難，為提高其魯棒性，將模糊控制和神經(jīng)網(wǎng)絡(luò)與PID相結(jié)合[8-9]。純跟蹤控制是根據(jù)當(dāng)前的位置偏差、航向偏差和車輛幾何學(xué)模型求得控制輸入，其結(jié)構(gòu)簡(jiǎn)單，實(shí)現(xiàn)方便，被廣泛應(yīng)用?；诩兏櫩刂频穆窂礁櫺Ч年P(guān)鍵在于參數(shù)前視距離的選取，而作業(yè)速度和跟蹤路徑曲率對(duì)該參數(shù)的選取影響較大，模糊控制被用來(lái)自適應(yīng)調(diào)整前視距離，提高其跟蹤精度[10-11]。模型預(yù)測(cè)控制(MPC)是基于預(yù)測(cè)模型、滾動(dòng)優(yōu)化和反饋矯正的控制策略，具有較強(qiáng)的魯棒性，且在優(yōu)化求解過(guò)程中可考慮控制量、控制增量等約束條件[12-14]。

考慮到拖拉機(jī)作業(yè)過(guò)程中，控制信號(hào)會(huì)由于信號(hào)通訊、執(zhí)行器等造成一定的延遲，使得當(dāng)前時(shí)刻產(chǎn)生的控制信號(hào)在未來(lái)時(shí)刻被執(zhí)行，而MPC是一種預(yù)測(cè)控制，可有效補(bǔ)償延遲問(wèn)題。本文采用了基于MPC的路徑跟蹤控制方法。文獻(xiàn)[15]指出了MPC中預(yù)測(cè)時(shí)域?qū)Ω櫺Ч绊戄^大，當(dāng)速度和跟蹤路徑發(fā)生變化時(shí)，應(yīng)對(duì)其適當(dāng)調(diào)整。文獻(xiàn)[16]設(shè)計(jì)了自適應(yīng)MPC控制器，根據(jù)參考軌跡曲率的變化自動(dòng)調(diào)節(jié)預(yù)測(cè)時(shí)域，但并未考慮速度變化時(shí)對(duì)預(yù)測(cè)時(shí)域的調(diào)整。本文針對(duì)拖拉機(jī)作業(yè)速度和路徑曲率變化時(shí)，基于固定預(yù)測(cè)時(shí)域MPC控制器的跟蹤效果可能變差的問(wèn)題，提出一種自適應(yīng)MPC控制器。當(dāng)作業(yè)場(chǎng)景變化時(shí)，利用改進(jìn)的粒子群優(yōu)化(PSO)算法求取最優(yōu)的預(yù)測(cè)時(shí)域，保證路徑跟蹤控制器的魯棒性和精確性。

1 拖拉機(jī)運(yùn)動(dòng)學(xué)模型

由于拖拉機(jī)的作業(yè)速度低，模型預(yù)測(cè)控制通常可采用結(jié)構(gòu)簡(jiǎn)單、易于建模的運(yùn)動(dòng)學(xué)模型。假設(shè)拖拉機(jī)左右對(duì)稱，四輪車模型可簡(jiǎn)化為二輪模型，拖拉機(jī)模型示意圖如圖1所示。在導(dǎo)航坐標(biāo)系下，得到拖拉機(jī)的運(yùn)動(dòng)學(xué)模型為

圖1 拖拉機(jī)模型示意圖

(1)

式中：x，y為拖拉機(jī)的質(zhì)點(diǎn)在導(dǎo)航坐標(biāo)系下的坐標(biāo)；φ為航向角；v為縱向速度；θ為車輪轉(zhuǎn)向角；L為前后輪軸距。

路徑跟蹤控制器中采用的誤差量為系統(tǒng)狀態(tài)變量，基于式(1)的線性化和離散化處理，可得該系統(tǒng)的狀態(tài)空間方程為

(2)

控制輸入會(huì)由于信號(hào)通訊、執(zhí)行器等造成一定的延遲，則狀態(tài)空間方程表示為

(3)

式中kd為輸入延遲時(shí)間，可通過(guò)試驗(yàn)測(cè)量獲得。

考慮到本文是為了驗(yàn)證拖拉機(jī)在不同作業(yè)速度和跟蹤路徑時(shí)，所提出的基于自適應(yīng)預(yù)測(cè)時(shí)域的MPC效果，忽略了輸入延遲，采用基于式(2)的狀態(tài)空間模型。

2 自適應(yīng)路徑跟蹤控制器設(shè)計(jì)

MPC是根據(jù)當(dāng)前時(shí)刻傳感器測(cè)量的拖拉機(jī)狀態(tài)信息與期望值的偏差，并通過(guò)優(yōu)化求解得到控制輸入來(lái)改變拖拉機(jī)的運(yùn)行狀態(tài)，進(jìn)而實(shí)現(xiàn)路徑跟蹤效果。MPC模型的初始化參數(shù)中的預(yù)測(cè)時(shí)域P對(duì)其解算結(jié)果的影響較大，當(dāng)拖拉機(jī)的速度或跟蹤軌跡有較大變化時(shí)，路徑跟蹤效果會(huì)受到影響，故本文采用了改進(jìn)的粒子群優(yōu)化算法對(duì)預(yù)測(cè)時(shí)域P進(jìn)行適當(dāng)調(diào)整?；谧赃m應(yīng)MPC的路徑跟蹤控制算法如圖2所示。

圖2 基于自適應(yīng)MPC的路徑跟蹤控制算法

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

MPC是結(jié)合拖拉機(jī)運(yùn)動(dòng)學(xué)模型、當(dāng)前狀態(tài)預(yù)測(cè)之后的優(yōu)化控制值，可在一定程度上補(bǔ)償控制器的延遲，且根據(jù)需要增加一定的約束條件。

(4)

假設(shè)預(yù)測(cè)時(shí)域?yàn)镻，控制時(shí)域?yàn)镃，則Δuk+C-1=Δuk+C=…=Δuk+P-1。根據(jù)上式可得時(shí)刻k的預(yù)測(cè)輸出為

(5)

令時(shí)刻k的控制序列為ΔUk，輸出序列為Zk+1，則：

模型預(yù)測(cè)控制的評(píng)價(jià)函數(shù)為

(7)

MPC的約束條件設(shè)置為

(8)

式中：umin和umax為控制量約束值；Δumin和Δumax為控制增量約束值。

通過(guò)優(yōu)化求解，可得控制時(shí)域內(nèi)時(shí)刻k的控制輸入增量ΔUk，則時(shí)刻k的控制輸入量為

uk=uk-1+Δuk。

(9)

2.2 改進(jìn)粒子群優(yōu)化算法

拖拉機(jī)作業(yè)過(guò)程中速度和跟蹤路徑是變化的，若模型預(yù)測(cè)控制中的參數(shù)設(shè)定為固定值，路徑跟蹤效果會(huì)受到一定的影響。為了提高路徑跟蹤的自適應(yīng)性，采用改進(jìn)粒子群優(yōu)化算法，根據(jù)工作場(chǎng)景對(duì)預(yù)測(cè)時(shí)域進(jìn)行優(yōu)化選取。

PSO算法中粒子的速度和位置更新軌跡為[17]

(10)

式中：Vj為粒子j的速度；Xj為粒子j的位置；w為慣性權(quán)重；c1、c2是加速度常數(shù)；r1、r2是隨機(jī)數(shù)；Xj,p為該粒子經(jīng)過(guò)的最優(yōu)位置；Xj,g是整個(gè)種群中粒子經(jīng)過(guò)的最優(yōu)位置；m是群體規(guī)模。

參數(shù)w反映了對(duì)全局和局部搜索的權(quán)重，對(duì)于PSO算法的尋優(yōu)過(guò)程尤為重要。為了提高尋優(yōu)性能，對(duì)傳統(tǒng)的粒子群優(yōu)化算法進(jìn)行改進(jìn)，將w設(shè)定為分段函數(shù)，第k次迭代的慣性權(quán)重值wk為

(11)

式中：Iλ為轉(zhuǎn)折迭代次數(shù)；Imax為最大迭代次數(shù)；wλ是轉(zhuǎn)折權(quán)重值；wmax是最大慣性權(quán)重值；wmin為最小慣性權(quán)重值。

3 仿真實(shí)驗(yàn)與分析

為了驗(yàn)證本文算法有效性，針對(duì)拖拉機(jī)不同的作業(yè)速度和跟蹤路徑，分別對(duì)基于固定預(yù)測(cè)時(shí)域和自適應(yīng)預(yù)測(cè)時(shí)域的MPC控制器進(jìn)行仿真實(shí)驗(yàn)和對(duì)比分析。選用東方紅-X1304拖拉機(jī)為仿真實(shí)驗(yàn)對(duì)象，其前后輪軸距L為2.688 5 m。MPC控制器的相應(yīng)參數(shù)設(shè)置是：采樣周期為T=0.05 s，控制量約束值分別為umin=-0.523 6 rad、umax=0.523 6 rad，控制增量約束值分別為Δumin=-0.087 3 rad、Δumax=0.087 3 rad，控制時(shí)域?yàn)镃=5。粒子群優(yōu)化算法的相應(yīng)參數(shù)是：加速度常數(shù)為c1=c2=1.494 55，最大慣性權(quán)重值為wmax=0.9，最小慣性權(quán)重值為wmin=0.4，轉(zhuǎn)折慣性權(quán)重值為wλ=0.6，轉(zhuǎn)折迭代次數(shù)為Iλ=5，種群規(guī)模m=10，最大迭代次數(shù)Imax=50。

3.1 直線跟蹤路徑

直線作業(yè)是拖拉機(jī)較普遍的作業(yè)方式，對(duì)作業(yè)速度v分別為1、2 m/s和變速v=1.5+0.5sin(2πt/30)，采用基于不同固定預(yù)測(cè)時(shí)域值和自適應(yīng)預(yù)測(cè)值的MPC控制器進(jìn)行仿真實(shí)驗(yàn)。初始位置偏差設(shè)定為3 m，初始航向偏差為π/3 rad。圖3給出了跟蹤直線路徑時(shí)，不同作業(yè)速度下的跟蹤路徑、航向角跟蹤誤差和控制輸入結(jié)果圖。從圖3可以看出，同一預(yù)測(cè)時(shí)域在速度不同時(shí)，預(yù)測(cè)效果不同，如預(yù)測(cè)時(shí)域P為10時(shí)，若速度為1 m/s效果較差，若速度為 2 m/s效果較好；控制輸入都在設(shè)定的約束條件內(nèi)，且基于自適應(yīng)預(yù)測(cè)時(shí)域的MPC控制器的控制輸入波動(dòng)小，收斂速度快；基于自適應(yīng)預(yù)測(cè)時(shí)域的MPC控制器在三種速度情況下，跟蹤效果都優(yōu)于基于三個(gè)固定預(yù)測(cè)時(shí)域的MPC控制器。

3.2 曲線跟蹤路徑

為了驗(yàn)證跟蹤不同路徑的控制器效果，對(duì)曲線路徑進(jìn)行仿真實(shí)驗(yàn)驗(yàn)證。曲線跟蹤路徑半徑設(shè)定為20 m的圓弧，初始位置誤差為3 m，初始航向偏差為0 rad。作業(yè)速度v分別為1、2 m/s和變速v=1.5+0.5sin(2πt/30)，跟蹤路徑、航向角跟蹤誤差和控制輸入結(jié)果如圖4所示。從圖4可以看出，拖拉機(jī)可以有效跟蹤曲線路徑；對(duì)于基于固定預(yù)測(cè)時(shí)域的MPC控制器，在速度不同時(shí)，最優(yōu)預(yù)測(cè)時(shí)域不同，如速度為1 m/s時(shí)，預(yù)測(cè)時(shí)域?yàn)?0要優(yōu)于其他兩個(gè)固定預(yù)測(cè)時(shí)域值，而速度為2 m/s時(shí)，預(yù)測(cè)時(shí)域?yàn)?0的控制器效果較好；三種速度下，基于自適應(yīng)預(yù)測(cè)時(shí)域的MPC控制器的跟蹤效果都優(yōu)于基于三種固定預(yù)測(cè)時(shí)域的MPC控制器。

(a) v=1 m/s

為了進(jìn)一步分析算法的有效性，對(duì)跟蹤誤差均值和收斂時(shí)間進(jìn)行了統(tǒng)計(jì)分析，其結(jié)果分別見表1和表2。其中，跟蹤誤差用行駛軌跡上的點(diǎn)到離期望路徑最近點(diǎn)的距離評(píng)價(jià)，收斂時(shí)間用跟蹤誤差收斂到0.05 m以內(nèi)的時(shí)間統(tǒng)計(jì)。表1表明，在基于固定預(yù)測(cè)時(shí)域的情況下，速度越小，預(yù)測(cè)時(shí)域的值設(shè)定為較大值時(shí)跟蹤誤差越小，而速度變大時(shí)，應(yīng)適當(dāng)減小預(yù)測(cè)時(shí)域；不同作業(yè)速度和跟蹤路徑的工作場(chǎng)景中，基于自適應(yīng)預(yù)測(cè)時(shí)域值的跟蹤誤差相對(duì)于其他固定時(shí)域的降低了2%～44%，跟蹤精度更高。表2表明，同一預(yù)測(cè)時(shí)域在速度或跟蹤路徑不同時(shí)，收斂效果不同，如預(yù)測(cè)時(shí)域P為20，跟蹤直線路徑時(shí)，若速度為1 m/s收斂速度要優(yōu)于預(yù)測(cè)時(shí)域?yàn)?0和10，若速度為 2 m/s收斂速度最慢；跟蹤曲線路徑時(shí)，若速度為1 m/s收斂速度居中。不同作業(yè)速度和跟蹤路徑時(shí)，基于自適應(yīng)預(yù)測(cè)時(shí)域值的收斂時(shí)間相對(duì)于三種固定時(shí)域下的收斂時(shí)間縮短了2%～71%，收斂速度最快。

表1 跟蹤誤差均值統(tǒng)計(jì)

表2 收斂時(shí)間統(tǒng)計(jì)

4 結(jié)束語(yǔ)

針對(duì)基于固定預(yù)測(cè)時(shí)域的MPC控制器，為提高控制器的自適應(yīng)性，本文結(jié)合工作場(chǎng)景和PSO算法對(duì)預(yù)測(cè)時(shí)域進(jìn)行適當(dāng)調(diào)整，并采用分段函數(shù)的慣性權(quán)重對(duì)PSO算法進(jìn)行了改進(jìn)。以東方紅X-1304拖拉機(jī)為仿真對(duì)象，分別對(duì)不同速度和路徑情況進(jìn)行了實(shí)驗(yàn)驗(yàn)證和對(duì)比分析。實(shí)驗(yàn)結(jié)果表明，相比于基于固定參數(shù)的MPC控制器，自適應(yīng)MPC控制器在不同工作場(chǎng)景下都具有較好的路徑跟蹤效果。該路徑跟蹤技術(shù)可提高拖拉機(jī)自主化作業(yè)的環(huán)境適應(yīng)性，為后續(xù)試驗(yàn)提供研究基礎(chǔ)。