吳 正 趙懷林

(上海應(yīng)用技術(shù)大學(xué)電氣與電子工程學(xué)院 上海 201499)

0 引 言

隨著無(wú)人駕駛領(lǐng)域和范圍的不斷擴(kuò)展，路徑規(guī)劃算法的研究越來(lái)越受到重視，路徑規(guī)劃主要分為基于環(huán)境信息已知的全局路徑規(guī)劃和依靠傳感器感知的局部路徑規(guī)劃。全局路徑規(guī)劃可以迅速規(guī)劃出障礙物已知情況下的路徑，但如果在突然有障礙物闖入的情況下容易發(fā)生不必要的碰撞，局部路徑規(guī)劃根據(jù)傳感器反饋的信息進(jìn)行處理，形成閉環(huán)控制，從而可以繞過(guò)環(huán)境中的動(dòng)態(tài)障礙物，但缺乏全局掌控能力。

基于柵格地圖的路徑規(guī)劃算法中，常用的有Dijkstra[1]、最佳優(yōu)先算法和A*算法等。其中Dijkstra算法以初始點(diǎn)開始迭代搜索初始點(diǎn)周圍的所有節(jié)點(diǎn)，將周圍的節(jié)點(diǎn)全部納入未搜索節(jié)點(diǎn)集，一圈圈向外擴(kuò)散，直到到達(dá)目標(biāo)節(jié)點(diǎn)，這樣可以保證得到最優(yōu)路徑，但效率極低。最佳優(yōu)先算法會(huì)選擇離目標(biāo)最近的節(jié)點(diǎn)，它基于貪心策略[2]向目標(biāo)點(diǎn)移動(dòng)即使它不是正確的路徑，所以不能保證路徑最短，但速度比Dijkstra快得多。A*算法將Dijkstra靠近初始點(diǎn)的節(jié)點(diǎn)與最佳優(yōu)先算法靠近目標(biāo)點(diǎn)的節(jié)點(diǎn)的信息塊結(jié)合起來(lái)。利用啟發(fā)函數(shù)信息來(lái)引導(dǎo)搜索方向，提高搜索效率[3]。

A*算法是路徑規(guī)劃中比較常用的全局路徑規(guī)劃方法，它是一種基于柵格地圖的啟發(fā)式搜索算法[4]，具有最優(yōu)性和完備性的特點(diǎn)[5]，但規(guī)劃出來(lái)的路徑不夠平滑，且不滿足車輛非完整性約束[6]，而且當(dāng)環(huán)境中突然出現(xiàn)動(dòng)態(tài)障礙物時(shí)[7]，就需要回溯重新計(jì)算，生成新的路徑，增加了計(jì)算量。但是A*對(duì)于不同的環(huán)境具有很強(qiáng)的擴(kuò)展性和適應(yīng)性，對(duì)項(xiàng)目工程來(lái)說(shuō)十分實(shí)用。針對(duì)A*算法，研究者對(duì)其進(jìn)行了大量的研究，如文獻(xiàn)[8]中的雙重A*算法同時(shí)完成全局和局部路徑規(guī)劃，解決了A*算法在動(dòng)態(tài)環(huán)境中規(guī)劃效果不佳的問(wèn)題，但是每次遇到障礙物都需要重新進(jìn)行一次A*搜索，效率不高；文獻(xiàn)[9]提出一種增加搜索節(jié)點(diǎn)的A*算法，減小了路徑長(zhǎng)度和轉(zhuǎn)折角度，使路徑更加平滑，但不滿足車輛非完整性約束；為了解決單個(gè)啟發(fā)式函數(shù)不能兼顧效率和最優(yōu)性的問(wèn)題，MHA*算法設(shè)置多個(gè)啟發(fā)式函數(shù)[10]，在不同情況下使用不同的啟發(fā)式函數(shù)，從而找到最優(yōu)解；為了滿足車輛模型需要，文獻(xiàn)[11]提出一種懲戒式混合A*算法，能夠得到安全的適合車輛行駛的路徑，但是不夠平滑，且對(duì)動(dòng)態(tài)障礙物沒有做特定處理。

本文針對(duì)A*算法規(guī)劃出來(lái)的軌跡不夠平滑、不滿足車輛非完整性約束、在動(dòng)態(tài)環(huán)境中規(guī)劃效果不佳等缺陷對(duì)A*算法做出一定改進(jìn)，省略了一些劣質(zhì)節(jié)點(diǎn)，使得A*算法規(guī)劃出來(lái)的軌跡轉(zhuǎn)折少且更加平滑，在運(yùn)動(dòng)中需要考慮物體的方向?qū)傩院蛯?shí)際運(yùn)動(dòng)約束，并配合lattice算法，將A*規(guī)劃出來(lái)的全局路徑作為lattice的參考線，進(jìn)行局部候選路徑的生成。然后設(shè)計(jì)一系列cost代價(jià)函數(shù)篩選出最優(yōu)局部路徑，解決A*算法對(duì)于動(dòng)態(tài)障礙物處理不靈活的問(wèn)題[12]。

