動態環境下多重A*算法的機器人路徑規劃方法

華 洪，張志安，施振穩，陳冠星

南京理工大學 機械工程學院，南京210094

路徑規劃技術是移動機器人技術的核心內容之一，近年來在許多領域都有著廣泛的應用，例如在制造領域中應用于生產線上下料搬運的自主引導運輸車，在電商工廠中應用廣泛的無人送貨小車，在家中負責清掃地板的掃地機器人等。它們都是在不同的環境下，按照指定要求，以感知、建模、規劃、執行的標準路徑規劃方法執行[1-2]。根據路徑規劃環境是否已知，可將路徑規劃算法分為全局路徑規劃與局部路徑規劃。根據路徑規劃環境中是否存在動態障礙物，可將路徑規劃分為靜態路徑規劃與動態路徑規劃[3-4]。

A*算法作為一種全局的靜態路徑規劃搜索算法，具有魯棒性好、反應快等優點，但受算法原理限制，規劃出來的路徑折線較多，且隨著環境變大搜索代價呈指數增長。文獻[5]提出對A*算法中的估價函數進行指數加權處理，并且對路徑進行5 次多項式平滑處理，有效解決了搜索效率低和路徑不平滑的問題，但機器人并不具備局部規劃能力與動態避障能力，在密集區域內容易陷入極小值。文獻[6]提出在A*全局規劃的基礎上，采用自適應步長調節方法的人工勢場法進行局部路徑優化，且在局部路徑規劃的同時增設虛擬目標點，讓機器人能夠逃離陷進點，但機器人并不具備動態避障能力，且規劃出來的路徑可能非最優。文獻[7]針對機器人在類似超市環境等密集區域下的路徑規劃問題，在A*算法基礎上提出了一種動態環境下機器人多狀態自主導航方法，在狹窄通道與動態障礙物的環境下，機器人都有良好的避障策略與局部規劃能力，但其考慮參數過多，容易導致全局路徑非最優。

針對以上方法的特點與不足，本文提出一種基于A*算法的改進的路徑規劃方法。首先在傳統的全局A*算法之上將路徑節點進行優化，按照刪除冗余點準則與新增節點準則，減小全局規劃任務，提高效率；再結合滾動窗口法的思想，在既定的滾動周期內，結合全局規劃的節點信息，按照一定準則在局部環境中生成子目標；在該環境內加入動態避障控制策略的同時再進行“全局路徑規劃”，根據傳感器探測范圍，如果環境范圍過大再生成子局部環境，依次遞歸，進行多重A*算法，直至機器人能夠到達子目標點。在Μatlab 軟件中建立柵格圖進行仿真，仿真結果表明，相比于傳統的A*算法，改進算法下機器人能在部分地圖信息未知的情況下實現局部路徑規劃，并擁有良好的避障功能，且消耗的時間更少，路徑的長度更短。

1 改進A*算法

A*算法是在Dijkstra 和BFS 兩種算法的基礎上提出的靜態網絡最優路徑搜索方法，它是一種啟發式的搜索算法[8]。傳統A*算法僅適用于全局的靜態環境，同時由于啟發式搜索的原理限制，并不會考慮到路徑節點連線的平滑度。針對傳統A*算法的局限性與不足，進行以下改進：

（1）優化路徑節點，平滑路徑軌跡；

（2）結合滾動窗口法，綜合節點信息確定局部路徑規劃區域；

（3）加入避障控制策略，在局部路徑規劃過程中保證機器人在較短時間內到達子目標點。

1.1 優化全局路徑節點

傳統A*算法是以最短路線優化目標，規劃出來的全局路徑在復雜的環境下通常會出現折線較多的情況，原因是其包含部分冗余點以及一些不必要的轉折點，易導致機器人運動的角速度及向心加速度發生變化，現采取以下方法準則對路徑節點進行優化：

刪除節點準則：若當前點與其父節點、下一節點在同一直線上，則判定該節點為可刪除節點；若當前點與其父節點、下一節點連線所成角大于α 小于180°，且父節點和下一節點的連線與不可通行區域無交點，則判定該節點為可刪除節點。找出可刪除節點剔除后，重新遍歷新節點，更新路徑。

新增節點準則：若當前節點和軌跡所成夾角與下一節點和軌跡所成夾角差值大于β，在兩節點中點處新增一個節點，并將該節點沿軌跡法線方向以減小β 角的方向平移γ 個柵格單位，重新遍歷新節點，確認路徑與不可通行區域無交點后，更新路徑。

