基于改進A*算法和lattice算法的路徑規劃方法

吳 正 趙懷林

(上海應用技術大學電氣與電子工程學院 上海 201499)

0 引 言

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

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

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

本文針對A*算法規劃出來的軌跡不夠平滑、不滿足車輛非完整性約束、在動態環境中規劃效果不佳等缺陷對A*算法做出一定改進，省略了一些劣質節點，使得A*算法規劃出來的軌跡轉折少且更加平滑，在運動中需要考慮物體的方向屬性和實際運動約束，并配合lattice算法，將A*規劃出來的全局路徑作為lattice的參考線，進行局部候選路徑的生成。然后設計一系列cost代價函數篩選出最優局部路徑，解決A*算法對于動態障礙物處理不靈活的問題[12]。

1 全局路徑生成單元

1.1 傳統A*算法

A*算法是從靜態網絡中求解最短路徑，是一種直接搜索方法，算法通過比較當前路徑柵格的8個鄰居的啟發式函數值F來逐步確定下一個路徑柵格。公式為：

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

(1)

式中：f(n)是從初始點經由節點n到目標點的估價函數;g(n)是在狀態空間中從初始節點到節點n的實際代價值;h(n)是從n到目標點最佳路徑的預計花費估計代價值。當從初始點向目標點移動時，A*權衡這兩者。

1.2 實際運動約束

在傳統A*算法中，一般不會考慮運動物體的方向和實際運動約束，只假設物體可以直接轉移到鄰近柵格中。但對于實際的車輛路徑規劃而言，必須要考慮物體運動的實際約束，所以它們不一定出現在格點的中心。從初始點位置出發，物體在下一步搜索中只能到達它能夠到達的位置，比如，在車輛模型中，受制于汽車轉向角的限制，如圖1所示。

圖1 實際運動約束軌跡圖

如圖2所示：(a)為傳統A*算法節點擴展方式，它將成本與單元格中心相關聯，并且只訪問與單元格中心相對應的狀態；(b)為D*算法節點擴展方式，它將成本與單元格角聯系起來，允許單元格間任意線性路徑；(c)為改進A*算法節點擴展方式，它將連續狀態與每個單元格關聯，單元格的得分是其關聯連續狀態的成本。

圖2 節點擴展圖

1.3 省略冗余點

1.3.1無必要遍歷點

在搜索過程中，會有很多劣質的點，相比于其他節點，代價明顯更高，對于這些點，可以直接刪除。如圖3所示，在a-b-c、a-d-c、a-e-c幾條路徑中，明顯a-b-c具有最優解，那么其他兩條軌跡沒有太大的估價意義，所以在傳統遍歷節點的基礎上直接刪除這些代價值大的點，既提高了效率也節省了計算量。

圖3 無必要遍歷點

1.3.2夾 點

所謂夾點，就是在兩個點連線之間的點，如圖4所示，b為a和c的夾點，可以直接刪除，最終獲得只包含起始點、轉折點、目標點的節點，減小計算量。

圖4 夾點示意圖

1.4 實際代價值優化

考慮到物體實際運動約束，對實際代價值G進行優化。

① 如果前進，則有:

G=G+S

(2)

② 如果后退，則有:

G=G+P1S

(3)

③ 如果有轉向，則有:

G=G+P2|δ(c)|

(4)

式中：S表示每次搜索走過的距離;P1、P2表示系數;δ代表車輛的轉向角;c表示輪距。

1.5 優化啟發式函數

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

1.6 改進后的A*算法流程

A*算法需要創建兩個列表：open list和close list，其中:open list用來存放當前位置周圍可以被考慮的路徑;close list用來存放不被考慮的路徑。改進后A*算法具體流程如圖5所示。

圖5 改進后A*算法流程

2 局部運動軌跡生成單元

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

2.1 lattice采樣生成軌跡

2.1.1 Frenet坐標系

通常在二維平面中采用X-Y坐標系來表示坐標，但是為了更好地規劃運動軌跡，本文坐標基于路徑，lattice采樣生成軌跡基于Frenet坐標系，需要用Frenet坐標系來表示車輛的狀態。作一條光滑的參考線如圖6所示，將汽車的坐標點投影到參考線上，得到一個參考線上的投影點。從參考線起始點到投影點的路徑長度就是汽車在Frenet坐標系下的縱向偏移量，用S表示；投影點到汽車位置的距離則是汽車在Frenet坐標系下的橫向偏移量，用L表示。因為參考線是足夠光滑的，也可通過汽車的朝向、速度、加速度來計算出Frenet坐標系下，橫向和縱向偏移量的一階導和二階導。

