張 勇，王同祥

(1.黃河交通學(xué)院，河南 焦作 454950; 2.鄭州宇通客車股份有限公司，河南 鄭州 450000)

0 引 言

半掛式車輛由于具備運貨效率高、能夠有效降低成本以及減少不必要的加工包裝環(huán)節(jié)等優(yōu)勢，在農(nóng)業(yè)機(jī)械、交通運輸?shù)阮I(lǐng)域的需求不斷增長[1]。然而，半掛式車輛的駕駛難度較大，容易導(dǎo)致駕駛員疲勞，成為影響其應(yīng)用推廣的瓶頸。

近年來，隨著計算機(jī)技術(shù)以及車輛控制技術(shù)的日益進(jìn)步，自主駕駛車輛技術(shù)迅速發(fā)展，并在商用車領(lǐng)域取得了長足進(jìn)步[2-3]。由于具備自主感知和理解環(huán)境、規(guī)劃和控制自身運動、完成預(yù)定任務(wù)的能力，相對于有人駕駛車輛，自主駕駛車輛可以降低或免除駕駛員的勞動強度，消除引起交通事故的潛在因素，成為汽車產(chǎn)業(yè)發(fā)展的一個重要趨勢[4]。因此研究半掛式車輛的自主駕駛對減輕駕駛員的工作量、改善交通安全具有重要意義。

針對半掛式車輛的軌跡跟蹤控制是實現(xiàn)其自主駕駛所面臨的重要挑戰(zhàn)之一。首先，相比于一般車輛，半掛式車輛的運動控制不僅需要考慮車輛的非完整性約束，還需要考慮到牽引車與拖車所構(gòu)成的欠驅(qū)動、非完整系統(tǒng)。其次，現(xiàn)有的慣性測量儀器成本較高，難以在商用車上大規(guī)模裝備，這導(dǎo)致半掛式車輛的航向難以直接測量，增加了其軌跡跟蹤的難度。最后，半掛式車輛的軌跡跟蹤控制還需要適應(yīng)不同的道路曲率。因此，研究半掛式車輛的軌跡跟蹤控制有顯著的理論價值。

目前，國內(nèi)外的學(xué)者在自主駕駛車輛的軌跡跟蹤控制領(lǐng)域做了大量的研究[4-9]。趙熙俊[5]和安林芳[6]等采用傳統(tǒng)的PID控制方法進(jìn)行車輛軌跡跟蹤，表現(xiàn)出較好的跟蹤能力，但是難以滿足復(fù)雜工況下的應(yīng)用；姜巖[7]等采用了預(yù)瞄方法進(jìn)行車輛的軌跡跟蹤研究，實車測試結(jié)果表明它對存在較大跟蹤偏差以及不連續(xù)性的軌跡具有較強的魯棒性，但是預(yù)瞄距離的自適應(yīng)選取仍是一個難點。游峰[8]等采用BackStepping算法設(shè)計自主駕駛車輛換道行為的軌跡跟蹤控制器。文獻(xiàn)[9-11]基于二次型最優(yōu)控制器解決車輛軌跡跟蹤問題，仿真結(jié)果表明由于車輛模型的線性化忽略了道路曲率影響，降低了其在曲率變化較大工況下的應(yīng)用。ZHANG[12]等在對車輛模型進(jìn)行迭代線性化的基礎(chǔ)上發(fā)展出迭代LQR方法，能夠降低車輛模型失配造成的問題。但是，上述方法不能有效適應(yīng)半掛式車輛的運動特性。

本文通過研究半掛式車輛轉(zhuǎn)向運動的特點，建立了運動學(xué)模型，并將其線性化得到軌跡跟蹤的線性誤差模型。針對半掛式車輛的航向難以測量的問題，基于擴(kuò)展卡爾曼濾波(EKF)設(shè)計了車輛狀態(tài)估計器。在對期望軌跡進(jìn)行自適應(yīng)擬合的基礎(chǔ)上，利用迭代LQR方法，通過使得在預(yù)測時域內(nèi)的軌跡跟蹤誤差最小化得到最優(yōu)的反饋控制序列，得到了針對半掛式車輛的軌跡跟蹤控制器，并在Simulink環(huán)境中進(jìn)行了仿真研究。

1 半掛式車輛建模

1.1 半掛式車輛的運動學(xué)模型

半掛式車輛的轉(zhuǎn)向過程如圖1所示。假設(shè)車輛在水平面內(nèi)做平面運動，且車輪與地面之間無相對滑動，大地坐標(biāo)系為OXY，X軸和Y軸分別指向正東方向和正北方向。

圖 1 半掛式車輛轉(zhuǎn)向過程示意圖

滿足車輛非完整約束的運動方程為

式中：(x,y)——半掛式車輛的后軸中心位置；

θ——半掛式車輛拖車與Y軸的夾角；

φ——牽引車與拖車車軸之間的夾角；

δf——牽引車的前輪轉(zhuǎn)向角；

