基于預(yù)見位姿信息的鉸接式車輛LQR-GA路徑跟蹤控制

孟 宇 汪 鈺 顧 青 白國星

(北京科技大學(xué)機(jī)械工程學(xué)院， 北京 100083)

0 引言

鉸接式車輛具有中央鉸接點(diǎn)連接，前后車體可相對(duì)轉(zhuǎn)動(dòng)的結(jié)構(gòu)特點(diǎn)，在低矮巷道和復(fù)雜工況行駛，具有靈活、機(jī)動(dòng)、高效等突出優(yōu)點(diǎn)。鉸接式車輛的路徑跟蹤是智能礦山領(lǐng)域的研究熱點(diǎn)。針對(duì)鉸接車的路徑跟蹤控制，趙翾等[1]針對(duì)鉸接式車輛使用Ackermann公式與指數(shù)趨近律設(shè)計(jì)滑?？刂七M(jìn)行路徑跟蹤，HEMAMI等[2]設(shè)計(jì)了一種基于誤差動(dòng)力學(xué)的路徑跟蹤控制器，PETROV等[3]在鉸接式車輛中應(yīng)用了線性反饋控制，BIGRAS等[4]提出一種基于線性矩陣不等式的控制框架，RIDLEY等[5]將極點(diǎn)配置方法應(yīng)用于鉸接車中，NAYL等[6]設(shè)計(jì)了一種采用切換模型的預(yù)測(cè)控制方案，邵俊愷等[7]提出一種基于強(qiáng)化學(xué)習(xí)的控制算法。其中文獻(xiàn)[2-3]沒有給出具體的誤差分析，文獻(xiàn)[1,4-7]的研究結(jié)果中位置偏差有較大超調(diào)，最大偏差大于0.5 m。過大的位置偏差可能會(huì)導(dǎo)致車輛在狹長、低矮巷道行駛時(shí)碰撞巷道壁，且最終穩(wěn)定跟蹤精度也有待提高。實(shí)現(xiàn)高精度的穩(wěn)定路徑跟蹤控制是保證無人駕駛鉸接車安全自主行駛的關(guān)鍵，有必要對(duì)其開展深入研究。

線性二次型最優(yōu)控制(LQR)是一種穩(wěn)定的控制方法，可利用較小的控制能量使系統(tǒng)狀態(tài)變量維持在零值附近[8]，且同時(shí)可以對(duì)不穩(wěn)定系統(tǒng)進(jìn)行整定，對(duì)于鉸接車多自由度運(yùn)動(dòng)、且行駛環(huán)境噪聲信號(hào)復(fù)雜的情況具有突出優(yōu)勢(shì)。為提高鉸接式車輛路徑跟蹤的跟蹤精度，本文結(jié)合鉸接式車輛的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)特點(diǎn)，基于其誤差動(dòng)力學(xué)模型提出一種基于預(yù)見信息的線性二次型最優(yōu)控制(LQR)方法。擬利用航跡推算方法對(duì)鉸接車的下一采樣時(shí)刻進(jìn)行位姿估計(jì)，將預(yù)見位姿信息作為控制器的部分輸入可有效提高控制對(duì)象的反應(yīng)速度。同時(shí)，將利用全局優(yōu)化算法遺傳算法對(duì)Q、R矩陣進(jìn)行優(yōu)化，獲得最優(yōu)反饋控制率，實(shí)現(xiàn)鉸接車的精確路徑跟蹤，并利用Simulink和ADAMS聯(lián)合仿真驗(yàn)證控制策略的有效性，且在模型樣機(jī)中進(jìn)行試驗(yàn)驗(yàn)證。

1 路徑跟蹤控制策略

1.1 鉸接式車輛運(yùn)動(dòng)學(xué)模型

γ=θf-θr

圖1 鉸接式車輛模型Fig.1 Articulated vehicle model

由于鉸接式車輛行駛速度較低，一般小于28 km/h[10],輪胎變形對(duì)行駛影響可忽略不計(jì)，假設(shè)α=β=0[11]，可得鉸接式車輛運(yùn)動(dòng)幾何關(guān)系

(1)

選取前橋中點(diǎn)Pf(Xf,Yf)為整車狀態(tài)參考點(diǎn)，可以得到整車運(yùn)動(dòng)學(xué)模型為

(2)

由式(2)可知，通過控制車速和鉸接角可以實(shí)現(xiàn)對(duì)鉸接式車輛位姿狀態(tài)的實(shí)時(shí)控制。本文所用35 t鉸接式車輛模型部分結(jié)構(gòu)參數(shù)如表1所示。

表1 鉸接式車輛模型基本參數(shù)Tab.1 Main parameters of articulated vehicle model

