一種基于A*算法的圖搜索路徑優化平滑方法

陳子豪

（杭州應用聲學研究所，杭州 311499）

路徑規劃作為地圖導航、無人平臺運動控制等多種技術領域的關鍵環節，一直都是研究的熱點。A*算法[1]、D*算法、人工勢場、快速搜索隨機樹（Rapidexploration Random Tree，RRT）等經典算法，在路徑搜索領域都有十分廣泛的應用。但是，各算法自身均存在不同的短板，導致這些算法從發布至今仍在不斷演變和衍生。例如，原始的A*算法生成路徑可能存在過于緊貼障礙物、拐點較多等問題[2-4]。本文基于A*算法提出了一種基于二階貝塞爾曲線的路徑優化平滑方法。

1 A*算法

由于A*算法的原理涉及篇幅較長且已有大量發表資料可供參考，這里不對A*算法的原理進行贅述，僅對A*算法中的關鍵步驟加以說明。

基于圖搜索的A*算法實現過程如下：

（1）讀取原始圖像進行灰度化，得到二值化圖像；

（2）以像素點為單位，遍歷整張圖片，將圖片中的點分類，白色點標志為可行域，黑色點標志為障礙；

（3）障礙物膨脹處理；

（4）設定起點和終點坐標，檢查起點和終點是否為障礙，若是，終止搜索，若處于可行域上，執行步驟5；

（5）將起點作為openlist中第一個節點，開始路徑搜索；

（6）將路徑搜索最終得到的路徑點保存在容器中；

（7）畫出路徑軌跡。

1.1 障礙物膨脹

對障礙物進行膨脹處理是由于初始算法規劃的路徑會出現緊貼障礙物的情況，需設置一定的危險半徑，避免過于靠近障礙物發生碰撞。障礙物膨脹也可通過簡單的圖像腐蝕膨脹實現。但是，實際應用場景中局部地圖的生成主要依靠激光或聲吶等傳感器測量到障礙物的實際距離，再選擇合適的地圖分辨率將探測到的障礙物映射到局部地圖上。映射時使用的地圖分辨率不同，在分辨率較大的情況下，單純腐蝕膨脹圖像容易造成原始地圖產生過大的變化，出現地圖精度損失。因此，本文選擇通過判斷距離閾值的方式進行障礙物膨脹，具體實現過程如圖1所示。

圖1 障礙物膨脹流程圖

需要注意，獲取的離最近障礙物的距離是圖像上像素點之間的歐式距離，因此安全距離閾值要由實際距離值進行轉換，轉換公式為

式中：D為圖像上的安全距離閾值；d為實際安全距離閾值；res為地圖的分辨率。

1.2 路徑搜索結果

以圖像坐標（0，0）點為起點，（800，600）點為終點，圖2和圖3分別為障礙物未進行膨脹處理搜索得到的路徑和障礙物膨脹后搜索得到的路徑。從圖2和圖3可以看出，未設置安全半徑的搜索路徑會出現緊貼障礙物的現象，而設置了安全半徑的搜索路徑會遠離障礙物，提高了系統可靠性和安全性。

圖2 搜索得到的原始路徑

圖3 障礙物膨脹后搜索得到的路徑

2 路徑優化

算法搜索得到的原始路徑中還存在許多不必要的拐點，需對路徑進行優化，舍去不必要的拐點，減少拐點數量，盡可能刪去多余的路徑點，僅保留必要的可通過直線段連接的路徑點，提高運動效率。優化策略的原理如圖4所示。

圖4 路徑優化策略原理

優化時需要篩選前向路徑段上無須避障時的無意義拐點。假設圖4中的黑色路徑點為初次搜索得到的所有路徑點，從起點開始，依次尋找能夠無碰撞連接的節點。若一個節點可以和起點無碰撞連接，此節點的下一個節點不能與起點無碰撞連接。忽略此節點之前的所有節點，記錄此點為q，再以q為起點，尋找下一個符合上述情況的節點q，直至終點和q能夠無碰撞連接，將所有的q點連接后得到優化后的路徑。判斷兩點是否可以無障礙直線連接的原理，如圖5所示。將待判斷的兩點形成的線段映射為地圖上的像素點，分離出線段上的所有像素點，判斷其是否為障礙物。以長邊為基準是為了在直線段上盡可能選取更多的點，提高判斷的可靠性，如圖6所示。

