基于預(yù)測(cè)模型的無(wú)人賽車路徑規(guī)劃算法研究*

龔國(guó)錚 鄭少武 鐘思祺 葉鳴 李巍華

（1.華南理工大學(xué)，廣州 510641；2.深圳裹動(dòng)智駕科技有限公司，深圳 518054；3.廣州華工機(jī)動(dòng)車檢測(cè)技術(shù)有限公司，廣州 510640）

主題詞：無(wú)人駕駛賽車 路徑規(guī)劃 模型預(yù)測(cè)控制 操縱穩(wěn)定性 最優(yōu)路徑

1 前言

保證無(wú)人駕駛車輛高速工況下的行駛安全性，是無(wú)人駕駛技術(shù)推廣的難點(diǎn)，而無(wú)人賽車更需要穩(wěn)定可靠的算法保證高速行駛安全性。無(wú)人賽車路徑規(guī)劃系統(tǒng)的首要目標(biāo)是在保證賽車穩(wěn)定性和安全性的前提下，充分利用輪胎極限以實(shí)現(xiàn)最小圈速，因此需要一種能夠在極限工況下兼顧賽車效率與安全性的路徑規(guī)劃算法，目前相關(guān)研究較少。

早期路徑規(guī)劃算法的研究以圖搜索算法、基于采樣的算法為主。圖搜索算法包括Dijkstra、A*等算法，基于采樣的算法以快速擴(kuò)展隨機(jī)樹（Rapidly Exploring Random Tree，RRT）為代表。目前，圖搜索算法構(gòu)建路網(wǎng)所需的時(shí)間較長(zhǎng)，而RRT 算法規(guī)劃路徑會(huì)導(dǎo)致隨機(jī)性和曲率不連續(xù)的問(wèn)題，基于最優(yōu)化理論的路徑規(guī)劃算法可有效解決這些問(wèn)題，因此近年來(lái)得到了廣泛應(yīng)用。

模型預(yù)測(cè)控制（Model Predictive Control，MPC）是廣泛應(yīng)用于無(wú)人駕駛車輛控制領(lǐng)域的算法，近年來(lái)也被用于路徑規(guī)劃，如無(wú)人駕駛避障。文獻(xiàn)[13]提出了一種基于MPC 的統(tǒng)一路徑規(guī)劃方法，該方法可以自動(dòng)確定車道保持、變道、匝道合并等多種不同的機(jī)動(dòng)模式并進(jìn)行統(tǒng)一的路徑規(guī)劃，但未考慮車輛的穩(wěn)定性約束。文獻(xiàn)[14]提出一種基于無(wú)源MPC 的方法來(lái)保持或改善在不確定性下路徑規(guī)劃的性能，通過(guò)引入無(wú)源約束提高路徑規(guī)劃穩(wěn)定性，但只控制了轉(zhuǎn)向，沒有考慮車速變化。文獻(xiàn)[15]提出了一種規(guī)劃控制耦合的方法，通過(guò)設(shè)計(jì)目標(biāo)函數(shù)和相關(guān)約束將規(guī)劃控制問(wèn)題轉(zhuǎn)化為最優(yōu)化問(wèn)題，但沒有考慮側(cè)偏角約束。上述方法大多針對(duì)乘用車工況，不適用于無(wú)人賽車，且除文獻(xiàn)[12]外，均未考慮行駛穩(wěn)定性約束，若直接用于無(wú)人賽車，將增加賽車在高速行駛或路面附著條件較差時(shí)失控的風(fēng)險(xiǎn)，此外，還需要考慮邊界約束使賽車保持在賽道內(nèi)行駛。

綜上，路徑規(guī)劃算法研究大多未綜合考慮邊界約束與車輛行駛穩(wěn)定性約束問(wèn)題，因此，本文面向無(wú)人駕駛電動(dòng)方程式賽車，提出一種基于預(yù)測(cè)模型的路徑規(guī)劃方法，將多種約束下的無(wú)人賽車路徑規(guī)劃問(wèn)題轉(zhuǎn)化為二次規(guī)劃問(wèn)題，基于預(yù)測(cè)模型完成問(wèn)題的求解，設(shè)計(jì)多組仿真測(cè)試，利用CarSim/Simulink 仿真驗(yàn)證算法性能，并與其他方法進(jìn)行對(duì)比。

