智能車輛避障路徑規(guī)劃方法研究

趙 穎,張 琪,俞 庭,林先卬,張穎娟,李云伍,冀 杰

(1.西南大學 工程技術學院,重慶 400715;2.中國汽車工程學會,北京 100176)

隨著無人駕駛技術的快速發(fā)展,路徑規(guī)劃與軌跡跟蹤快速成為智能車輛的核心技術之一[1,2]。在車輛行駛過程中,采用合適的路徑規(guī)劃算法不僅能夠安全、準確、快速地避障,且具有較高的乘坐舒適性,從而提高出行效率。

國內外學者已在路徑規(guī)劃與軌跡跟蹤方面作了大量研究[3,4]。在避障路徑的擬合方面,幾何法和數(shù)值優(yōu)化法的采用較為廣泛。幾何法是通過傳感器獲取障礙物的位置信息,從而生成多條無障礙幾何曲線段,并在一定條件下連接形成避障代價最小路徑的方法,多用于靜態(tài)避障[5]。Liu等[6]通過分析直線路徑上障礙物的位置信息,提出了一種由3段圓弧構成的小障礙物避障方法。劉宇峰等[7]通過傳感器定位障礙物,結合拖拉機運動學模型規(guī)劃避障圓弧,并采用分段圓弧連接的方法得到避障代價最小的路徑。魏爽等[8]建立了基于預瞄點搜索的純追蹤模型,用于直線和曲線預設的局部跟蹤路徑規(guī)劃。采用幾何法進行避障路徑規(guī)劃簡單、易實現(xiàn),但在多段圓弧的連接處,路徑曲率發(fā)生突變,引起原地轉向現(xiàn)象,使得車輛轉向穩(wěn)定性降低。數(shù)值優(yōu)化法是通過采用多項式函數(shù),或插值函數(shù)生成多條曲率連續(xù)變化的曲線,并在一定條件下確定控制點,從而得到最優(yōu)避障路徑的方法。貝塞爾曲線和樣條曲線的使用尤為廣泛。Elhoseny等[9]提出了一種貝塞爾曲線與改進遺傳算法相融合的方法,并對車輛進行了動態(tài)路徑規(guī)劃。余伶俐等[10]采用五次貝塞爾曲線平滑規(guī)劃路徑并進行多段曲線的光滑拼接,以曲線參數(shù)作為中間映射量,構建路徑長度與軌跡坐標之間的狀態(tài)映射模型,設計移動機器人的非時間運動參考量。李紅洛等[11]設計了一種基于五階貝塞爾曲線的前車換道路徑規(guī)劃方法。連建芳等[12]提出了一種基于三次樣條插值的路徑規(guī)劃方法,并提出了一種新穎的混沌自適應粒子群優(yōu)化算法來優(yōu)化三次樣條插值中的控制點。強寧等[13]提出了一種粒子群優(yōu)法(Particle swarm optimization,PSO)算法與三次樣條插值相融合的路徑規(guī)劃方法。于洋等[14]采用三次B樣條曲線對無人車進行軌跡優(yōu)化。夏晨等在靜態(tài)突發(fā)威脅工況下,基于三次樣條生成一系列候選路徑。在動態(tài)突發(fā)威脅工況下,構建動態(tài)突發(fā)威脅相關模型,最終采用A*算法建立總代價函數(shù),生成最優(yōu)避障路徑。

上述文獻通過仿真分析方法驗證了采用幾何法或數(shù)值優(yōu)化法進行避障路徑規(guī)劃的可行性,其中,幾何法所規(guī)劃的多為曲率不連續(xù)的路徑,而貝塞爾曲線和樣條曲線因其曲率連續(xù)的特性,可作為避障路徑規(guī)劃中的緩和曲線,使所規(guī)劃的路徑更平滑。對于以上兩種常用的避障曲線,進一步比較兩種曲線在車輛轉向性能、駕駛穩(wěn)定性、乘坐舒適性方面的優(yōu)劣性,為后續(xù)開展車輛多種避障規(guī)劃算法的融合具有一定指導意義。