圖5 判斷兩點是否能無障礙連接流程圖

圖6 以長邊為基礎在直線上取點

實際過程中，由于地圖中的障礙物進行過膨脹處理，障礙物的邊界存在一定的變形，這對判斷兩點間是否能無障礙直線連接會有一定的影響，有可能會出現在某段區域內路徑點比較密集的情況，如圖7所示。

圖7 第一次優化后剩余的路徑點

冗余的路徑點在平滑階段會增加算法的時間復雜度，進而增加系統的運算量。為避免上述情況的發生，要對路徑進行二次優化，并在此過程中引入距離閾值。具體流程為：遍歷第一次優化后的路徑點容器，若當前節點為第i個結點，比較它與第i+1個節點的距離，若小于設定閾值，判斷第i+1個節點為無效節點，從容器中刪去第i+1個節點，后續節點自動順移；重新比較第i個節點與第i+1個節點距離，若大于設定閾值，以i+1個節點開始繼續比較，直到容器中所有的點都被查詢。二次優化后的路徑點如圖8所示。

圖8 第二次優化后剩余的路徑點

3 路徑平滑

優化后的路徑點仍有可能存在拐點處曲率突變的情況。路徑拐點處曲率突變會對運動平臺的路徑跟蹤造成較大影響，因此對二次優化后的路徑進行平滑得到最終的搜索路徑。若使用3次樣條插值或3次B樣條插值算法進行路徑平滑，會導致軌跡直線部分也存在曲率變化，在直線運動時反而有可能降低軌跡跟蹤精度。本文只在路徑拐角處使用二階貝賽爾曲線將折線段平滑成曲線，在進行路徑平滑縮短航程的同時盡可能保留原始路徑點，減小實際運動時的軌跡誤差[5]。

以二階貝賽爾曲線為例介紹其原理，具體過程如圖9所示[6]。在平面內選3個不同點A、B、C，且依次用線段連接。AB和BC上分別確定點D和點E，使得AD/AB=BE/BC成立。連接DE，并在DE上找出一點F，使得DF/DE=AD/AB=BE/BC。讓選取的點D在第一條線段上從起點A移動到終點B，找出所有點F，將所有的點F連起來，會得到一條非常光滑的曲線，即貝塞爾曲線。

圖9 貝賽爾曲線

二階貝賽爾曲線的公式建立在一階貝賽爾曲線的基礎上。一階貝賽爾曲線實際上是點A到點B的連續點，描述的是一條線段，與兩點間的線性插值類似，可表示為

二階貝賽爾曲線為A至B的連續點D與B至C的連續點E組成線段DE上的連續點F，可表示為

式中：A、B、C分別為平面坐標系下的二維坐標點，是一個2×1的矩陣。

假設包括起點和終點在內需要連接A、B、C、D、E，不包括起點和終點，在每個路徑點兩側進行線性插值，各插入一個子節點。以C點為例，在CB段插入點C1，CD段插入點C2，使得

這樣原路徑A—B—C—D—E被A—B1—B2—C1—C2—D1—D2—E所替代，新的路徑可以視為由直線段和曲線段拼接而成，即AB1+B1B2+B2C1+C1C2+C2D1+D1D2+D2E，依次連接一段直線和一段二階貝賽爾曲線，直至用直線連接終點和最后一個插值點，如圖10所示。

圖10 路徑平滑原理圖

按上述方法連接優化后的所有路徑點，最終的軌跡規劃效果圖如圖11所示。

圖11 路徑規劃最終效果圖

4 結語

本文以A*算法為例，提出了一種基于二階貝塞爾曲線的路徑優化平滑方法，并選取實際地形圖對算法效果進行了演示和驗證，證明了該方法可有效改善原始算法存在的問題。