包套圖，曾 岑

（1.渤海船舶職業(yè)學(xué)院，遼寧 葫蘆島 125000；2.大連理工大學(xué)，遼寧 大連 116024）

1 引言

汽車保有量的提高嚴(yán)重增加了交通擁堵和交通事故的概率，自動(dòng)駕駛技術(shù)的提出在一定程度上解決了由于飲酒、疲勞等人為因素導(dǎo)致的事故［1］。車輛運(yùn)動(dòng)控制是自動(dòng)駕駛的關(guān)鍵與末端技術(shù)，是自動(dòng)駕駛的主體行為［2］。因此，研究車輛運(yùn)動(dòng)控制技術(shù)對(duì)促進(jìn)自動(dòng)駕駛技術(shù)的成熟和落地具有重要意義。

車輛跟蹤控制一般解耦為縱向控制和橫向控制問(wèn)題，縱向控制用于消除縱向的速度偏差，橫向控制用于消除車輛質(zhì)心與期望軌跡點(diǎn)的位置偏差［3］。橫向控制是車輛跟蹤控制的核心問(wèn)題，分為無(wú)預(yù)瞄控制和預(yù)瞄控制兩個(gè)大方面。無(wú)預(yù)瞄控制中Stanley方法［4］最為經(jīng)典，它以前軸中心與參考軌跡最短距離為參考，計(jì)算前輪轉(zhuǎn)角。但是由于輪胎側(cè)偏的存在，使得橫向控制與橫向位移存在延遲，這種延遲在車速較高時(shí)會(huì)引起控制波動(dòng)和駕駛軌跡振蕩。預(yù)瞄控制分為單點(diǎn)預(yù)瞄和多點(diǎn)預(yù)瞄兩種情況［5］，單點(diǎn)預(yù)瞄在路徑前方選擇一個(gè)預(yù)瞄點(diǎn)，繪制一條當(dāng)前位置與預(yù)瞄點(diǎn)相切的軌跡并跟蹤。文獻(xiàn)［6］以加權(quán)方式將無(wú)預(yù)瞄和單點(diǎn)預(yù)瞄控制方法結(jié)合，實(shí)現(xiàn)了車輛在不同工況下的路徑跟蹤。多點(diǎn)預(yù)瞄是指在路徑點(diǎn)上選擇多個(gè)參考點(diǎn)，通過(guò)加權(quán)將各點(diǎn)橫向偏差進(jìn)行綜合的方法，常用的多點(diǎn)預(yù)瞄方法包括線性二次規(guī)劃和模型預(yù)測(cè)控制等。文獻(xiàn)［7］以自動(dòng)清掃車為研究對(duì)象，基于多點(diǎn)預(yù)瞄方法對(duì)車輛運(yùn)動(dòng)進(jìn)行控制，經(jīng)實(shí)驗(yàn)驗(yàn)證該方法能夠滿足清掃車輛控制精度需要。上述方法能夠得到較為精確的路徑跟蹤控制結(jié)果，但是控制過(guò)程中沒(méi)有關(guān)注能耗、終點(diǎn)控制精度等因素，因此有必要進(jìn)行進(jìn)一步研究。

針對(duì)智能車輛的路徑跟蹤控制問(wèn)題，建立了車輛的動(dòng)力學(xué)模型和單點(diǎn)預(yù)瞄控制系統(tǒng)模型，設(shè)計(jì)了分層協(xié)調(diào)最優(yōu)控制的路徑跟蹤方法，其中上層控制器基于系統(tǒng)狀態(tài)求出最優(yōu)控制律，下層控制器實(shí)現(xiàn)對(duì)最優(yōu)控制律的準(zhǔn)確跟蹤，最終實(shí)現(xiàn)了車輛在不同工況下對(duì)路徑高精度、低能耗的跟蹤控制。

2 車輛跟蹤系統(tǒng)建模

2.1 汽車動(dòng)力學(xué)建模

為了建立車輛的動(dòng)力學(xué)模型，首先作以下4點(diǎn)假設(shè)：

（1）忽略車輛的俯仰和滾動(dòng)運(yùn)動(dòng)；

（2）車輛前輪負(fù)責(zé)轉(zhuǎn)向，后輪沒(méi)有轉(zhuǎn)向能力；

（3）忽略車輛行駛過(guò)程中的空氣組合和坡道阻力；

（4）左右車輪均工作在線性區(qū)域，且特性相同。