1 全局路徑生成單元

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

A*算法是從靜態(tài)網(wǎng)絡(luò)中求解最短路徑，是一種直接搜索方法，算法通過(guò)比較當(dāng)前路徑柵格的8個(gè)鄰居的啟發(fā)式函數(shù)值F來(lái)逐步確定下一個(gè)路徑柵格。公式為：

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

(1)

式中：f(n)是從初始點(diǎn)經(jīng)由節(jié)點(diǎn)n到目標(biāo)點(diǎn)的估價(jià)函數(shù);g(n)是在狀態(tài)空間中從初始節(jié)點(diǎn)到節(jié)點(diǎn)n的實(shí)際代價(jià)值;h(n)是從n到目標(biāo)點(diǎn)最佳路徑的預(yù)計(jì)花費(fèi)估計(jì)代價(jià)值。當(dāng)從初始點(diǎn)向目標(biāo)點(diǎn)移動(dòng)時(shí)，A*權(quán)衡這兩者。

1.2 實(shí)際運(yùn)動(dòng)約束

在傳統(tǒng)A*算法中，一般不會(huì)考慮運(yùn)動(dòng)物體的方向和實(shí)際運(yùn)動(dòng)約束，只假設(shè)物體可以直接轉(zhuǎn)移到鄰近柵格中。但對(duì)于實(shí)際的車輛路徑規(guī)劃而言，必須要考慮物體運(yùn)動(dòng)的實(shí)際約束，所以它們不一定出現(xiàn)在格點(diǎn)的中心。從初始點(diǎn)位置出發(fā)，物體在下一步搜索中只能到達(dá)它能夠到達(dá)的位置，比如，在車輛模型中，受制于汽車轉(zhuǎn)向角的限制，如圖1所示。

圖1 實(shí)際運(yùn)動(dòng)約束軌跡圖

如圖2所示：(a)為傳統(tǒng)A*算法節(jié)點(diǎn)擴(kuò)展方式，它將成本與單元格中心相關(guān)聯(lián)，并且只訪問(wèn)與單元格中心相對(duì)應(yīng)的狀態(tài)；(b)為D*算法節(jié)點(diǎn)擴(kuò)展方式，它將成本與單元格角聯(lián)系起來(lái)，允許單元格間任意線性路徑；(c)為改進(jìn)A*算法節(jié)點(diǎn)擴(kuò)展方式，它將連續(xù)狀態(tài)與每個(gè)單元格關(guān)聯(lián)，單元格的得分是其關(guān)聯(lián)連續(xù)狀態(tài)的成本。

圖2 節(jié)點(diǎn)擴(kuò)展圖

1.3 省略冗余點(diǎn)

1.3.1無(wú)必要遍歷點(diǎn)

在搜索過(guò)程中，會(huì)有很多劣質(zhì)的點(diǎn)，相比于其他節(jié)點(diǎn)，代價(jià)明顯更高，對(duì)于這些點(diǎn)，可以直接刪除。如圖3所示，在a-b-c、a-d-c、a-e-c幾條路徑中，明顯a-b-c具有最優(yōu)解，那么其他兩條軌跡沒有太大的估價(jià)意義，所以在傳統(tǒng)遍歷節(jié)點(diǎn)的基礎(chǔ)上直接刪除這些代價(jià)值大的點(diǎn)，既提高了效率也節(jié)省了計(jì)算量。

圖3 無(wú)必要遍歷點(diǎn)

1.3.2夾 點(diǎn)

所謂夾點(diǎn)，就是在兩個(gè)點(diǎn)連線之間的點(diǎn)，如圖4所示，b為a和c的夾點(diǎn)，可以直接刪除，最終獲得只包含起始點(diǎn)、轉(zhuǎn)折點(diǎn)、目標(biāo)點(diǎn)的節(jié)點(diǎn)，減小計(jì)算量。

圖4 夾點(diǎn)示意圖

1.4 實(shí)際代價(jià)值優(yōu)化

考慮到物體實(shí)際運(yùn)動(dòng)約束，對(duì)實(shí)際代價(jià)值G進(jìn)行優(yōu)化。

