基于貝塞爾曲線的智能汽車避障軌跡規劃方法研究*

曹昌盛，曹昊天

（1.國電南瑞南京控制系統有限公司，江蘇 南京 210000；2.湖南大學 機械與運載工程學院，湖南 長沙 410082）

0 引言

近年來，智能汽車因其能夠提高通行效率并減少交通事故而受到研究人員的廣泛關注[1-2]。目前，百度、谷歌和特斯拉等公司在智能駕駛領域已經取得了一些研究成果，并開始進行路測，但由于技術的復雜性和法規的限制，智能汽車離真正的商業化應用還有一段距離[3]。智能汽車主要包括環境感知、行為決策、軌跡規劃和運動控制四大模塊，其中軌跡規劃作為智能汽車的核心技術也涌現了大量研究成果[1,4]。局部軌跡規劃是利用全局路徑信息和車載傳感器信息為智能汽車實時規劃安全舒適的行駛軌跡（包括參考路徑和速度）。

目前常見的局部軌跡規劃算法有基于圖搜索的方法、人工勢場法、直接優化法和曲線插值法等。A*、Dijkstra 和IWD 算法是典型的基于圖搜索的方法，通過將駕駛場景劃分成網格形式，并用某些單元表示障礙物，然后在當前位置和目標位置之間搜索最優軌跡[5-7]。這類方法的計算代價會隨著搜索空間的增加而急劇增加，因此，多用于封閉低速場景。人工勢場法因其可方便地通過設置斥力函數來處理障礙物約束而被廣泛應用于軌跡規劃，但其采用梯度下降法確定行駛路徑，易陷入局部最優解，進而導致路徑的震蕩[8]。為了克服這個缺點，曹昊天等提出了一種基于調和速度勢的主動避障方法[9-10]。然而，基于勢函數的方法只能生成無碰撞的路徑，而不能保證路徑的長度和曲率是最優的，這將可能影響到路徑的跟隨精度。Li 和Saravanakumar 等[11-13]在Frénet 坐標系使用直接優化法進行了局部軌跡規劃，但他們使用一般的曲線生成軌跡很難在節點處保持連續曲率，這可能會影響駕駛的穩定性和舒適性。

綜上分析，本文將利用貝塞爾曲線的曲率處處連續的優點并結合直接優化法，探討一種基于四階貝塞爾曲線的軌跡規劃算法。路徑規劃時，將以路徑的長度、曲率、連續性和車輛碰撞風險為代價函數，其中引入危險勢場理論，描述車輛碰撞危險程度；速度規劃時，將以行車效率和舒適性為規劃目標進行速度規劃，該方法可通過調整各子目標函數的權重來滿足不同駕駛需求。

1 道路模型建立

為了軌跡規劃時能適應直道和彎道情形，本文使用以車道中心線弧長為參數的三次多項式來描述車道線中心線，在Frénet 坐標系中描述道路上的任意點。Frénet 坐標系將道路中心線定義為基線，將橫向偏移定義為與基線的垂直距離，允許車輛在與基線平行的道路上行駛。假設道路信息由感知系統準確獲得，則車道線中心線可表示為：

式中，xc(s)和yc(s)分別為道路中心線在x和y方向的參數方程。Frénet 坐標系中，道路上任意一點與車道中心線的關系示意如圖1 所示。

由圖1 可知，道路上任意一點與道路中心線的關系為：

式中，d為路面上任意點到車道線中心線對應點切線的距離。由式（1）和式（2）可知路面上任意點的笛卡爾坐標(x,y)可映射為Frénet 坐標(s,d)。θ為車道線中心線航向角，由式（3）確定：

將式（3）代入式（2）可得：

車道中心線的航向角和曲率為：

2 軌跡規劃算法

2.1 規劃路徑生成方法

2.1.1 貝塞爾曲線及其性質

如圖2 所示，一條由兩個端點P0和P4，3 個控制點所唯一確定的四階貝塞爾曲線，通過調整中間3 個控制點的位置即可得到滿足舒適性和安全性需求的路徑。四階貝塞爾曲線可表示為：

式中：Pi表示各控制點，為伯恩斯坦多項式，其表達式為：

貝塞爾曲線的主要性質有：

（1）貝塞爾曲線必經過P0和P4兩端點，即：

（3）曲線在P0處的曲率為：

（4）貝塞爾曲線具有放射變換不變的特性。該性質結合性質（3）有助于生成一條曲率處處連續的路徑。

2.1.2 貝塞爾曲線控制點推導