1.2 預(yù)見位姿推算模型

在控制中根據(jù)車輛當(dāng)前行駛的位姿狀態(tài)信息，利用航跡推算技術(shù)建立預(yù)見位姿狀態(tài)模型，進(jìn)行下一時(shí)刻采樣點(diǎn)位置的位姿估計(jì)，在車輛路徑跟蹤中加入預(yù)見信息，可提高路徑跟蹤控制的反應(yīng)速度[12]。

鉸接式車輛行駛曲線和參數(shù)定義示意圖如圖2所示，曲線A為車輛行駛曲線;前車體中點(diǎn)為P，預(yù)見車輛位置點(diǎn)為P′；前后車體轉(zhuǎn)向中心分別為Or、Of;前后車體的航向角分別為θf、θr，轉(zhuǎn)向半徑分別為Rf、Rr[13]。

圖2 鉸接式車輛行駛曲線和參數(shù)定義Fig.2 Articulated vehicle modeling configuration

通過鉸接車的運(yùn)動(dòng)學(xué)模型分析，得到前車體航向角變化速度為

(3)

假定采樣間隔為Δt，前車體的預(yù)見航向角為

(4)

假定車輛逆時(shí)針轉(zhuǎn)動(dòng)為正方向，計(jì)算可得前車體的轉(zhuǎn)向半徑為

(5)

為區(qū)分車輛轉(zhuǎn)動(dòng)和直線行駛狀態(tài)，設(shè)置車體轉(zhuǎn)動(dòng)狀態(tài)量ts，且滿足以下條件

(6)

其中，設(shè)置的閾值ε通常取值較小，當(dāng)鉸接角γ處在該閾值范圍內(nèi)，認(rèn)為車體沒有轉(zhuǎn)動(dòng)。如圖2所示，車體由P(x,y)點(diǎn)移動(dòng)到P′(x′,y′)所用時(shí)間為Δt，前車體的轉(zhuǎn)向角速度為ωf，則前車體轉(zhuǎn)過角度為

λ=|ωf|Δt

(7)

且由幾何關(guān)系可得車體移動(dòng)距離

(8)

可推算下一時(shí)刻車輛前車體中點(diǎn)坐標(biāo)P′(x′,y′)為[13]

(9)

(10)

(11)

式中δ——預(yù)見航向角與當(dāng)前航向角偏差

1.3 誤差動(dòng)力學(xué)模型

鉸接角和車速是路徑跟蹤控制的控制變量，本文重點(diǎn)是實(shí)現(xiàn)鉸接角的實(shí)時(shí)控制。需要建立合適的偏差量作為控制輸入，并能夠完整地反映跟蹤控制精度。車輛勻速行駛速度為v，根據(jù)車輛行駛中的實(shí)際路徑和理想路徑的偏差情況建立的行駛示意圖如圖3所示[5]。

圖3 鉸接式車輛行駛示意圖Fig.3 Graphical representation of driving of articulated vehicle

偏差定義與分析如下[3]：

(1)位置偏差εd：鉸接車參考定位點(diǎn)p與參考軌跡上對(duì)應(yīng)點(diǎn)P(與參考軌跡距離最近)的橫向位置差值

(12)

(2)行駛方向偏差εθ：鉸接車定位參考點(diǎn)p的航向角(速度方向與車輛坐標(biāo)系x軸之間的夾角)與參考軌跡上對(duì)應(yīng)點(diǎn)P的航向角(對(duì)應(yīng)點(diǎn)的切線與車輛坐標(biāo)系x軸之間的夾角)的差值

(13)

(3)曲率偏差εc：鉸接車定位參考點(diǎn)p與參考軌跡上對(duì)應(yīng)點(diǎn)P的曲率差值

(14)

整理式(12)～(14)，根據(jù)L=Lf+Lr，可得

(15)

鉸接式車速較低，且鉸接角變化較小，對(duì)鉸接角速度的變化率通?？梢院雎裕瑢?duì)式(15)簡化，得到鉸接車誤差動(dòng)力學(xué)方程為

(16)

在恒速運(yùn)動(dòng)情況下，鉸接車誤差動(dòng)力學(xué)模型為單輸入的線性時(shí)不變模型，通過控制偏差量可實(shí)現(xiàn)對(duì)車輛鉸接角的實(shí)時(shí)控制。

1.4 轉(zhuǎn)向運(yùn)動(dòng)分析與數(shù)學(xué)建模