2 車輛動(dòng)力學(xué)預(yù)測(cè)模型

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

復(fù)雜的車輛模型會(huì)降低算法效率，為兼顧效率與精度，使用自行車模型對(duì)賽車進(jìn)行建模，為簡(jiǎn)化計(jì)算，只考慮車輛的平面運(yùn)動(dòng)，忽略車輛的俯仰與側(cè)傾運(yùn)動(dòng)，建立如圖1所示的車輛動(dòng)力學(xué)模型，基本動(dòng)力學(xué)方程為：

圖1 車輛動(dòng)力學(xué)模型

式中，為車輛質(zhì)量；I為車輛繞軸的轉(zhuǎn)動(dòng)慣量；、分別為質(zhì)心到前軸和后軸的距離；()為全局坐標(biāo)系下質(zhì)心的坐標(biāo)；為質(zhì)心航向角；v、v分別為車輛坐標(biāo)系下質(zhì)心的縱向、橫向速度；為車輛橫擺角速度；為前輪轉(zhuǎn)角；、分別為前、后輪的側(cè)向力；F為車輛所受縱向力；=(-)為附加扭矩，由比例控制器建模；=δv/(+)為運(yùn)動(dòng)學(xué)理想目標(biāo)角速度；為比例控制系數(shù)，與底層控制器有關(guān)。

考慮賽車與路面的相互作用，選擇魔術(shù)公式輪胎模型對(duì)輪胎側(cè)向力進(jìn)行建模與計(jì)算；假設(shè)賽車動(dòng)力由直流電機(jī)提供，且為后輪驅(qū)動(dòng)，賽車縱向力F由直流電機(jī)模型及所受阻力建模。計(jì)算公式分別為：

2.2 預(yù)測(cè)模型構(gòu)建

為對(duì)車輛行為進(jìn)行預(yù)測(cè)，需要對(duì)此非線性動(dòng)力學(xué)模型進(jìn)行預(yù)處理，將其轉(zhuǎn)化為線性預(yù)測(cè)模型。定義系統(tǒng)狀態(tài)量=(X,Y,φ,v,v,ω)，系統(tǒng)輸入量=()，利用一階泰勒展開對(duì)式（1）車輛模型進(jìn)行線性化處理，接著利用零階保持器（Zero-Order Hold，ZOH）進(jìn)行離散化處理，車輛的動(dòng)力學(xué)模型可表示為：

3 模型預(yù)測(cè)路徑規(guī)劃算法

為設(shè)計(jì)無(wú)人賽車的路徑規(guī)劃算法，需要分析無(wú)人賽車的行駛目標(biāo)，通過(guò)目標(biāo)函數(shù)設(shè)計(jì)及施加約束，將路徑規(guī)劃問(wèn)題轉(zhuǎn)化為二次規(guī)劃（Quadratic Programming，QP）問(wèn)題并求解，得到滿足賽車行駛要求的路徑。

3.1 設(shè)計(jì)系統(tǒng)輸出量

設(shè)計(jì)系統(tǒng)輸出量是模型預(yù)測(cè)控制問(wèn)題的關(guān)鍵，首先假設(shè)所有道路中心線坐標(biāo)及對(duì)應(yīng)的道路寬度已知。

算法解決的是在中心線已知情況下的賽車路徑優(yōu)化問(wèn)題，首先對(duì)中心線建模，具體做法是對(duì)其進(jìn)行參數(shù)化處理，定義∈[0,]為參考路徑弧長(zhǎng)變量，其中為路徑總長(zhǎng)度，通過(guò)三次樣條插值可以將中心線坐標(biāo)(,)表示成關(guān)于的函數(shù)()和()，可進(jìn)一步求出中心線的方向角()。

令為道路起點(diǎn)到車輛最近中心線的弧長(zhǎng)，其計(jì)算公式為：

MPC 問(wèn)題的系統(tǒng)輸出量通常為橫向誤差與縱向誤差，但與的求解是非線性問(wèn)題，且需要涉及的求解，盡管與車輛當(dāng)前位置對(duì)應(yīng)的可通過(guò)求解式（6）獲得，但對(duì)于預(yù)測(cè)優(yōu)化問(wèn)題，難以利用（6）求出預(yù)測(cè)時(shí)域內(nèi)每個(gè)時(shí)刻的。

