融合改進(jìn)A*算法和貝塞爾曲線優(yōu)化的路徑規(guī)劃算法

謝春麗，高勝寒，孫學(xué)志

(東北林業(yè)大學(xué)交通學(xué)院，哈爾濱 150006)

0 引言

路徑規(guī)劃是機(jī)器人自動(dòng)行駛的一個(gè)重要組成部分，它的主要目的是在特定的具有障礙地圖環(huán)境下，在給定起始點(diǎn)以及終點(diǎn)的條件下，在盡可能短的時(shí)間內(nèi)生成一條最短無碰撞路徑的過程，由于汽車自動(dòng)駕駛是在機(jī)器人以及自動(dòng)引導(dǎo)汽車AGV(automated guided vehicle)的自動(dòng)行駛基礎(chǔ)上進(jìn)行的應(yīng)用，所以路徑規(guī)劃也是汽車自動(dòng)駕駛的必不缺少的組成部分。在路徑規(guī)劃領(lǐng)域，許多學(xué)者做了大量的算法研究，探索出了很多算法及改進(jìn)，這些算法各有優(yōu)劣，常用的路徑規(guī)劃方法主要有以下幾種：Dijksta算法、A*算法、蟻群算法、RRT算法、LPA*算法和D*lite算法等。Dijksta算法是貪心算法模式應(yīng)用于路徑規(guī)劃，優(yōu)點(diǎn)是產(chǎn)生最優(yōu)解的路徑，但缺點(diǎn)占用內(nèi)存大，運(yùn)行時(shí)間長[1]；A*算法是現(xiàn)在最成熟、應(yīng)用最廣的算法，通過引用啟發(fā)性函數(shù)，平衡運(yùn)行速度以及最優(yōu)解之間的關(guān)系，缺點(diǎn)是容易陷入局部最優(yōu)解[2]；蟻群算法是模仿自然界蟻群尋找食物的過程，添加信息素信息正反饋進(jìn)而尋找路徑，優(yōu)點(diǎn)是不易產(chǎn)生局部最優(yōu)，易于找到最優(yōu)解，缺點(diǎn)是收斂速度慢[3]；RRT算法探索空間能力較好，但容易受到狹窄通道影響而找不到解[4]；LPA*算法與D*lite算法同為增量啟發(fā)式算法，但前者為正向搜索，而后者為反向搜索，兩者一般適用于動(dòng)態(tài)地圖環(huán)境[5]。

本文主要以A*算法為基礎(chǔ)，對AGV的路徑規(guī)劃進(jìn)行研究，使用A*算法的改進(jìn)算法跳點(diǎn)搜索算法(jumppointssearch，JPS)進(jìn)行啟發(fā)性函數(shù)改良，從而減少Open_list以及Close_list中不必要的內(nèi)存占用，同時(shí)大幅度減少了算法的運(yùn)算時(shí)間，然后通過貝塞爾曲線(Bézier curve)進(jìn)行全局路徑平滑性優(yōu)化，再通過python仿真，驗(yàn)證在給定一些地圖參數(shù)的情況下，能得出有關(guān)最優(yōu)路徑的相關(guān)數(shù)據(jù)，使該算法在應(yīng)用于自動(dòng)引導(dǎo)汽車時(shí)，可以盡量排除模型理想化的影響。

1 傳統(tǒng)A*算法

對于路徑規(guī)劃算法方面，學(xué)者們在多年來進(jìn)行不斷研究，提出了許多穩(wěn)定成熟的算法，這些算法原理簡單，而且可以進(jìn)行大量實(shí)際運(yùn)用，如在Dijksta算法基礎(chǔ)上提出的A*算法，其公式如下：

f(n)=g(n)+h(n)

(1)

式中：f(n)為起始點(diǎn)到目標(biāo)點(diǎn)的估計(jì)代價(jià)；g(n)為起始點(diǎn)到中間點(diǎn)n的實(shí)際代價(jià)；h(n)為啟發(fā)函數(shù)，同時(shí)表示中間點(diǎn)n到目標(biāo)點(diǎn)的估計(jì)代價(jià)。A*算法主要是通過確定起始點(diǎn)和目標(biāo)點(diǎn)，建立Open_list以及Close_list 2個(gè)列表，由起點(diǎn)出發(fā)，把當(dāng)前點(diǎn)放入Open_list作為父節(jié)點(diǎn)，然后在當(dāng)前節(jié)點(diǎn)的8個(gè)附近節(jié)點(diǎn)進(jìn)行遍歷尋找子節(jié)點(diǎn)，將父節(jié)點(diǎn)加入Close_list，新的子節(jié)點(diǎn)作為父節(jié)點(diǎn)并計(jì)算f、g、h3個(gè)值，反復(fù)進(jìn)行運(yùn)算直至Open_list列表中存在目標(biāo)點(diǎn)，然后進(jìn)行從目標(biāo)點(diǎn)到起始點(diǎn)的回溯，并從f值選取最合適的路徑，得出最優(yōu)路徑。A*算法的流程如圖1所示。

