基于貝塞爾曲線的自動駕駛汽車避障路徑規(guī)劃

化祖旭 張文海

（重慶交通大學(xué)機電與車輛工程學(xué)院，重慶 400074）

主題詞：路徑規(guī)劃 貝塞爾曲線 自動駕駛

1 引言

自動駕駛車輛目前按國際標(biāo)準(zhǔn)通常劃分為6個等級，能夠?qū)崿F(xiàn)無人駕駛技術(shù)的無人車一般都已達到L3級別以上。目前，全球能夠?qū)崿F(xiàn)自動駕駛技術(shù)的企業(yè)主要有百度、特斯拉、谷歌Waymo、GM Cruise、奧迪，國內(nèi)外自動駕駛系統(tǒng)多數(shù)正處于輔助駕駛L2 級別。自動駕駛汽車主要有視覺感知、規(guī)劃決策、運動控制3大研究方向。其中，路徑規(guī)劃又分為全局路徑規(guī)劃和局部路徑規(guī)劃，局部路徑規(guī)劃是在全局路徑規(guī)劃的基礎(chǔ)上根據(jù)當(dāng)前實際行駛環(huán)境規(guī)劃出一條無碰撞且滿足各種約束的局部路徑[1]。

常用的局部路徑規(guī)劃方法主要有人工勢場法、貝塞爾曲線法、B 樣條曲線法、曲線插值法以及基于MPC算法的動態(tài)路徑規(guī)劃。其中，貝塞爾曲線具有曲率連續(xù)、易于生成的優(yōu)點，被廣泛應(yīng)用于自動駕駛汽車局部路徑規(guī)劃。張金煒等[2]提出了蟻群算法與四次貝塞爾曲線融合的路徑規(guī)劃方法，其先利用蟻群算法規(guī)劃全局最短路徑，然后利用多個貝塞爾曲線對路徑進行擬合，提高了蟻群算法規(guī)劃路徑的平滑度。張新鋒等[3]通過遺傳算法對貝塞爾曲線進行了優(yōu)化，最后通過Trucksim 軟件驗證了其規(guī)劃路徑的可行性。高嵩等[4]基于貝塞爾曲線建立了避障路徑規(guī)劃，以最大最小曲率的差值為優(yōu)化目標(biāo)，通過序列二次優(yōu)化(SQR)算法對控制點參數(shù)進行優(yōu)化求解，并通過實驗驗證了其方法的可行性。余星寶等[5]基于4階貝塞爾曲線優(yōu)化了A*算法，通過尋找A*算法中路徑曲率突變點，用4階貝塞爾曲線去替代曲率過大的路徑，通過仿真驗證這種方法可以有效減少A*算法中的曲率突變和減短路徑長度。Song等[6]提出了粒子群算法與高階貝塞爾曲線相結(jié)合的方法設(shè)計了更加平滑的規(guī)劃路徑，Michael Schwung 等[7]提出了基于事件的規(guī)避碰撞方法，并使用貝塞爾曲線規(guī)劃軌跡，實驗驗證了方法的可行性。Vahid Hassani 等[8]提出了利用貝塞爾曲線來生成船舶的行駛路徑，并設(shè)立成本函數(shù)來反應(yīng)其動態(tài)性能，并通過仿真驗證了算法的有效性。

本文為了簡化算法，使用2 段3 次貝塞爾曲線完成避障路徑規(guī)劃，第2 段變道路徑曲線由第1 段避撞路徑曲線旋轉(zhuǎn)得到，并建立了安全距離模型，確保車輛在輪胎滑移與側(cè)向風(fēng)等不確定因素的影響下與障礙物不會碰撞。通過加入車輛目標(biāo)狀態(tài)曲率約束、側(cè)滑約束、執(zhí)行機構(gòu)約束確保車輛行駛舒適性，最后利用仿真軟件MATLAB進行仿真驗證。

2 路徑規(guī)劃

2.1 貝塞爾曲線介紹

貝塞爾曲線通過控制點的選取來改變曲線的形狀，具有軌跡曲率連續(xù)可導(dǎo)、易于跟蹤、滿足汽車動力學(xué)約束且僅需少量控制點就可生成軌跡等優(yōu)點[9]。

通常定義n+1 個控制點組成n階貝塞爾曲線，其表達式如式(1)[10]:

式中，Pi與t分別是控制點的坐標(biāo)值與參數(shù)，Bi,n(t)為Bernstein多項式，其表達式如式(2)[11]：

式中，為二次項系數(shù)。

3次貝塞爾曲線的參數(shù)方程表達式可以表述為式(3)：

式中，Pi(xi，yi)；xi和yi分別代表控制點的橫縱坐標(biāo)。

2.2 控制點的選取

