田圣哲，賀藝斌，竇勝月

智能車軌跡跟蹤控制方法研究

田圣哲，賀藝斌，竇勝月

（長(zhǎng)安大學(xué)汽車學(xué)院，陜西 西安 710064）

隨著信息化的發(fā)展，智能駕駛技術(shù)應(yīng)用前景廣闊。軌跡跟蹤控制作為智能駕駛技術(shù)中的一環(huán)，受到越來越多的關(guān)注。文章介紹了智能汽車的運(yùn)動(dòng)學(xué)以及動(dòng)力學(xué)模型，詳細(xì)闡述了目前常用的幾種跟蹤控制方法的理論推導(dǎo)，分析了其各自的適用場(chǎng)景及優(yōu)缺點(diǎn)。從而可知，無論哪一種控制方法都有其弊端，建模方法以及行駛速度的不同，都會(huì)影響控制效果。因此，需要結(jié)合實(shí)際情況進(jìn)行改進(jìn)，使智能汽車的控制更有適用性。

智能駕駛；跟蹤控制；純跟蹤；模型預(yù)測(cè)

引言

智能駕駛研究的重點(diǎn)之一就是軌跡跟蹤控制，國內(nèi)外學(xué)者對(duì)此進(jìn)行了深入的研究。本文介紹的軌跡跟蹤控制方法有純跟蹤控制法、前輪反饋控制、MPC（模型預(yù)測(cè)控制）還有LQR（線性二次調(diào)節(jié)器）控制法。前面兩者都是建立在汽車運(yùn)動(dòng)幾何模型的一種控制，而后面兩者可以在汽車動(dòng)力學(xué)模型的基礎(chǔ)上進(jìn)行控制。不同的控制方法有不同的適用場(chǎng)景，例如純跟蹤和前輪反饋控制算法比較適用于中低速場(chǎng)景，有較好的跟蹤效果。因此，掌握不同控制算法的特點(diǎn)，具有重要的意義[1]。

1 汽車運(yùn)動(dòng)學(xué)模型

汽車運(yùn)動(dòng)學(xué)模型如下圖所示，在大地坐標(biāo)系下，（X,Y）和（X,Y）分別為車輛前后軸軸心坐標(biāo)，為車體的航向角，δ為前輪轉(zhuǎn)角，v和v代表前后軸中心速度。車輛的運(yùn)動(dòng)學(xué)模型為：

圖1 汽車運(yùn)動(dòng)學(xué)模型

2 汽車動(dòng)力學(xué)模型

汽車動(dòng)力學(xué)模型就是從力學(xué)的角度來分析作用于目標(biāo)自身的力和運(yùn)動(dòng)之間的關(guān)系，在描述動(dòng)力學(xué)的過程中，我們盡可能簡(jiǎn)化，以減少控制算法的計(jì)算量。在此經(jīng)簡(jiǎn)化后汽車具有三個(gè)方向的運(yùn)動(dòng)，分別為沿軸的縱向運(yùn)動(dòng)，沿軸的橫向運(yùn)動(dòng)和繞軸橫擺運(yùn)動(dòng)，即汽車三自由度動(dòng)力學(xué)模型?？梢越⑾聢D所示的汽車單軌模型：

圖2 汽車單軌動(dòng)力學(xué)模型

根據(jù)牛頓第二定律，分別得到沿軸，軸和繞軸的受力平衡方程。

3 純跟蹤算法

純跟蹤控制是根據(jù)車輛的幾何模型來建立的，只有在低速下才能滿足車輛的實(shí)時(shí)響應(yīng)，不適合在高速工況來進(jìn)行跟蹤控制。該算法以車后軸為切點(diǎn)，在參考路徑上匹配一個(gè)預(yù)瞄點(diǎn)，如下圖點(diǎn)，假設(shè)車輛后輪中心點(diǎn)沿著一定的轉(zhuǎn)彎半徑行駛，然后根據(jù)三角幾何關(guān)系來確定前輪轉(zhuǎn)角的大小。

圖3 純跟蹤幾何模型