在以上4點(diǎn)假設(shè)的前提下，車輛可以被簡(jiǎn)化為具有3個(gè)自由度的單軌模型，如圖1所示。

圖1 車輛簡(jiǎn)化模型Fig.1 Simplified Vehicle Model

圖中：OXY—道路坐標(biāo)系；（Xc，Yc）—車輛質(zhì)心在道路坐標(biāo)系的坐標(biāo)；oxyz—車輛坐標(biāo)系；x軸—車輛縱向前進(jìn)方向；z軸—天向；y軸—按右手準(zhǔn)則確定的方向；vx—車輛縱向速度；vy—車輛橫向速度；lf—車輛前軸距；lr—車輛后軸距；δf—前輪轉(zhuǎn)角；φ—車輛橫擺角。根據(jù)力平衡公式和力矩平衡公式，得到車輛動(dòng)力學(xué)模型為：

式中：m—車輛質(zhì)量；ax—車輛縱向加速度；Cαf、Cαr—前后輪側(cè)偏剛度；Iz—車輛繞Z軸的轉(zhuǎn)動(dòng)慣量。

2.2 預(yù)瞄模型距離設(shè)定

與非預(yù)瞄控制比，預(yù)瞄控制具有魯棒性好、控制精度高等優(yōu)點(diǎn)［8］，因此這里使用單點(diǎn)預(yù)瞄控制方式。單點(diǎn)預(yù)瞄控制模型，如圖2所示。

圖2 預(yù)瞄跟蹤控制模型Fig.2 Preview Tracking Control Model

圖中：L—預(yù)瞄距離；ye—橫向距離誤差；εe—方向誤差。

預(yù)瞄距離的設(shè)定對(duì)路徑跟蹤控制效果的影響具有一定規(guī)律性：當(dāng)預(yù)瞄距離較大時(shí)，路徑跟蹤系統(tǒng)的穩(wěn)定性較好；當(dāng)預(yù)瞄距離較小時(shí)，路徑跟蹤精度較高。為了兼顧系統(tǒng)的穩(wěn)定性和跟蹤精確性，經(jīng)過(guò)不斷實(shí)驗(yàn)和調(diào)整，將預(yù)瞄距離設(shè)置為橫向速度的分段函數(shù)，為：

2.3 預(yù)瞄模型及線性化

根據(jù)圖2所示的單點(diǎn)預(yù)瞄跟蹤控制示意圖，得到單預(yù)瞄點(diǎn)的跟蹤模型為：

式中：ωr—車輛橫擺角速度；KL—對(duì)應(yīng)于預(yù)瞄點(diǎn)的轉(zhuǎn)換系數(shù)。

結(jié)合式（1）和式（3），選擇路徑跟蹤控制系統(tǒng)的狀態(tài)量為X={φ，vx，vy，ye，εe}，控制量u={ax，δf}，可以得到系統(tǒng)狀態(tài)空間方程為：

由式（4）表達(dá)式可知，路徑跟蹤系統(tǒng)的狀態(tài)空間方程為非線性形式。在(X0(t)，u0(t))位置進(jìn)行泰勒展開(kāi)，得：

式中：A—系統(tǒng)矩陣；B—控制矩陣，且有：

3 分層協(xié)調(diào)最優(yōu)控制器設(shè)計(jì)

3.1 最優(yōu)控制器結(jié)構(gòu)

這里的控制目標(biāo)關(guān)注控制精度和能量消耗兩個(gè)方面，為了實(shí)現(xiàn)此控制目標(biāo)，設(shè)計(jì)了分層協(xié)調(diào)最優(yōu)控制器，包括上層控制器和下層控制器兩個(gè)控制器，如圖3所示。

圖3 最優(yōu)控制器結(jié)構(gòu)Fig.3 Structure of Optimal Controller

圖3 中虛線框內(nèi)為分層協(xié)調(diào)最優(yōu)控制器，上層控制器使用LQR最優(yōu)控制方法，得到控制精度、能量消耗綜合最優(yōu)意義下的控制量uop={ }arop，δfop。下層控制使用參數(shù)智能整定PID控制器，以剎車氣動(dòng)壓力Pb、油門節(jié)氣門開(kāi)度αth為控制量，實(shí)現(xiàn)車輛對(duì)最優(yōu)控制量uop={arop，δfop}的精確跟蹤。

3.2 LQR最優(yōu)控制器設(shè)計(jì)