lc,lt——牽引車前軸到聯(lián)結(jié)點的距離和拖車后軸到聯(lián)結(jié)點的距離；

v——車輛前輪中心的速度。

其中，f為連續(xù)可微的方程。

1.2 半掛式車輛路徑跟蹤的線性誤差模型

由于式(2)為非線性模型，這里相對于期望軌跡對其進(jìn)行線性化。假設(shè)期望軌跡表示為

將期望軌跡上的期望軌跡點(χr,ur)泰勒展開，并忽略高階部分，可得

對式(5)進(jìn)行離散化，可得到其離散線性時變模型：

2 基于EKF的狀態(tài)估計

由于成本的限制，半掛式車輛的航向角難以直接測量，針對這一問題，設(shè)計基于EKF的狀態(tài)觀測器?？紤]過程噪聲，則式(2)可以表示為：

其中，w=[w1w2w3w4]T為過程噪聲。

由于半掛式車輛后軸的中心坐標(biāo)(x,y)可以通過GPS測量，牽引車與拖車車軸之間的夾角 φ可以通過編碼器獲得，可以得到車輛狀態(tài)的測量方程

其中，Z=[z1z2z3]T為測量得到的后軸中心坐標(biāo)以及牽引車與拖車車軸之間的夾角。v=[v1v2v3]T為測量噪聲。假設(shè)wk和vk均為零均值的高斯白噪聲，則有

其中，Mk和Nk分別為wk和vk的協(xié)方差矩陣。

基于EKF的車輛狀態(tài)估計器實現(xiàn)步驟如下：

1）初始化

E——相應(yīng)矢量或矩陣的期望。

2）狀態(tài)更新

3）狀態(tài)校正

式中：Pk和Kk——協(xié)方差矩陣和估計增益；

I——單位矩陣。

3 半掛式車輛的軌跡跟蹤控制

半掛式車輛自主駕駛軌跡跟蹤可以描述為設(shè)計一個控制器，通過確定車輛控制輸入u=[vδf]T，使得車輛到達(dá)并跟隨給定的期望軌跡χr=[xryrθrφr]T。

在每一采樣時刻k根據(jù)車輛當(dāng)前位置和車速可以在期望軌跡上得到一系列的期望軌跡點，然后根據(jù)車輛的軌跡跟蹤誤差模型可以預(yù)測其在未來一段時域的狀態(tài)[13]。通過設(shè)計性能評價指標(biāo)并使其最小化，獲得最優(yōu)的控制量序列。最終半掛式車輛的軌跡跟蹤控制器只采用最優(yōu)控制序列中的第一個元素。半掛式車輛軌跡跟蹤的示意圖如圖2所示。

圖 2 半掛式車輛路徑跟蹤示意圖

3.1 基于三次Bezier曲線的自適應(yīng)擬合

為了得到軌跡跟蹤的前饋控制量，需要對離散的期望軌跡路點進(jìn)行擬合。三次Bezier曲線由于具備良好的擬合能力和極強的描述能力，非常適用于期望軌跡的平滑與擬合，其表達(dá)式為：

式中：q(τi)——參數(shù) τi處的插值點，τi∈ [0,1]；

C0和C3——期望軌跡上的控制點；

C1和C2——曲線擬合的中間點。

在自適應(yīng)擬合的初始化階段將期望軌跡的起止點作為三次Bezier曲線兩端的控制點。每次迭代擬合分別根據(jù)式（13）和式（14）求解各段Bezier曲線的中間點以及對應(yīng)的插值點，并計算插值點與原軌跡點之間的殘差，并在殘差最大的插值點所對應(yīng)的軌跡點處將期望軌跡分段，直到擬合精度滿足要求。

3.2 迭代LQR軌跡跟蹤控制算法

針對半掛式車輛的軌跡跟蹤控制，設(shè)計k時刻的優(yōu)化性能指標(biāo)為：

其中，Np表示預(yù)測時域，對角矩陣，t，R分 別為狀態(tài)誤差權(quán)重，終端誤差權(quán)重和控制量權(quán)重，其對角線上的元素表示了對相應(yīng)誤差分量的懲罰程度。

引入拉格朗日乘子λ，式(15)可改寫為最優(yōu)化方程V：

構(gòu)造哈密爾頓函數(shù)，令

結(jié)合式(17)和式(16)可得：

當(dāng)評價函數(shù)取最小值時，函數(shù)V需滿足：

結(jié)合式(15)和式(19)，根據(jù)已知的SNp=t，νNp=0和終端狀態(tài)(Np)，可以求得最優(yōu)反饋控制量：

最終得到半掛式車輛最優(yōu)控制控制量為：

其中，參考輸入矢量ur可以通過對期望軌跡進(jìn)行基于Beizer曲線的自適應(yīng)擬合獲得。

4 仿真系統(tǒng)設(shè)計與結(jié)果分析

4.1 仿真系統(tǒng)設(shè)計

在Simulink環(huán)境下搭建虛擬仿真平臺，通過軌跡跟蹤仿真實驗驗證本文所提出的跟蹤算法的有效性。軌跡跟蹤虛擬仿真平臺框架如圖3所示。