1.2 局部路徑規劃

局部路徑規劃中移動機器人需要知曉環境信息與自身位置信息，利用外界傳感器構造滾動窗口探測環境信息。本文結合滾動窗口法與A*算法進行局部路徑規劃的思想為：在構造的柵格地圖環境中，設定起始點S、目標點E、傳感器環境探測距離R、滾動窗口Vision，在規劃完全局路徑后，結合節點信息與窗口范圍，選取若干局部路徑規劃區域與子目標點，在當前窗口內基于原規劃路徑實時進行局部路徑規劃，到達子目標點后更新局部路徑區域，重復以上過程，直到最后一個滾動窗口刷新，到達目標點E，完成路徑規劃。

下面在構造的柵格地圖環境中，對某個局部路徑規劃窗口進行數學建模。設柵格地圖的環境大小為m×n，柵格尺寸為δ，工作空間為柵格集合J ，機器人當前位置柵格為sEn，目標柵格為sEn+1，機器人傳感器探測距離R 視作矩形距離RV，如式（1）所示；滾動窗口Jn 為以柵格sEn中心點為圓心，RV為半徑的圓形區域，d 為歐幾里德距離，滾動窗口Vision 為滾動窗口Jn內柵格x 的集合，如式（2）所示。

由式（1）可知，本文研究的移動機器人的探測柵格的規模t=RV/δ ，機器人的有效視野窗口Vision 的矩陣模型如下所示：

如圖1所示，機器人處在一個10×10的柵格環境中，設定柵格尺寸δ=1,RV=3，當前機器人處于sEn點，也是坐標為（2，1）的柵格的中心，可計算得t=3,m=n=10。由式（2）和式（3）可推得機器人在該環境下的有效滾動窗口視野柵格包括：Vision={(1,1);(2,1);(3,1);(4,1);(1,2);(2,2);(3,2);(4,2);(1,3);(2,3);(3,3)}。

圖1 滾動窗口環境下的柵格環境模型

假設整個路徑規劃過程完成了n 次局部路徑規劃，第m 次局部路徑規劃的路徑為D(m)，則全程的路徑可以表示為：

由式（4）可知，局部子目標點sE 的選取對于路徑通往終點E 具有導向作用，需要建立起始點、局部子目標點sE 與終點E 的聯系。因此，局部路徑規劃過程的關鍵點在于局部子目標點的選取。結合全局路徑節點，本文提出將局部子目標點所處位置分為多種情況討論。

（1）局部子目標點與節點重合但處于可行域內

若當前環境過小，將會出現目標點E 在第一個滾動窗口中的情況，此時目標點E 與局部子目標點重合，這種情況即為全局路徑規劃。若目標點E 并未出現在第一個滾動窗口中，則從起點開始，選取在每個滾動窗口中出現的序列號最大的節點作為第一個子目標點。

（2）局部子目標點與節點不重合但處于可行域內

若在某個滾動窗口中，因環境過大或在優化全局路徑時刪除的序列點過多，導致序列點過于稀疏，傳感器檢測不到下一節點信息，此時新增的局部子目標點設定于機器人滾動窗口范圍Vision 與原規劃路徑的交點處。

（3）局部子目標點處于不可行區域內

若在某個滾動窗口中，檢測到在全局路徑規劃之前并未記錄到的障礙物，且障礙物恰好與本應設定的局部子目標點重合，這種情況出現時需要加入新的障礙物信息，更新地圖，重新進行全局路徑規劃，直至本次滾動窗口的局部子目標點設定在可行域內。

下面給出局部子目標選取的數學模型：

滾動窗口Vision 的柵格集合可由式（2）和（3）推得：

其中，s ∈{1,2,…,n},Vision包含柵格總數為t2=(RV/δ)2。

由式（5）可以推出滾動窗口視野中柵格點的坐標集合為：

式中，X、Y 為柵格環境中所有柵格點的橫縱坐標值所組成的集合。

滾動窗口中任意柵格點到目標點的距離函數為：

為了研究方便，移動機器人在柵格環境中的移動方向可看成是前、后、左、右、左前、右前、左后、右后8個方向，即從當前柵格向鄰近柵格行徑有8 個方向，不妨設左前方是不可通行點，則機器人可運動方向共有7 個，可表示為：

