夏長高,丁偉兵,韓江義

(江蘇大學(xué) 汽車與交通工程學(xué)院， 江蘇 鎮(zhèn)江 212000)

0 引言

路徑規(guī)劃是智能車輛的核心技術(shù)，在一定要求下快速生成由初位置達(dá)到目標(biāo)的安全無碰撞路徑[1-2]。常用的路徑規(guī)劃算法有ACO、A-star、粒子群、RRT、人工勢場法(APF)等。其中ACO、A-star、粒子群等算法在應(yīng)用中存在計(jì)算量較大、迭代時(shí)間長，以及在未知環(huán)境下規(guī)劃實(shí)時(shí)性較差等不足[3-5]。RRT算法基于概率采樣向目標(biāo)生成節(jié)點(diǎn)，存在規(guī)劃路徑不平滑、規(guī)劃時(shí)間長的不足[6]。APF算法最早由Khatib[7]提出，應(yīng)用于機(jī)器人避障領(lǐng)域，因其計(jì)算量小且具有較好的實(shí)時(shí)性等優(yōu)點(diǎn)而被廣泛運(yùn)用。但是APF算法應(yīng)用于智能車輛也存在一些不足，如易陷入局部最小值、目標(biāo)不可達(dá)、路徑曲率不連續(xù)、超出道路邊界、不滿足車輛動(dòng)力學(xué)要求等[8-10]。張珂等[11]將汽車設(shè)置為橢圓形障礙物，有效降低路徑曲率，但是所規(guī)劃的路徑局部出現(xiàn)曲率變化較大，會(huì)導(dǎo)致跟蹤效果不理想。劉美辰等[12]在解決局部最小值時(shí)，通過給車輛方向盤一個(gè)轉(zhuǎn)角，以擺脫局部最小值點(diǎn)。余騰偉等[13]基于RRT-APF的融合算法，通過RRT算法得到多個(gè)虛擬目標(biāo)點(diǎn)，這樣可以避免APF算法時(shí)陷入局部最小值點(diǎn)，但是因?yàn)樾枰獞?yīng)用RRT與APF的2次規(guī)劃，需要的計(jì)算時(shí)間較長，實(shí)時(shí)性較低。

針對以上算法存在的不足，本文提出一種基于B樣條-改進(jìn)APF算法，首先加入車輛與目標(biāo)距離的調(diào)節(jié)因子；設(shè)計(jì)可變邊界斥力勢場以及中心引力勢場，以滿足無人車應(yīng)用于果園等非結(jié)構(gòu)類型的道路，不僅可以防止車輛在避障中超出邊界以及貼近中心線行駛，而且可以規(guī)劃出平滑的路徑；將3次準(zhǔn)均勻B樣條優(yōu)化算法嵌入到改進(jìn)的APF算法中，調(diào)用設(shè)計(jì)的B樣條曲線優(yōu)化算法，實(shí)現(xiàn)二階導(dǎo)數(shù)的連續(xù)、路徑曲率連續(xù)。

1 傳統(tǒng)人工勢場

1.1 傳統(tǒng)人工勢場法基本原理

人工勢場法廣泛應(yīng)用于路徑規(guī)劃中，基本原理是在目標(biāo)點(diǎn)周圍構(gòu)建引力勢場，在障礙物周圍構(gòu)建斥力勢場，通過引力與斥力的合力來控制車輛向目標(biāo)點(diǎn)移動(dòng)[14]，傳統(tǒng)人工勢場的受力示意圖如圖1。

圖1 傳統(tǒng)人工勢場受力分析示意圖

1.2 不足及改進(jìn)思路

基于人工勢場法原理以及結(jié)合相關(guān)文獻(xiàn)，在傳統(tǒng)人工勢場中，只考慮了控制對象與障礙物、目標(biāo)的歐幾里得距離，距離障礙物越近，斥力越強(qiáng)，距離目標(biāo)越遠(yuǎn)，引力越強(qiáng)[15]。但是在實(shí)際應(yīng)用中存在一些問題：