圖中l表示車輛后軸位置到預(yù)瞄點(diǎn)的距離，表示車身姿態(tài)和目標(biāo)路點(diǎn)的夾角，在△中，根據(jù)正弦定理有：

在阿克曼轉(zhuǎn)向關(guān)系△中，

聯(lián)立上式有：

此外，定義e為車輛當(dāng)前狀態(tài)和預(yù)瞄點(diǎn)在橫向上的誤差：

e=lsin（6）

聯(lián)立（3）和（4），考慮小角度假設(shè)：

所以可看出純跟蹤控制器是一個(gè)橫向轉(zhuǎn)角控制器，受參數(shù)l響較大，通常l被認(rèn)為是車速的關(guān)系，定義為關(guān)于速度的一次多項(xiàng)式。

4 前輪反饋控制

前輪反饋控制（Front wheel feedback）也稱Stanley控制，其核心思想是利用前輪中心的路徑跟蹤偏差量對(duì)方向盤轉(zhuǎn)向控制量進(jìn)行計(jì)算[2]。Stanley算法是一種非線性控制算法，依據(jù)前輪中心控制點(diǎn)到參考軌跡目標(biāo)點(diǎn)P2的側(cè)向位移誤差e和航向角誤差e來進(jìn)行設(shè)計(jì)的。該算法可以實(shí)現(xiàn)橫向跟蹤誤差指數(shù)收斂于0。

圖4 stanley算法幾何模型

如上圖4所示，可以根據(jù)車輛的位姿與給定的參考軌跡的相對(duì)幾何關(guān)系來獲得車輛方向盤轉(zhuǎn)角的控制變量。

不考慮橫向跟蹤誤差情況下，前輪偏角應(yīng)當(dāng)與給定路徑的切線方向一致。即有δ=θ，其中θ是車輛航向與最近路徑線之間的夾角。

不考慮航向跟蹤誤差情況下，橫向跟蹤誤差越大，前輪轉(zhuǎn)向角越大。假設(shè)車輛預(yù)期軌跡在前輪處與給定路徑上最近點(diǎn)相切線相交，由圖中幾何關(guān)系可得到如下非線性比例函數(shù)：

式中為車速，為增益系數(shù)，所以前輪轉(zhuǎn)角為：

有文獻(xiàn)表明，和純跟蹤算法相比較，stanley算法更適用于相對(duì)較高車速的工況，但道路曲線不光滑時(shí)容易出現(xiàn)車輛響應(yīng)超調(diào)過大的問題。該算法不適用用于倒車的場(chǎng)景，自動(dòng)泊車等不適用。

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

模型預(yù)測(cè)控制（Model Predictive Control）是一種比較先進(jìn)的過程控制方法。其處理多目標(biāo)約束條件問題較強(qiáng)，成為智能駕駛領(lǐng)域研究問題的熱點(diǎn)[3]。模型預(yù)測(cè)控制的實(shí)現(xiàn)依賴于過程的動(dòng)態(tài)線性模型，既可以對(duì)當(dāng)前時(shí)態(tài)進(jìn)行優(yōu)化，又考慮未來時(shí)刻，反復(fù)優(yōu)化，考慮整個(gè)時(shí)域內(nèi)的優(yōu)化求解。

模型預(yù)測(cè)控制的實(shí)現(xiàn)主要有三個(gè)步驟，分別是預(yù)測(cè)模型、滾動(dòng)優(yōu)化和反饋矯正[4]。

圖5 模型預(yù)測(cè)控制算法原理示意圖

（1）預(yù)測(cè)模型：預(yù)測(cè)模型作為模型預(yù)測(cè)控制的基礎(chǔ)，可以用來描述系統(tǒng)的動(dòng)態(tài)行為。預(yù)測(cè)模型的形式?jīng)]有較高的要求，通常狀態(tài)方程、傳遞函數(shù)都可以作為預(yù)測(cè)模型，對(duì)于無人駕駛汽車，預(yù)測(cè)模型一般是車輛動(dòng)力學(xué)模型或者運(yùn)動(dòng)學(xué)模型。

