基于預測模型的無人賽車路徑規劃算法研究*

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

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

主題詞：無人駕駛賽車 路徑規劃 模型預測控制 操縱穩定性 最優路徑

1 前言

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

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

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

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

2 車輛動力學預測模型

2.1 車輛動力學建模

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

圖1 車輛動力學模型

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

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

2.2 預測模型構建

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

3 模型預測路徑規劃算法

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

3.1 設計系統輸出量

設計系統輸出量是模型預測控制問題的關鍵，首先假設所有道路中心線坐標及對應的道路寬度已知。

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

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

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

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

3.2 線性時變模型預測局部規劃算法

3.2.1 目標函數

對于系統輸出量，首先作以下設定：

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

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

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

3.2.2 邊界約束

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

圖3 邊界約束

邊界約束可表示為：

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

3.2.3 橫、縱向行駛穩定性組合約束

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

3.2.3.1 滑移穩定性約束

滑移穩定性約束主要保證車輛橫向穩定性，基于安全駕駛包絡，在橫向動力學模型的v-相軌跡圖中劃出一個使車輛狀態收斂于平衡點的平行四邊形區域，如圖4所示。滑移穩定性約束如下：

圖4 安全駕駛包絡范圍

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

3.2.3.2 輪胎極限約束

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

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

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

圖5 加速度摩擦圓

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

3.2.4 構建優化問題

通過目標函數設計并綜合考慮多種約束，可以構建如下最優化問題：

4 仿真分析

4.1 局部規劃算法仿真

表1 車輛模型參數

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

圖6 仿真路徑

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

圖7 車輛行駛軌跡

圖8 行駛速度對比

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

圖9 質心加速度aX-aY散點圖

圖10 前、后輪側偏角對比

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

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

圖11 求解時間直方圖

4.2 方法對比

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

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

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

圖12 本文算法與RRT*算法的對比結果

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

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

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

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

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

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

5 結束語

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