基于模型預(yù)測控制的無人駕駛履帶車輛軌跡跟蹤方法研究

胡家銘， 胡宇輝， 陳慧巖， 劉凱

(北京理工大學(xué) 機械與車輛學(xué)院, 北京 100081)

0 引言

無人駕駛履帶車輛，作為未來高機動地面無人戰(zhàn)斗系統(tǒng)的一個重要組成部分，具有廣泛的應(yīng)用前景[1]。在無人駕駛履帶車輛研究中，軌跡跟蹤控制是其最重要的內(nèi)容之一，特別是在復(fù)雜環(huán)境下的高精度軌跡跟蹤是保證其行駛安全和完成相關(guān)任務(wù)的基礎(chǔ)[2]。然而，在實際軌跡跟蹤控制中，特別是在越野環(huán)境下，無人駕駛履帶車輛仍然面臨很多挑戰(zhàn)[3]。首先，履帶車輛是一個包含復(fù)雜非完整性約束的非線性耦合系統(tǒng)，而且履帶相對于地面的滑移、滑轉(zhuǎn)以及側(cè)滑運動難以準(zhǔn)確測量；其次，無人駕駛履帶車輛的軌跡跟蹤控制還受到行駛道路的環(huán)境約束以及執(zhí)行機構(gòu)的飽和約束；最后，無人駕駛履帶車輛高精度的軌跡跟蹤控制需要充分考慮建模誤差、參數(shù)擾動等系統(tǒng)不確定性以及外部干擾的影響。因此，研究無人駕駛履帶車輛的軌跡跟蹤控制具有重要的理論及現(xiàn)實意義。

目前，國內(nèi)外學(xué)者對履帶車輛的轉(zhuǎn)向過程做了大量研究。如文獻[4-8]建立了履帶與地面之間存在剪切作用關(guān)系的轉(zhuǎn)向過程模型，并通過實車轉(zhuǎn)向試驗驗證了所建立模型的準(zhǔn)確性。文獻[9]建立了基于瞬時轉(zhuǎn)向中心的履帶車輛運動學(xué)模型，并采用Levenberg-Marquardt算法進行滑動參數(shù)的實時估計，用于預(yù)測車輛行駛軌跡。文獻[10] 運用二次優(yōu)化理論和狀態(tài)反饋控制原理搭建車輛動力學(xué)模型和控制器。文獻[11]提出了基于非線性模型預(yù)測控制的滾動預(yù)測范圍估計方法。上述研究實現(xiàn)了對履帶車輛轉(zhuǎn)向過程的建模，并能夠為其轉(zhuǎn)向控制提供理論支持。

考慮到路網(wǎng)文件提供的參考路徑是一系列不包含曲率信息的離散路點這一情況，前期研究已經(jīng)證明忽略道路曲率或?qū)⑵湟暈槎ㄖ档穆窂礁櫡椒〞a(chǎn)生較大的跟蹤誤差，并導(dǎo)致系統(tǒng)振蕩[12]。因此，多項式曲線、樣條曲線和Bezier曲線等參數(shù)化的路徑擬合方法被廣泛地應(yīng)用于為路徑跟蹤提供的道路曲率信息[13-14]。然而，參數(shù)化的曲線擬合方法存在確定曲線階次的難題。一方面選擇較高的曲線階次雖然能夠?qū)崿F(xiàn)較好的擬合精度，但是增加了求解的復(fù)雜程度，加重了優(yōu)化計算量，從而影響路徑跟蹤算法的實時性；另一方面，選擇較低的曲線階次雖然能夠降低計算量，但是在擬合復(fù)雜路徑時容易產(chǎn)生較大的擬合誤差，進而增加無人駕駛車輛發(fā)生碰撞危險的可能性。在這種情況下，為了降低參考路徑的擬合誤差，通常將其分成多段進行擬合，但分段過多也會增加擬合的計算量。因此，需要研究一種參考路徑分段數(shù)目自適應(yīng)調(diào)整的參考路徑擬合方法。