鉸接車的ADAMS模型僅能反映車輛的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)特性，為了使仿真過程符合鉸接車的實(shí)際轉(zhuǎn)向過程，對(duì)鉸接車的轉(zhuǎn)向機(jī)構(gòu)進(jìn)行運(yùn)動(dòng)分析和建模。鉸接車的轉(zhuǎn)向機(jī)構(gòu)為電機(jī)驅(qū)動(dòng)的液壓轉(zhuǎn)向系統(tǒng)，將轉(zhuǎn)向系統(tǒng)分為4個(gè)環(huán)節(jié)，并分別針對(duì)各個(gè)環(huán)節(jié)建立了比例電磁閥模型、閥芯受力平衡模型、閥控液壓缸-負(fù)載模型及轉(zhuǎn)向系統(tǒng)運(yùn)動(dòng)學(xué)摸型，分別推導(dǎo)了電磁閥控制壓力與占空比關(guān)系、液控力與閥芯位移關(guān)系、活塞輸出位移與閥芯輸入位移關(guān)系以及活塞輸出位移與車輛鉸接角關(guān)系，最終確定了電信號(hào)即占空比與輸出鉸接角的關(guān)系，并最終獲得勻速轉(zhuǎn)向的轉(zhuǎn)向機(jī)構(gòu)數(shù)學(xué)模型。鉸接車轉(zhuǎn)向系統(tǒng)數(shù)學(xué)仿真模型如圖4所示，仿真結(jié)果如圖5所示，鉸接角速度為0.14 rad/s，完全符合實(shí)車的轉(zhuǎn)向特性。

圖4 轉(zhuǎn)向系統(tǒng)仿真模型Fig.4 Steering system simulation model

圖5 轉(zhuǎn)向模型仿真結(jié)果Fig.5 Steering model response in simulation

圖6 路徑跟蹤控制系統(tǒng)Fig.6 Path tracking control system

2 路徑跟蹤控制器

針對(duì)以上分析，建立路徑跟蹤控制系統(tǒng)如圖6所示。根據(jù)鉸接式車輛的運(yùn)動(dòng)學(xué)模型分析進(jìn)行速度和鉸接角控制來實(shí)時(shí)控制車輛的行駛位姿狀態(tài)。由式(16)可看出，在恒速情況下，鉸接車的誤差動(dòng)力學(xué)模型為線性時(shí)不變模型，而鉸接車在地下巷道行駛過程中車速較低且很少出現(xiàn)較大幅度的加減速情況，因此本研究中車輛以3 m/s的速度勻速行駛，采用PID速度控制建立速度控制器，使車速維持恒穩(wěn)，再以恒速條件下的線性時(shí)不變的誤差動(dòng)力學(xué)模型作為鉸接角控制模型。對(duì)于鉸接角的控制，根據(jù)車輛的當(dāng)前行駛位姿信息進(jìn)行下一采樣點(diǎn)的位姿估計(jì)可以得到預(yù)見位姿信息，且通過當(dāng)前行駛狀態(tài)和預(yù)見行駛狀態(tài)與理想路徑的偏差可獲得車輛的當(dāng)前狀態(tài)跟蹤偏差和預(yù)見狀態(tài)跟蹤偏差，建立線性時(shí)不變的單輸入偏差狀態(tài)方程進(jìn)行LQR-GA控制，實(shí)現(xiàn)理想鉸接角輸入。通過PID速度控制和LQR-GA鉸接角控制實(shí)現(xiàn)鉸接式車輛車的實(shí)時(shí)路徑跟蹤。

2.1 LQR控制器

LQR最優(yōu)控制可利用較低成本使系統(tǒng)達(dá)到預(yù)期性能指標(biāo)，且也可以同時(shí)對(duì)不穩(wěn)定系統(tǒng)進(jìn)行整定，對(duì)線性時(shí)不變系統(tǒng)的狀態(tài)反饋控制問題中，往往可取得良好控制效果，已被應(yīng)用于許多工程和科學(xué)領(lǐng)域[14-16]，同時(shí)也被運(yùn)用于眾多復(fù)雜系統(tǒng)[17-20]。建立線性系統(tǒng)模型為

(17)

式中A——系統(tǒng)狀態(tài)矩陣，A∈Rn×n

B——輸入矩陣，B∈Rn×n

C——輸出矩陣，C∈Rp×n

D——狀態(tài)反饋矩陣，D∈Rp×m

u(t)、X(t)、Y(t)——系統(tǒng)輸入量、狀態(tài)量以及輸出量

根據(jù)式(17)以及表1中鉸接式車輛模型參數(shù)可得鉸接式車輛的矩陣為

由此，可控可觀性分析為

(18)

矩陣均滿秩證明該系統(tǒng)具有可控可觀性，結(jié)合式(16)、(17)，鉸接式車輛的LQR控制器模型為線性時(shí)不變單輸入系統(tǒng)模型為