當前點柵格到相鄰柵格的距離可表示為：

由式（5）～（9）可推得，局部子目標sE 選取規則的數學模型為：

1.3 避障控制策略

移動機器人如何在未知環境下自主躲避動態障礙物一直是路徑規劃問題的難點。本文為了研究方便，將動態障礙物的運動做勻速直線運動處理，同時對于障礙物的體積和形狀做點化處理，即不考慮障礙物的具體尺寸和大小。

自主移動機器人避障技術發展以來，可將其避障控制結構分為兩大類：反應型控制結構和慎思型控制結構[9-10]。反應型控制結構是指機器人的感應器在接收到障礙物信息后，執行器直接執行設定好的指令，即“感知-執行”的過程，這種控制結構優點是反應迅速，缺點是缺少全局觀。如文獻[11]與文獻[12]中采用的避障策略，在復雜環境下容易無法到達最優點。慎思型控制結構為“感知-規劃-執行”，這種控制結構的機器人一定要具備分布式的嵌入式控制結構，感應器的信息首先傳入上層決策層，經過決策層分析處理后才傳入執行機構做出動作，這種控制結構的智能化程度雖然提升了，但是控制結構過于臃腫，往往在設備性能不高的情況下難以實現。如文獻[13]與文獻[14]就是這種控制結構，它將避障環境分為兩種情形，每種情形下障礙物不同的運動方向避障策略也截然不同，這在真實環境下是難以實現的。

結合前文中的路徑規劃方法，本文綜合反應型控制結構和慎思型控制結構的優點，在局部路徑規劃過程中，基于云的協作定位系統，使用卷積層和ConvLSTΜ層的組合算法確定動態障礙物與局部路徑軌跡的關系[15]，混合使用上述兩種避障控制結構。

（1）動態障礙物軌跡與路徑軌跡重合，如圖2和圖3所示。

圖2 障礙物軌跡與路徑方向相反

圖3 障礙物軌跡與路徑方向相同

圖中紅線表示機器人原定的路徑軌跡，紅色方塊(R)表示移動機器人，藍色方塊(M)表示膨化處理后的障礙物，褐色物塊(X) 表示膨化處理后的碰撞位置。圖2表示障礙物的行進方向與機器人的行進方向相反，此時采用反應型控制策略，機器人向路徑法線方向平移一個柵格單位的距離，在與障礙物距離開始遞增并且大于一定閾值時，機器人返回原軌跡行駛。圖3表示障礙物的行進方向與機器人的行進方向相同，當障礙物的速度大于機器人的行駛速度，易知機器人可按原軌跡行進，當障礙物速度小于機器人的行駛速度，機器人采用慎思型控制結構，計算出碰撞點位置后，將碰撞點膨化處理為不可行點，重新進行路徑規劃，更新路徑。

（2）動態障礙物與路徑交為一個點或多個點，如圖4和圖5所示。

圖4 障礙物速度小于機器人行進速度

圖5 障礙物速度大于機器人行進速度

當動態障礙物行徑與路徑軌跡的交點非碰撞點時，易知機器人可按原路徑行駛，不予討論。當障礙物的行徑與機器人有一個碰撞點時，若動態障礙物速度遠小于機器人行進速度，采用慎思型控制結構，如圖4所示，可按照障礙物的速度大小將碰撞點做相應的膨化處理，將膨化區域視作不可行區域，重新進行局部路徑規劃，更新路徑。若障礙物速度遠大于機器人速度，將導致膨化區域過大，應采用反應型控制策略，如圖5所示，機器人在碰撞點前等待，待障礙物穿過機器人規劃路徑后，再按原路徑行駛。

2 實現流程

2.1 地圖及算法環境處理

為了便于研究，本文對移動機器人及環境地圖做以下設定：

（1）采用柵格法對環境地圖進行數學建模，可行進區域與不可行進區域分別用白色柵格與黑色柵格表示。

（2）地圖環境的起點、終點已知，地圖中部分障礙物信息未知。

（3）將移動機器人與動態障礙物點化后膨化處理。

（4）地圖環境中的動態障礙物做勻速直線運動，但障礙物速度方向、出現規律未知。

（5）移動機器人帶有傳感器，能夠感知有限范圍內地圖的信息，如障礙物的方位、速度等。

（6）移動機器人的運動為勻速運動，且可全向運動。

2.2 算法流程

動態環境下的多重A*算法流程如下：