圖1 傳統(tǒng)A*算法的流程框圖

在傳統(tǒng)A*算法當(dāng)中，一般將g值計(jì)算值表征為路徑所經(jīng)過柵格地圖的節(jié)點(diǎn)數(shù)，而啟發(fā)性函數(shù)選取節(jié)點(diǎn)到目標(biāo)點(diǎn)距離，一般為歐里幾德距離度量移動(dòng)代價(jià)：

(2)

式中：(x1，y1)，(x2，y2)分別為節(jié)點(diǎn)n1，n2的坐標(biāo)。對于估價(jià)函數(shù)f(n)，當(dāng)g(n)=0時(shí)，f(n)=h(n)，估價(jià)函數(shù)完全取決于啟發(fā)函數(shù)，A*算法變成使用貪心策略的廣度優(yōu)先搜索，此時(shí)計(jì)算速度快，但不一定能獲得最優(yōu)解；當(dāng)h(n)=0時(shí)，f(n)=g(n)，此時(shí)算法變成Dijksta算法，往往能獲得最優(yōu)解，但此時(shí)的計(jì)算占用內(nèi)存大，計(jì)算速度慢。

A*算法作為經(jīng)典路徑規(guī)劃算法，雖然可以較穩(wěn)定地解決路徑尋優(yōu)問題，但也容易陷入“死循環(huán)”，不考慮實(shí)際車輛運(yùn)行情況而產(chǎn)生復(fù)雜且繁多轉(zhuǎn)折點(diǎn)以及在存在動(dòng)態(tài)障礙物的地圖環(huán)境中容易碰撞動(dòng)態(tài)障礙物等問題。趙曉等[6]在 A*算法的基礎(chǔ)上，結(jié)合跳點(diǎn)搜索算法提出一種改進(jìn)的A*算法，該算法通過篩選跳點(diǎn)進(jìn)行擴(kuò)展，直到生成最終路徑，擴(kuò)展過程中使用跳點(diǎn)代替A*算法中大量可能被添加到Open_list和Close_list的不必要節(jié)點(diǎn)，從而減少計(jì)算量。陳豪等[7]提出了基于二維柵格地圖的改進(jìn)A*算法，其引入了方向矢量和并行搜索，使智能汽車路徑搜索同時(shí)具備方向性和并行性。宣仁虎[8]針對A*算法在進(jìn)行全局路徑規(guī)劃時(shí)路徑較長，轉(zhuǎn)折角度過大，路徑不夠平滑的缺點(diǎn)，提出了利用智能蟻群算法對A*算法中的路徑進(jìn)行迭代優(yōu)化的改進(jìn)算法。Zhong等[9]提出了一種新的混合路徑規(guī)劃方法，該方法將A*算法與自適應(yīng)窗口方法相結(jié)合，可以在大規(guī)模動(dòng)態(tài)環(huán)境中為移動(dòng)智能汽車進(jìn)行全局路徑規(guī)劃，實(shí)時(shí)跟蹤和避障。張陽偉等[10]提出了一種建立正反向遍歷的Open_list和Close_list一共4個(gè)列表，然后采取通過變步長雙向運(yùn)算，當(dāng)找到正方向的Open_list和反方向的Close_list存在公共節(jié)點(diǎn)時(shí)，算法結(jié)束得出最優(yōu)路徑。

綜上所述，A*算法主要存在以下不足：

1)冗余節(jié)點(diǎn)會(huì)進(jìn)行過多代價(jià)運(yùn)算，會(huì)導(dǎo)致算法運(yùn)行時(shí)間過長；

2)由于Open_list訪問節(jié)點(diǎn)過多，所以在運(yùn)行過程中，會(huì)占用大量內(nèi)存使用空間。

本文的改進(jìn)算法與A*算法相比，運(yùn)行時(shí)間會(huì)大幅縮短以及評估節(jié)點(diǎn)數(shù)量會(huì)在地圖環(huán)境較大或高障礙率的條件下大幅減少。

2 融合改進(jìn)A*算法和貝塞爾曲線優(yōu)化

2.1 柵格地圖模型建立