針對無人駕駛履帶車輛的軌跡跟蹤問題，模型預(yù)測控制由于具備能夠系統(tǒng)地考慮模型非線性，且易于處理多約束優(yōu)化問題等優(yōu)勢，在無人駕駛車輛的運動規(guī)劃與控制等領(lǐng)域得到了廣泛的應(yīng)用[15]。然而，由于履帶車輛在實際轉(zhuǎn)向時，履帶相對于地面的滑移、滑轉(zhuǎn)及側(cè)滑運動受到行駛速度、轉(zhuǎn)彎半徑和地面條件等復(fù)雜因素的影響，很難準(zhǔn)確測量。同時，由于載荷變化、重心轉(zhuǎn)移等造成的模型參數(shù)改變以及外部干擾等不利因素增加了軌跡跟蹤控制的難度，甚至造成系統(tǒng)的不穩(wěn)定。

本文研究履帶車輛的滑動轉(zhuǎn)向特性，建立了基于瞬時轉(zhuǎn)向中心的履帶車輛運動學(xué)模型。針對參考路徑是離散的路點序列這一特點，提出了一種基于3次Bezier曲線的參考路徑自適應(yīng)擬合方法，在實現(xiàn)路徑平滑基礎(chǔ)上提供道路的曲率信息。設(shè)計了無人駕駛履帶車輛的模型預(yù)測軌跡跟蹤控制器，系統(tǒng)地處理無人駕駛履帶車輛建模誤差、環(huán)境約束以及執(zhí)行機構(gòu)約束。同時，在軌跡跟蹤控制器中引入了跟蹤誤差反饋校正因子，有效地抑制模型不確定性和外部干擾帶來的影響。

1 基于瞬時中心的履帶車輛運動學(xué)建模和控制系統(tǒng)問題描述

1.1 履帶車輛運動學(xué)模型

本文研究的無人駕駛履帶車輛由某型裝甲車改裝而成，是一種典型的差速轉(zhuǎn)向車輛，通過調(diào)節(jié)左、右兩側(cè)主動輪的轉(zhuǎn)速來控制航向和速度。履帶車輛左、右兩側(cè)主動輪各有一套獨立的動力傳動系統(tǒng)，由直流電機、兩擋位機械自動變速箱、制動系統(tǒng)和減速器等組成，如圖1所示。

圖1 無人駕駛履帶車輛的動力傳動系統(tǒng)Fig.1 Power transmission system of unmanned tracked vehicle

圖2 履帶車輛轉(zhuǎn)向過程的運動學(xué)關(guān)系Fig.2 Kinematics of tracked vehicle during steering

僅考慮無人駕駛履帶車輛在水平地面上的運動，令η=[xo,yo,ψ]T為系統(tǒng)狀態(tài)量，(xo,yo)表示履帶車輛幾何中心o在大地坐標(biāo)系下的位置?？紤]到履帶車輛滑動轉(zhuǎn)向的特性，即轉(zhuǎn)向過程始終伴隨著高速側(cè)履帶的滑轉(zhuǎn)與低速側(cè)履帶的滑移。針對這一問題，采用文獻[9]提出的基于瞬時轉(zhuǎn)向中心的履帶車輛運動學(xué)模型：

(1)

1.2 控制系統(tǒng)問題描述

ηe(k+1)=η(k)-ηd(k+1)+TsR(ψ)u(k),

(2)

式中：Ts為系統(tǒng)控制周期。

研究目標(biāo)可以表述為基于瞬時轉(zhuǎn)向中心的履帶車輛運動學(xué)模型(1)式和誤差跟蹤模型(2)式設(shè)計模型預(yù)測的軌跡跟蹤控制器，使得無人駕駛履帶車輛能夠有效地跟蹤參考軌跡ηd，且能抑制模型不確定性和外界干擾的影響。

為了充分利用參考路徑所提供的信息，得到更準(zhǔn)確的參考狀態(tài)量ηd，提出了一種基于3次Bezier曲線的參考路徑自適應(yīng)擬合方法。同時，結(jié)合基于瞬時轉(zhuǎn)向中心的履帶車輛運動學(xué)模型，設(shè)計了基于模型預(yù)測的軌跡跟蹤控制器。無人駕駛履帶車輛控制系統(tǒng)框圖如圖3所示，本文主要關(guān)注基于3次Bezier曲線的自適應(yīng)路徑擬合以及基于模型預(yù)測的軌跡跟蹤控制器兩個模塊。