步驟1 算法的初始化。初始化系統參數：路徑規劃的起始位置S，目標位置E，柵格尺寸δ，柵格數量n，生成工作空間柵格集合J ；初始化算法參數：機器人傳感器探測距離R，路徑優化參數α、β、γ。

步驟2 基于已知的環境信息，生成全局路徑并優化全局路徑。以傳統算法生成路徑后，經刪除節點、新增節點優化路徑，生成可參照節點信息，基于該部分的節點信息進行初步局部路徑規劃。

步驟3 選取當前環境下滾動窗口的子目標點，判斷當前子目標點信息，若當前子目標點處恰好為不可通行柵格，則轉到步驟2，若子目標點處為可通行柵格區域，則轉到步驟4。

步驟4 在當前局部路徑規劃區域內采用起始點為sEn、目標點為sEn+1 的多重A*算法，同時引入避障策略，根據動態障礙物信息的不同，混合使用反應型控制結構和慎思型控制結構。

步驟5 在到達局部子目標點后，判斷當前位置是否與目標終點E 重合，若不重合，則跳到步驟3；若重合，說明機器人已到達目標位置，算法結束。

3 仿真分析

3.1 多重A*算法與原算法的比較

為了驗證多重A*算法有較好的全局路徑規劃能力與局部路徑規劃能力，本文在不同環境下將多重A*算法與A*算法進行實驗仿真比較。實驗在CPU為I7-8700，RAΜ 為16 GB 的計算機上運行，算法通過Μatlab 編程仿真實現。

3.1.1 路徑軌跡的比較

本文算法相比于傳統A*算法，在靜態全局路徑規劃問題上主要是對規劃的路徑軌跡進行優化處理。在20×20 的柵格環境下，以（1，1）為起點，（20，20）為終點進行傳統A*算法的全局路徑規劃后，用本文算法優化路徑節點。

優化路徑節點的關鍵在于參數α、β、γ 的選取，α、β 越小理論上優化效果越好，但α 過小容易導致節點少，路徑不平滑，β 過小規劃軌跡容易與不可行域相交，同時α、β、γ 的選取也與環境大小、障礙物覆蓋率相關。在20×20 的柵格環境下，經過多組數據仿真比較，選取參數α=126°,β=58°,γ=2 下路徑的優化效果最好，優化軌跡的參考標準有累積轉角、平均轉角、路徑長度。圖6為路徑節點的優化過程，紅線表示生成的全局路徑。圖6（a）為傳統A*算法生成的路徑軌跡，圖6（b）為刪除部分冗余點后生成的路徑軌跡，圖6（c）為新增節點后生成的路徑軌跡，即為本文算法生成的路徑。

由表1可知，經過本文的路徑優化策略，對比傳統A*算法累積轉角降低了81.23%，平均轉角降低了37.94%，路徑長度降低了9.58%。文獻[13]與文獻[14]對于軌跡僅做了本文路徑優化策略的第一步，即只對路徑算法做了刪除冗余點處理。由表1可知，雖然本文算法在路徑長度上相比于文獻[13]與文獻[14]有少許增加，但是累積均轉角與平均轉角相比其他文獻降低了23.64%和15.82%，使得路徑軌跡更加平滑，更符合移動機器人的運動學規律。除此之外，新增的節點縮短了節點間的平均距離，更有利于后續局部路徑規劃范圍的選取。

表1 不同算法路徑的優化性能比較

圖6 優化路徑過程

3.1.2 算法效率的比較

Dynamic A*是由Stentz在1994年提出的一種在傳統A*算法的基礎上發展而來的動態路徑搜索算法，一種比較典型的動態路徑規劃，擁有局部路徑規劃能力[16-17]。Dynamic A*算法是一種動態環境下的廣義A*算法，將其與本文算法比較時間效率與空間效率。

為了減少環境的變化給實驗帶來的影響，進一步提高實驗的客觀性，本文采用不同數據在同一臺計算機上進行算法仿真模擬，并建立3個不同復雜程度的環境將本文算法與Dynamic A*算法進行比對。環境的復雜程度設定為環境的大小與障礙物的覆蓋率，如圖7 及表2所示。圖7（a）表示環境面積小且障礙物覆蓋率低的環境，圖7（b）表示環境面積適中、障礙物覆蓋率大的環境，圖7（c）表示環境面積大、障礙物覆蓋率高的環境。