局部路徑規劃就是生成一條引導智能汽車從初始狀態到達目標狀態的路徑。車輛初始狀態X0=(x0,y0,φ0,κ0)和目標狀態XT=(xT,yT,φT)都包含位置、航向角及曲率信息。根據貝塞爾曲線性（1）可知，曲線兩個端點P0和P4分別為車輛初始位置和目標位置。為簡化計 算，將車輛 初始狀 態旋轉 平移至X0=(0,0,0,κ0)。如圖2 所示，在X′OY′坐標系中，車輛初始位置即P0處的航向角為0，由性質（2）可得控制點P1的坐標表達式為：

式中，a為P0和P1兩點間的距離。

此外，當已知P0與P1的坐標時，由性質（3）可進一步求得控制點P2的坐標為：

且由貝塞爾曲線仿射變換不變性可知，旋轉平移變換并不會改變曲線特性，結合性質（2）可得控制點P4和P3的坐標表達式：

式中，c為P3和P4間的距離。

由上述分析可知，四階貝塞爾曲線的未知參數僅剩a、b和c，通過設置合理的目標函數，求解對應的優化問題可得到最優路徑。

2.2 規劃路徑優化機制

為了適應不同形狀道路，令：

式中，s0和sp分別為規劃時刻的初始距離和規劃距離。路徑規劃時，以路徑質量（路徑的長度、曲率和連續性）和行車安全為目標。

（1）路徑的質量指標：包括路徑的長度、曲率和連續性，其數學描述如下：

①路徑長度：

式中，S為偽路徑長度，即Frénet 坐標系下規劃路徑的長度。

②路徑曲率平方和：

③路徑的連續性：

式中，θ、θpre分別為當前路徑和上次規劃路徑的航向角。

（2）行車安全指標：綜合考慮道路邊界、靜止障礙物和動態障礙物的影響、構建危險勢場函數，用以評估車輛的碰撞風險?？倓莺瘮悼杀硎緸椋?/p>

式中，n代表障礙物的數量，Ko是與障礙物相關的危險系數，B是車道寬度，Kl、Kr是分別與道路左右邊緣相關的危險系數，Di為：

式中，(so,i,do,i)是第i個障礙物車輛在Frénet 坐標系中的坐標，E1、E2分別為尺度因子。在Frénet 坐標 系中(50,0)處有一障礙物時，碰撞風險的分布情況如圖3 所示。

圖3 在Frénet 坐標系中車輛碰撞風險分布情況

將長度為sp的規劃路徑等分成長度為?s的Np段，可得具有Np+1 個參考點的規劃路徑，其碰撞風險代價J4為：

最終，規劃路徑時的總代價函數可表示為：

式中，wi(i=1,2,3,4)分別為路徑的長度、平滑性、連續性和安全性代價函數的權重。因此，路徑規劃優化問題可由如下方程描述：

采用序列二次規劃算法（Sequential Quadratic Programming，SQP）求解帶約束的非線性優化問題（23）。SQP 通過構建近似的二次模型和迭代優化來尋找最優解，廣泛應用于許多領域的工程和科學問題中；在每次迭代中，SQP 會計算目標函數的梯度和約束條件的梯度，并構建求解一個二次模型獲得下一個迭代點；這個過程不斷重復，直到滿足停止準則為止。此外，為了確保車輛行駛穩定性，車輛轉彎半徑應大于最小轉彎半徑rmin。

2.3 速度規劃

式中，t∈[0,1]為參變量，∈B(i=0,1,…,4)為貝塞爾曲線的端點和控制點的坐標。=s-s0，s，s0分別為行駛路程和規劃時初始距離。

進一步計算V隨行駛路程的變化率A和加速度隨路程的變化率Z分別為：

速度規劃時，車輛的初始狀態用車輛速度、加速度、加速度變化率、規劃時刻初始距離和規劃距離表示，即(v0,a0,z0,s0,sp)。選擇合適的貝塞爾曲線控制點橫坐標(i=1,2,3) 確保式（25）和（26）分母不為0，由貝塞爾曲線性質可得V(0)==v0，==0，==sp。初始加速度和加速度變化率為：

考慮到通行效率和舒適性，速度規劃的目標是使每個參考點的速度最大，同時使規劃距離內的平均速度和加速度變化率最小。顯然，此為多目標規劃問題，一般不存在唯一最優解。但在決策時，可為每個目標函數設定一個目標值，然后根據駕駛員駕駛需求確定優先級，對目標函數值進行選擇。此時，該問題可進一步轉化為式（29）所示的極大極小問題[14]，即：

其中，gi,i=1,2,3 分別為子目標函數?i的目標值，q={qi},i=1,2,3 分別代表每個目標函數的權重系數，并且，

且滿足以下約束：