考慮自動(dòng)引導(dǎo)汽車(AGV)主要是在平面空間進(jìn)行運(yùn)動(dòng)，可以進(jìn)行以下表述：設(shè)智能汽車為A，所在的平面空間為Q，整個(gè)平面空間可以分為b行b列進(jìn)行均勻分割b2個(gè)均勻方塊區(qū)域，對于各個(gè)均勻方塊區(qū)域，可以進(jìn)行建立二維坐標(biāo)系，得到諸多二維坐標(biāo)，再對每個(gè)均勻方塊賦予一個(gè)特征值，柵格地圖由柵格M={Mij|Mij=0，1，2，3}組成，0代表該區(qū)域?yàn)樽詣?dòng)引導(dǎo)汽車所在起始柵格qgoal；1代表柵格地圖中自動(dòng)引導(dǎo)汽車的目的柵格qint，2代表自由區(qū)域B，可以進(jìn)行路徑規(guī)劃；3代表存在障礙物的柵格區(qū)域C，該區(qū)域無法進(jìn)行路徑規(guī)劃，在進(jìn)行仿真模擬時(shí)躲避。則在該平面空間Q中的避障空間就可以表示成為Qa={q∈W|A(q)∩C=?}，其中q為該空間的一點(diǎn)，A(q)表示的是智能汽車在空間所在位置q點(diǎn)。而路徑規(guī)劃的目標(biāo)即尋找由初始位置qint到目標(biāo)位置qgoal的最短避障路徑。

首先對障礙及其周邊區(qū)域的柵格進(jìn)行賦值，從而確定模擬環(huán)境邊界、障礙以及起始點(diǎn)，這樣智能汽車在進(jìn)行路徑規(guī)劃時(shí)，可以通過更準(zhǔn)確的信息進(jìn)行規(guī)劃，降低AGV撞到障礙的風(fēng)險(xiǎn)。下面為模型的處理。

空間Q的劃分：將平面二維空間均勻劃分成獨(dú)立的柵格。

區(qū)域邊緣的處理：將區(qū)域邊緣視作障礙區(qū)域，在規(guī)劃路徑過程中將區(qū)域邊緣不予以考慮。

2.2 改進(jìn)A*算法

2.2.1堆結(jié)構(gòu)優(yōu)化

當(dāng)AGV進(jìn)行JPS算法過程中，要進(jìn)行大量節(jié)點(diǎn)代價(jià)運(yùn)算，同時(shí)還需要通過代價(jià)值進(jìn)行Open_list以及Close_list中的節(jié)點(diǎn)坐標(biāo)元組的增加和刪除，如果每次都進(jìn)行坐標(biāo)的代價(jià)比較，將會(huì)產(chǎn)生大量的計(jì)算，同時(shí)會(huì)增加較長運(yùn)行時(shí)間。

堆是一種特殊的數(shù)據(jù)結(jié)構(gòu)，定義為：n個(gè)元素的序列{k1，k2，ki，…，kn}當(dāng)且僅當(dāng)滿足下關(guān)系時(shí)，即ki≤k2i且ki≤k2i+1或者ki≥k2i且ki≥k2i+1，則序列{k1，k2，ki，…，kn}為堆。圖2為最小堆的數(shù)據(jù)結(jié)構(gòu)的運(yùn)算過程。二叉樹的子節(jié)點(diǎn)的數(shù)值若是小于父節(jié)點(diǎn)的數(shù)值，則會(huì)進(jìn)行位置進(jìn)行置換，以此類推，直至符合所有父節(jié)點(diǎn)都小于子節(jié)點(diǎn)的數(shù)值。

圖2 最小堆的數(shù)據(jù)結(jié)構(gòu)的運(yùn)算過程

于是堆總是滿足以下2個(gè)性質(zhì)：

1)堆中某個(gè)結(jié)點(diǎn)的值總是不大于或不小于其父結(jié)點(diǎn)的值；

2)堆總是一顆完整的二叉樹。

于是可以通過最小堆(父節(jié)點(diǎn)小于子節(jié)點(diǎn)的堆)進(jìn)行最小代價(jià)值的篩選，并快速完成Open_list以及Close_list 2個(gè)列表的中節(jié)點(diǎn)的篩選。

2.2.2JPS算法

減少A*算法的主要途徑就是減少Open_list以及Close_list中冗雜節(jié)點(diǎn)的遍歷過程，而通過跳點(diǎn)可以減少遍歷列表中無關(guān)節(jié)點(diǎn)的占用內(nèi)存，馬小陸等[11]提出了在當(dāng)前節(jié)點(diǎn)與目標(biāo)點(diǎn)之間有無障礙物2種情況進(jìn)行討論跳點(diǎn)的篩選，本文將對JPS算法對跳點(diǎn)的選擇進(jìn)行討論。

