半掛式車輛自主駕駛的軌跡跟蹤控制研究

張 勇，王同祥

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

0 引 言

半掛式車輛由于具備運貨效率高、能夠有效降低成本以及減少不必要的加工包裝環節等優勢，在農業機械、交通運輸等領域的需求不斷增長[1]。然而，半掛式車輛的駕駛難度較大，容易導致駕駛員疲勞，成為影響其應用推廣的瓶頸。

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

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

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

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

1 半掛式車輛建模

1.1 半掛式車輛的運動學模型

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

圖 1 半掛式車輛轉向過程示意圖

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

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

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

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

δf——牽引車的前輪轉向角；

lc,lt——牽引車前軸到聯結點的距離和拖車后軸到聯結點的距離；

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

其中，f為連續可微的方程。

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

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

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

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

2 基于EKF的狀態估計

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

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

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

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

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

基于EKF的車輛狀態估計器實現步驟如下：

1）初始化

E——相應矢量或矩陣的期望。

2）狀態更新

3）狀態校正

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

I——單位矩陣。

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

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

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

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

3.1 基于三次Bezier曲線的自適應擬合

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

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

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

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

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

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

針對半掛式車輛的軌跡跟蹤控制，設計k時刻的優化性能指標為：

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

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

構造哈密爾頓函數，令

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

當評價函數取最小值時，函數V需滿足：

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

最終得到半掛式車輛最優控制控制量為：

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

4 仿真系統設計與結果分析

4.1 仿真系統設計

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

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

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

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

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

4.2 仿真結果分析

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

表 1 軌跡跟蹤控制器的參數說明

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

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

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

圖 5 軌跡跟蹤的仿真結果

軌跡跟蹤仿真過程中掛車的航向角及其與牽引車之間的夾角如圖6所示。可以看出掛車航向變化平滑，且掛車與牽引車之間的夾角小于0.5 rad，能夠避免掛車與牽引車發生內部碰撞。

圖 6 跟蹤過程中掛車運動狀態

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

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

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

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

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

軌跡跟蹤過程中基于EKF的狀態估計誤差如圖9所示。可以看出，半掛式車輛狀態觀測器對于車輛x和y坐標的估計誤差在±0.2 m以內，同時對于拖車航向角的估計誤差保持在-0.02～0 rad之間，滿足控制精度的要求。

圖 9 軌跡跟蹤過程的狀態估計誤差

5 結束語

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

下一步工作將深入研究半掛式車輛的動力學模型，并結合模型預測控制方法實現半掛式車輛在更高速度下的穩定性操控。