圖3 無人駕駛履帶車輛控制系統(tǒng)框圖Fig.3 Block diagram of control system of unmanned tracked vehicle

2 自適應(yīng)參考路徑擬合

由于Bezier曲線具備控制簡便、具有極強的描述能力和易于生成復(fù)雜的平滑曲線等優(yōu)勢，因此常被用來進行參考路徑的平滑。Bezier曲線可以表示為

(3)

式中：m為Bezier曲線階次；q(τi)為參數(shù)τi處的插值點；Pk為第k個控制點。通過對參數(shù)τi在[0, 1]內(nèi)取值，可以得到在第1個控制點P0和最后1個控制點Pm之間生成任意個插值點。其中，3次Bezier曲線由于具備良好的擬合能力和簡單的表達形式等優(yōu)點，受到廣泛關(guān)注，其表達式為

(4)

式中：τi∈[0,1]。

為保證后續(xù)段曲線和前一段曲線在連接處的光滑(在連接點處切矢量方向相同，大小不等，即G1連續(xù))，必須使得擬合出來的相鄰兩段曲線在接點處的3個控制點共線，如圖4所示。圖4以3次Bezier曲線為例，Qk、Pk(k=0, 1, 2, 3)分別為前后兩段曲線的控制點，其中，P0、P3為端點，P1、P2為中間點。曲線G1連續(xù)要求：

(5)

式中：Gb為方向矢量；s為待定的標(biāo)量參數(shù)。

圖4 兩段3次Bezier曲線的G1連續(xù)Fig.4 G1 continuity of two third-order Bezier curves

由于參考路徑的曲線擬合不要求擬合曲線嚴格地通過各個離散路點，因此選擇最小二乘法擬合各段參考路徑的中間點。設(shè)pi為參考路徑給定的離散路點，則最小二次方程可以表示為

(6)

(7)

在此基礎(chǔ)上，基于3次Bezier曲線的自適應(yīng)參考路徑擬合流程如圖5所示。在路徑擬合的初始化階段將參考路徑的起止點作為3次Bezier曲線的第1個和最后1個控制點。在每次曲線擬合的迭代過程中，首先根據(jù)(7)式求解各段Bezier曲線中間控制點的位置。然后根據(jù)(4)式可以得到與原路徑點對應(yīng)的插值點，并計算插值點與原路徑點之間的殘差ρ. 若Bezier曲線中間插值點處的殘差大于人為設(shè)定的閾值β，即ρ≥β，則選擇在殘差最大的中間點對應(yīng)的路徑點處將參考路徑分段。這個過程不斷重復(fù)，直到擬合得到的各段曲線中間點處殘差滿足擬合精度ε的要求。

圖5 自適應(yīng)的參考路徑曲線擬合流程圖Fig.5 Flow chart of adaptive reference path curve fitting

3 基于模型預(yù)測的軌跡跟蹤控制器

3.1 軌跡跟蹤控制器設(shè)計

圖6 模型預(yù)測控制原理示意圖Fig.6 Shematic diagram of model predictive control

模型預(yù)測控制器的設(shè)計思想就是將實際控制問題轉(zhuǎn)化為帶約束條件的目標(biāo)函數(shù)，通過對該目標(biāo)函數(shù)進行最優(yōu)化求解得到未來一段時域內(nèi)的控制序列，然后將其中第1個元素施加到被控系統(tǒng)上，模型預(yù)測控制的基本原理如圖6所示。

控制過程中，始終存在一條參考路徑，如圖6中長虛線所示。圖6中，k表示時刻，以時刻k作為當(dāng)前時刻，ηc和uc分別表示實際執(zhí)行得到的狀態(tài)量和控制量與通過車輛模型預(yù)測得到的最優(yōu)狀態(tài)量和最優(yōu)控制量之間的誤差，ηp和up分別為上一控制周期預(yù)測得到的最優(yōu)狀態(tài)量和控制量,ηk和um分別為當(dāng)前控制周期狀態(tài)量的測量值和控制量實際執(zhí)行后的測量值，u*表示最優(yōu)控制序列?？刂破鹘Y(jié)合當(dāng)前的測量值ηk和um，預(yù)測系統(tǒng)未來一段時域內(nèi)[k,k+Np] 的輸出，Np為預(yù)測時域，如圖6中實線所示。通過求解滿足目標(biāo)函數(shù)以及各種約束的優(yōu)化問題，得到在預(yù)測時域內(nèi)一系列的控制序列u*，如圖6中的短虛線所示，將該控制序列的第一個元素u*(0)作為執(zhí)行控制量。當(dāng)來到下一個時刻k+1時，重復(fù)上述過程，滾動地完成帶約束的優(yōu)化問題，通過優(yōu)化問題求解工具進行求解[16]。

