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

(1.西南大學(xué) 工程技術(shù)學(xué)院,重慶 400715;2.中國(guó)汽車工程學(xué)會(huì),北京 100176)

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

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

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

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

1 避障路徑規(guī)劃

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

1.1 貝塞爾曲線控制點(diǎn)

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

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

貝塞爾曲線表達(dá)式[17]如下

(1)

(2)

式中：Pi代表控制點(diǎn)坐標(biāo),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)

曲線上任意點(diǎn)曲率[1]可表示如下

(4)

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

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

(5)

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

(6)

求解得

(7)

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

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

(8)

則膨脹后半徑R可表達(dá)為

R=R0+ΔR

(9)

設(shè)第六個(gè)控制點(diǎn)狀態(tài)坐標(biāo)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],以路徑平均曲率最小為目標(biāo),對(duì)參數(shù)x1、x2、x3、x4、x5進(jìn)行優(yōu)化。同理,限制避障路徑的起點(diǎn)、終點(diǎn)和航線的連續(xù)性,選取圖2中P0、P1、P4、P5四點(diǎn),即可確定三次貝塞爾曲線。

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

圖3為基于五次樣條曲線的避障路徑規(guī)劃,以車輛避障初始點(diǎn)為原點(diǎn),車輛縱向?yàn)閄方向,車輛橫向?yàn)閅方向建立坐標(biāo)系。車輛在初始避障時(shí)的狀態(tài)為Pstart,車輛在避障終點(diǎn)時(shí)的狀態(tài)為Pend。

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

設(shè)避障路徑為

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

(12)

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

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

(13)

代入式(13),可求得

(14)

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

(15)

2 仿真分析

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

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

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

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

圖5 避障路徑示意圖

圖6 四種避障路徑曲線曲率對(duì)比

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

3 實(shí)車試驗(yàn)

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

圖7 無人駕駛試驗(yàn)平臺(tái)

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

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

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

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

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

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

4 結(jié)束語

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

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

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

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

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