陳子豪

（杭州應(yīng)用聲學(xué)研究所，杭州 311499）

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

1 A*算法

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

基于圖搜索的A*算法實現(xiàn)過程如下：

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

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

（3）障礙物膨脹處理；

（4）設(shè)定起點和終點坐標(biāo)，檢查起點和終點是否為障礙，若是，終止搜索，若處于可行域上，執(zhí)行步驟5；

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

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

（7）畫出路徑軌跡。

1.1 障礙物膨脹

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

圖1 障礙物膨脹流程圖

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

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

1.2 路徑搜索結(jié)果

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

圖2 搜索得到的原始路徑

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

2 路徑優(yōu)化

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

圖4 路徑優(yōu)化策略原理

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

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

圖6 以長邊為基礎(chǔ)在直線上取點

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

圖7 第一次優(yōu)化后剩余的路徑點

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

圖8 第二次優(yōu)化后剩余的路徑點

3 路徑平滑

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

以二階貝賽爾曲線為例介紹其原理，具體過程如圖9所示[6]。在平面內(nèi)選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 貝賽爾曲線

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

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

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

假設(shè)包括起點和終點在內(nèi)需要連接A、B、C、D、E，不包括起點和終點，在每個路徑點兩側(cè)進行線性插值，各插入一個子節(jié)點。以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 路徑平滑原理圖

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

圖11 路徑規(guī)劃最終效果圖

4 結(jié)語

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