1) 局部最小值。當(dāng)障礙物的合斥力與目標(biāo)所產(chǎn)生的引力在數(shù)值上大小相等，在同一直線上且方向相反，這時(shí)被控對象陷入局部最優(yōu)值點(diǎn)，無法完成完整的路徑規(guī)劃。

2) 目標(biāo)不可達(dá)。當(dāng)在目標(biāo)附近有障礙物存在時(shí)，因?yàn)榭拷繕?biāo)點(diǎn)，目標(biāo)產(chǎn)生的引力變小，而此時(shí)障礙物產(chǎn)生的斥力超過目標(biāo)的引力，使被控對象在目標(biāo)點(diǎn)處抖動(dòng)，無法到達(dá)目標(biāo)點(diǎn)。

3) 超出道路邊界、偏離行駛路徑中心線。在復(fù)雜的道路上，避開多障礙物過程中由于多個(gè)障礙物的合斥力的作用會(huì)出現(xiàn)被控對象超出道路邊界的情況；此外，在果園等作業(yè)道路中行駛，需要沿著作業(yè)道路的中間線行駛，但是傳統(tǒng)人工勢場法中主要依靠目標(biāo)的吸引力行進(jìn)，導(dǎo)致實(shí)際的路徑無法沿著道路中心。

4) 規(guī)劃避障路徑曲率不連續(xù)。在靠近障礙物時(shí)，產(chǎn)生較大的斥力勢場，而導(dǎo)致避障路徑曲率不連續(xù)。

對于上述不足，本文提出了一種B樣條-改進(jìn)APF算法，可以很好地解決以上問題，該算法針對局部最小值引入虛擬障礙物；依據(jù)果園等非結(jié)構(gòu)化道路對斥力場函數(shù)進(jìn)行設(shè)計(jì)，并加入可變邊界斥力勢場、中心線引力勢場；通過設(shè)計(jì)的3次B樣條優(yōu)化算法，確定避障控制點(diǎn)，對規(guī)劃的路徑進(jìn)行擬合優(yōu)化，得到曲率連續(xù)且滿足車輛動(dòng)力學(xué)要求的路徑。

2 算法改進(jìn)

2.1 障礙物斥力場的改進(jìn)

傳統(tǒng)APF的斥力場函數(shù)，在斥力場模型中，加入調(diào)節(jié)因子，使在靠近目標(biāo)的過程中，斥力隨著引力減小而減小，并同時(shí)減小為零，從而可以解決目標(biāo)不可達(dá)的問題，并且增加一個(gè)由車輛指向目標(biāo)的斥力，有助于使斥力矢量與引力矢量之間的角度減小，促進(jìn)規(guī)劃路徑的平滑。改進(jìn)的斥力場函數(shù)表示為[16]：

(1)

斥力函數(shù)為斥力場函數(shù)負(fù)梯度[17]：

(2)

(3)

(4)

式中：Freq1為障礙物指向車輛的斥力；Freq2為車輛指向目標(biāo)點(diǎn)的斥力；Freq為合斥力；Vb為障礙物指向車輛斥力的單位向量；Vg為車輛指向目標(biāo)斥力的單位向量。

解決局部最小值點(diǎn)問題，在本文的改進(jìn)APF算法中，當(dāng)控制對象處于局部最小值時(shí)，建立虛擬障礙物提供逃脫斥力；反之逃脫斥力為零。該逃脫斥力函數(shù)為：

(5)

式中：Freq3為虛擬障礙物指向車輛的斥力；Vy為虛擬障礙物指向車輛斥力的單位向量；Fsum為當(dāng)前車輛所受合力大小。

2.2 可變邊界斥力場的建立

農(nóng)業(yè)無人車在進(jìn)行避障的過程中很容易出現(xiàn)越過道路邊界的問題，通過建立道路邊界斥力場，不僅可以避免越出道路邊界的問題，還可以促進(jìn)車輛向中間區(qū)域行駛，使規(guī)劃路徑更加平緩。道路邊界斥力場模型為：

