基于五階Bezier曲線的無人車避障軌跡規劃

陳鵬宇, 孔令安,2,3， 彭憶強,2,3*

(1.西華大學汽車與交通學院， 成都 610039； 2.汽車測控與安全四川省重點實驗室， 成都 610039； 3.四川省新能源汽車智能控制與仿真測試技術工程研究中心， 成都 610039)

據公安部統計，截至2021年底全國機動車保有量達3.95億輛，其中汽車3.02億輛；2021年全國新注冊登記機動車3 674萬輛，比2020年增加346萬輛，增長10.38%[1]。在為出行增加便利的同時，隨之帶來的交通擁堵和安全問題開始變得日益突出。面對機動車不斷快速增長帶來的問題，各界普遍認為發展以計算機控制代替人工駕駛的自動駕駛汽車會是有效解決手段之一[2]。

無人駕駛作為主要的技術包括環境感知、路徑規劃、跟蹤控制3個方面，其中路徑規劃作為中間環節，在無人駕駛系統中有著極為重要的作用[3]。根據規劃的范圍來劃分，路徑規劃又可以分為全局路徑規劃和局部路徑規劃。全局路徑規劃是在整個地圖上規劃出一條從起點到終點的可行駛的靜態路徑，局部路徑規劃是在全局路徑規劃的基礎上根據當前實際行駛的環境規劃出來的一條隨環境變化無碰撞且滿足各種約束的實時路徑。

經過多年的技術累計，無人車的局部路徑規劃技術也有了很大的突破。綜合來看，無人車局部路徑規劃技術主要可以分為以下4大類：基于圖搜索的局部路徑規劃方法主要有 A*算法、混合 A*算法、D*算法及一系列衍生算法等[4-5]；基于隨機搜索算法的局部路徑規劃方法主要就是快速隨機搜索樹(rapidly-exploring random tree， RRT)，基于曲線插值的局部路徑規劃方法，常用的插值曲線有直線、圓弧、多項式曲線、Bezier曲線、B樣條曲線等[6]，以及基于數值優化方法的局部路徑規劃方法，人工勢場法、最優控制等方法都是基于數值優化進行局部路徑規劃常采用的一些方法[7]。

由公式may=kv2(m、v、ay分別為車輛的質量、速度、橫向加速度，k為道路曲率)可知,車輛通過的路徑曲率應該是連續的，車輛的運動軌跡應滿足側向加速度可導，因此需要規劃出來的軌跡曲線曲率可導，由此可以知道路徑函數最低三階可導。Latip等[8]研究了三階Bezier曲線，但其規劃的軌跡在起始點及終止點的道路曲率以及起始側向加速度均不能為零，不能使車輛在實際行駛中很好地跟隨軌跡； Xu等[9]基于四階Bezier曲線提出了一種移動機器人避障路徑方法，該方法規劃出的路徑平滑利于移動機器人跟蹤，但相同情況下其軌跡曲率最大值較大；Bae等[10]對各階Bezier曲線做了對比研究，得出滿足起始點和終止點曲率均為零的最小階Bezier曲線為五階。為了使所規劃軌跡曲線在起始點位置和終止點位置曲率均為零，采用五階Bzeier曲線進行局部路徑規劃。

將五階Bezier曲線應用于無人車局部路徑規劃中的問題。由于現有方法生成的軌跡曲線不能滿足起始位置及終止位置曲率為零的情況，因此，車輛在跟蹤開始及結束時會產生較大的側向加速度，影響車輛軌跡跟蹤的平順性。為此，將基于五階Bezier曲線方法加入“車輛性能邊界條件”及“前車碰撞邊界條件”約束，使生成的軌跡曲線在起始位置和終止位置曲率值為零，讓車輛能夠平穩跟蹤軌跡行駛，且針對不同換道初始行駛速度，能夠實現動態調整換道軌跡。

1 無人駕駛汽車避障策略

當無人車在按照預先規劃好的全局路徑道路上行駛時，必然會有各種各樣的障礙物出現在前方道路中，為了使無人車在公路上順利行駛，需要針對不同類型的障礙物建立不同的避障策略，典型的避障策略包括停車避障、換道避障以及跟車避障[11]。

首先，車輛將會對前方障礙物進行是否靜止的判斷，當前方障礙物處于靜止狀態時，將會優先選擇換道避障，在沒有可行駛道路時將會采取停車避障；當前方障礙物為動態車輛時，將會判斷前車速度是否高于本車車速，優先選擇跟車避障，否則選擇換道或停車避障。無人車在公路上行駛時的避障策略如圖1所示。

圖1 無人駕駛汽車避障策略Fig.1 Obstacle avoidance strategies for driverless vehicles