(19)

(20)

式中Q——系統(tǒng)狀態(tài)量權(quán)陣

R——系統(tǒng)控制量權(quán)陣

J1反映鉸接車偏離平衡狀態(tài)的程度，J2反映控制能量大小?？刂颇繕?biāo)是使最優(yōu)性能指標(biāo)取得最小值，即以最小的控制能量使路徑跟蹤偏差達(dá)到最小，建立反饋控制率u=-KX(t)，滿足K=R-1BTP，建立Riccati方程

ATP+PA+Q-PBR-1BTP=0

(21)

式中P——對(duì)稱正定矩陣

通過求解最優(yōu)Q、R矩陣可求得最優(yōu)反饋控制率，實(shí)現(xiàn)理想鉸接角輸入。路徑跟蹤控制器的設(shè)計(jì)如圖7所示。

圖7 鉸接式車輛路徑跟蹤控制器Fig.7 Articulated vehicle path tracking controller

其中，當(dāng)前偏差量為當(dāng)前時(shí)刻車輛位姿與理想路徑中位姿的偏差，預(yù)見偏差量為下一時(shí)刻的預(yù)見位姿與理想路徑位姿的偏差，為避免引入的預(yù)見信息量值過大，導(dǎo)致控制過度引起車輛抖振，在控制器中加入柔化操作環(huán)節(jié)，綜合考慮當(dāng)前偏差量信息和預(yù)見的偏差量信息，以提高控制器的穩(wěn)定性和反應(yīng)速度，即LQR控制器的最終輸入為

ε(t)=aε1(t)+bε2(t)

(22)

其中

式中ε1(t)——當(dāng)前跟蹤偏差矩陣

ε2(t)——預(yù)見信息偏差矩陣

a、b——柔化系數(shù)

ε(t)——LQR控制器輸入

2.2 GA算法優(yōu)化

遺傳算法(GA)是一種用于解決最優(yōu)化的一種搜索啟發(fā)式算法，具有良好的全局搜索能力，并且利用其內(nèi)在并行性，可以方便地進(jìn)行分布式計(jì)算，加快求解速度。

在LQR控制中存在狀態(tài)量權(quán)重矩陣Q和控制能量權(quán)重矩陣R的選取兩個(gè)重要問題，不同的選取結(jié)果將直接影響控制效果，在傳統(tǒng)優(yōu)化方法中，Q、R的計(jì)算通常會(huì)耗費(fèi)較長時(shí)間且多依賴于專家經(jīng)驗(yàn)[21]。遺傳算法作為一種全局優(yōu)化算法，可對(duì)Q、R矩陣進(jìn)行優(yōu)化，克服傳統(tǒng)LQR控制的缺點(diǎn)，LQR-GA控制在倒立擺控制、復(fù)合電力系統(tǒng)和飛行器俯仰控制等[22-24]問題中已取得較大進(jìn)展。

圖8 遺傳算法流程Fig.8 Block diagram of genetic algorithm scheme

評(píng)價(jià)函數(shù)選擇為LQR控制的最優(yōu)性能指標(biāo)，當(dāng)最優(yōu)性能指標(biāo)取得最小值時(shí)，即可達(dá)到最優(yōu)控制效果。且設(shè)置約束條件為

(23)

(1)編碼方式

因?yàn)槎M(jìn)制編碼通常會(huì)在目標(biāo)函數(shù)中引入附加的多峰性，使得編碼后的目標(biāo)函數(shù)比原始問題更復(fù)雜，并且在函數(shù)優(yōu)化和約束優(yōu)化領(lǐng)域，實(shí)數(shù)編碼比二進(jìn)制和Gray編碼更為有效，實(shí)數(shù)編碼在基因型空間和表現(xiàn)型空間中是一致的，在本文中采用實(shí)數(shù)編碼。

(2)群體取值范圍

(24)

由于采用無限時(shí)間狀態(tài)調(diào)節(jié)器的控制系統(tǒng),其閉環(huán)系統(tǒng)是漸進(jìn)穩(wěn)定的，n變化范圍為1～100，變化間隔為10，系統(tǒng)的閉環(huán)極點(diǎn)位置。

表2顯示了系統(tǒng)的閉環(huán)極點(diǎn)位置與狀態(tài)反饋增益矩陣K隨著n變化的趨勢(shì)。由閉環(huán)極點(diǎn)位置變化可以判斷，n>30后繼續(xù)增大對(duì)于主導(dǎo)極點(diǎn)的影響甚微，僅對(duì)于系統(tǒng)的輔助極點(diǎn)有一定影響，使其遠(yuǎn)離虛軸原點(diǎn),對(duì)系統(tǒng)的性能影響不大，因此，n應(yīng)取較小值，即取n≤30。狀態(tài)反饋增益矩陣K的變化如表2所示，隨著n增大而增大，對(duì)于狀態(tài)變量的變化約束加強(qiáng)。