(6)

式中：kedge為邊界斥力勢場增益系數(shù)；Yl、Yr分別為車輛質(zhì)心到左右邊界的距離;w為車輛寬度。

由式(6)知，當(dāng)靠近邊界時(shí)受到的斥力增大，可以防止無人車在避障中出現(xiàn)道路越界的問題[18]。

2.3 可變中心線引力勢場的建立

中心線引力勢場函數(shù)表示為：

(7)

2.4 3次B樣條曲線優(yōu)化規(guī)劃路徑

B樣條曲線是B-樣條基函數(shù)的線性組合，k階B樣條曲線可實(shí)現(xiàn)k-1次導(dǎo)數(shù)連續(xù)，因此，B樣條曲線常應(yīng)用于路徑優(yōu)化與軌跡規(guī)劃中。應(yīng)用B樣條曲線對規(guī)劃的路徑進(jìn)行優(yōu)化，不僅可以指定階次，而且改變控制點(diǎn)僅會(huì)對部分曲線形狀產(chǎn)生改變，保留Bezier曲線優(yōu)點(diǎn)的同時(shí)，彌補(bǔ)了Bezier曲線在局部無法進(jìn)行修改的不足。

設(shè)存在n+1個(gè)控制點(diǎn)Pi(i=0,1,2,…,n)，節(jié)點(diǎn)向量T=[t0,t1,…,tn+k+1]，k階B樣條曲線可以定義如下[19]：

(8)

式中：Bi,k(t)是第i個(gè)k階B樣條基函數(shù)在參數(shù)t處的值，k為B樣條基函數(shù)階數(shù)(k=0,1,…,m)；下標(biāo)i為B樣條的序號；Pi為控制點(diǎn)；t為位置參數(shù)，0≤t≤1。

其中，Bi,k(t)表達(dá)式為[19]：

高等教育要符合大學(xué)生的身心發(fā)展和教育自身的規(guī)律。構(gòu)建科學(xué)的漸進(jìn)式專業(yè)認(rèn)知教育體系需要吸取系統(tǒng)論、協(xié)同論、人的全面發(fā)展理論知識(shí)，全面充分的認(rèn)識(shí)大學(xué)生的成長成才教育規(guī)律。

(9)

在綜合優(yōu)化效果和計(jì)算復(fù)雜度，采用3次準(zhǔn)均勻B樣條曲線[20-22]對改進(jìn)APF規(guī)劃的路徑進(jìn)行曲線優(yōu)化，準(zhǔn)均勻B樣條曲線保留了Bezier曲線在2個(gè)端點(diǎn)處的性質(zhì)，其節(jié)點(diǎn)矢量中兩端節(jié)點(diǎn)具有重復(fù)度k+1，即t0=t1=…=tk，tn+1=tn+2=…=tn+k+1，如T=[0,0,0,0.2,0.4,0.6,0.8,1,1,1]。其他定義域在[tk,tn+1]內(nèi)的節(jié)點(diǎn)均勻分布，重復(fù)度為1節(jié)點(diǎn)區(qū)間長度為Δi=常數(shù)>0(i=k,k+1,…,n)。

取k=3，則有3次B樣條曲線表達(dá)式：

(10)

基函數(shù)表達(dá)式如下：

(11)

式中：t為位置參數(shù)。

將式 (11)代入式(10)，可得3次B樣條曲線表達(dá)式：

t2(3P0-6P1+3P2)+

t(-3P0+3P2)+

(P0+4P1+P2)]

(12)

將式(11)用矩陣形式表達(dá)：

(13)

式中：P0、P1、P2、P3為控制點(diǎn)。