2 車輛運動學建模

動力學主要研究作用于物體的力與物體運動的關系，而車輛運動學模型一般用于分析車輛的平順性和車輛操縱的穩定性。車輛運動學轉向模型如圖2所示。

A點表示車輛前軸軸心位置；B點表示后軸軸心位置；(Xf,Yf)、(Xf,Yf)分別表示車輛前軸軸心和后軸軸心的坐標；φ為車輛航向角； vf為車輛前軸中心速度, vr為車輛后軸中心速度;δf為前輪轉角；車輛轉向時，瞬時轉動中心為O點所在位置；R為后輪轉向半徑圖2 車輛運動學模型[12]Fig.2 Vehicle kinematics mode[12]

道路曲率k、轉向半徑r、車輛軸距l以及前輪轉角δf之間的關系為

(1)

在后輪行駛軸心(Xr，Yr)處，速度為

(2)

在后軸軸心B處的運動學約束為

(3)

聯立式(2)和式(3)，得

(4)

根據前、后輪的幾何關系得

(5)

則由式(2)～式(4)可得橫擺角速度

(6)

前輪轉角為

(7)

聯立式(4)和式(6)，可得車輛運動模型為

(8)

通過運動學模型就能將無人車在任意t時刻下的狀態通過笛卡爾坐標(x,y)、航向角φ和道路曲率k這4個變量表示。通過這4個變量表達，則軌跡可以看作是由初始狀態到終止狀態的一組變量變化，設初始狀態X0=[x0y0φ0k0]和終止狀態XT=[xTyTφTkT][13]。

3 基于Bezier曲線的局部路徑規劃

3.1 五階Bezier曲線公式

Bezier曲線對應于二維平面的數學曲線，因其控制簡單，能處理光滑曲線，開始應用于無人車的局部路徑規劃中，通過設定曲線的起始點，并在始末點之間添加不同數量的控制點，通過改變控制點的位置坐標進而達到控制曲線彎曲方向以及彎曲角度的目的[14]。

Bezier曲線通過使用多個線段的端點坐標P0、P1、…、Pn來定義其曲線，由給定點P0、P1、…、Pn的n階Bezier曲線可表示為通式[15]

(9)

由Bzier通式可得五次Bezier曲線公式為

(10)

(11)

將式(11)代入式(10)可得

P(t)=(1-t)5P0+5t(1-t)4P1+

10t2(1-t)3P2+10t3(1-t)2P3+

5t4(1-t)P4+t5P5

(12)

在笛卡爾坐標系中，將車輛位置(x,y)表示成參數t的函數，則五階Bezier曲線可以表示為

(13)

則曲線上任意一點的曲率[10]為

(14)

式(14)中：x′(t)為縱向車速；y′(t)為橫向車速；x″(t)為縱向加速度；y″(t)為橫向加速度。五階Bezier曲線軌跡示意圖如圖3所示。

圖3 五階Bezier曲線規劃軌跡示意圖Fig.3 Fifth order Bezier curve planning trajectory diagram

3.2 換道避障軌跡邊界條件

根據Bezier曲線的性質，需確定控制點的位置。因此，設計一種典型的需要超車換道的工況如圖4所示。在封閉雙車道道路內，前方車輛(或障礙物)C0靜止停靠在道路右側位置，后方車輛(本車)C1以恒定車速v1行駛，當車輛傳感器檢測到前方車輛靜止且影響到本車行駛時，按照前文提到的避障策略，本車域控制器將從當前位置進行局部路徑規劃開始換道避障策略來躲避前車(或障礙物)，通過考慮車輛性能約束及前車碰撞約束來確定Bezier曲線的左右邊界。

圖4 典型超車換道工況Fig.4 Typical overtaking lane change condition

3.2.1 考慮車輛性能的邊界

在車輛換道轉向過程中，需要考慮車輛性能極限，保證車輛在轉向側翻的指標是側向加速度的大小，如果當車輛開始變道時就以最大側向加速度aymax=0.28gm/s2進行轉向(g為重力加速度)[16]，考慮車輛性能約束的左邊界圓弧半徑R1計算公式為

(15)

如圖5所示為性能邊界所示。

Lxl表示車輛變道開始位置到左邊界位置的距離圖5 軌跡規劃可行域邊界Fig.5 Feasible region boundary of trajectory planning

3.2.2 考慮前車碰撞的邊界

當前方車輛(或障礙物)靜止停靠在路邊時，需考慮本車與障礙物碰撞的問題，將車輛假設為包裹車身的幾何圓，則換道過程中本車與障礙物的幾何圓的相切點就是不產生碰撞的約束點[17]。在本車與障礙物不發生碰撞的極限條件下開始轉向，就可得到考慮碰撞問題的右邊界。如圖5中碰撞邊界所示。