Case1節(jié)點(diǎn)周圍不存在障礙物

如圖3(a)所示，x，g點(diǎn)為柵格地圖上的2個(gè)點(diǎn)，xpred為x的父節(jié)點(diǎn)，若要由xpred到達(dá)g最直接最短路徑就是由xpred→x→g路線進(jìn)行行進(jìn)，但是還有其他的路徑規(guī)劃方案，例如xpred→a2→a3→g，這種路線明顯可知不是最優(yōu)路徑，而且進(jìn)行了復(fù)雜節(jié)點(diǎn)的運(yùn)算從而導(dǎo)致A*算法最常見的周圍臨節(jié)點(diǎn)的重復(fù)訪問。由圖3(a)可知，和第二種的路徑規(guī)劃方案類似的非最優(yōu)方案都會(huì)經(jīng)過圖中的灰色柵格，而要進(jìn)行跳點(diǎn)的篩選，必須對這些灰色柵格所在的不必要節(jié)點(diǎn)進(jìn)行刪除。所以，當(dāng)節(jié)點(diǎn)的周圍不存在障礙物節(jié)點(diǎn)的時(shí)候，對于與當(dāng)前節(jié)點(diǎn)在同一水平線或者豎直線上的節(jié)點(diǎn)，存在以下的篩選條件：

L(xprep，…，n|x)≤L(xprep，x，n)

(3)

式中：函數(shù)L表示路徑的長度；則表示的鄰節(jié)點(diǎn)；L(xprep，…，n|x)表示的是以xprep為起點(diǎn)；n為目標(biāo)點(diǎn)但不經(jīng)過x的路徑集合；L(xprep，x，n)則表示的是路徑xprep→x→n。

當(dāng)目標(biāo)點(diǎn)在非直線柵格方向上如圖3(b)所示，同時(shí)在當(dāng)前節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)之間并未存在障礙物，同理依據(jù)式(4)可得此時(shí)目標(biāo)點(diǎn)在當(dāng)前節(jié)點(diǎn)的非直線柵格方向上的跳點(diǎn)篩選條件：

L(xprep，…，n|x)

(4)

由圖3可知僅由a2，b2，b3為篩選符合條件的跳點(diǎn)，而剩余的灰色節(jié)點(diǎn)將被刪除，以免導(dǎo)致計(jì)算內(nèi)存過大和運(yùn)算繁瑣。

圖3 跳點(diǎn)篩選示意圖

對于這2種情況給出以下條件進(jìn)行跳點(diǎn)篩選：

1)n不是x的自然鄰節(jié)點(diǎn)；

2)L(xprep，…，n|x)>L(xprep，x，n)

(5)

此時(shí)節(jié)點(diǎn)x周圍存在障礙物柵格，即圖4(a)以及圖4(b)中的黑色柵格，障礙物柵格創(chuàng)建了強(qiáng)制鄰節(jié)點(diǎn)(所有滿足篩選條件的x的鄰節(jié)點(diǎn)n)，即圖4(a)以及圖4(b)中的紅色柵格。這些鄰節(jié)點(diǎn)被創(chuàng)建，是因?yàn)橐恍〇鸥裼捎跂鸥竦奶娲窂奖蛔枞荒鼙徊眉簦瑘D4(a)中的a3和圖4(b)中的a1都是強(qiáng)制鄰節(jié)點(diǎn)，在篩選跳點(diǎn)的時(shí)候都放進(jìn)列表。

圖4 強(qiáng)制鄰節(jié)點(diǎn)篩選的一種情況

起始點(diǎn)中間存在障礙物的柵格地圖模型如圖5所示，淺藍(lán)色部分為篩選出來的強(qiáng)制性鄰節(jié)點(diǎn)，即跳點(diǎn)。對于路徑最優(yōu)規(guī)劃，分別將建立的跳點(diǎn)集合進(jìn)Open_list，從起點(diǎn)qint以及終點(diǎn)qgoal2個(gè)點(diǎn)作為A*算法的起始點(diǎn)，然后通過式(1)，進(jìn)行A*算法的運(yùn)算找尋最優(yōu)路徑。

圖5 JPS算法跳點(diǎn)的篩選

2.2.3改進(jìn)啟發(fā)函數(shù)