按照LQR最優(yōu)控制器設(shè)計(jì)原理，目標(biāo)函數(shù)應(yīng)包括終端誤差、過(guò)程累積誤差、控制量約束等方面。但是考慮到路徑跟蹤控制過(guò)程可以視為無(wú)限時(shí)間的線性二次型系統(tǒng)，因此目標(biāo)函數(shù)設(shè)計(jì)為［9］：

式中：J—目標(biāo)函數(shù)；X—狀態(tài)量；U—控制量；t0—時(shí)間起點(diǎn)；Q—狀態(tài)量加權(quán)矩陣；R—控制量加權(quán)矩陣。

當(dāng)權(quán)值矩陣Q、R為單位對(duì)角陣時(shí)，各狀態(tài)量和控制量的重視程度一致。但是在車輛行駛過(guò)程中，當(dāng)車速較小時(shí)，車輛操縱穩(wěn)定性較好，此時(shí)應(yīng)更加關(guān)注車輛對(duì)給定路徑的跟蹤精度；當(dāng)車速較大時(shí)，車輛操縱穩(wěn)定性變差，此時(shí)應(yīng)降低對(duì)跟蹤精度的關(guān)注，適當(dāng)降低車速而提高車輛操作穩(wěn)定性和行駛安全性?；谏鲜隹紤]，將權(quán)值矩陣Q、R設(shè)置為車速v的函數(shù)，為：

3.3 參數(shù)智能整定PID控制器

對(duì)于下層控制器，使用參數(shù)智能整定PID控制器進(jìn)行設(shè)計(jì)?；贚QR得到的最優(yōu)控制量arop，得到車輛的縱向控制邏輯，如圖4所示。將車輛實(shí)際加速度計(jì)為ar，設(shè)計(jì)一個(gè)變量a?r=ar-arop，當(dāng)a?r≤0即實(shí)際加速度小于最優(yōu)加速度時(shí)，則使用發(fā)動(dòng)機(jī)提供動(dòng)力，進(jìn)一步增大加速度；當(dāng)a?r＞0即實(shí)際加速度大于最優(yōu)加速度時(shí)，則使用制動(dòng)器提供阻力，從而減小加速度。

圖4 縱向控制邏輯Fig.4 Vertical Control Logic

使用參數(shù)智能整定PID控制器，基于變量a?r得到發(fā)動(dòng)機(jī)節(jié)氣門開(kāi)度αth或剎車氣動(dòng)壓力Pb。由于兩個(gè)PID控制器的設(shè)計(jì)原理完全一致，因此以參數(shù)智能整定PID控制器1為例對(duì)控制器設(shè)計(jì)原理進(jìn)行介紹。參數(shù)智能整定PID控制器原理，如圖5所示。

圖5 參數(shù)智能整定PID控制器Fig.5 Parameters Intelligence Calibration PID Controller

基于PID控制的節(jié)氣門開(kāi)度αth計(jì)算方法為［10］：

式中：αth(t)—t時(shí)刻的節(jié)氣門開(kāi)度；kp—比例系數(shù)；ki—積分系數(shù)；kd—微分系數(shù)。

傳統(tǒng)PID控制器使用固定的比例系數(shù)、積分系數(shù)、微分系數(shù)，但是控制對(duì)象狀態(tài)和工況是實(shí)時(shí)變化的，固定的PID參數(shù)難以對(duì)被控對(duì)象具有實(shí)時(shí)適應(yīng)性。為了解決這一問(wèn)題，這里使用粒子群算法實(shí)時(shí)整定PID參數(shù)。粒子群算法原理較為簡(jiǎn)單，這里不再介紹，僅對(duì)粒子編碼方式和優(yōu)化目標(biāo)函數(shù)進(jìn)行明確。粒子使用十進(jìn)制3維度編碼方式，即粒子編碼為(kp，ki，kd)。優(yōu)化的目標(biāo)函數(shù)設(shè)置為：

式中：f—目標(biāo)函數(shù)；j—時(shí)刻；t—當(dāng)前時(shí)刻。式（9）表示優(yōu)化的目標(biāo)為車輛實(shí)際加速度與最優(yōu)加速度的累積誤差最小。按照上述設(shè)置方法，可以得到具有實(shí)時(shí)適應(yīng)性的PID控制器參數(shù)kp，ki，kd。

4 路徑跟蹤驗(yàn)證與分析

4.1 車輛參數(shù)設(shè)置

為了驗(yàn)證分層協(xié)調(diào)最優(yōu)控制器的路徑跟蹤控制性能，在Carsim環(huán)境中建立車輛模型，并基于Matlab搭建控制器模型，車輛參數(shù)，如表1所示。