當(dāng)有n個(gè)控制點(diǎn)Pi(i=0,1,…,n-1)，以P0、P1、P2、P3作為控制點(diǎn)得到首條3次B樣條曲線；再以P1～P4繪制第2條曲線；類推以Pn-4～Pn-1繪制第n-3條曲線，繪制的曲線自然銜接，有二階導(dǎo)數(shù)連續(xù)的特性。

本文設(shè)計(jì)的B樣條優(yōu)化算法對于控制點(diǎn)的選取如圖2所示，該部分講述的是局部路徑控制點(diǎn)的選取，其余控制點(diǎn)也是依據(jù)該方法進(jìn)行選取。其中P1-P4的控制點(diǎn)都是通過改進(jìn)路徑規(guī)劃算法所得到的路徑節(jié)點(diǎn)，控制點(diǎn)P1在規(guī)劃路徑的轉(zhuǎn)向起始點(diǎn)；控制點(diǎn)P2為車輛行駛軸線方向距離障礙最近的點(diǎn)；控制點(diǎn)P3為控制點(diǎn)P2與P4的中間點(diǎn)；控制點(diǎn)P4為避障過程中車身回正后的質(zhì)心位置；控制點(diǎn)P5是經(jīng)過控制點(diǎn)P3且垂直于P3與障礙物連線的直線，與過控制點(diǎn)P5所在的水平軸線所形成的交點(diǎn)。

圖2 局部路徑控制點(diǎn)選取示意圖

2.5 算法流程

本文設(shè)計(jì)的算法流程如下：

步驟1算法初始化，設(shè)置初始化參數(shù)，如初始位置Ps、目標(biāo)位置Pg；給定相關(guān)參數(shù)，如引力、斥力的增益系數(shù)katt、kp、步長d0以及障礙物影響范圍ρ0等。

步驟2更新車輛與障礙物、目標(biāo)的位置向量以及計(jì)算距離。

步驟3計(jì)算障礙物斥力與邊界斥力矢量和Freq、目標(biāo)引力與中心線引力矢量和Fatt，獲得作用于車輛的矢量和Fsum。

步驟4判斷當(dāng)前是否處于局部最優(yōu)，如若判斷是，設(shè)立虛擬障礙物提供逃脫斥力，并進(jìn)行下一步；反之逃脫斥力為零，執(zhí)行下一步。

步驟5計(jì)算車輛下一時(shí)刻的位置，沿著合力矢量方向行進(jìn)一個(gè)步長，并將路徑點(diǎn)更新在Path數(shù)組中。

步驟6獲得路徑數(shù)組中Bezier控制點(diǎn)并存儲(chǔ)數(shù)組中，調(diào)用設(shè)計(jì)的B樣條曲線優(yōu)化算法模塊對路徑進(jìn)行優(yōu)化，并更新優(yōu)化的路徑。

步驟7判斷與目標(biāo)的距離是否小于常數(shù)值K(K數(shù)值設(shè)定小于步長，因?yàn)榛谠O(shè)置的步長難以正好到達(dá)目標(biāo)點(diǎn))，如果判斷認(rèn)定到達(dá)目標(biāo)，則跳出循環(huán)，并將目標(biāo)路徑更新在Path數(shù)組中；反之，若與目標(biāo)的距離大于K，跳到步驟2執(zhí)行。

3 仿真及結(jié)果分析

3.1 針對局部最小值的改進(jìn)仿真分析

為檢驗(yàn)上述算法的可行性，本文將黃海某小型園藝電動(dòng)拖拉機(jī)整車參數(shù)作為仿真檢驗(yàn)對象，其軸距為1.6 m，寬度為1.2 m，前輪轉(zhuǎn)角范圍設(shè)置在(-π/4,π/4),最小轉(zhuǎn)彎半徑約為2 m,所以理論規(guī)劃的路徑曲率應(yīng)低于0.5 m-1，設(shè)置障礙物的影響距離為2 m。接下來基于Matlab 2019b進(jìn)行路徑規(guī)劃的仿真試驗(yàn)，分別使用本文設(shè)計(jì)的算法，與A-APF、B-APF、傳統(tǒng)人工勢場法進(jìn)行對比分析，設(shè)置基礎(chǔ)參數(shù)如表1所示。