表2 閉環(huán)極點(diǎn)位置及狀態(tài)反饋矩陣K隨n的變化Tab.2 Change of K with n in closed-loop pole position and state feedback matrix

綜合以上分析，取n≤30，即群體中個(gè)體最大值為30。

(3)交叉方式

采用離散交叉的方式，即選定一個(gè)基因位，如選定q1，然后將選定的2個(gè)染色體在這個(gè)位置之后的基因進(jìn)行交換。

(4)變異

在任意一個(gè)基因位，如選定q1，隨機(jī)產(chǎn)生2個(gè)整數(shù)相交換等來進(jìn)行變異。

(5)參數(shù)選取

設(shè)定種群數(shù)為100，精英數(shù)為10，交叉概率為0.4，連續(xù)進(jìn)化代數(shù)限制為20，遺傳算法迭代終止代數(shù)為50，采用隨機(jī)一致選擇、順序排序。

建立適應(yīng)度函數(shù)為

(25)

自適應(yīng)函數(shù)值偏差預(yù)設(shè)為0.01，為避免搜索陷入局部最小值點(diǎn)，設(shè)置自適應(yīng)函數(shù)值L1=L′+10，遺傳算法具體的迭代過程如圖9所示。

圖9 遺傳算法迭代過程Fig.9 Genetic algorithm iterative process

從圖9遺傳算法迭代過程可看出，在迭代8代以后的適應(yīng)度函數(shù)值等于平均適應(yīng)度函數(shù)值，評(píng)價(jià)函數(shù)值已無明顯變化，即至少迭代8代即可求出最優(yōu)解，即最優(yōu)反饋控制率。

3 聯(lián)合仿真分析

圖12 鉸接車路徑跟蹤仿真結(jié)果Fig.12 Articulated vehicle path tracking based circular path

利用ADAMS與Simulink的聯(lián)合仿真來初步驗(yàn)證提出的控制策略的有效性。在Solidworks中建立包括鉸接車、左右轉(zhuǎn)向液壓缸、前后車體、左右轉(zhuǎn)向推桿等的鉸接車三維模型，為了彌補(bǔ)實(shí)體模型導(dǎo)入后丟失的原有屬性，在導(dǎo)入ADAMS模型后，定義各個(gè)零部件的質(zhì)量，并為其添加約束。在ADAMS模型中，考慮鉸接式車輛運(yùn)行的巷道環(huán)境，采用二維隨機(jī)不平路面，并參考E級(jí)路面修改路面譜系數(shù)，建立約束和驅(qū)動(dòng)，基于ADAMS的鉸接式車輛模型如圖10所示。

圖10 基于ADAMS的35 t鉸接式車輛模型Fig.10 35 t articulated vehicle model based on ADAMS

搭建ADAMS和Simulink的聯(lián)合仿真模型，本研究中的聯(lián)合仿真路線如圖11所示。

圖11 聯(lián)合仿真流程Fig.11 Block diagram of unity simulation scheme

根據(jù)圖6所示的鉸接式車輛路徑跟蹤控制策略進(jìn)行聯(lián)合仿真。在聯(lián)合仿真中，對(duì)于鉸接角的控制，根據(jù)地下巷道鉸接車的安全行駛要求，車輛一側(cè)與巷道壁間距最小為10 cm，且由于鉸接車的結(jié)構(gòu)限制，其前車體轉(zhuǎn)角范圍為[-45°，45°]，結(jié)合車身尺寸信息和規(guī)劃理想路徑信息，計(jì)算得maxεd=10 cm,maxεθ=12 rad,maxεc=0.038 4 m-1。

車輛跟蹤的理想路徑為圓心在全局坐標(biāo)系原點(diǎn)，半徑為25 m的圓形路徑，車輛起點(diǎn)坐標(biāo)為(35,10) m，車速選擇3 m/s,仿真時(shí)長設(shè)為100 s，仿真結(jié)果如圖12～16所示。圖12a為鉸接車跟蹤理想路徑效果圖，在仿真時(shí)間100 s中，行駛路徑與理想路徑基本吻合且路徑平滑，圖12b為路徑跟蹤局部放大圖，可看出，當(dāng)預(yù)見路徑的偏差為負(fù)，行駛路徑偏差為正時(shí)，行駛的路徑偏差有明顯的減小趨勢(shì)。反映了加入預(yù)見信息可補(bǔ)償固有反應(yīng)，加快反應(yīng)速度。圖13中車輛速度能在較短時(shí)間內(nèi)達(dá)到并維持在3 m/s，表明速度控制有效；圖14～16為路徑跟蹤偏差情況。