式中，v0、a0和j0分別為主車在規劃時刻的速度、加速度和加速度變化率。Vmax,i是參考點處的最大允許速度，為了防止車輛發生側滑，最大速度應滿足：

其中，μ是道路附著系數，ay_usr是決策者期望的最大側向加速度，Κ是路徑曲率。

3 軌跡規劃算法驗證

為了驗證軌跡規劃方法的有效性，設計了針對良好天氣條件下，智能車輛在低風險交通環境和結構化道路上，包括直線和曲線道路上的動態避障場景。軌跡規劃算法在安裝MATLAB R2018b 的計算機硬件平臺上編程實現，計算機操作系統為64 位Windows 10，CPU 為Intel-i76700K @4.0 GHz，內存32 GB；然后將軌跡規劃結果導入到專業車輛動力學軟件CarSim?中實現路徑跟隨和速度跟蹤。軌跡規劃方法仿真驗證的整體框架如圖4 所示，仿真參數設置如表1 所示。

表1 軌跡規劃算法參數設置

圖4 軌跡規劃框架

3.1 直道連續避障軌跡規劃仿真

如圖5(a)所示，假設下車道中心線的y坐標為0。主車起始位置的坐標為(0,0)，初始速度為25 m/s，靜態障礙物中心的Frénet 坐標為(250,2)，動態障礙物中心初始位置的Frénet 坐標為(50,-2)，并以恒定速度10 m/s 沿車道中心線行駛。單次規劃路徑的長度為60 m，車輛每1 s 重新規劃一次，最大允許側向加速度設為6 m/s2，主車最大速度為25 m/s。

圖5 直道避障軌跡規劃結果

實驗結果如圖5 所示，由圖5(a)可知，所規劃的路徑可以保證主車成功避開動態和靜態障礙車且可被很好地跟隨。同時由圖5(b)可看出，整個避障過程，加速度和加速度變化率變化光滑平緩。為了驗證車輛在避障過程中的行駛穩定性，將軌跡規劃結果導入到CarSim?中進行跟隨仿真實驗，由圖5(c)可知，跟隨過程主車方向盤轉角和車輪控制力矩最大絕對值分別為36°和328 N·m左右，且保持平緩。由圖5(d)可知，跟隨過程中縱向和側向加速度的最大絕對值分別為0.27 m/s2和4.0 m/s2左右，且變化平緩光滑，說明主車運動平穩、舒適。綜上，本文軌跡規劃方法在直道動態避障時具有良好的效果。

3.2 彎道連續軌跡規劃仿真

如圖6(a)所示，假設下車道中心線起止點y坐標為0。主車起始位置坐標為(0,0)，初始速度為15 m/s，靜態障礙物中心的Frénet 坐標為(200,2)，動態障礙物中心初始位置的Frénet 坐標為(50,-2)，并以恒定速度10 m/s 沿車道中心線行駛，單次規劃路徑的長度為60 m，車輛每行駛1 s 重新規劃一次，主車最大允許側向加速度為5 m/s2，最大允許速度為20 m/s。

圖6 彎道避障軌跡規劃仿真結果

實驗結果如圖6 所示。由圖6(a)可知，所規劃的路徑可以保證主車成功的避開動態和靜態障礙車。同時由圖6(a)的局部放大圖可知規劃路徑可以被很好地跟隨。由圖6(b)可看出加速度和加速度變化率在整個行駛過程中光滑平緩。為了驗證車輛在避障過程中的行駛穩定性，將軌跡規劃結果導入到CarSim?中進行跟隨仿真實驗，由圖6(c)可知，跟隨過程主車方向盤轉角和車輪控制力矩最大絕對值分別為40°和700 N·m 左右，且總體保持平緩。由圖6(d)可知，跟隨過程中主車縱向和側向加速度的最大絕對值分別為1.0 m/s2和3.2 m/s2左右，且總體變化平緩光滑，說明主車運動平穩。

4 結論

本文針對一般結構化道路環境下的智能車動態避障工況，提出了一種基于四階貝塞爾曲線的避障軌跡規劃方法；基于Frénet 坐標，構建有關路徑長度、曲率、連續性和行車風險的綜合指標，采用SQP 方法求解非線性優化問題獲得最優路徑；基于通行效率和駕駛舒適性指標，采用多目標優化方法獲得最優速度。仿真結果表明：在保證車輛穩定行駛和安全避障的前提下，提出的軌跡規劃方法還能盡可能滿足通行效率和舒適性的不同需求。此外，本研究也尚存不足，目前僅局限于低風險交通環境下的動態避障工況，后續研究中將著重解決復雜交通流場景下的最優決策和規劃問題。