由于履帶車輛的滑轉(zhuǎn)率、滑移率很難測得所導(dǎo)致的建模誤差以及外部干擾等因素，在實際應(yīng)用中實際執(zhí)行得到的狀態(tài)量和通過車輛模型預(yù)測得到的最優(yōu)狀態(tài)量之間存在一定的誤差，即

(8)

為了補償這一誤差，將反饋校正因子引入基于模型預(yù)測控制的軌跡跟蹤控制器，如圖7所示。圖7中，虛線框表示的是模型預(yù)測控制器的主體，主要由履帶車輛線性誤差模型、系統(tǒng)約束以及目標(biāo)函數(shù)等組成。履帶車輛線性誤差模型由(2)式得到，控制器設(shè)計的主要內(nèi)容是確定合適的目標(biāo)函數(shù)和系統(tǒng)約束。

圖7 帶反饋校正的模型預(yù)測軌跡跟蹤控制器Fig.7 Model predictive trajectory tracking controller with feedback correction

3.2 約束條件

履帶車輛由直流電機通過傳動系統(tǒng)對左、右兩側(cè)的主動輪進行驅(qū)動，因此需要考慮軌跡跟蹤過程中的執(zhí)行器飽和約束，約束表達形式為

(9)

式中：umin(k)和umax(k)分別表示控制量的最小約束和最大約束。

另外，考慮參考路徑的邊界約束，對履帶車輛的狀態(tài)量進行約束

(10)

式中：ηmin(k)和ηmax(k)分別表示狀態(tài)量的最小約束和最大約束。

3.3 模型預(yù)測控制問題

目標(biāo)函數(shù)要能夠保證智能車輛快速且平穩(wěn)的追蹤上期望軌跡。因此，需要加入對系統(tǒng)狀態(tài)量的偏差和控制量的優(yōu)化，采用如下形式的目標(biāo)函數(shù)：

(11)

4 實車試驗

試驗主要針對以下兩個方面：基于3次Bezier曲線的自適應(yīng)參考路徑擬合算法的擬合效果；本文提出的基于模型預(yù)測的軌跡跟蹤控制器對參考軌跡跟蹤的跟蹤能力。

4.1 試驗平臺

無人駕駛履帶車輛平臺以及試驗路面如圖8所示。平臺安裝的數(shù)據(jù)采集系統(tǒng)主要包括在主動輪上的輪速傳感器、高精度差分全球定位系統(tǒng)(DGPS)、慣性測量單元。其中：輪速傳感器測量兩側(cè)主動輪轉(zhuǎn)速，經(jīng)換算可以得到左、右側(cè)履帶的線速度；DGPS 系統(tǒng)用來測量車輛的運動軌跡，能測量其在大地坐標(biāo)系下的坐標(biāo)值; 慣性測量單元用來測量車輛的航向。履帶車輛及基于模型預(yù)測的軌跡跟蹤控制器參數(shù)如表1所示。

表1 履帶車輛及模型預(yù)測軌跡跟蹤控制器的參數(shù)

4.2 自適應(yīng)參考路徑擬合算法效果

采集越野環(huán)境下的參考路徑以及經(jīng)過基于3次Bezier曲線的自適應(yīng)參考路徑擬合后的路徑如圖9(a)所示。

圖9 越野環(huán)境下參考路徑的自適應(yīng)擬合結(jié)果Fig.9 Adaptive fitting results of reference paths in off-road environment