圖14所示為鉸接車路徑跟蹤控制中加入預(yù)見信息和未加入預(yù)見信息的跟蹤位置偏差效果圖，通過對(duì)比可看出基于預(yù)見信息的路徑跟蹤可以有效減小位置偏差的超調(diào)，提高反應(yīng)速度，橫向位置偏差最大偏差量不到0.2 m，最終偏差控制在0.03 m,相對(duì)于輪距2.3 m，誤差僅為1.3%，對(duì)于最終穩(wěn)態(tài)誤差雖然仍處于工作允許范圍內(nèi)，但仍有提高空間。

圖13 車速控制結(jié)果Fig.13 Speed control results

圖14 鉸接車路徑跟蹤位置偏差Fig.14 Position error of articulated vehicle

圖15 行駛方向偏差Fig.15 Deviation of driving direction

圖15所示為鉸接車路徑跟蹤控制中加入預(yù)見信息和未加入預(yù)見信息的跟蹤行駛方向偏差效果圖，行駛方向偏差最終穩(wěn)定在1.5×10-4rad，相比于鉸接車轉(zhuǎn)向角極值45°，誤差為0.19%，通過對(duì)比可看出基于預(yù)見信息的路徑跟蹤對(duì)于行駛方向偏差的校正十分明顯，加快了調(diào)節(jié)速度也減小了穩(wěn)態(tài)誤差，提高了車輛行駛的穩(wěn)定性。

圖17 USTB CYV-Ⅱ試驗(yàn)平臺(tái)尺寸明細(xì)(單位：mm)Fig.17 Test platform dimension details of USTB CYV-Ⅱ

圖16所示為鉸接車路徑跟蹤控制中加入預(yù)見信息和未加入預(yù)見信息的跟蹤曲率偏差效果圖，通過對(duì)比可看出基于預(yù)見信息的路徑跟蹤減小了曲率偏差的最大超調(diào)，且同樣快速將曲率偏差調(diào)節(jié)至0，波動(dòng)范圍小于0.003 m-1。仿真結(jié)果證明了提出的控制策略的有效性，試驗(yàn)中將進(jìn)一步驗(yàn)證。

圖16 曲率偏差Fig.16 Curvature deviation

4 試驗(yàn)

為進(jìn)一步驗(yàn)證本文中鉸接車路徑跟蹤控制策略的有效性，采用USTB CYV-Ⅱ作為試驗(yàn)平臺(tái)進(jìn)行試驗(yàn)分析，車體結(jié)構(gòu)和尺寸明細(xì)如圖17所示。USTB CYV-Ⅱ試驗(yàn)平臺(tái)的車體結(jié)構(gòu)與尺寸效仿鉸接式車輛結(jié)構(gòu)設(shè)計(jì)，運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)特性與鉸接式車輛相符，可作為鉸接式車輛路徑跟蹤控制策略的試驗(yàn)平臺(tái)進(jìn)行試驗(yàn)分析，試驗(yàn)場(chǎng)地選為樓道的部分區(qū)域，平面圖如圖18所示，跟蹤路徑選為有2個(gè)轉(zhuǎn)彎的路徑以充分驗(yàn)證控制系統(tǒng)性能。

USTB CYV-Ⅱ的行駛過程如圖19所示，由于地面過于光滑且轉(zhuǎn)彎半徑較小，為保證車輛良好跟蹤理想路徑，車速選為1 m/s勻速行駛。

圖18 試驗(yàn)區(qū)平面示意圖Fig.18 Test area plane

圖19 USTB CYV-Ⅱ的行駛過程Fig.19 Driving process of USTB CYV-Ⅱ

圖20 USTB CYV-Ⅱ跟蹤路徑Fig.20 Path tracking of USTB CYV-Ⅱ

圖21 跟蹤速度與鉸接角變化Fig.21 Change of tracking speed and articulation angle

圖22 跟蹤偏差Fig.22 Tracking error