表1 車輛參數(shù)Tab.1 Vehicle Parameters

車輛常用行駛工況為變道-直線行駛、小曲率轉(zhuǎn)彎、大曲率轉(zhuǎn)彎等工況，因此這里通過(guò)這3個(gè)工況對(duì)控制器進(jìn)行驗(yàn)證。

4.2 變道-直線跟蹤

給定一個(gè)直線路徑，如圖6所示。圖中實(shí)線為道路邊界線，虛線為道路中心線，該道路為同向雙車道路徑。黑色矩形表示行駛車輛，車輛行駛過(guò)程中發(fā)現(xiàn)前方障礙物，規(guī)劃并跟蹤避障路徑，避障路徑按照文獻(xiàn)［11］方法規(guī)劃得到，圖6中細(xì)實(shí)線為避障路徑，實(shí)心點(diǎn)為避障路徑結(jié)束點(diǎn)，避障結(jié)束后沿直線行駛。

圖6 變道-直線路徑Fig.6 Lane Change-Straight Path

分別使用這里分層協(xié)調(diào)最優(yōu)控制器（控制器1）、LQR最優(yōu)控制+傳統(tǒng)PID控制（控制器2）、文獻(xiàn)［12］的優(yōu)化控制方法（控制器3）進(jìn)行路徑跟蹤，統(tǒng)計(jì)3種控制器的最大橫向偏差，結(jié)果如下：控制器1跟蹤的最大橫向偏差為0.027m，控制器2跟蹤的最大橫向偏差為0.036m，控制器3跟蹤的最大橫向偏差為0.033m。

跟蹤誤差表明：控制器1、控制器2、控制器3在變道-直線行駛工況中，都能夠?qū)o定路徑進(jìn)行跟蹤。從跟蹤精度看，控制器1的跟蹤精度高于控制器2 和控制器3，這是因?yàn)榭刂破? 中的LQR最優(yōu)控制綜合考慮了控制精度和控制耗能，反應(yīng)在控制過(guò)程中，表現(xiàn)為控制精度更高、控制量幅值更小，因此控制器1的控制精度整體優(yōu)于控制器2和控制器3。

4.3 小曲率轉(zhuǎn)彎

給定一個(gè)U形軌跡，如圖7所示。圖中虛線為軌道的中心線，為本節(jié)跟蹤的路徑；虛線兩側(cè)的實(shí)線為道路邊界線。

圖7 U形路徑Fig.7 U-Shaped Path

分別使用控制器1、控制器2、控制器3進(jìn)行路徑跟蹤，其中控制器1和控制器2得到的最優(yōu)控制律一致，3種控制器得到的控制量，如圖8所示。

圖8 不同控制方法的控制量Fig.8 Control Quantity of Different Control Method

圖8（a）中，縱坐標(biāo)數(shù)值為正時(shí)表示發(fā)動(dòng)機(jī)節(jié)氣門開(kāi)度，數(shù)值為負(fù)時(shí)表示剎車氣動(dòng)壓力。

由圖8可以看出，這里分層協(xié)調(diào)最優(yōu)控制器得到的控制量在幅值上整體小于控制器3，這是因?yàn)槭褂肔QR最優(yōu)控制方法計(jì)算控制律時(shí)，平衡了控制精度和控制量能耗，因此控制律在整體幅值上小于控制器3的優(yōu)化控制方法。

統(tǒng)計(jì)控制器1、控制器2和控制器3對(duì)給定路徑跟蹤的最大橫向偏差和最大方向偏差結(jié)果，如表2所示。

表2 最大偏差Tab.2 Maximum Error

由表2種數(shù)據(jù)可知，控制器1的最大橫向誤差和最大方向誤差最小，表示控制器1的跟蹤控制精度最高，控制器3的跟蹤控制精度次之，控制器2的跟蹤控制精度最差。這是因?yàn)椋刂破?和控制器2相比，控制器2使用傳統(tǒng)PID控制，控制器1使用的是參數(shù)智能整定PID控制，參數(shù)智能整定PID控制能夠根據(jù)系統(tǒng)狀態(tài)實(shí)時(shí)調(diào)節(jié)PID參數(shù)，使PID控制器對(duì)系統(tǒng)狀態(tài)和工況具有實(shí)時(shí)適應(yīng)性，因此控制器1比控制器2的精度高。