本文的JPS算法是先進(jìn)行地圖建模然后進(jìn)行地圖中跳點(diǎn)的篩選，然后在跳點(diǎn)的基礎(chǔ)上進(jìn)行A*算法的改進(jìn)啟發(fā)性函數(shù)代價(jià)運(yùn)算，從而實(shí)現(xiàn)最優(yōu)路徑尋找。A*算法一般的h(n)為歐里幾德距離(euclidean distance)，或者切比雪夫距離(chebyshev distance)：

d(x，y)=Max{|x1-x2|，|y1-y2|}

(6)

或者Octile距離：

d(x，y)=Max{|x1-x2|，|y1-y2|}+

(7)

或者曼哈頓距離(manhattan distance)：

d(x，y)=|x1-x2|+|y1-y2|

(8)

對于柵格地圖，若為四方向移動(dòng)，曼哈頓距離(manhattan distance)是最合適的啟發(fā)函數(shù)。若八方向(相較于四方向增加對角線方向)移動(dòng)，可以用切比雪夫距離(chebyshev distance)作為啟發(fā)性函數(shù)。歐里幾德距離(euclidean distance)適合方向自由度更高的地圖作為其啟發(fā)函數(shù)，但是歐式距離中具有一個(gè)開方的過程，計(jì)算量會(huì)較其他算法更大。而Octile距離主要是用于45°轉(zhuǎn)彎，比較適合本文中的八方向地圖環(huán)境，而且不存在開方等復(fù)雜計(jì)算。

在地圖環(huán)境中的最小估計(jì)函數(shù)會(huì)被地圖中的障礙物影響，h(n)不僅會(huì)因?yàn)樘S點(diǎn)之間的代價(jià)而改變，而且還會(huì)因?yàn)檎系K物的分布而改變[12]，但該文獻(xiàn)對障礙率對數(shù)化后未絕對值化，導(dǎo)致h(n)一直是負(fù)優(yōu)化。本文為了估計(jì)最小路徑距離值，設(shè)置了障礙率代表地圖環(huán)境。障礙率即當(dāng)前地圖環(huán)境下障礙節(jié)點(diǎn)數(shù)與整體地圖環(huán)境節(jié)點(diǎn)總數(shù)的比，為了減少邊界無關(guān)區(qū)域障礙的影響，此時(shí)障礙率P如式(6)所示，也可以稱之為有效障礙率，是由于將起始點(diǎn)所形成矩形區(qū)域以外的障礙物除去，其中式(9)中起始節(jié)點(diǎn)為(xs，ys)，終點(diǎn)為(xg，yg)。同時(shí)為了使數(shù)據(jù)更平滑，對障礙率進(jìn)行對數(shù)化處理，然后再與Octile距離進(jìn)行結(jié)合處理作為h(n)，即式(10)。

(9)

(10)

由上式可知，該啟發(fā)性函數(shù)在障礙率較低時(shí)，h(n)將會(huì)在f(n)中占比較小，此時(shí)搜索速度較快，但尋優(yōu)性較差，但也符合低障礙率的路徑規(guī)劃情景；該啟發(fā)性函數(shù)在障礙率較高時(shí)，h(n)將會(huì)在f(n)中占比較大，此時(shí)搜索速度較慢，但尋優(yōu)性較為良好。

由于本文主要是以八方向的移動(dòng)為主，所以不考慮曼哈頓距離，通過在相同地圖模型環(huán)境，相同起始點(diǎn)進(jìn)行JPS算法，h(n)分別采用以上距離進(jìn)行仿真，具體數(shù)據(jù)如表1所示。以下數(shù)據(jù)搜索時(shí)間僅為主算法時(shí)間，不包括地圖環(huán)境構(gòu)建以及可視化運(yùn)行時(shí)間。地圖環(huán)境為圖6，此時(shí)有效障礙率38.4%。表1為不同啟發(fā)性函數(shù)在相同地圖環(huán)境下的運(yùn)行數(shù)據(jù)，3種啟發(fā)性函數(shù)中，在較高的障礙率的地圖中，所生成路徑長度一致，但是切比雪夫距離的評估節(jié)點(diǎn)數(shù)量較另外2種算法多20.31%，占用內(nèi)存較大。而改進(jìn)的啟發(fā)性函數(shù)，在高障礙率地圖環(huán)境中，與歐里幾德距離相比較，評估節(jié)點(diǎn)數(shù)量和路徑長度一致，但是搜索時(shí)間上減少38.01%，通過表1仿真數(shù)據(jù)決定本文采取改進(jìn)啟發(fā)性函數(shù)作為啟發(fā)式函數(shù)。

圖6 測試啟發(fā)式函數(shù)地圖環(huán)境