由圖9(a)可以看出，擬合后得到的路徑與給定的參考路徑基本重合。圖9中+表示Bezier曲線的起點P0和終點P3，·表示Bezier曲線的中間控制點P1和P2. 通過自適應(yīng)參考路徑擬合可以看出，為滿足擬合精度閾值要求，該測試道路一共被劃分為12段，每段最后一個控制點(即終點)和下一段的起始控制點(即起點)重合。將大量路徑點通過少數(shù)控制點進行表征，使得該自適應(yīng)參考路徑擬合算法能夠以使用較少的控制點實現(xiàn)對參考路徑的準(zhǔn)確描述。圖9(b)顯示參考路徑的擬合殘差保持在0.25 m以下，滿足了曲線擬合精度β約束。根據(jù)擬合后得到的曲線方程可以計算出參考路徑切線方向和參考曲率，為更準(zhǔn)確的軌跡跟蹤提供了重要信息。

4.3 無人駕駛履帶車輛越野道路跟蹤測試

將本文所提出的無人駕駛履帶車輛軌跡跟蹤控制器用于實車越野環(huán)境測試，在試驗中無人駕駛履帶車輛能夠適應(yīng)各種復(fù)雜的越野環(huán)境，保持可靠的運行。路徑跟蹤結(jié)果如圖10所示。

圖10 無人駕駛履帶車輛越野環(huán)境下軌跡跟蹤結(jié)果Fig.10 Track tracking results of unmanned tracked vehicle in off-road environment

將本文所提出的無人駕駛履帶車輛軌跡跟蹤控制器用于實車越野環(huán)境測試。為了分析引入反饋校正對軌跡跟蹤器的影響，分別使用以下兩種控制器跟蹤圖10所示的參考路徑：

1)帶反饋校正的模型預(yù)測軌跡跟蹤控制器，記為控制器1；

2)不帶反饋校正的模型預(yù)測軌跡跟蹤控制器，記為控制器2.

在測試過程中，將期望車速設(shè)置為10 km/h，結(jié)果顯示無人駕駛履帶車輛能夠適應(yīng)各種復(fù)雜的越野環(huán)境，保持可靠的運行。

由圖10可以看出，本文提出的基于模型預(yù)測的軌跡跟蹤算法能夠很好地跟隨參考路徑。通過對比跟蹤結(jié)果，相比于未采用反饋校正的模型預(yù)測軌跡跟蹤控制器(控制器2)，采用反饋校正的軌跡跟蹤控制器(控制器1)能夠更好地跟蹤參考路徑，證明了引入反饋校正能夠在一定程度上抑制由建模誤差和外部干擾等因素所引起的跟蹤誤差，提高跟蹤精度。

圖10和圖11給出了無人駕駛履帶車輛越野環(huán)境下軌跡跟蹤結(jié)果，從中可以看出通過在模型預(yù)測控制器中加入狀態(tài)反饋，在求解時間無明顯提升的狀態(tài)下可有效地提高跟蹤精度。由表2可以看出，跟蹤誤差平均值在無狀態(tài)反饋時為0.527 4 m，有狀態(tài)反饋時為0.442 7 m，通過狀態(tài)反饋，跟蹤誤差平均值減少了16%. 從圖10(a)可見，尤其在轉(zhuǎn)彎工況下可快速進行路徑跟蹤。實車試驗證實該方法可應(yīng)用于實車測試，通過狀態(tài)反饋可有效提高跟蹤精度。

圖11 軌跡跟蹤計算時間Fig.11 Calculational time of trajectory tracking

表2 無人駕駛履帶車輛越野環(huán)境下軌跡跟蹤誤差統(tǒng)計

5 結(jié)論

1)本文建立了基于瞬時轉(zhuǎn)向中心的履帶車輛運動學(xué)模型，提出了一種基于3次Bezier曲線的參考路徑自適應(yīng)擬合方法，從而能夠充分利用參考路徑的曲率信息，實現(xiàn)更精確的軌跡跟蹤。

2)考慮履帶車輛的環(huán)境約束、執(zhí)行機構(gòu)飽和約束，設(shè)計了基于模型預(yù)測的軌跡跟蹤控制器，系統(tǒng)地處理車輛模型非線性以及多約束優(yōu)化問題。針對履帶車輛的建模誤差及外部干擾，引入了反饋校正因子進行抑制。實車測試證明，本文算法可以實時地在非線性模型和多約束下得到最優(yōu)運動軌跡和控制序列，并且能有效抑制建模誤差及外部干擾的影響。