（2）滾動(dòng)優(yōu)化：模型預(yù)測(cè)控制的滾動(dòng)優(yōu)化也叫在線優(yōu)化，對(duì)系統(tǒng)的目標(biāo)函數(shù)進(jìn)行反復(fù)求解，這也是模型預(yù)測(cè)控制獨(dú)有的特點(diǎn)。

（3）反饋校正：反饋校正也稱誤差校正。模型失配與環(huán)境不符等因素可能引起失控，需要計(jì)算出系統(tǒng)輸出值和模型預(yù)估值來得到誤差，從而通過反饋校正來進(jìn)行修正，進(jìn)行新的優(yōu)化。

智能車的模型預(yù)測(cè)控制通常需要先進(jìn)行運(yùn)動(dòng)學(xué)建模或者動(dòng)力學(xué)建模，再加入自身的一些約束進(jìn)行控制，從而提高目標(biāo)車輛行駛的穩(wěn)定性。車輛建模得到的是一個(gè)非線性模型，如果進(jìn)行線性控制，還需要進(jìn)行線性化，再進(jìn)行離散化處理。

一般離散化的模型：

可以得到一個(gè)新的狀態(tài)空間表達(dá)式：

各矩陣的定義如下：

進(jìn)一步簡(jiǎn)化，做如下假設(shè)：

設(shè)定系統(tǒng)的預(yù)測(cè)時(shí)域?yàn)?i>N，控制時(shí)域?yàn)?i>N，預(yù)測(cè)時(shí)域內(nèi)的狀態(tài)量和系統(tǒng)輸出量可用以下式子計(jì)算：

最終，系統(tǒng)輸出矩陣為：

由上式可看出預(yù)測(cè)時(shí)域內(nèi)的狀態(tài)量和輸出量都可以根據(jù)系統(tǒng)當(dāng)前時(shí)刻的狀態(tài)量和控制時(shí)域內(nèi)的控制量來計(jì)算得到。

在實(shí)際中，需要設(shè)定合適的優(yōu)化目標(biāo)并進(jìn)行求解，得到控制時(shí)域內(nèi)控制序列。模型預(yù)測(cè)控制中再把優(yōu)化目標(biāo)函數(shù)經(jīng)過處理轉(zhuǎn)化為二次規(guī)劃問題進(jìn)行求解。在控制系統(tǒng)中，加入滿足系統(tǒng)狀態(tài)的量以及控制量的約束條件，這樣就形成了一個(gè)完整的表達(dá)式。在反饋機(jī)制中，可以得到一系列控制增量，系統(tǒng)執(zhí)行此控制量到下一時(shí)刻。新的時(shí)刻，系統(tǒng)根據(jù)狀態(tài)信息不斷更新來預(yù)測(cè)下一段時(shí)域輸出。

模型預(yù)測(cè)控制模型建立相對(duì)方便，可以處理多變量，有約束的復(fù)雜問題，魯棒性好。

模型預(yù)測(cè)控制現(xiàn)在被廣泛的應(yīng)用到軌跡規(guī)劃中，考慮了車輛動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)特性，提高了跟蹤控制的執(zhí)行力。但是在實(shí)車測(cè)試中，車輛的約束維度較多時(shí)，會(huì)使得計(jì)算量偏大，尤其速度增加時(shí)，求解量增加，實(shí)時(shí)性較差。

6 LQR控制

LQR（線性二次調(diào)節(jié)器）控制是現(xiàn)代控制理論中最成熟的一種狀態(tài)空間設(shè)計(jì)法。LQR控制算法可得到狀態(tài)線性最優(yōu)控制規(guī)律，使控制系統(tǒng)達(dá)到最優(yōu)[5]。

線性二次型問題，都是對(duì)線性系統(tǒng)確定一個(gè)最優(yōu)反饋控制率*()=－()使得下式性能指標(biāo)最?。?/p>

其中為狀態(tài)權(quán)重矩陣，為半正定矩陣，為控制量權(quán)重系數(shù)，為正定矩陣。在使用中，需要調(diào)節(jié)矩陣和矩陣來使控制效果最優(yōu)。

為了求出，假設(shè)存在常量矩陣，在系統(tǒng)穩(wěn)定條件下有：

= (+BPB)－1BPA（19）