考慮碰撞問題的右邊界圓弧半徑R2的計算公式如下。

(16)

(17)

(18)

(19)

(20)

式中:ay2為側向加速度；Lxr表示車輛變道開始位置到右邊界位置的距離；yd為兩條道路中心線之間的距離，取3.75 m；d為兩個幾何圓圓心之間的縱向距離；r為車輛幾何圓半徑；B為車寬；L為車長；ld為考慮碰撞問題時本車幾何圓中心到障礙車后方的直線距離。在此邊界條件下可得到安全換道車距S=Lxr+ld。

通過分析得到的考慮車輛性能約束的左邊界圓弧半徑R1和考慮前方靜止障礙物避撞的右邊界圓弧半徑R2，就可得到換道軌跡的左右可行域邊界。通過兩個邊界條件的約束就可得到換道軌跡的可行域范圍，如圖5中紅色實線所圍成的區域所示。

3.3 換道最優軌跡曲線選擇

道路軌跡曲線規劃采用五階Bezier曲線，將選取6個不同的控制點，道路軌跡曲線則根據所選的6個控制點的改變而改變。在本研究中，以坐標原點為起始點，即P0=(x0,y0)=(0,0)；根據換道對稱性，有P1=(x1,y1)=(x1,0)；P2=(x2,y2)=(x2,0)；P3=(x3,y3)=(x,yd)；P4=(x4,y4)=(2x2-x1,yd)；P5=(x5,y5)=(2x2,yd)。其中yd為兩條道路中心線之間的距離，取3.75 m。因此，決定道路軌跡曲線形狀的變量為選取的第二個和第三個點的縱坐標值x1和x2。

初步的左右邊界判定僅僅只滿足了獲得車輛不側翻以及不碰撞前車的極限情況，如果要生成的軌跡更利于車輛的跟蹤行駛，那么將會加入更多的約束。為了獲得最安全舒適的路徑，建立了目標優化函數[18]為

J=min(ω1J1+ω2J2+ω3J3+ω4J4)

(21)

式(21)中：ω1、ω2、ω3和ω4為權重系數。

所以，目標函數公式(21)可以轉化為

(22)

各優化指標的約束條件為

(23)

(24)

通過分析求解建立的目標函數，便可以得到不同車速下對應的最佳x1和x2，即得到6個最優坐標點信息，規劃出最優行駛軌跡曲線。

3.4 換道最優軌跡曲線求解

序列二次規劃(sequential quadratic programming，SQP)算法是處理規模不大非線性規劃問題的最實用算法之一，此算法能夠將之前推導得到的等式約束及不等式約束轉換為較為簡單的二次規劃問題來求解[21]。整理第3.3節中所推導得到的式(23)和式(24)可得到優化目標函數為

(25)

通過SQP方法對式(25)進行最優化求解來優化控制點的選取，使最終的行駛軌跡收斂到局部最優解，得到的軌跡可使軌跡曲線更加平滑，跟蹤過程滿足駕乘人員乘坐舒適和車輛換道迅速的特點。

4 仿真分析

由前面分析可知，軌跡曲線形狀和終止點位置由第二個和第三個控制點的縱坐標值x1和x2來決定。為分析選取不同x1和x2的值對軌跡曲線的影響進行MATLAB仿真，生成不同x1點產生的軌跡簇(取換道縱向距離為100 m，x1取值范圍為16～40 m，間隔距離為6 m，x1為換道距離的中點)，得到圖6所示的軌跡曲線和圖7所示的軌跡曲線曲率；不同x2點產生的軌跡簇(x2取值范圍為38～70 m，間隔距離為8 m，x1點為x2的中點)，得到圖8所示的軌跡曲線和圖9所示的軌跡曲線曲率。

圖6 不同x1點產生的軌跡曲線簇Fig.6 Cluster of trajectory curves generated by different x1 points

圖7 不同x1點產生的軌跡曲線曲率Fig.7 Curvature of the trajectory curve generated by different x1 points

圖8 不同x2點產生的軌跡曲線簇Fig.8 Cluster of trajectory curves generated by different x2 points

圖9 不同x2點產生的軌跡曲線曲率Fig.9 Curvature of the trajectory curve generated by different x2 points

由圖6～圖9可知，相較于不同的x1點，不同x2點產生的軌跡曲線曲率變化范圍更小，換道距離范圍更大，對車輛跟蹤影響更重要。所以，選擇選取解最優x2點的方式來驗證本方法對不同車速情況下前方出現障礙物的路徑規劃能力，進行如下仿真分析。