表1 同啟發(fā)性函數(shù)在相同地圖環(huán)境下的運(yùn)行數(shù)據(jù)

3 貝塞爾曲線的車輛路徑軌跡優(yōu)化

3.1 貝塞爾曲線相關(guān)特性

貝塞爾曲線是由伯恩斯坦多項(xiàng)式(bernstein polynomial)推導(dǎo)而來[13]，貝塞爾曲線一般用來進(jìn)行折線的平滑化處理，本文運(yùn)用貝塞爾曲線對改進(jìn)的算法產(chǎn)生出來的折線路徑進(jìn)行平滑處理。

伯恩斯坦多項(xiàng)式的第n階項(xiàng)有如下形式：

(11)

(12)

其中，βi叫做伯恩斯坦系數(shù)。

由文獻(xiàn)[14]中的結(jié)論，高階貝塞爾曲線的數(shù)值穩(wěn)定性較差，為了解決數(shù)值穩(wěn)定性，同時(shí)為了滿足AGV的曲率約束，采取二階貝塞爾曲線和三階貝塞爾曲線相結(jié)合進(jìn)行路徑平滑性優(yōu)化，針對二階和三階貝塞爾曲線如下屬性進(jìn)行使用。

1)二階貝塞爾曲線表達(dá)式如式(13)所示，三階貝塞爾曲線表達(dá)式如式(14)所示：

P(t)=P0(1-t)2+2P1(1-t)t+P2t2，

t∈[0，1]

(13)

P(t)=P0(1-t)3+3P1(1-t2)t+

3P2(1-t)t2+P3t3，t∈[0，1]

(14)

2)二階貝塞爾曲線會(huì)經(jīng)過第1、3個(gè)控制點(diǎn)；三階貝塞爾曲線經(jīng)過第1、4個(gè)控制點(diǎn)。

3)曲線在端點(diǎn)切向量表達(dá)式如下：

P′(t)=-2P0(1-t)+2P1(1-2t)+2P2t2，

t∈[0，1]

(15)

P′(t)=-3P0(1-t)2+3P1(3t2-4t+1)-

6P2(1-t)t+3P3t2，t∈[0，1]

(16)

4)貝塞爾曲線上任意一點(diǎn)曲率為：

(17)

5)貝塞爾曲線軌跡起始點(diǎn)曲率為：

(18)

6)仿射變換不變特性

通過式(17)二階貝塞爾曲線上曲率和三階貝塞爾曲線上曲率可知，路徑軌跡曲線連續(xù)條件為x′(t)、y′(t)不同時(shí)為0，若x′(t)、y′(t)同時(shí)為0，可知κ(t)=0，而這使得路徑軌跡無法到達(dá)后續(xù)控制點(diǎn)，但這與貝塞爾曲線定義相矛盾，所以由二階貝塞爾曲線確定的軌跡曲線和由三階貝塞爾曲線確定的軌跡曲線曲率處處連續(xù)。

對于AGV，控制其貝塞爾曲線軌跡的每一個(gè)控制點(diǎn)的曲率是不太容易的，但對于AGV最大前輪轉(zhuǎn)向角是有一定范圍的，通過式(19)，可以知道控制點(diǎn)曲率與最大前輪轉(zhuǎn)向角的關(guān)系。

(19)

式中：κ為曲率;γ為曲率半徑即轉(zhuǎn)彎半徑;α為前輪轉(zhuǎn)向角;L為前后輪軸距，所以存在式(20)，即為曲率有界條件。

Kmin≤κ(τ)≤Kmax

(20)

式中：Kmin和Kmax分別為最小、最大曲率。

貝塞爾曲線具有以下性質(zhì)：擬合曲線的起始點(diǎn)和終止點(diǎn)與被平滑化處理的折線的起始點(diǎn)和終止點(diǎn)重合；折線點(diǎn)一旦確定，擬合曲線唯一。貝塞爾曲線平滑處理后，使生成路徑與折線相比更貼近實(shí)際，更適合AGV的實(shí)際運(yùn)行。

3.2 貝塞爾曲線優(yōu)化問題以及改進(jìn)

由于高階貝塞爾曲線數(shù)值穩(wěn)定性較差，所以一般的路徑優(yōu)化都采取二階和三階貝塞爾曲線優(yōu)化[14]。在不同控制點(diǎn)的情況，會(huì)存在二階貝塞爾曲線以及三階貝塞爾曲線之間的優(yōu)化選擇，如圖7所示，對前3個(gè)點(diǎn)進(jìn)行二階貝塞爾曲線優(yōu)化，則第4個(gè)點(diǎn)的優(yōu)化會(huì)被遺漏，從而導(dǎo)致更適合的三階貝塞爾曲線優(yōu)化被二階貝塞爾曲線優(yōu)先替代，不能達(dá)到最優(yōu)。