表1 仿真基礎(chǔ)參數(shù)設(shè)置

對于上文所提出的問題，分別將本文設(shè)計(jì)的改進(jìn)算法與傳統(tǒng)算法以及其他學(xué)者算法進(jìn)行對比，其中文獻(xiàn)[17]中所改進(jìn)的人工勢場算法(記作A-APF)、文獻(xiàn)[9]中改進(jìn)的人工勢場算法(記作B-APF)、以及傳統(tǒng)人工勢場算法進(jìn)行多次對比分析，從而驗(yàn)證本文提出的B樣條-改進(jìn)APF算法在路徑規(guī)劃有著良好的可行性。其中，B-APF算法的改進(jìn)主要是在傳統(tǒng)斥力勢場中加入調(diào)節(jié)因子；A-APF算法在B-APF基礎(chǔ)上，對斥力場函數(shù)改進(jìn)，并加入了道路邊界斥力勢場。

針對局部最小值的仿真如圖3所示。對于傳統(tǒng)人工勢場，在合力作用下，控制對象在X=6 m處陷入了局部最優(yōu)，導(dǎo)致無法到達(dá)目標(biāo)點(diǎn)；A-APF算法解決了局部最小值的問題，但避障路徑曲率不連續(xù)；本文算法通過建立的虛擬障礙物提供的逃脫斥力，彌補(bǔ)局部最小值的不足，并基于設(shè)計(jì)的B樣條優(yōu)化模塊，得到平滑且曲率連續(xù)的路徑。

圖3 局部最小值仿真曲線

對于多個(gè)障礙物的局部最小值問題，具體仿真結(jié)果如圖4所示。由仿真結(jié)果可知，在傳統(tǒng)APF算法中控制對象陷于局部最小值位置，無法到達(dá)目標(biāo)位置；在改進(jìn)APF算法中，增加由對象指向目標(biāo)的斥力，可以順利達(dá)到目標(biāo)位置。

圖4 局部最小值仿真曲線

3.2 針對目標(biāo)不可達(dá)改進(jìn)仿真分析

對于解決目標(biāo)不可達(dá)的問題，如圖5所示。在仿真結(jié)果中可以看出，應(yīng)用傳統(tǒng)APF算法由于在目標(biāo)點(diǎn)處受到的斥力大于引力，導(dǎo)致無法到達(dá)目標(biāo)點(diǎn)。本文算法在斥力場函數(shù)中加入調(diào)節(jié)因子，在接近目標(biāo)點(diǎn)附近時(shí)，受到的障礙物斥力也會(huì)同時(shí)減小，可以達(dá)到目標(biāo)點(diǎn)。

圖5 目標(biāo)不可達(dá)問題仿真曲線

3.3 加入中心線引力場模型的仿真分析

車輛在果園等非結(jié)構(gòu)化道路環(huán)境中進(jìn)行，作業(yè)行駛路線應(yīng)沿著道路中間線，如圖6所示?？梢姡疚母倪M(jìn)的算法可以使車輛在無障礙行駛中，沿著中心線行進(jìn)作業(yè)。

圖6 無障礙路徑規(guī)劃算法曲線

3.4 針對超出邊界的仿真分析

針對超出邊界的問題，仿真結(jié)果如圖7所示，在所設(shè)置的道路環(huán)境中，利用傳統(tǒng)APF算法所規(guī)劃的路徑在X=15～20 m出現(xiàn)較大的縱向偏離現(xiàn)象；而通過引入可變道路邊界斥力勢場和中心線引力勢場，規(guī)劃的路徑?jīng)]有出現(xiàn)越過道路邊界的現(xiàn)象，障礙物斥力較小的情況下，可以貼近中間線行駛，此外，所規(guī)劃的路徑相比較傳統(tǒng)APF算法更加連續(xù)、平滑。