表2 不同復雜程度下的環境參數

如表3 所示，本文從路徑規劃時間（單位為程序執行的周期次數T）與規劃的路徑長度兩個指標對比Dynamic A*算法與本文算法，可知在20×20的柵格環境下本文算法路徑規劃時間與路徑長度分別減少了12.3%和11.0%，在50×50的柵格環境下本文算法路徑規劃時間與路徑長度分別減少了18.6%和16.4%，在100×100的柵格環境下本文算法路徑規劃時間與路徑長度分別減少了50.4%和49.1%。

實驗數據表明，隨著柵格環境的增大，環境越復雜，本文算法的優勢越加明顯。如圖8所示，隨著環境面積的增加，在柵格數目超過140 左右，Dynamic A*算法所花費的時間與空間將呈指數型增長，而本文算法則在柵格數目超過180后斜率才慢慢增加，理論上也可證實隨著環境增大，本文算法效率越來越優于Dynamic A*算法。這是因為A*算法是通過等勢線逐級擴展的方式進行搜索，搜索空間較大，這是一種長度優先算法，生成的路徑容易在小范圍區域內出現多次轉彎的現象，環境越復雜，這種現象越加明顯[18-19]。而本文算法的路徑更新僅發生在每個滾動窗口內，隨著空間環境變化，耗費值的增加僅來源于滾動窗口內部路徑的變化以及滾動窗口數量的增加，大大降低了路徑規劃的時間和空間成本。

圖7 不同復雜程度環境下的柵格地圖

表3 不同環境下算法性能對比

圖8 規劃時間與路徑長度隨環境變化折線圖

3.2 動態環境仿真

為了驗證本文算法的動態規劃能力，在算法仿真中引入未先驗的動態障礙物，建立簡單環境進行仿真驗證。在該環境下首先根據先驗的地圖信息，以S(1,1)為起始點、E(20,20)為終點，進行全局路徑規劃，優化路徑后得到如圖9所示的路徑，用紅線表示。在該路徑下選取合適的局部子目標點，增加的局部子目標點數目為2個，設為A、B。

圖9 先驗環境下的路徑

在該環境下路徑被分割成3個子路徑，分別是SA、AB、BE。機器人在起始位置時，在第一個滾動窗口內未檢測到地圖信息有變化，機器人沿先驗環境下的路徑由S 點行進到第一個局部子目標點A。此時，機器人在第二個滾動窗口內檢測地圖信息，此時檢測到動態障礙物M（藍色方塊），根據傳感器的信息反饋，機器人將在X 位置（紅色柵格）與障礙物相撞，如圖10（a）所示。

根據傳感器的檢測信息，該動態障礙物M 的行進速度小于機器人的行駛速度，M 的行徑軌跡與路徑軌跡有一個交點，采用慎思型控制結構，在局部區域中重新進行路徑規劃，更新子路徑AB。如圖10（b）所示，原路徑用虛線表示，機器人的行駛路徑用實線表示。當到達第二個子目標點B 時，障礙物位于機器人左側位置，完成動態避障過程。

表4 為在圖9 所示的20×20 的柵格環境下，引入未先驗的動態障礙物時不同算法的定量實驗比對。傳統A*算法不具備避障能力，在遇到未先驗的障礙物后路徑規劃失敗，不可到達目標位置。Dynamic A*算法與本文算法在遇到未先驗障礙物時，都有重新規劃路徑的能力，可到達目標位置，且本文算法的規劃時間與路徑長度相對于Dynamic A*在該實驗環境下減少了12.9%與8.1%。

圖10 動態避障

表4 動態環境下不同算法比對

4 結束語

傳統的A*算法是一種全局的靜態路徑規劃算法，存在路徑不平滑、搜索效率低等缺點，本文針對A*算法的不足，提出了一種適用于局部動態環境的路徑規劃的多重A*算法。多重A*算法首先在原有全局路徑之上優化了全局路徑軌跡并提取了局部子目標點，在每個滾動窗口內進行局部路徑規劃，并結合反應型和慎思型避障控制策略的優點進行實時避障。本文將改進后的算法與原算法進行了比對，并在動態環境下進行了仿真分析，在20×20的柵格環境下改進后算法相比于原算法累積轉角降低了81.23%，平均轉角降低了37.94%，路徑長度降低了9.58%，規劃時間降低了12.3%，且在未知環境中能有效地避開動態障礙物。