基于此,本文針對五次貝塞爾曲線、三次貝塞爾曲線、五次樣條曲線和三次樣條曲線,進行避障路徑規(guī)劃研究,探究四種避障路徑規(guī)劃的特點及其工程實用性。首先,確定控制點并分別擬合出以上四種避障路徑;其次,采用Matlab和Carsim聯(lián)合仿真分析的方法對這四種避障路徑的曲率進行對比分析;最后,基于無人駕駛試驗平臺,采用純跟蹤預瞄模型開展實車試驗,以此對比探究四種避障路徑的工程實用性,解決車輛在避障時由于路徑曲率不連續(xù)易發(fā)生原地轉向的問題。

1 避障路徑規(guī)劃

避障路徑規(guī)劃是根據(jù)工況設計可行的避障路徑,并要求路徑曲率連續(xù),進而有效避免車輛在繞過障礙物時出現(xiàn)原地轉向的現(xiàn)象,最終使車輛能夠安全、快速、平穩(wěn)地避過障礙物,繼續(xù)行駛[16]。

1.1 貝塞爾曲線控制點

為簡化計算,現(xiàn)以車輛避障的起始位置作為坐標原點,選取避障前半部分進行局部路徑規(guī)劃,如圖1所示為避障路徑規(guī)劃示意圖。

圖1 避障路徑規(guī)劃示意圖

貝塞爾曲線表達式[17]如下

(1)

(2)

式中：Pi代表控制點坐標,t代表參數(shù),n為有理正整數(shù)。

五次貝塞爾曲線參數(shù)方程表示如下

P(t)=P0(1-t)5+5P1(1-t)4t+10P2(1-t)3t2+

10P3(1-t)2t3+5P4(1-t)t4+P5t5

(3)

曲線上任意點曲率[1]可表示如下

(4)

為獲得五次貝塞爾曲線,需確定控制點坐標,基于貝塞爾曲線仿射變換的不變特性,首先,設定車輛在避障初始時刻的狀態(tài)坐標

P0=(x0,y0)=(0,0)

(5)

設避障初始狀態(tài)及最終狀態(tài)車輛航向角均為0,則第二個控制點坐標P1(x1,0),設第三個控制點坐標P2(x2,y2)。根據(jù)式(4),當t=0時

(6)

求解得

(7)

基于五次貝塞爾曲線的避障路徑如圖2所示,其中,陰影部分代表障礙物,以車輛避障起點為P0,設障礙物的幾何半徑為R0,車寬為2w,車長為2l,對障礙物進行膨脹,膨脹寬度為車輛的幾何半徑ΔR

圖2 基于五次貝塞爾曲線的避障路徑

(8)

則膨脹后半徑R可表達為

R=R0+ΔR

(9)

設第六個控制點狀態(tài)坐標P5為

P5=(x5,y5)=(x5,R)

(10)

根據(jù)式(4)可求得

P3=(x3,y3)=

P4=(x4,y4)=(x4,R)

(11)

基于GILL P E提出的穩(wěn)定序列二次規(guī)劃(Stabilized sequential quadratic programming,SQP)算法[19],以路徑平均曲率最小為目標,對參數(shù)x1、x2、x3、x4、x5進行優(yōu)化。同理,限制避障路徑的起點、終點和航線的連續(xù)性,選取圖2中P0、P1、P4、P5四點,即可確定三次貝塞爾曲線。

1.2 樣條曲線路徑規(guī)劃

圖3為基于五次樣條曲線的避障路徑規(guī)劃,以車輛避障初始點為原點,車輛縱向為X方向,車輛橫向為Y方向建立坐標系。車輛在初始避障時的狀態(tài)為Pstart,車輛在避障終點時的狀態(tài)為Pend。

圖3 基于三次樣條曲線的避障路徑

設避障路徑為

P(s)=Pstart+s(Pend-Pstart)

(12)