圖 3 軌跡跟蹤的仿真平臺框架

通過非線性車輛運動模型、測量得到的車輛狀態(tài)量和未來一定時間內(nèi)的控制量序列可以預(yù)測車輛的狀態(tài)。這是一個迭代的過程，由于控制序列不可知，就需要找到一個顯式的迭代方程對微分方程進(jìn)行近似求解。在工程實際中，使用較為廣泛的數(shù)值解法有歐拉法(Euler method)和4階龍格-庫塔(Runge-Kutta)算法。

單步的歐拉法形式簡單，但精度較低。為了得到較高的精度，本文采用4階龍格-庫塔算法，其一般表述形式為：

4階龍格-庫塔算法的迭代步長越小，其計算精度越高，但計算速度也會下降。

4.2 仿真結(jié)果分析

基于上節(jié)所述的虛擬仿真平臺，采用表1所示的參數(shù)進(jìn)行軌跡跟蹤仿真實驗，仿真所用的期望軌跡具有連續(xù)變化的曲率。

表 1 軌跡跟蹤控制器的參數(shù)說明

首先將擬合殘差的閾值β設(shè)為0.25 m，對期望軌跡進(jìn)行基于三次Bezier曲線的自適應(yīng)擬合，擬合結(jié)果如圖4(a)所示，可以看出期望軌跡被劃分為6段進(jìn)行擬合，體現(xiàn)了自適應(yīng)軌跡擬合算法的描述能力。期望軌跡的擬合殘差如圖4(b)所示，可以看出，擬合殘差小于0.25 m，滿足了滿足給定殘差閾值的要求。通過擬合曲線方程可以得到軌跡跟蹤的前饋控制量。

圖 4 基于三次Bezier曲線的參考軌跡擬合結(jié)果

半掛式車輛軌跡跟蹤的仿真結(jié)果如圖5(a)所示，圖5(b)為掛車中心的軌跡與期望軌跡對比，可以看出半掛式車輛能夠很好地適應(yīng)期望軌跡的曲率變化，到達(dá)并跟隨期望軌跡。

圖 5 軌跡跟蹤的仿真結(jié)果

軌跡跟蹤仿真過程中掛車的航向角及其與牽引車之間的夾角如圖6所示?？梢钥闯鰭燔嚭较蜃兓交?，且掛車與牽引車之間的夾角小于0.5 rad，能夠避免掛車與牽引車發(fā)生內(nèi)部碰撞。

圖 6 跟蹤過程中掛車運動狀態(tài)

在軌跡跟蹤過程中，掛車相對于期望軌跡的橫向跟蹤偏差和航向跟蹤偏差如圖7所示。可以看出，半掛式車輛自主駕駛的軌跡跟蹤控制器可使得掛車的橫向跟蹤偏差在±0.2 m以內(nèi)，航向跟蹤偏差保持在±0.15 rad以內(nèi)，具有較高的跟蹤精度。

圖 7 軌跡跟蹤過程的跟蹤偏差

軌跡跟蹤過程中的控制量，即車速與前輪偏角，如圖8所示?？梢钥闯?，在仿真過程中半掛式車輛的車速始終小于2 m/s，且車輛的前輪偏角控制量小于0.3 rad，滿足給定的速度與前輪偏角的閾值約束。

圖 8 軌跡跟蹤過程的控制量

從圖8還可以看出，當(dāng)仿真進(jìn)行到80 s之后，車輛緩慢下降到0 m/s，即車輛停止。這是由于當(dāng)車輛行駛80 s后已經(jīng)接近期望軌跡給定的終點所造成的，當(dāng)車輛到達(dá)終點后半掛式車輛停止運動。

軌跡跟蹤過程中基于EKF的狀態(tài)估計誤差如圖9所示?？梢钥闯觯霋焓杰囕v狀態(tài)觀測器對于車輛x和y坐標(biāo)的估計誤差在±0.2 m以內(nèi)，同時對于拖車航向角的估計誤差保持在-0.02～0 rad之間，滿足控制精度的要求。

圖 9 軌跡跟蹤過程的狀態(tài)估計誤差

5 結(jié)束語

本文建立了半掛式車輛的非線性運動學(xué)模型并將其線性化得到了車輛軌跡跟蹤的線性誤差模型。設(shè)計了基于EKF的車輛狀態(tài)觀測器，有效的估計拖車的航向角。通過對期望軌跡進(jìn)行基于三次Bezier曲線的自適應(yīng)擬合獲取了軌跡跟蹤的前饋控制量。針對半掛式車輛的軌跡跟蹤，設(shè)計了基于迭代LQR的控制器。仿真結(jié)果顯示本文所提出的軌跡跟蹤控制器能夠滿足半掛式車輛自主駕駛的需求。

下一步工作將深入研究半掛式車輛的動力學(xué)模型，并結(jié)合模型預(yù)測控制方法實現(xiàn)半掛式車輛在更高速度下的穩(wěn)定性操控。