矩陣同時(shí)滿足黎卡提方程：

=+APA－APB(+BPB)－1BPA （20）

求解黎卡提方程得到矩陣，經(jīng)過一系列的迭代求出符合要求的最優(yōu)解。

在二自由度動(dòng)力學(xué)模型中，LQR算法一般需要調(diào)節(jié)的參數(shù)更多，不僅要獲得車輛自身的模型參數(shù)，還需要調(diào)節(jié)目標(biāo)函數(shù)的，矩陣。在汽車不同的車速下，、的不同會(huì)對(duì)算法的效果有重要的影響，所以為了獲得較優(yōu)的跟蹤性能，、的選取十分重要。但LQR控制的優(yōu)點(diǎn)也比較突出，通過加上前饋轉(zhuǎn)角，可以有效解決曲線行駛時(shí)的穩(wěn)態(tài)跟蹤誤差，尤其在中等速度行駛時(shí)，跟蹤效果更好。

7 結(jié)論

各種算法在使用場(chǎng)景上都有其局限性。純跟蹤算法是一種基于車輛運(yùn)動(dòng)學(xué)模型的控制方法，基于對(duì)前輪轉(zhuǎn)角進(jìn)行控制來消除橫向誤差，主要適用于低速工況。與純跟蹤算法相比較，Stanley算法考慮了橫擺角誤差，跟蹤效果相對(duì)較佳。模型預(yù)測(cè)控制是現(xiàn)在應(yīng)用最流行的控制算法，是自動(dòng)駕駛軌跡跟蹤領(lǐng)域的研究重點(diǎn)。MPC和LQR控制方法中系統(tǒng)建模、內(nèi)部權(quán)重矩陣、仿真工況等的不同對(duì)最終結(jié)果都有影響。各種方法都有優(yōu)缺點(diǎn)，在實(shí)際過程中，綜合使用可能更有效果。

[1] 陳慧巖,陳舒平,龔建偉.智能汽車橫向控制方法研究綜述[J].兵工學(xué)報(bào),2017,38(06):1203-1214.

[2] 熊璐,楊興,卓桂榮,等.無人駕駛車輛的運(yùn)動(dòng)控制發(fā)展現(xiàn)狀綜述[J].機(jī)械工程學(xué)報(bào),2020,56(10):127-143.

[3] 龔建偉,姜巖,徐威.無人駕駛車輛模型預(yù)測(cè)控制[M].北京:北京理工大學(xué)出版社,2014:35-50.

[4] 吳欣欣.基于模型預(yù)測(cè)控制的智能車輛軌跡跟蹤控制方法研究[D].合肥:合肥工業(yè)大學(xué),2020.

[5] 高琳琳,唐風(fēng)敏,郭蓬,等.自動(dòng)駕駛橫向運(yùn)動(dòng)控制的改進(jìn)LQR方法研究[J].機(jī)械科學(xué)與技術(shù),2021,40(03):435-441.

Research on Tracking Control Method of Intelligent Vehicle

TIAN Shengzhe, HE Yibin, DOU Shengyue

( College of Automobile, Chang’an University, Shaanxi Xi’an 710064 )

With the development of informatization, the application prospect of intelligent driving techn- ology is broad. As a part of intelligent driving technology, trajectory tracking control has

more and more attention. This article introduces the kinematics and dynamics models of smart cars, elaborates the theoretical derivation of several commonly used tracking control methods, and analyzes their respec- tive applicable scenarios and advantages and disadvantages. It can be seen that no matter which control method has its drawbacks, the difference in modeling method and driving speed will affect the control effect. Therefore, it is necessary to make improvements in combination with the actual situation to make the control of smart cars more applicable.

Intelligent driving; Tracking control; Pure pursuit; Model prediction

A

1671-7988(2022)01-36-04

U495

A

1671-7988(2022)01-36-04

CLC NO.: U495

田圣哲，碩士研究生，就讀于長(zhǎng)安大學(xué)汽車學(xué)院，研究方向：自動(dòng)駕駛軌跡規(guī)劃及控制。

10.16638/j.cnki.1671-7988.2022.001.009