圖2 車輛橫、縱向誤差真實(shí)值與近似值的對(duì)比

3.2 線性時(shí)變模型預(yù)測(cè)局部規(guī)劃算法

3.2.1 目標(biāo)函數(shù)

對(duì)于系統(tǒng)輸出量，首先作以下設(shè)定：

設(shè)計(jì)目標(biāo)函數(shù)時(shí)，考慮到賽車在指定時(shí)間內(nèi)的行駛距離盡可能遠(yuǎn)，而可通過(guò)控制控制賽車的行駛距離，因此首先需要最大化，同時(shí)，為減少的估計(jì)誤差，需要最小化；此外，僅考慮行駛距離求解得到的軌跡過(guò)于曲折，并非理想軌跡，相較于平滑軌跡，其總體角速度較大。為得到平滑軌跡，將角速度作為懲罰項(xiàng)加入到目標(biāo)函數(shù)中，得到的目標(biāo)函數(shù)為：

式中，=(,)；q=(,)；、分別為橫向誤差和縱向誤差的權(quán)重，與MPC 算法不同，橫向誤差將被賦予較小的權(quán)重，使車輛能充分利用寬賽道的優(yōu)勢(shì)搜索出行駛距離最遠(yuǎn)的軌跡；V、Δu為預(yù)測(cè)時(shí)域內(nèi)第個(gè)控制量；ω為預(yù)測(cè)時(shí)域內(nèi)第個(gè)角速度，與上述分析相對(duì)應(yīng)，ω為為保證軌跡的平滑，防止軌跡變化不平緩而加入到目標(biāo)函數(shù)中的懲罰項(xiàng)；q為角速度的權(quán)重；為狀態(tài)量的權(quán)重；R為控制量的權(quán)重矩陣；、、q分別為第個(gè)狀態(tài)橫向誤差、縱向誤差、角速度的權(quán)重。

將式（9）代入式（10）中并化簡(jiǎn)，目標(biāo)函數(shù)可進(jìn)一步整理成標(biāo)準(zhǔn)二次型的形式：矩陣。

3.2.2 邊界約束

采用半空間約束的方法對(duì)賽車的行駛邊界進(jìn)行限制。假設(shè)預(yù)測(cè)時(shí)域內(nèi)第個(gè)點(diǎn)(X,Y)對(duì)應(yīng)的為，將邊界約束定義為左、右邊界點(diǎn)((),())、((),())切線組成的半空間，如圖3所示。

圖3 邊界約束

邊界約束可表示為：

式（12）的物理含義見圖3。在圖3a中，中心線右側(cè)是車輛預(yù)測(cè)軌跡，單個(gè)點(diǎn)對(duì)應(yīng)的邊界半空間約束是兩側(cè)邊界點(diǎn)的切線所包圍的區(qū)域。

3.2.3 橫、縱向行駛穩(wěn)定性組合約束

為避免車輛運(yùn)行過(guò)程中失去操縱穩(wěn)定性，基于橫向、縱向行駛穩(wěn)定性設(shè)計(jì)了線性行駛穩(wěn)定性組合約束，包括滑移穩(wěn)定性約束和輪胎極限約束。

3.2.3.1 滑移穩(wěn)定性約束

滑移穩(wěn)定性約束主要保證車輛橫向穩(wěn)定性，基于安全駕駛包絡(luò)，在橫向動(dòng)力學(xué)模型的v-相軌跡圖中劃出一個(gè)使車輛狀態(tài)收斂于平衡點(diǎn)的平行四邊形區(qū)域，如圖4所示?；品€(wěn)定性約束如下：

圖4 安全駕駛包絡(luò)范圍

圖4 中的①、③為最大橫擺角速度約束，對(duì)應(yīng)式（13）第1 個(gè)不等式；②、④為給定縱向速度的最大輪胎側(cè)偏角約束，對(duì)應(yīng)式（13）第2、第3個(gè)不等式，其中，第2個(gè)不等式約束是后輪側(cè)偏角的約束，第3個(gè)不等式約束是前輪側(cè)偏角約束，圖中只繪制了第2個(gè)不等式約束的情況。