首先，利用MATLAB對算法進行仿真，本方法取具有代表性的3個道路速度，“低速”工況，速度設定為36 km/h；“中速”工況，速度設定為72 km/h；“高速”工況，速度設定為108 km/h。換道過程中允許的最大側向加速度aymax=0.28gm/s2；車道寬度yd=3.75 m；車寬B=1.88 m；車長L=4.325 m；換道安全距離S=3v1，ω1=0.3，ω2=0.3，ω3=0.4，ω4=0，不考慮換道時間的影響。根據第3節所提出的邊界條件及優化目標函數，得到對應不同車速下生成的軌跡曲線及曲線曲率，如圖10和圖11所示。

圖10 不同車速規劃軌跡曲線圖Fig.10 Planned trajectory curves for different vehicle speeds

圖11 不同車速規劃軌跡曲線曲率圖Fig.11 Curvature diagram of trajectory curves planned for different vehicle speeds

為驗證此方法在后續實車上的應用是否有效，本文還利用車輛動力學軟件CarSim與MATLAB/Simulink軟件聯合仿真，對所研究算法進行驗證，自車車輛分別設置速度為36、72、108 km/h在正常雙向直行道路行駛(道路中心線之間的距離為3.75 m)的工況來模擬避障過程。聯合仿真實驗采用CarSim軟件自帶的C-Class Hatchback作為實驗車輛，此模型具體車輛參數如表1所示。

表1 實驗車輛模型參數Table 1 Experimental vehicle model parameters

根據3種工況實驗結果顯示，無人車在避障過程中避障效果良好，軌跡跟蹤偏差較小且實際車輛軌跡靠道路中心線；3種工況下車輛最大側向加速度均在0.18g左右，小于側翻閾值0.28g，滿足車輛性能要求。

仿真效果如圖12、圖13所示。

圖12 車速不同時軌跡跟蹤效果圖Fig.12 Trajectory tracking and at different speeds

(2)本文方法在無人車行駛過程中前方出現靜止障礙物采用換道避障策略時，通過考慮車輛動力學性能極限和前車碰撞條件來規劃無人車可行軌跡，所規劃的最優換道軌跡曲線更加平滑，跟蹤過程滿足乘客乘坐舒適和車輛換道迅速的特點，為無人車采取換道避障策略時規劃出了可行的換道軌跡。

圖13 車速不同時側向加速度圖Fig.13 Lateral acceleration at different speeds

5 結論

綜上仿真得到以下結論。

(1)基于五階Bezier曲線規劃的換道軌跡可以滿足曲線在起始位置和終止位置曲率值為零的條件，車輛能夠平穩跟蹤軌跡行駛，且針對不同換道初始行駛速度，能夠實現動態調整換道軌跡，突破了傳統曲線插值局部路徑規劃法在換道前需要提前知道終止點位置的局限。

[1]中華人民共和國公安部. 2021年全國機動車保有量達3. 95億 新能源汽車同比增59.25%[EB/OL]. (2022-1-11)[2022-1-15]. https: //app. mps. gov. cn/gdnps/pc/content. jsp?id=8322369.

Ministry of Public Security of the People’s Republic of China. In 2021, the number of motor vehicles in China will reach 395 million new energy vehicles increased 59. 25% year-on-year [EB/OL]. (2022-1-11) [2022-1-15]. https: //app. mps. gov. cn/gdnps/pc/content. jsp?id=8322369.

[2]趙安晨, 陳朋. 試論無人駕駛技術對城市道路交通及規劃產生的影響與建議[C]//面向高質量發展的空間治理——2021中國城市規劃年會論文集(6城市交通規劃). 北京: 中國城市規劃學會, 2021: 112-121.

Zhao Anchen, Chen Peng. Discussion on the influence of unmanned driving technology on urban road traffic and planning and suggestions [C]//Spatial Governance for High-quality Development： The Proceedings of 2021 China Urban Planning Annual Conference (6 Urban Traffic Planning). Beijing: Urban Planning Society of China, 2021: 112-121.

[3]熊璐, 康宇宸, 張培志, 等. 無人駕駛車輛行為決策系統研究[J]. 汽車技術, 2018(8): 1-9.

Xiong Lu, Kang Yuchen, Zhang Peizhi, et al. Research on behavior decision system of autonomous vehicle [J]. Automobile Technology, 2018(8): 1-9.

[4]Majumder S, Prasad M S, Shankar R. Three dimensional D*algorithm for incremental path planning in uncooperative environment[C]//International Conference on Signal Processing & Integrated Networks. New York: IEEE, 2016: 431-435.