式中：s=s(t),t∈[0,T],s∈[0,1],T表示整個避障過程所用的總時長。s的值域范圍決定了該規(guī)劃方式下的樣條曲線的橫向超調量始終為0,而若采用多項式函數(shù)規(guī)劃樣條曲線,將不可避免地產生橫向超調量,故采取式(12)所示的樣條路徑,設五次樣條曲線公式為

s(t)=b1t5+b2t4+b3t3+b4t2+b5t+b6

(13)

代入式(13),可求得

(14)

同理,可得三次樣條曲線公式如下

(15)

2 仿真分析

在對本文所述兩種避障曲線進行仿真前,首先對多項式函數(shù)規(guī)劃的樣條曲線,在不同始末距離前提下,選取四個控制點,擬合三次多項式樣條曲線,避障曲線仿真結果如圖4所示。

圖4 不同始末距離的三次多項式樣條曲線規(guī)劃

由圖4可知,其他條件一定時,三次多項式樣條曲線存在一定的橫向超調量,且始末距離越大,根據(jù)三次多項式擬合樣條曲線的橫向超調量越小。同理,五次多項式樣條曲線及其它階數(shù)的多項式樣條曲線均存在橫向超調量,而上文所提出的三次及五次樣條曲線規(guī)劃方法因其參數(shù)函數(shù)的值域范圍特性,不存在橫向超調量。故采用上文提出的三次及五次樣條曲線規(guī)劃進行仿真和試驗分析。

圖5為仿真環(huán)境下三次樣條曲線、五次樣條曲線、三次貝塞爾曲線和五次貝塞爾曲線的避障路徑示意圖。

圖5 避障路徑示意圖

圖6 四種避障路徑曲線曲率對比

由圖6(a)可知,三次貝塞爾曲線和五次貝塞爾曲線的曲率曲線均存在兩個波峰和一個波谷,波峰處為車輛轉向處路徑的曲率變化,波谷的存在是由于車輛轉向方向發(fā)生變化,路徑曲率由1.176×10-6m-1(三次貝塞爾曲線),1.154×10-6m-1(五次貝塞爾曲線)逐漸降為0,再由0逐漸增大。三次貝塞爾曲線的曲率在兩次波峰處發(fā)生突變,而五次貝塞爾曲線的曲率全程連續(xù)變化。由圖6(b)可知,三次樣條曲線和五次樣條曲線的曲率曲線均存在兩個波峰和一個波谷,波峰處為車輛轉向處路徑的曲率變化,波谷的存在是由于車輛轉向方向發(fā)生變化,路徑曲率分別由2.97×10-4m-1(三次樣條曲線)、2.89×10-4m-1(五次樣條曲線)逐漸降為0,再由0逐漸增大。由圖6(c)可知,五次貝塞爾曲線曲率和五次樣條曲線的曲率在避障全程中均不存在突變,變化連續(xù)且穩(wěn)定,不產生原地轉向現(xiàn)象,乘坐舒適性較好。由圖6(d)可知,三次曲線的曲率存在突變,產生原地轉向現(xiàn)象,乘坐舒適性較差。由此可見,不論是貝塞爾曲線還是樣條曲線,三次曲線的曲率在換向處會發(fā)生突變,產生原地轉向現(xiàn)象,乘坐舒適性較差,而五次曲線的曲率變化連續(xù)。綜上,三次曲線比五次曲線的曲率波動幅度更大,曲率不連續(xù)引起原地轉向,曲率變化率過大導致車輛的乘坐舒適性較差;五次曲線的曲率變化較穩(wěn)定。

3 實車試驗

為驗證貝塞爾曲線和樣條曲線在局部避障路徑規(guī)劃的工程實用性,選用搭載高精度GNSS/INS組合導航系統(tǒng)的無人駕駛平臺進行試驗,如圖7所示。

圖7 無人駕駛試驗平臺

圖8 基于四種曲線的車輛橫向加速度變化對比

圖9 基于四種曲線的車輛角速度變化對比

表1 兩種避障軌跡下車輛橫向加速度分析

表2 兩種避障軌跡下車輛角速度分析