3.2.3.2 輪胎極限約束

為了保持賽車的操作穩(wěn)定性，需將橫、縱向力約束在附著橢圓內(nèi)。因此，提出一種簡(jiǎn)化輪胎極限約束的方法，來(lái)約束整車橫、縱向加速度對(duì)輪胎力進(jìn)行限制：

式中，a、a分別為車輛質(zhì)心處的縱向、橫向加速度。

加速度矢量被限制在半徑為的摩擦圓中，如圖5所示。

圖5 加速度摩擦圓

為簡(jiǎn)化求解，將式（14）轉(zhuǎn)化為線性約束，在時(shí)刻求解縱向控制量時(shí)，將變量v、v、、視為常量，即第個(gè)預(yù)測(cè)時(shí)域采用估計(jì)值進(jìn)行替代，將輪胎極限約束轉(zhuǎn)化為只關(guān)于縱向控制量的線性約束：

3.2.4 構(gòu)建優(yōu)化問(wèn)題

通過(guò)目標(biāo)函數(shù)設(shè)計(jì)并綜合考慮多種約束，可以構(gòu)建如下最優(yōu)化問(wèn)題：

4 仿真分析

4.1 局部規(guī)劃算法仿真

表1 車輛模型參數(shù)

后軸側(cè)滑比前軸嚴(yán)重，因此設(shè)定前輪側(cè)偏角峰值大于后輪。算法使用的仿真路徑如圖6所示，設(shè)定仿真時(shí)間為200 s，更新頻率為500 Hz，預(yù)測(cè)時(shí)域=90 s，預(yù)測(cè)時(shí)域時(shí)間間隔=0.05 s。

圖6 仿真路徑

仿真結(jié)果如圖7～圖10 所示。由圖7 可以看出，高附著系數(shù)（=0.85）條件下，未加入約束的賽車在11 號(hào)彎道處出現(xiàn)了嚴(yán)重的后軸側(cè)滑，賽車因未能預(yù)知側(cè)滑危險(xiǎn)而導(dǎo)致失控，由圖10 可看出，此時(shí)賽車前、后輪側(cè)偏角出現(xiàn)較大的峰值，說(shuō)明賽車因過(guò)于追求速度而忽略了其穩(wěn)定性；由圖8 可看出，加入穩(wěn)定性約束后賽車的行駛速度降低，提高了行駛的穩(wěn)定性；圖9顯示，賽車行駛過(guò)程中充分利用了地面的附著力，加速度矢量基本在摩擦圓內(nèi)。由圖7a 可知，加入約束后賽車軌跡更加平滑，為保證車輪抓地力的穩(wěn)定，在各彎道處，賽車的轉(zhuǎn)彎半徑會(huì)適當(dāng)增大，使賽車順利過(guò)彎，更符合實(shí)際行駛狀況。

圖7 車輛行駛軌跡

圖8 行駛速度對(duì)比

中等附著系數(shù)（=0.50）和低附著系數(shù)（=0.30）條件下，未加入約束的賽車在多個(gè)彎道處（如5號(hào)彎道）出現(xiàn)不同程度的后軸側(cè)滑，由圖9 可知，算法均高估了地面所能提供的附著力，使得賽車在過(guò)彎時(shí)地面附著力不足，因此出現(xiàn)一定程度的前后軸側(cè)滑，圖10中的側(cè)偏角峰值也說(shuō)明了這一問(wèn)題。另一個(gè)需要注意的問(wèn)題是這2種工況下11號(hào)彎道并沒有出現(xiàn)如圖7a一樣嚴(yán)重的側(cè)滑，原因是預(yù)測(cè)模型在計(jì)算輪胎側(cè)偏力時(shí)也會(huì)考慮附著系數(shù)，為保證邊界約束，在中低附著系數(shù)時(shí)賽車過(guò)11號(hào)彎道不會(huì)加速到很高的速度。而高附著系數(shù)（=0.85）時(shí)因?yàn)樗惴ǜ吖赖孛娓街Φ淖饔?，同時(shí)未能通過(guò)預(yù)測(cè)模型預(yù)知可能的側(cè)滑失控危險(xiǎn)，導(dǎo)致在11 號(hào)彎道處加速到較高的車速后無(wú)法及時(shí)減速；加入行駛穩(wěn)定性約束后雖然降低了行駛速度（圖8），但是保證了賽車在邊界范圍內(nèi)穩(wěn)定行駛，由圖7c 可看出，在8～10 號(hào)彎道處賽車出現(xiàn)了駛出賽道的情況，若在實(shí)際行駛中可能已經(jīng)發(fā)生了碰撞，但加入約束后的結(jié)果顯示賽車行駛狀況良好，且能夠有效利用地面附著力（圖9）。由于本文算法并非車輛控制算法且未以最小化橫向控制誤差作為優(yōu)化目標(biāo)，因此沒有定量比較不同地面附著條件下的橫向誤差。