圖7 二階貝塞爾曲線與三階貝塞爾曲線判定以及優(yōu)化

圖8為常見的二階貝塞爾曲線以及三階貝塞爾曲線的優(yōu)化。至于更多控制點(diǎn)的優(yōu)化則采取n組二階貝塞爾曲線以及m組三階貝塞爾曲線進(jìn)行組合優(yōu)化而不采用數(shù)值不穩(wěn)定高階貝塞爾曲線。

圖8 二階貝塞爾曲線與三階貝塞爾曲線的判定特定節(jié)點(diǎn)及其優(yōu)化

將JPS算法所生成路徑，代入到貝塞爾曲線改進(jìn)程序中，由于二階貝塞爾曲線和三階貝塞爾曲線所需控制點(diǎn)數(shù)量不同，應(yīng)先判斷能否進(jìn)行三階貝塞爾曲線優(yōu)化，若能則進(jìn)行優(yōu)化，否則進(jìn)行二階貝塞爾曲線優(yōu)化，而對于能否進(jìn)行三階貝塞爾曲線優(yōu)化的判斷，通過記錄控制點(diǎn)坐標(biāo)，然后記錄方向也即4個(gè)控制點(diǎn)的3個(gè)方向向量，當(dāng)存在如下9種情況時(shí)，即可進(jìn)行三階貝塞爾曲線優(yōu)化。

(21)

(22)

(23)

(24)

(25)

(26)

(27)

(28)

(29)

以上的這些公式中的dxn為第n個(gè)橫坐標(biāo)方向變化量，dyn為第n個(gè)縱坐標(biāo)方向變化量。

這樣，全局路徑規(guī)劃產(chǎn)生的路徑將會(huì)被二階貝塞爾曲線和三階貝塞爾曲線共同優(yōu)化，而不會(huì)產(chǎn)生三階貝塞爾曲線優(yōu)化中控制點(diǎn)不足以及二階貝塞爾曲線優(yōu)化中容易遺漏一個(gè)控制點(diǎn)而未進(jìn)行三階貝塞爾曲線優(yōu)化這2種情況導(dǎo)致的曲線優(yōu)化不理想的情況。

4 仿真與分析

4.1 改進(jìn)算法仿真

本文在Window 10，處理器為i7-6900H的環(huán)境下通過python3.9進(jìn)行仿真，為驗(yàn)證本文的改進(jìn)JPS算法的可行性，地圖環(huán)境的障礙物位置均為隨機(jī)選取，在隨機(jī)的地圖環(huán)境能更好地驗(yàn)證算法的有效性以及先進(jìn)性。仿真分別在20*20、30*30、50*50的有效障礙率為20%的低障礙率的地圖環(huán)境以及20*20、30*30、50*50的有效障礙率為40%左右的高障礙率地圖環(huán)境，分別進(jìn)行傳統(tǒng)A*算法、JPS算法以及改進(jìn)JPS算法仿真。同時(shí)為驗(yàn)證改進(jìn)算法的路徑生成效果以及貝塞爾曲線的優(yōu)化效果，在驗(yàn)證啟發(fā)性函數(shù)的30*30，有效障礙率為38.4%的地圖環(huán)境下進(jìn)行相應(yīng)的仿真運(yùn)行，仿真結(jié)果如圖9—12所示。

圖9 傳統(tǒng)A*算法的路徑搜索

4.2 評價(jià)指標(biāo)

為從實(shí)驗(yàn)結(jié)果中分析算法的有效性，在仿真實(shí)驗(yàn)中選取路徑規(guī)劃算法評估節(jié)點(diǎn)數(shù)量、搜索時(shí)間以及產(chǎn)生路徑長度作為算法的評價(jià)指標(biāo)。傳統(tǒng)A*算法、JPS算法、改進(jìn)JPS算法在較低障礙率地圖環(huán)境下測試結(jié)果見表2，在高障礙率地圖環(huán)境下測試結(jié)果如表3。數(shù)據(jù)運(yùn)行時(shí)間只是主算法運(yùn)行時(shí)間，不包括地圖建模時(shí)間，可視化時(shí)間以及曲線優(yōu)化時(shí)間。同時(shí)運(yùn)行時(shí)間過短會(huì)產(chǎn)生波動(dòng)較大，所以如下數(shù)據(jù)為進(jìn)行5次仿真取的平均值。同時(shí)由于不同障礙率的地圖環(huán)境的障礙分布不同，也會(huì)對時(shí)間產(chǎn)生一些影響。

