龔國錚 鄭少武 鐘思祺 葉鳴 李巍華

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

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

1 前言

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

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

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

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

2 車輛動力學預測模型

2.1 車輛動力學建模

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

圖1 車輛動力學模型

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

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

2.2 預測模型構(gòu)建

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

3 模型預測路徑規(guī)劃算法

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

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

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

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

令為道路起點到車輛最近中心線的弧長，其計算公式為：

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

圖2 車輛橫、縱向誤差真實值與近似值的對比

3.2 線性時變模型預測局部規(guī)劃算法

3.2.1 目標函數(shù)

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

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

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

將式（9）代入式（10）中并化簡，目標函數(shù)可進一步整理成標準二次型的形式：矩陣。

3.2.2 邊界約束

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

圖3 邊界約束

邊界約束可表示為：

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

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

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

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

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

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

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

3.2.3.2 輪胎極限約束

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

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

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

圖5 加速度摩擦圓

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

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

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

4 仿真分析

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

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

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

圖6 仿真路徑

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

圖7 車輛行駛軌跡

圖8 行駛速度對比

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

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

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

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

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

圖11 求解時間直方圖

4.2 方法對比

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

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

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

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

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

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

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

圖13 本文算法與文獻[8]所提出的方法的對比

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

表3 本文算法與文獻[8]所提出的方法規(guī)劃路徑的指標對比

5 結(jié)束語

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