跟蹤結(jié)果及跟蹤誤差如圖20所示，行駛路徑與理想路徑能夠較好的貼合，且行駛路徑較為平滑，車輛能夠很好地沿著理想路徑行駛，由局部放大圖可知，預(yù)見位姿往往會(huì)呈現(xiàn)與理想路徑間有更大的誤差，從而使車輛能夠得到一個(gè)更大的糾偏信號(hào)加快糾偏時(shí)間，使鉸接車沿著理想路徑行駛。圖21為行駛速度和鉸接角變化曲線，速度數(shù)據(jù)為控制器的輸出數(shù)據(jù)，停車時(shí)由于車輛慣性導(dǎo)致了車輛滑行；鉸接角變化平穩(wěn)，兩個(gè)彎道路段鉸接角變化趨勢(shì)相同，車輛具有較好的轉(zhuǎn)彎特性。圖22a為加入預(yù)見信息的位置偏差、行駛方向偏差和曲率偏差的變化情況，3個(gè)偏差均在直線行駛和曲線行駛時(shí)出現(xiàn)抖振，是由于車輛響應(yīng)速度較慢以及樓道地面較為光滑，車輛出現(xiàn)側(cè)滑導(dǎo)致，但仍符合鉸接車的實(shí)際運(yùn)行要求，在其他路段偏差均較小，其中，位置偏差基本維持在零值上下，波動(dòng)約±0.03 m，與仿真結(jié)果一致，行駛方向偏差基本收斂于零值上下，波動(dòng)約為±0.004 rad，曲率偏差基本在零值上下，最大值不超過0.003 m-1。試驗(yàn)中起始偏差量均較小是因?yàn)檐囕v在理想路徑起點(diǎn)開始行駛，而仿真中車輛起點(diǎn)不在理想路徑上。圖22b為未加入預(yù)見信息的跟蹤偏差結(jié)果，由圖22a和圖22b的對(duì)比可看出，加入預(yù)見信息減小了位置偏差和行駛方向偏差的超調(diào)，對(duì)于行駛方向偏差的改善效果十分明顯，對(duì)于曲率偏差的調(diào)節(jié)作用較小，與仿真結(jié)果吻合，加入預(yù)見信息對(duì)于路徑跟蹤控制有一定的調(diào)節(jié)作用。試驗(yàn)結(jié)果表明了本研究中的鉸接式車輛的路徑跟蹤控制策略的有效性，可用于鉸接車的路徑跟蹤。

5 結(jié)束語

提出了一種基于預(yù)見信息的鉸接式車輛LQR-GA路徑跟蹤控制方法，通過引入預(yù)見信息提高控制系統(tǒng)的反應(yīng)速度，加入柔化以防止控制量過大。同時(shí)，提出了一種基于跟蹤偏差的LQR-GA路徑跟蹤控制方法，GA算法對(duì)LQR中的狀態(tài)量權(quán)重矩陣進(jìn)行優(yōu)化且達(dá)到了最優(yōu)的控制效果。所提出的控制策略在仿真和試驗(yàn)中均取得了較好的控制效果，路徑的跟蹤位置偏差的誤差為1.3%，行駛方位偏差誤差為0.19%，曲率偏差基本維持在零值，極大地改善了鉸接車路徑跟蹤精度，試驗(yàn)結(jié)果符合車輛巷道行駛要求，可用于鉸接式車輛路徑跟蹤控制。

1 趙翾, 楊玨, 張文明,等. 農(nóng)用輪式鉸接車輛滑模軌跡跟蹤控制算法[J/OL]. 農(nóng)業(yè)工程學(xué)報(bào), 2015, 31(10):198-203.http:∥www.tcsae.org/nygcxb/ch/reader/view_abstract.aspx?file_no=20151026&flag=1. DOI:10.11975/j.issn.1002-6819.2015.10.026.

ZHAO Xuan, YANG Jue, ZHANG Wenming, et al. Sliding mode control algorithm for path tracking of articulated dump truck[J/OL]. Transactions of the CSAE, 2015, 31(10):198-203.(in Chinese)

2 HEMAMI A, POLOTSKI V. Path tracking control problem formulation of an LHD loader[J]. The International Journal of Robotics Research, 1998, 17(2): 193-199.

3 PETROV P, BIGRAS P. A practical approach to feedback path control for an articulated mining vehicle[C]∥2001 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2001, 4: 2258-2263.

4 BIGRAS P, PETROV P, WONG T. A LMI approach to feedback path control for an articulated mining vehicle[C]∥7th International Conference on Modeling and Simulation of Electric Machines, Converters and Systems (Electrimacs), 2002:1-6.

5 RIDLEY P, CORKE P. Load haul dump vehicle kinematics and control[J]. Journal of Dynamic Systems, Measurement, and Control, 2003, 125(1): 54-59.

6 NAYL T, NIKOLAKOPOULOS G, GUSTFSSON T. Switching model predictive control for an articulated vehicle under varying slip angle[C]∥2012 20th Mediterranean Conference on Control & Automation (MED), 2012: 890-895.