圖10 JPS算法所篩選的跳點(diǎn)

圖11 改進(jìn)JPS算法的路徑搜索

圖12 改進(jìn)貝塞爾曲線所生成的改進(jìn)JPS算法所生成路徑

表2 傳統(tǒng)A*算法、JPS算法以及改進(jìn)JPS算法的路徑搜索在較低障礙率地圖環(huán)境下的數(shù)據(jù)

表3 傳統(tǒng)A*算法、JPS算法以及改進(jìn)JPS算法的路徑搜索在高障礙率地圖環(huán)境下的數(shù)據(jù)

由表2可知，在低障礙率地圖環(huán)境下，在地圖環(huán)境簡單的20*20的條件下，無論是評估節(jié)點(diǎn)個(gè)數(shù)還是搜索時(shí)間，改進(jìn)算法與A*算法以及JPS算法相比會(huì)存在劣勢，但生成的路徑長度所差不大。當(dāng)?shù)貓D環(huán)境變大時(shí)，評估節(jié)點(diǎn)數(shù)會(huì)大于A*算法，但會(huì)小于JPS算法，而運(yùn)行時(shí)間相對另2種算法在30*30的地圖環(huán)境下分別減少99.5%和14.1%，在50*50，20%有效障礙率的地圖環(huán)境下分別減少99.7%和9.3%。對于后2組數(shù)據(jù)，體現(xiàn)改進(jìn)算法在有效障礙率變大時(shí)，評估節(jié)點(diǎn)數(shù)會(huì)大幅減少，而且運(yùn)行時(shí)間相較于本身也減少11.2%。

通過表3發(fā)現(xiàn)，雖然在路徑長度上可能略長于A*算法，但路徑長度基本與其他2種算法無異，而且改進(jìn)算法不論是在評估節(jié)點(diǎn)數(shù)量與其他2個(gè)算法相比，分別減少了14.9%和25%；在搜索時(shí)間上，相較于其他2種算法，分別減少99.8%和59.2%。

為了驗(yàn)證評估節(jié)點(diǎn)數(shù)量對貝塞爾曲線的擬合效果影響，分別進(jìn)行了對低障礙率的改進(jìn)算法的20%障礙率的20*20、30*30和50*50的地圖環(huán)境產(chǎn)生的路徑進(jìn)行貝塞爾曲線優(yōu)化。優(yōu)化結(jié)果如圖13—15所示，其中綠點(diǎn)為起點(diǎn)，粉色點(diǎn)為終點(diǎn)，發(fā)現(xiàn)評估節(jié)點(diǎn)數(shù)量并不影響擬合效果。

圖13 在20*20地圖環(huán)境下的貝塞爾曲線擬合曲線

圖14 在30*30地圖環(huán)境下的貝塞爾曲線擬合曲線

圖15 在50*50地圖環(huán)境下的貝塞爾曲線擬合曲線

5 結(jié)論

對AGV的路徑規(guī)劃來說，可行性是前提，快速性是其目標(biāo)。本文改良JPS算法實(shí)現(xiàn)過程中，對軌跡進(jìn)行了貝塞爾曲線優(yōu)化，以保證規(guī)劃的路徑滿足AGV運(yùn)動(dòng)可行性。同時(shí)使用了最小堆數(shù)據(jù)結(jié)構(gòu)對Open_list取最小值進(jìn)行優(yōu)化，該方法降低了內(nèi)存消耗，加快了算法的運(yùn)算。對比傳統(tǒng)的算法，改良JPS算法的改進(jìn)之處在于啟發(fā)性函數(shù)這一改善。在python編程環(huán)境下，進(jìn)行較低有效障礙率以及高障礙率的20*20、30*30、50*50地圖環(huán)境下的仿真得出的試驗(yàn)結(jié)果表明，該算法在有效障礙率較低時(shí)，運(yùn)算時(shí)間短，尋優(yōu)結(jié)果較好；在有效障礙率較高時(shí)，運(yùn)算時(shí)間也較對照組相比更短，訪問節(jié)點(diǎn)數(shù)較少，節(jié)約了內(nèi)存空間，搜索時(shí)間大幅度縮短。對于仿真生成的路徑軌跡，通過改進(jìn)的貝塞爾曲線優(yōu)化，滿足了曲率連續(xù)有界約束，驗(yàn)證了算法的有效性與高效性。