圖6 Frenet坐標投影

2.1.2 lattice軌跡生成

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

圖7 多項式軌跡

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

(5)

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

(6)

2.2 lattice評價函數

本文設計了5個cost評價函數：

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

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

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

(4) 縱向加速度cost：加速度是速度對時間的導數，表示加速度的變化率。本文用加速度的最大值D來表示縱向加速度cost值。

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

最終的cost值如下：

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

(7)

式中：a為橫向偏移cost的權重系數;b為障礙物水平距離cost的權重系數;c為障礙物垂直距離cost的權重系數;d為縱向加速度cost的權重系數;e為橫向加速度cost的權重系數。結合這五個cost評價函數，篩選出代價最低的軌跡作為最優軌跡。

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

圖8 lattice算法流程

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

3 實驗與結果分析

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

圖10 融合算法系統框架

根據該路徑規劃方法，本路徑規劃方法仿真建立在機器人操作系統ROS下，使用Ubuntu16.04+kinetic版本進行實驗。

3.1 實際運動約束實驗

不考慮物體實際運動約束的A*算法與考慮物體實際運動約束的A*算法效果對比如圖11和圖12所示。

圖11 不考慮物體實際運動約束的A*算法路徑規劃圖

圖12 考慮物體實際運動約束的A*算法路徑規劃圖

可以看出，在增加了車輛實際運動約束，即最小轉彎半徑之后，規劃出來的路徑明顯更利于車輛行駛，不會發生規劃出來的路徑車輛跟蹤不了的問題。本文最小轉彎半徑為4 m。

3.2 計算速度優化實驗

針對A*算法計算量大、計算速度不夠的問題，對A*算法的冗余點進行了刪除，減少了計算量，增加了路徑規劃的速度。本文對圖13的環境，設定相同起始點和目標點，進行了20次規劃然后取平均值，得到了平均每次規劃的時間如表1所示。

圖13 普通A*算法效果圖

表1 路徑規劃計算速度對比測試

可以看出，雖然普通A*算法的速度是最快的，但是它沒有考慮實際運動約束，規劃出來的路徑對于車輛來說可能無法行駛，改進后的A*算法速度要快于普通混合A*算法，所以本文刪除冗余點的方法有效果。

3.3 路徑平滑優化實驗

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

圖14 結合實際運動約束的A*算法效果圖

圖15 改進后的A*算法效果圖

3.4 多代價函數軌跡優化實驗

本文為lattice算法設定5個cost函數去約束軌跡，最后篩選出最優軌跡，對于每個cost函數設定不同的權重，通過實驗測試出最佳權重比例分配。

表2中避障即為躲避障礙物的性能；穩定性表示規劃出來的路徑是否穩定，是否會一直跳變；舒適度代表規劃出來的軌跡與當前行駛軌跡相差是否過大，過大則需要猛打方向盤，影響乘客舒適度。該路徑規劃方法經過調參，可以規劃出兼備避障性能、穩定性、舒適度的軌跡供車輛行駛。

表2 代價值權重比例分配測試

3.5 實時避障效果實驗

實驗結果如圖16-圖18所示，圖中的坐標軸代表車輛，黑線為全局路徑，白線為最優局部軌跡。從效果上來看，該路徑規劃方法可以在不同的環境下很好地躲避障礙物，解決了A*算法在動態環境下需要重新規劃的問題，減小了計算量，提高了A*算法的靈活性。

圖16 實時避障實驗1

圖17 實時避障實驗2

圖18 實時避障實驗3

4 結 語

針對A*算法缺乏動態性、規劃出來的軌跡不夠平滑、不滿足車輛非完整約束等問題，本文提出一種改進A*算法和lattice算法相結合的路徑規劃方法，消除傳統A*算法中的無必要遍歷點和夾點，同時考慮物體的方向屬性和實際運動約束，優化啟發式函數最終生成全局路徑，然后把全局路徑作為lattice的參考線，采樣生成一系列候選路徑，并結合障礙物信息和其他因素計算候選路徑的代價值，能夠選出平滑且無障礙的局

部軌跡。仿真實驗表明了本文的路徑規劃方法效果顯著，能快速規劃出一條平滑、安全且滿足車輛非完整性約束的運動路徑。