本文規(guī)劃路徑由2 段3 次貝塞爾曲線構(gòu)成，分別為避撞路徑與變道路徑。如圖1 所示，每段具有4 個控制點，2段共8個控制點，其中控制點P3為前一段與后一段路徑共用，以確保曲率連續(xù)。這里只需求取前4 個控制點坐標(biāo)，后4 個控制點坐標(biāo)直接由前段曲線旋轉(zhuǎn)得到。

圖1 3次貝塞爾曲線避障軌跡

避撞路徑的3次貝塞爾曲線參數(shù)方程，如式(4)[12]：

（4）對式（4）分別求一階導(dǎo)與二階導(dǎo)，其表達式如下：

貝塞爾曲線上任意一點的曲率為[13]：

假設(shè)前方為靜止障礙物，初始時刻目標(biāo)車輛勻速行駛，縱向速度為v0，且其初始時刻的橫向速度與加速度為零，則初始狀態(tài)下有如下關(guān)系：

由此可得出前3 個控制點P0=（0，0），P1=（v/3，0），P2=（2v/3，0），接下來求取控制點P3的坐標(biāo)，為了簡化算法本文設(shè)定控制點P3在道路中心線上，則確定P3點坐標(biāo)為（x3，L/2），L為單車道寬度?？紤]避障時車輛輪胎滑移與側(cè)向風(fēng)等不確定因素的影響，建立以下安全距離模型，如式(8)[14]：

式中，d為預(yù)留安全距離；d0為目標(biāo)車輛緊急停車時與障礙物的最小安全距離，一般取2～3 m；v為車速，障礙物危險系數(shù)t1∈[0,1]。

2.3 路徑優(yōu)化

路徑優(yōu)化是為確保規(guī)劃的路徑易于車輛跟蹤，且同時具有安全性和舒適性[15]。為保證規(guī)劃路徑的安全性，令x3=D+a-d，且設(shè)置如式(9)：

式中，D為目標(biāo)車輛與障礙物之間的初始距離，a為車輛長度。

為保證側(cè)滑約束，設(shè)置如式(10)[16]：

式中，ay為橫向加速度，v為汽車車速，u為道路摩擦系數(shù)，g為重力加速度。

為簡化第2 段變道路徑規(guī)劃算法，其由前一段避撞路徑旋轉(zhuǎn)得出，此時第2段規(guī)劃路徑起始控制點為P3，且P2，P3，P4處于同一條直線上，保證車輛的橫擺角連續(xù)無突變，由此可求得控制點P4=（2x3,3.5），P5=（2x3+v/3,3.5），P6=（2x3+2v/3,3.5），此時控制點P4，P5與P6共線，可確保變道后車輛目標(biāo)狀態(tài)的曲率為零。至此2段3次貝塞爾曲線全部規(guī)劃完成。

3 仿真

下面利用MATLAB 軟件對規(guī)劃路徑進行仿真，設(shè)定車輛長度a=4 m，車輛寬度b=1.8 m，道路寬度L=3.5 m，路面附著系數(shù)u取0.8。本文規(guī)劃路徑可隨速度變化而變化，如圖2所示，初始距離D選取為26 m，車輛速度分別為10 m/s，15 m/s，20 m/s，由圖2 可看出車輛速度越快，車輛在結(jié)束避撞路徑時的X軸坐標(biāo)越小，增大了汽車與障礙物之間的距離，確保了汽車的安全性。圖3中的車速設(shè)定為15 m/s,可看出初始距離D變化，規(guī)劃路徑的曲率變化率也有所不同，初始距離越大路徑的曲率越小，舒適性越高，可適應(yīng)不同的駕駛風(fēng)格。圖4至圖5中車速設(shè)定為15 m/s，初始距離D設(shè)定為26 m。圖4為避撞路徑中車輛橫向加速度隨X軸的變化曲線，可看出橫向加速度最大為1.81 m/s2，則規(guī)劃路徑的橫向加速度滿足式（9）所規(guī)定的側(cè)滑約束，滿足車輛變道安全性，變道路徑與避撞路徑的加速度大小相同方向相反，圖5為車輛橫擺角，其最大值為0.11 rad，滿足車輛變道舒適性要求。

圖2 不同車速下3次貝塞爾曲線避障軌跡

圖3 不同初始距離下三次貝塞爾曲線避障軌跡

圖4 車輛橫向加速度曲線

圖5 車輛橫擺角變化

4 總結(jié)

本文基于2段3次貝塞爾曲線完成了自動駕駛汽車避障路徑規(guī)劃，建立了安全距離模型，增加了車輛安全性。同時利用旋轉(zhuǎn)曲線的方法簡化了避障路徑的算法。仿真結(jié)果驗證：本文的路徑規(guī)劃方法可以在不同初始距離D與不同車速下進行，且規(guī)劃的避障路徑滿足一定的舒適性與安全性。