從橫向加速度指標分析,由表1可知,采用三次貝塞爾曲線時,車輛的橫向加速度方差比采用三次樣條曲線時減小73%,采用五次貝塞爾曲線時,車輛的橫向加速度方差比采用五次樣條曲線時減小58%。如圖8所示,采用三次樣條路徑規(guī)劃時,橫向加速度均值最小,為-0.001 6 m/s2,采用三次樣條路徑規(guī)劃時,橫向加速度均值最大,為0.010 1 m/s2,采用三次和五次貝塞爾路徑規(guī)劃時,車輛的橫向加速度方差均比采用三次和五次樣條路徑規(guī)劃時車輛的橫向加速度方差小。此外,采用貝塞爾曲線時,車輛橫向加速度變化全程較穩(wěn)定,而采用樣條曲線時,車輛橫向加速度在t=0和t=8.5 s處存在突變。因此,不論是三次曲線還是五次曲線,采用貝塞爾曲線時,車輛的橫向控制比采用樣條曲線時更加穩(wěn)定。

從角速度指標分析,由表2可知,采用三次貝塞爾曲線時,車輛的角速度均值比采用三次樣條曲線時減小41%,角速度方差比采用三次樣條曲線時減小90%,采用五次貝塞爾曲線時,車輛的角速度均值比采用五次樣條曲線時減小77%,角速度方差比采用三次樣條曲線時減小83%;采用五次貝塞爾曲線時,車輛的角速度均值比采用三次貝塞爾曲線時減小66%,采用五次樣條曲線時,車輛的角速度均值比采用五次樣條曲線時減小13%。如圖9所示,采用五次貝塞爾路徑規(guī)劃時,車輛角速度均值最小,為0.006 1 rad/s,采用三次樣條路徑規(guī)劃時,車輛角速度均值最大,為0.030 0 rad/s。此外,采用不同階數(shù)的同類曲線,車輛的角速度變化差異不大,轉向性能均良好。進一步,不論是三次曲線還是五次曲線,采用貝塞爾曲線時,車輛角速度變化全程較穩(wěn)定;采用樣條曲線時,車輛角速度在t=4 s和t=9 s時存在突變,即車輛在轉向時導致角速度變化幅度較大。

4 結束語

為解決車輛在避障時易發(fā)生的軌跡曲率不連續(xù)所導致的原地轉向問題,本文選取三次貝塞爾曲線、三次樣條曲線、五次貝塞爾曲線和五次樣條曲線作為局部避障路徑進行研究。通過Carsim與Matlab仿真軟件對兩種方法所得的軌跡及其曲率進行對比,并通過實車試驗驗證兩種避障方法的相關技術指標,試驗結果表明如下。

(1)由于樣條曲線及貝塞爾曲線的數(shù)學特性,采用兩類曲線進行避障路徑規(guī)劃,車輛的橫向超調量均為0,車輛行駛平穩(wěn)性較好。

(2)同類曲線中,三次曲線比五次曲線的曲率波動幅度更大,曲率不連續(xù)引起原地轉向,曲率變化率過大導致車輛的乘坐舒適性較差。五次曲線的曲率變化較穩(wěn)定;

(3)同階數(shù)曲線中,采用三次貝塞爾曲線時,車輛的橫向加速度方差比采用三次樣條曲線時減小73%,采用五次貝塞爾曲線時,車輛的橫向加速度方差比采用五次樣條曲線時減小58%。因此,采用貝塞爾曲線進行避障路徑規(guī)劃時,車輛的乘坐舒適性更高。

(4)同階數(shù)曲線中,采用三次貝塞爾曲線時,車輛的角速度均值比采用三次樣條曲線時小41%,角速度方差比采用三次樣條曲線時減小90%,采用五次貝塞爾曲線時,車輛的角速度均值比采用五次樣條曲線時減小77%,角速度方差比采用三次樣條曲線時減小83%。因此,采用貝塞爾曲線進行避障路徑規(guī)劃時,車輛的駕駛穩(wěn)定性,及轉向性能更加優(yōu)越。