① 如果前進(jìn)，則有:

G=G+S

(2)

② 如果后退，則有:

G=G+P1S

(3)

③ 如果有轉(zhuǎn)向，則有:

G=G+P2|δ(c)|

(4)

式中：S表示每次搜索走過(guò)的距離;P1、P2表示系數(shù);δ代表車輛的轉(zhuǎn)向角;c表示輪距。

1.5 優(yōu)化啟發(fā)式函數(shù)

啟發(fā)式函數(shù)可以控制A*的行為。傳統(tǒng)的A*算法都使用曼哈頓距離和歐氏距離定義啟發(fā)式函數(shù)，本文使用Reeds-Shepp曲線、Dubins曲線和曼哈頓距離三種cost解算出來(lái)的最大值來(lái)作為A*的預(yù)期花費(fèi)估計(jì)代價(jià)值。其中Reeds-Shepp曲線由幾段半徑固定的圓弧和一段直線段拼接組成，而且圓弧的半徑就是車輛模型的最小轉(zhuǎn)向半徑，它是車輛模型行駛的最短路徑；Dubins曲線和Reeds-Shepp曲線相比，多了一個(gè)約束條件：車輛模型只能朝前開，不能后退；曼哈頓距離即為普通A*算法的預(yù)估代價(jià)值。

1.6 改進(jìn)后的A*算法流程

A*算法需要?jiǎng)?chuàng)建兩個(gè)列表：open list和close list，其中:open list用來(lái)存放當(dāng)前位置周圍可以被考慮的路徑;close list用來(lái)存放不被考慮的路徑。改進(jìn)后A*算法具體流程如圖5所示。

圖5 改進(jìn)后A*算法流程

2 局部運(yùn)動(dòng)軌跡生成單元

局部運(yùn)動(dòng)軌跡規(guī)劃總的來(lái)說(shuō)是在全局路徑規(guī)劃模塊下，結(jié)合改進(jìn)A*算法生成的全局路徑生成局部路徑的模塊。局部路徑規(guī)劃算法有好多種，例如人工勢(shì)場(chǎng)法、動(dòng)態(tài)窗口法等，但是動(dòng)態(tài)窗口法只適合能夠自由轉(zhuǎn)向的差速機(jī)器人運(yùn)動(dòng)，不滿足車輛非完整性約束，針對(duì)動(dòng)態(tài)窗口法的局限性[12]，使用基于采樣的局部軌跡生成算法，將改進(jìn)A*算法生成的全局路徑作為參考線，將各時(shí)刻的縱向偏移量和橫向偏移量還原成二維平面內(nèi)的軌跡點(diǎn)，從而形成一系列軌跡，然后通過(guò)cost評(píng)價(jià)函數(shù)選取最優(yōu)軌跡。

2.1 lattice采樣生成軌跡

2.1.1 Frenet坐標(biāo)系

通常在二維平面中采用X-Y坐標(biāo)系來(lái)表示坐標(biāo)，但是為了更好地規(guī)劃運(yùn)動(dòng)軌跡，本文坐標(biāo)基于路徑，lattice采樣生成軌跡基于Frenet坐標(biāo)系，需要用Frenet坐標(biāo)系來(lái)表示車輛的狀態(tài)。作一條光滑的參考線如圖6所示，將汽車的坐標(biāo)點(diǎn)投影到參考線上，得到一個(gè)參考線上的投影點(diǎn)。從參考線起始點(diǎn)到投影點(diǎn)的路徑長(zhǎng)度就是汽車在Frenet坐標(biāo)系下的縱向偏移量，用S表示；投影點(diǎn)到汽車位置的距離則是汽車在Frenet坐標(biāo)系下的橫向偏移量，用L表示。因?yàn)閰⒖季€是足夠光滑的，也可通過(guò)汽車的朝向、速度、加速度來(lái)計(jì)算出Frenet坐標(biāo)系下，橫向和縱向偏移量的一階導(dǎo)和二階導(dǎo)。

圖6 Frenet坐標(biāo)投影

2.1.2 lattice軌跡生成

(2) 將t0起始狀態(tài)和t1末狀態(tài)做多項(xiàng)式擬合，分別形成縱向多項(xiàng)式軌跡和橫向多項(xiàng)式軌跡,如圖7所示。

圖7 多項(xiàng)式軌跡

縱向軌跡的五次多項(xiàng)式s(t)，表示為:

(5)

橫向軌跡的五次多項(xiàng)式l(s)，表示為:

(6)

2.2 lattice評(píng)價(jià)函數(shù)