綜合控制器1和控制器3，控制器1的控制精度好于控制器3，且控制律幅值也整體小于控制器3，這是因?yàn)榭刂破? 中的LQR最優(yōu)控制綜合考慮了控制精度和控制耗能，反應(yīng)在控制過(guò)程中，表現(xiàn)為控制精度更高、控制量幅值更小，因此控制器1的控制精度和幅值整體優(yōu)于控制器3。

4.4 大曲率轉(zhuǎn)彎

為了驗(yàn)證分層協(xié)調(diào)最優(yōu)控制器的魯棒性，設(shè)計(jì)一個(gè)具有連續(xù)轉(zhuǎn)彎、大曲率的復(fù)雜道路工況，如圖9所示。圖中實(shí)線為道路邊界線，虛線為道路中心線，為這里需要跟蹤的路徑。圖中矩形位置為路徑起點(diǎn)，箭頭方向?yàn)闄C(jī)器人前進(jìn)方向。

圖9 復(fù)雜道路工況Fig.9 Complex Road Conditions

分別使用控制器1和控制器3對(duì)復(fù)雜道路進(jìn)行跟蹤，其中控制器1能夠?qū)崿F(xiàn)對(duì)給定復(fù)雜路徑的跟蹤，而控制器3在第1個(gè)大曲率轉(zhuǎn)彎時(shí)對(duì)路徑跟蹤失敗，控制器1和控制器3的復(fù)雜路徑跟蹤結(jié)果，如圖10所示。圖10（a）中道路中心虛線為待跟蹤路徑，藍(lán)色實(shí)線為控制器1的實(shí)際跟蹤結(jié)果；圖10（b）中粗實(shí)線為控制器3的跟蹤結(jié)果。

圖10 兩種控制器的跟蹤結(jié)果Fig.10 Tracking Result of the Two Controllers

由圖10可以明顯看出，控制器1實(shí)現(xiàn)了對(duì)連續(xù)轉(zhuǎn)彎、大曲率等復(fù)雜道路的精確跟蹤，在道路轉(zhuǎn)彎處的跟蹤誤差略大于直線位置?？刂破?在第1個(gè)大曲率轉(zhuǎn)彎位置，實(shí)際跟蹤路徑就超出了道路邊界，說(shuō)明無(wú)法實(shí)現(xiàn)對(duì)大曲率轉(zhuǎn)彎路徑的跟蹤。經(jīng)統(tǒng)計(jì)，控制器1對(duì)復(fù)雜路徑跟蹤的最大橫向偏差為0.103m、最大方向偏差為1.987°，實(shí)驗(yàn)數(shù)據(jù)和實(shí)驗(yàn)結(jié)果表明，控制器1對(duì)復(fù)雜道路也具有較強(qiáng)的高精度跟蹤能力，即分層協(xié)調(diào)最優(yōu)控制器具有較強(qiáng)的跟蹤能力和較好的魯棒性。

5 結(jié)論

這里研究了無(wú)人車輛行駛路徑跟蹤控制問(wèn)題，建立了車輛動(dòng)力學(xué)模型和單點(diǎn)預(yù)瞄控制系統(tǒng)狀態(tài)空間方程，設(shè)計(jì)了分層協(xié)調(diào)最優(yōu)控制器用于路徑跟蹤。經(jīng)實(shí)驗(yàn)驗(yàn)證得出以下結(jié)論：（1）在變道跟蹤直線路徑和U型路徑轉(zhuǎn)彎中，分層協(xié)調(diào)最優(yōu)控制器、LQR最優(yōu)控制+傳統(tǒng)PID控制、文獻(xiàn)［12］優(yōu)化控制都能夠?qū)崿F(xiàn)路徑跟蹤；但是分層協(xié)調(diào)最優(yōu)控制器的跟蹤精度優(yōu)于LQR最優(yōu)控制+傳統(tǒng)PID控制、文獻(xiàn)［12］優(yōu)化控制；（2）對(duì)于具有連續(xù)轉(zhuǎn)彎、大曲率的復(fù)雜道路，分層協(xié)調(diào)最優(yōu)控制器實(shí)現(xiàn)了精度較高的路徑跟蹤控制，文獻(xiàn)［12］的優(yōu)化控制無(wú)法跟蹤上述復(fù)雜道路工況；（3）從變道跟蹤、U形轉(zhuǎn)彎和大曲率連續(xù)轉(zhuǎn)彎等工況可以看出，分層協(xié)調(diào)最優(yōu)控制器具有較強(qiáng)的跟蹤能力和較好的魯棒性。