7 邵俊愷, 趙翾, 楊玨,等. 無人駕駛鉸接式車輛強(qiáng)化學(xué)習(xí)路徑跟蹤控制算法[J/OL]. 農(nóng)業(yè)機(jī)械學(xué)報(bào), 2017,48(3):376-382.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20170348&journal_id=jcsam. DOI: 10.6041/j.issn.1000-1298.2017.03.048.

SHAO Junkai,ZHAO Xuan,YANG Jue,et al.Reinforcement learning algorithm for path following control of articulated vehicle[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2017,48(3):376-382.(in Chinese)

8 OGATA K, YANG Y. Modern control engineering[M]. 4th ed. Upper Saddle River, NJ:Prentice Hall, 2002: 826-910.

9 SHIROMA N, ISHIKAWA S. Nonlinear straight path tracking control for an articulated steering type vehicle[C]∥ICCAS-SICE, 2009: 2206-2211.

10 POLOTSKI V, HEMAMI A. Control of articulated vehicle for mining applications: modeling and laboratory experiments[C]∥Proceedings of the 1997 IEEE International Conference on Control Applications, 1997: 318-323.

11 DRAGT B J, CRAIG I K, CAMISANI-CALZOLARI F R. Navigation of autonomous underground mine vehicles[C]∥1st African. Control Conference, 2003: 3-5.

12 王亮, 盧永錦. 引用預(yù)見信息的人機(jī)耦合優(yōu)化方法[J]. 北京航空航天大學(xué)學(xué)報(bào), 2008, 34(4):361-364.

WANG Liang, LU Yongjin. Optimal method of human-machine coupling with preview information[J].Journal of Beijing University of Aeronautics and Astronautics, 2008, 34(4):361-364.(in Chinese)

13 石峰, 戰(zhàn)凱, 顧洪樞,等. 地下鏟運(yùn)機(jī)跟蹤軌跡推算模型研究[J]. 有色金屬：礦山部分, 2010, 62(6):66-69.

SHI Feng, ZHAN Kai, GU Hongshu, et al. A study on underground loader tracking track calculation model[J].Nonferrous Metals: Mining Section, 2010, 62(6):66-69.(in Chinese)

14 DA FONSECA NETO J V, ABREU I S, DA SILVA F N. Neural-Genetic synthesis for state-space controllers based on linear quadratic regulator design for eigenstructure assignment[J]. IEEE Transactions on Systems, Man, and Cybernetics, Part B (Cybernetics), 2010, 40(2): 266-285.

15 GARGOURI L, ZAAFOURI A, KOCHBATI A, et al. LQG/LTR control of a direct current motor[C]∥ IEEE International Conference on Systems, Man and Cybernetics, 2002.

16 KO H S, LEE K Y, KIM H C. An intelligent based LQR controller design to power system stabilization[J]. Electric Power Systems Research, 2004, 71(1): 1-9.

17 WANG L, NI H, ZHOU W, et al. MBPOA-based LQR controller and its application to the double-parallel inverted pendulum system[J]. Engineering Applications of Artificial Intelligence, 2014, 36: 262-268.

18 NIKNEZHADI A, ALLUéFANTOVA M, KUNUSCH C, et al. Design and implementation of LQR/LQG strategies for oxygen stoichiometry control in PEM fuel cells based systems[J]. Journal of Power Sources, 2011, 196(9): 4277-4282.

19 HASANZADEH A, EDRINGTON C S, LIU Y, et al. An LQR based optimal tuning method for IMP-based VSI controller for electric vehicle traction drives[C]∥ Vehicle Power and Propulsion Conference (VPPC), 2011 IEEE, 2011: 1-7.

20 USTA M A, AKYAZI ?, AKPINAR A S. Aircraft roll control system using LQR and fuzzy logic controller[C]∥ 2011 International Symposium on Innovations in Intelligent Systems and Applications (INISTA), IEEE, 2011: 223-227.

21 TSAI S J, HUO C L, YANG Y K, et al. Variable feedback gain control design based on particle swarm optimizer for automatic fighter tracking problems[J]. Applied Soft Computing, 2013, 13(1): 58-75.

22 OHRI J. GA tuned LQR and PID controller for aircraft pitch control[C]∥ 2014 IEEE 6th India International Conference on Power Electronics (IICPE), 2014: 1-6.

23 GHAZI R, KHAJEH A. GA-based optimal LQR controller to improve LVRT capability of DFIG wind turbines[J]. Iranian Journal of Electrical and Electronic Engineering, 2013, 9(3): 167-176.

24 WONGSATHAN C, SIRIMA C. Application of GA to design LQR controller for an inverted pendulum system[C]∥ IEEE International Conference on Robotics and Biomimetics, ROBIO 2008. IEEE, 2009: 951-954.