本文設(shè)計(jì)了5個(gè)cost評(píng)價(jià)函數(shù)：

(1) 橫向偏移cost：離中心軌跡越遠(yuǎn)的軌跡cost越高。計(jì)算各候選局部路徑和改進(jìn)A*算法生成的全局路徑的距離A作為橫向偏移cost值。

(2) 障礙物水平距離cost：計(jì)算車輛離障礙物沿著中心線軌跡的水平距離B作為障礙物水平距離cost值。

(3) 障礙物垂直距離cost：計(jì)算障礙物的各個(gè)輪廓點(diǎn)距離各候選路徑的垂直距離，然后進(jìn)行加權(quán)得到C作為障礙物垂直距離cost值。

(4) 縱向加速度cost：加速度是速度對(duì)時(shí)間的導(dǎo)數(shù)，表示加速度的變化率。本文用加速度的最大值D來(lái)表示縱向加速度cost值。

(5) 橫向加速度cost：為了平穩(wěn)換道，打方向盤力度越大，橫向加速度cost越大。將方向盤轉(zhuǎn)角E作為橫向加速度cost值。

最終的cost值如下：

cost=(a×A+b×B+c×C+d×D+e×E)/5

(7)

式中：a為橫向偏移cost的權(quán)重系數(shù);b為障礙物水平距離cost的權(quán)重系數(shù);c為障礙物垂直距離cost的權(quán)重系數(shù);d為縱向加速度cost的權(quán)重系數(shù);e為橫向加速度cost的權(quán)重系數(shù)。結(jié)合這五個(gè)cost評(píng)價(jià)函數(shù)，篩選出代價(jià)最低的軌跡作為最優(yōu)軌跡。

lattice總體算法流程如圖8所示。

圖8 lattice算法流程

局部軌跡生成效果如圖9所示，圖中黑色為生成的候選軌跡，白色為cost最小的最優(yōu)軌跡，可以看出，此算法可以有效避開障礙物且軌跡最優(yōu)。

3 實(shí)驗(yàn)與結(jié)果分析

本文將改進(jìn)后的A*算法和lattice算法融合，將改進(jìn)后的A*算法規(guī)劃出來(lái)的全局路徑作為參考線，作為lattice算法的輸入，lattice算法將根據(jù)參考線在二維平面內(nèi)還原一系列軌跡點(diǎn)，從而得到軌跡。這樣的結(jié)合方法全局與局部兼顧，且軌跡平滑，利于車輛行駛。系統(tǒng)整體框架如圖10所示。

圖10 融合算法系統(tǒng)框架

根據(jù)該路徑規(guī)劃方法，本路徑規(guī)劃方法仿真建立在機(jī)器人操作系統(tǒng)ROS下，使用Ubuntu16.04+kinetic版本進(jìn)行實(shí)驗(yàn)。

3.1 實(shí)際運(yùn)動(dòng)約束實(shí)驗(yàn)

不考慮物體實(shí)際運(yùn)動(dòng)約束的A*算法與考慮物體實(shí)際運(yùn)動(dòng)約束的A*算法效果對(duì)比如圖11和圖12所示。

圖11 不考慮物體實(shí)際運(yùn)動(dòng)約束的A*算法路徑規(guī)劃圖

圖12 考慮物體實(shí)際運(yùn)動(dòng)約束的A*算法路徑規(guī)劃圖

可以看出，在增加了車輛實(shí)際運(yùn)動(dòng)約束，即最小轉(zhuǎn)彎半徑之后，規(guī)劃出來(lái)的路徑明顯更利于車輛行駛，不會(huì)發(fā)生規(guī)劃出來(lái)的路徑車輛跟蹤不了的問(wèn)題。本文最小轉(zhuǎn)彎半徑為4 m。

3.2 計(jì)算速度優(yōu)化實(shí)驗(yàn)

針對(duì)A*算法計(jì)算量大、計(jì)算速度不夠的問(wèn)題，對(duì)A*算法的冗余點(diǎn)進(jìn)行了刪除，減少了計(jì)算量，增加了路徑規(guī)劃的速度。本文對(duì)圖13的環(huán)境，設(shè)定相同起始點(diǎn)和目標(biāo)點(diǎn)，進(jìn)行了20次規(guī)劃然后取平均值，得到了平均每次規(guī)劃的時(shí)間如表1所示。

圖13 普通A*算法效果圖

表1 路徑規(guī)劃計(jì)算速度對(duì)比測(cè)試