圖7 針對超出邊界問題的算法曲線

3.5 不同道路環(huán)境下路徑規(guī)劃算法對比分析

表2為各算法在不同道路環(huán)境仿真時(shí)間，本文設(shè)計(jì)的算法在復(fù)雜道路總運(yùn)算時(shí)間僅為6.982 ms，依然有著APF算法運(yùn)算快捷的特性。

表2 不同道路環(huán)境算法仿真時(shí)間

針對單一障礙物環(huán)境的路徑規(guī)劃，設(shè)置起點(diǎn)坐標(biāo)為(0,0)，目標(biāo)坐標(biāo)為(12,0)，將各算法進(jìn)行對比分析，仿真結(jié)果如圖8所示。其中，圖8(a)為路徑規(guī)劃仿真圖，可以直觀看出，本文設(shè)計(jì)的算法所規(guī)劃的避障路徑更加平滑。圖8(b)、8(c)為路徑的切向斜率、曲率，與A-APF算法相比，本文所設(shè)計(jì)的算法在路徑的最大切向斜率、平均切向斜率分別降低30.64%、34.24%，在路徑的最大曲率、平均曲率分別降低44.80%、39.13%；與B-APF算法相比，路徑的最大切向斜率、平均切向斜率分別降低49.79%、49.00%，而B-APF規(guī)劃路徑的曲率不連續(xù)。

圖8 單一障礙物障礙路徑規(guī)劃仿真結(jié)果曲線

針對復(fù)雜障礙物環(huán)境的路徑規(guī)劃，設(shè)置起點(diǎn)坐標(biāo)為(0,0)，目標(biāo)坐標(biāo)為(27.5,0)，將各算法進(jìn)行對比分析，仿真結(jié)果如圖9所示。與A-APF算法相比，本文設(shè)計(jì)算法在路徑最大切向斜率、平均切向斜率分別降低71.25%、8.80%，在路徑最大曲率、平均曲率分別降低58.64%、33.47%；與B-APF算法相比，在路徑的最大切向斜率、平均切向斜率分別降低73.23%%、34.41%，而B-APF規(guī)劃的路徑曲率不連續(xù)。

圖9 多離散障礙物路徑規(guī)劃仿真結(jié)果曲線

4 結(jié)論

針對傳統(tǒng)APF在果園等非結(jié)構(gòu)化道路路徑規(guī)劃中存在的不足，提出了一種B樣條優(yōu)化與改進(jìn)APF的融合算法，加入了車輛與目標(biāo)距離的調(diào)節(jié)因子；對斥力勢場函數(shù)進(jìn)行了改進(jìn)，引入了可變邊界斥力勢場以及中心引力斥力勢場，滿足無人作業(yè)車應(yīng)用于果園等非結(jié)構(gòu)類型的道路；將3次準(zhǔn)均勻B樣條優(yōu)化算法嵌入到改進(jìn)的APF算法中，對規(guī)劃的路徑進(jìn)行優(yōu)化，規(guī)劃平滑并曲率符合車輛動(dòng)力學(xué)約束的路徑。

仿真實(shí)驗(yàn)表明，所設(shè)計(jì)的B樣條-改進(jìn)APF算法相比其他改進(jìn)的路徑規(guī)劃算法，曲率連續(xù)，在復(fù)雜路徑下的最大曲率與平均曲率為0.230 2、0.065 2 m-1，滿足仿真對象黃海某小型園藝電動(dòng)拖拉機(jī)的動(dòng)力學(xué)約束。此外，所設(shè)計(jì)的算法在復(fù)雜路徑下運(yùn)算時(shí)間僅為6.982 ms，依然有著人工勢場法計(jì)算快速的特性，該算法可以應(yīng)用于在線軌跡規(guī)劃。本文設(shè)計(jì)主要針對果園等非結(jié)構(gòu)化道路的避障路徑規(guī)劃，下一步會(huì)融入自適應(yīng)模塊，使算法可以兼容多種類型的道路。