圖9 質(zhì)心加速度aX-aY散點(diǎn)圖

圖10 前、后輪側(cè)偏角對(duì)比

由圖10可看出，在3種路面附著條件下，側(cè)偏角約束使前、后輪側(cè)偏角，尤其是后輪側(cè)偏角大幅減小，避免了前、后軸側(cè)滑的風(fēng)險(xiǎn)，前輪側(cè)偏角總體位于[-0.2,0.2]rad區(qū)間內(nèi)，后輪側(cè)偏角總體位于[-0.1,0.1]rad區(qū)間內(nèi)，符合滑移穩(wěn)定性約束。設(shè)置前輪側(cè)偏角大于后輪側(cè)偏角主要有兩方面原因：首先是后軸側(cè)滑會(huì)引發(fā)更大的危險(xiǎn)，車輛容易因甩尾而引發(fā)失控；其次是實(shí)踐中發(fā)現(xiàn)，如果前輪側(cè)偏角設(shè)置得過(guò)小，優(yōu)化結(jié)果不能滿足約束條件，難以求解，同時(shí)，約束前輪側(cè)偏角會(huì)間接約束前輪轉(zhuǎn)角，過(guò)大的約束會(huì)出現(xiàn)轉(zhuǎn)彎困難的問(wèn)題。側(cè)偏角在[-0.1,0.1]rad區(qū)間時(shí)，輪胎側(cè)向力處于線性區(qū)域，對(duì)結(jié)果進(jìn)行統(tǒng)計(jì)分析發(fā)現(xiàn)，后輪側(cè)偏角基本位于該區(qū)間內(nèi)，前輪側(cè)偏角90%以上也位于該區(qū)間內(nèi)，側(cè)向力位于線性區(qū)域內(nèi)可減小輪胎模型線性化處理后的精度損失。

對(duì)于路徑規(guī)劃算法，求解時(shí)間是算法評(píng)估的另一個(gè)重要指標(biāo)，算法使用開源求解器HPIPM 求解，算法平均單次二次規(guī)劃的求解時(shí)間為46.4 ms，其求解時(shí)間直方圖如圖11 所示，可知算法在90%以上情況下能在85.5 ms 內(nèi)給出結(jié)果，表明求解時(shí)間不會(huì)出現(xiàn)較大的波動(dòng)，算法穩(wěn)定性良好，且具有較高的計(jì)算效率。

圖11 求解時(shí)間直方圖

4.2 方法對(duì)比

為說(shuō)明本文方法的優(yōu)勢(shì)，與2 種不同方法進(jìn)行對(duì)比。首先與傳統(tǒng)方法RRT*進(jìn)行對(duì)比，為驗(yàn)證算法在復(fù)雜環(huán)境下的性能，選取圖6 中的1 號(hào)U 型彎道進(jìn)行算法對(duì)比，本文方法參數(shù)設(shè)置與上一節(jié)相同，附著系數(shù)=0.85，對(duì)比結(jié)果如圖12所示。

由圖12可看出，本文算法較RRT*算法規(guī)劃的路徑更加平滑，為進(jìn)行量化評(píng)估，對(duì)所規(guī)劃的路徑進(jìn)行曲率等指標(biāo)的對(duì)比，結(jié)果如表2所示。由表2可知，在已知相同賽道信息的情況下，本文算法能以更短的路徑駛過(guò)更遠(yuǎn)的中心線距離，表內(nèi)的通過(guò)時(shí)間是指車輛在已知速度規(guī)劃結(jié)果的情況下駛完規(guī)劃路徑所需的時(shí)間，為進(jìn)行公平對(duì)比，均使用文獻(xiàn)[22]所提出的速度規(guī)劃算法。