可以看出，雖然普通A*算法的速度是最快的，但是它沒有考慮實(shí)際運(yùn)動(dòng)約束，規(guī)劃出來(lái)的路徑對(duì)于車輛來(lái)說(shuō)可能無(wú)法行駛，改進(jìn)后的A*算法速度要快于普通混合A*算法，所以本文刪除冗余點(diǎn)的方法有效果。

3.3 路徑平滑優(yōu)化實(shí)驗(yàn)

下面介紹改進(jìn)后的A*算法與結(jié)合實(shí)際運(yùn)動(dòng)約束的A*算法效果對(duì)比實(shí)驗(yàn)情況。圖13-圖15為同一幅柵格地圖，相同的起始點(diǎn)和目標(biāo)點(diǎn)進(jìn)行的路徑規(guī)劃效果圖，為了測(cè)試算法的實(shí)用性，該環(huán)境設(shè)置了大量的隔板作為障礙物，增加了路徑規(guī)劃的難度。圖13為普通A*算法效果圖，可以看出，雖然路徑比較平滑，但是沒有考慮到實(shí)際運(yùn)動(dòng)約束，不利于車輛行駛。圖14為結(jié)合實(shí)際運(yùn)動(dòng)約束的A*算法規(guī)劃的路徑，該路徑考慮了車輛最小轉(zhuǎn)彎半徑為4 m，存在明顯的轉(zhuǎn)折痕跡，不利于車輛行駛。圖15為改進(jìn)后的A*規(guī)劃的路徑，考慮了車輛最小轉(zhuǎn)彎半徑，且路徑較為平滑，利于車輛行駛。

圖14 結(jié)合實(shí)際運(yùn)動(dòng)約束的A*算法效果圖

圖15 改進(jìn)后的A*算法效果圖

3.4 多代價(jià)函數(shù)軌跡優(yōu)化實(shí)驗(yàn)

本文為lattice算法設(shè)定5個(gè)cost函數(shù)去約束軌跡，最后篩選出最優(yōu)軌跡，對(duì)于每個(gè)cost函數(shù)設(shè)定不同的權(quán)重，通過(guò)實(shí)驗(yàn)測(cè)試出最佳權(quán)重比例分配。

表2中避障即為躲避障礙物的性能；穩(wěn)定性表示規(guī)劃出來(lái)的路徑是否穩(wěn)定，是否會(huì)一直跳變；舒適度代表規(guī)劃出來(lái)的軌跡與當(dāng)前行駛軌跡相差是否過(guò)大，過(guò)大則需要猛打方向盤，影響乘客舒適度。該路徑規(guī)劃方法經(jīng)過(guò)調(diào)參，可以規(guī)劃出兼?zhèn)浔苷闲阅?、穩(wěn)定性、舒適度的軌跡供車輛行駛。

表2 代價(jià)值權(quán)重比例分配測(cè)試

3.5 實(shí)時(shí)避障效果實(shí)驗(yàn)

實(shí)驗(yàn)結(jié)果如圖16-圖18所示，圖中的坐標(biāo)軸代表車輛，黑線為全局路徑，白線為最優(yōu)局部軌跡。從效果上來(lái)看，該路徑規(guī)劃方法可以在不同的環(huán)境下很好地躲避障礙物，解決了A*算法在動(dòng)態(tài)環(huán)境下需要重新規(guī)劃的問(wèn)題，減小了計(jì)算量，提高了A*算法的靈活性。

圖16 實(shí)時(shí)避障實(shí)驗(yàn)1

圖17 實(shí)時(shí)避障實(shí)驗(yàn)2

圖18 實(shí)時(shí)避障實(shí)驗(yàn)3

4 結(jié) 語(yǔ)

針對(duì)A*算法缺乏動(dòng)態(tài)性、規(guī)劃出來(lái)的軌跡不夠平滑、不滿足車輛非完整約束等問(wèn)題，本文提出一種改進(jìn)A*算法和lattice算法相結(jié)合的路徑規(guī)劃方法，消除傳統(tǒng)A*算法中的無(wú)必要遍歷點(diǎn)和夾點(diǎn)，同時(shí)考慮物體的方向?qū)傩院蛯?shí)際運(yùn)動(dòng)約束，優(yōu)化啟發(fā)式函數(shù)最終生成全局路徑，然后把全局路徑作為lattice的參考線，采樣生成一系列候選路徑，并結(jié)合障礙物信息和其他因素計(jì)算候選路徑的代價(jià)值，能夠選出平滑且無(wú)障礙的局

部軌跡。仿真實(shí)驗(yàn)表明了本文的路徑規(guī)劃方法效果顯著，能快速規(guī)劃出一條平滑、安全且滿足車輛非完整性約束的運(yùn)動(dòng)路徑。