[5]鄭亮, 孫龍龍, 陳雙. 一種改進工業自動導引車路徑規劃算法[J]. 科學技術與工程, 2021, 21(16): 6758-6763.

Zheng Liang, Sun Longlong, Chen Shuang. An improved path planning algorithm for industrial automatic guided vehicle [J]. Science Technology and Engineering, 201, 21(16): 6758-6763.

[6]張甫城, 胡敏, 江書真. 智能汽車換道軌跡規劃曲線研究[J]. 汽車實用技術, 2021, 46(1): 26-28.

Zhang Fucheng, Hu Min, Jiang Shuzhen. Research on lane change trajectory planning curve of intelligent vehicle [J]. Automobile Practical Technology, 2021, 46(1): 26-28.

[7]彭曉燕, 謝浩, 黃晶. 無人駕駛汽車局部路徑規劃算法研究[J]. 汽車工程, 2020, 42(1): 1-10.

Peng Xiaoyan, Xie Hao, Huang Jing. Research on local path planning algorithm for autonomous vehicle [J]. Automotive Engineering, 2020, 42(1): 1-10.

[8]Latip N B A, Omar R. Feasible path generation using bezier curves for car-like vehicle[J]. IOP Conference Series: Materials Science and Engineering, 2017, 226(1): 012133.

[9]Xu L, Cao M Y, Song B Y. A new approach to smooth path planning of mobile robot based on quartic Bezier transition curve and improved PSO algorithm[J]. Neurocomputing, 2022, 473: 98-106.

[10]Bae I, Moon J, Park H, et al. Path generation and tracking based on a Bézier curve for a steering rate controller of autonomous vehicles[C]//International IEEE Conference on Intelligent Transportation Systems. New York: IEEE, 2013: 436-441.

[11]洪班豪. 汽車主動避障系統控制與仿真研究[D]. 長沙: 湖南大學, 2020.

Hong Banhao. Control and simulation research of vehicle active obstacle avoidance system [D]. Changsha: Hunan University, 2020.

[12]龔建偉, 姜巖, 徐威. 無人駕駛車輛模型預測控制[M]. 第2版. 北京: 北京理工大學出版社, 2020: 27-31.

Jianwei Gong, Yan Jiang, Wei Xu. Model predictive control for autonomous vehicle [M]. 2nd ed. Beijing: Beijing Institute of Technology Press, 2020: 27-31.

[13]張新鋒, 陳建偉, 左思. 基于貝塞爾曲線的智能商用車換道避障軌跡規劃[J]. 科學技術與工程, 2020, 20(29): 12150-12157.

Zhang Xinfeng, Chen Jianwei, Zuo Si. Intelligent commercial vehicle lane change and obstacle avoidance trajectory planning based on Bessel curve [J]. Science Technology and Engineering, 2020, 20(29): 12150-12157.

[14]Prautzsch H, Boehm W, Paluszny M. Bézier and B-spline techniques[M]. Berlin: Spring, 2017.

[15] Vahide B. Path planning for autonomous ground vehicles based on quintic trigonometric Bézier curve[J]. Journal of the Brazilian Society of Mechanical Sciences and Engineering, 2021, 43(2): 1-14.

[16]Li H, Luo Y. Study on steering angle input during the automated lane change of electric vehicle: 2017-01-1962[R].Minneapolis: SAE Technical Paper, 2017.

[17]李洪硌. 無人駕駛汽車高速工況智能決策、軌跡規劃與跟蹤研究[D]. 廣州: 華南理工大學, 2020.

Li Hongzuo. Research on intelligent decision-making, trajectory planning and tracking of unmanned vehicle under high-speed driving conditions [D]. Guangzhou: South China University of Technology, 2020.

[18]Farouki R T, Pelosi F, Sampoli M L. Optimization of corner blending curves[J]. Computer-aided Design, 2019, 117: 102739.

[19]Korzeniowski D, Laski G. Method of planning a reference trajectory of a single lane change manoeuver with Bezier curve[J]. IOP Conference Series: Materials Science and, Engineering, 2016, 148(1): 012012.

[20]游峰, 谷廣. 面向無人駕駛的車輛協同換道軌跡規劃[J]. 科學技術與工程, 2018, 18(15): 155-161.

You Feng, Gu Guang. Collaborative lane change trajectory planning for unmanned vehicles [J]. Science Technology and Engineering, 2018, 18(15): 155-161.

[21]Gill P E, Kungurtsev V, Robinson D P. A stabilized SQP method: superlinear convergence[J]. Mathematical Programming, 2017, 163(1-2): 369-410.