表2 數(shù)據(jù)顯示，在規(guī)劃起始點(diǎn)相同的情況下，本文算法的規(guī)劃路徑更加平滑，數(shù)值上表現(xiàn)為最大曲率和平均曲率均較小，因而經(jīng)過(guò)速度規(guī)劃，算法能夠得到更高的平均速度，以更短的時(shí)間到達(dá)路徑的終點(diǎn)。表內(nèi)的求解時(shí)間是算法10 次運(yùn)行的平均時(shí)間，可以看出，在求解時(shí)間上RRT*算法比本文算法更具優(yōu)勢(shì)，因?yàn)楸疚乃惴ㄐ枰蠼舛我?guī)劃問(wèn)題，且參照序列二次規(guī)劃（Sequential Quadratic Programming，SQP）算法進(jìn)行循環(huán)優(yōu)化，本文共進(jìn)行了5 次迭代優(yōu)化，平均每次求解時(shí)間為25 ms 左右，也符合圖11 的分布，算法最終收斂到圖12b的結(jié)果。

圖12 本文算法與RRT*算法的對(duì)比結(jié)果

表2 本文算法與RRT*算法規(guī)劃路徑的指標(biāo)對(duì)比

此外，還與同樣基于最優(yōu)化理論的文獻(xiàn)[8]所提出的方法進(jìn)行了對(duì)比，結(jié)果如圖13所示。

圖13b 是在CarSim 上利用本文方法得到的實(shí)際行駛軌跡，參數(shù)設(shè)置同上一小節(jié)，附著系數(shù)=0.85。由圖13b 可看出，在多個(gè)U 型彎道處，本文算法能夠充分利用車輛動(dòng)力學(xué)模型與賽道寬度信息，使賽車轉(zhuǎn)彎半徑盡可能大以保持較高的車速。表3顯示了2種方法在性能上的差異。

圖13 本文算法與文獻(xiàn)[8]所提出的方法的對(duì)比

表3中的圈速是指車輛通過(guò)賽道一圈所用時(shí)間，最優(yōu)二次軌跡規(guī)劃算法圈速也是經(jīng)文獻(xiàn)[22]的速度規(guī)劃算法后求得的，為使計(jì)算結(jié)果更接近真實(shí)值，限制規(guī)劃速度的最大值與本文算法在CarSim仿真過(guò)程中能達(dá)到的最大值相等。由于所得的圈速只是理論值，所以對(duì)比方法的圈速必然比表中的60.747 6 s 長(zhǎng)，也比本文算法在仿真過(guò)程的實(shí)際圈速57.765 2 s長(zhǎng)，同時(shí)，本文算法在路徑曲率上的數(shù)據(jù)更小，因此更具優(yōu)勢(shì)。此外，由于最優(yōu)二次軌跡規(guī)劃算法是全局規(guī)劃的算法，求解時(shí)間比本文算法平均單次求解時(shí)間要長(zhǎng)。

表3 本文算法與文獻(xiàn)[8]所提出的方法規(guī)劃路徑的指標(biāo)對(duì)比

5 結(jié)束語(yǔ)

本文提出了一種基于預(yù)測(cè)模型的路徑規(guī)劃算法，充分考慮賽車的行駛穩(wěn)定性，解決了高速行駛時(shí)無(wú)人賽車在不同路面附著條件下的路徑規(guī)劃問(wèn)題。仿真驗(yàn)證結(jié)果表明：在不同附著條件下，所提出的算法均能使賽車在賽道內(nèi)穩(wěn)定行駛；與未加入穩(wěn)定性約束的算法相比，該算法可以有效減小車輛前、后軸側(cè)滑的風(fēng)險(xiǎn)，且求解時(shí)間能夠滿足實(shí)時(shí)性要求；將本文方法與傳統(tǒng)方法和基于最優(yōu)化理論的方法進(jìn)行對(duì)比，結(jié)果顯示，本文方法在道路曲率、通過(guò)時(shí)間、圈速上均優(yōu)于對(duì)比方法，證明了其優(yōu)越性。