基于視野域的快速動態路徑規劃

鐘志峰， 周 霖， 柯 偉

（湖北大學 計算機與信息工程學院， 湖北 武漢 430062）

0 引 言

伴隨科技的發展和現實的需要，路徑規劃應用范圍越來越廣，如日常生活中的手機導航、倉儲的自動物流分揀、智能倉儲系統等。在路徑規劃的系統要求中，如何在最短時間內自主規劃一條最短、行走最快的路徑是評判路徑規劃算法有效性的重要標準之一。而蟻群算法作為一種群智能算法[1]，具有很強的魯棒性和搜索較好解的能力，因此廣泛地被國內外學者應用于自主路徑規劃中。但是隨著時代的發展，基礎算法面對越來越多的不同領域的應用需求，出現了如容易陷入局部最優、計算時間較長、收斂慢以及轉彎角度過多等問題。因此，針對該算法的改進一直是熱門方向[2]。

對于陷入局部最優的改進，提出了諸如在選擇決策中通過對螞蟻轉向角度的限制減少轉角次數，從而減少路徑長度[3]；文獻[4]通過設置不均等的初始化信息素濃度，使螞蟻在最有可能出現最優解的區域進行重點搜索，提高算法效率；文獻[5-6]結合人工勢場，能有效地繞開障礙物，并加大了啟發因子的影響。以上文獻雖然對于自主尋跡的下一步移動策略有較大的優化，但都忽略了環境因素的影響，在復雜的環境條件下，仍然會存在收斂速度慢、難以找到最優解等問題。文獻[7]考慮到環境的影響，應用負反饋機制增大搜索范圍，但也增加了陷入死胡同的概率。文獻[8]采用回退策略來減少螞蟻進入凹形障礙物的概率，但這種方法計算量大且不能完全杜絕進入凹形障礙物的情況。

對于減少轉折角的改進，首先是提出加入轉角數判斷因子[9]，結合轉角和最短路徑來選擇最優路徑，但是算法中單步步長[10-11]的約束使其轉折角的數量無法進一步降低。因此繼續提出步長優化算法[12-13]，通過對指定步長的優化來減少轉角數量，但是指定步長不夠靈活，固定步長難以適應不同的場合。在此基礎上，馬飛宇等人進一步提出變步長算法[14]，將判斷區域和行走步長分開來，擴大判斷區域來實現動態步長。但是判斷區域的范圍已被指定，導致步長只能在判斷區域范圍內進行行走，對于步長選擇優化不明顯。

本文提出一種基于視野域的動態快速路徑規劃算法，優先對于地圖進行處理，采用填充法解決了螞蟻陷入凹陷問題，也提高了螞蟻行走的效率。再采取改進的負反饋思想，結合負信息素減少沒有找到終點路徑和最差路徑上的后續螞蟻行走概率。最后借鑒自然界中兵蟻和工蟻的現象，給工蟻加上視覺，引進視野域的思想優化當前找出的最優路徑。在實際行走中若存在障礙物動態變化和多個新增障礙物的現象，重新規劃部分路徑時結合DWA 算法找出新的最優路徑。實驗結果表明，本文提出的改進算法能夠很好地解決基礎算法出現的不足，具有迭代次數少、轉角少、行走時間短、能快速重新規劃新的最優路徑等優點。

1 相關算法介紹

1.1 建立柵格地圖環境

常用的環境建模方法有柵格圖法、自由空間法等。本文選擇簡單直觀的柵格圖法對目標二維運動空間進行環境建模。柵格由0、1 矩陣組成，0 代表自由柵格，表示無障礙物，顏色為白色；1 代表障礙柵格，表示存在障礙物，無法通過，顏色為黑色。設置自由柵格和禁忌柵格都為邊長為1 的正方形，如圖1 所示。同時設置螞蟻的行走方向為8 個方向角，但是存在2 個障礙物夾角是不可以通過的，如圖2 所示。圖中右上角位置的箭頭方向代表不可通過，其他箭頭代表可以通過。

圖1 柵格地圖

圖2 方向圖

對于圖1 建立的柵格圖，為便于在后續計算中標識螞蟻的位置，需將n×n柵格矩陣中的每一個柵格按照從左上角開始從上往下，再從左往右進行編號，編號i和其坐標(x,y)的對應關系如下：

1.2 負反饋蟻群算法

負反饋蟻群算法[15]是針對基礎蟻群算法中沒有找到目標點螞蟻，提出的一種在這些死去的螞蟻的路徑上釋放一種負信息的思想，其螞蟻移動概率公式如下：

式中增加了負反饋信息因子δ-φij(t)。其中：δ為常數，表示負信息素濃度的上限；φij(t)為負信息素矩陣。設信息素揮發系數為ρ（0＜ρ＜1），則公式（3）分別表示正、負信息素的更新規則。

1.3 自適應動態窗口算法

動態窗口算法（DWA）是一種局部避障算法[16-17]，將機器人的位置控制優化為速度控制，然后將角速度旋轉和速度控制范圍結合起來得出所有可能路線，如圖3 所示。再根據目標函數選出其中合適的路線。針對經典DWA 算法不能兼顧速度與安全，以及在高速通過障礙密集區時通過性較差的問題，提出一種自適應DWA 算法，其在不同障礙物密度環境下的目標函數中的速度權值不同，如下：

圖3 動態路線選擇

式中：heading 代表與障礙物的距離；dist 代表與當前目標點的距離；v、w代表當前速度和角速度；α、β、γd代表不同的權值。γd的取值如下：

2 基于視野域的快速動態路徑規劃的蟻群算法模型

2.1 地圖環境填充優化

圖1 中這種存在大量凹陷的障礙物，會使算法中的螞蟻更容易陷入凹形陷阱當中，需要對其進行優化，這也是為后文中的視野域算法降低圖形復雜度和減少計算量。為解決這一類問題，本文采用的環境優化算法通過分解-填充-合并的方式將地圖環境中的凹形障礙物進行處理。

分解過程：將地圖中連在一起的點作為一個整體提取出來。分解過程表達式如下：

式中：f(x,y)函數的含義為當點(x,y)處存在障礙物時，將與點(x,y)相鄰且存在障礙物的點的坐標放在一個集合S(x,y)中；f(i1,j1) ?f(i2,j2) 表示若集合S(i1,j1)與集合S(i2,j2)存在重疊坐標點時，則將集合S(i2,j2)中的元素放入集合S(i1,j1)中；unique 函數作用是去除集合中的重復元素。

通過上面的公式最終可以得到多個集合，每個集合存放的便是一塊獨立障礙物的位置坐標信息（即每個集合就是一塊獨立障礙物的子地圖），對其進行標注序號，然后對每個子地圖進行填充處理。其公式如下：

式中：Xmin、Xmax分別表示集合S中x的最小值和最大值；Ymin、Ymax分別表示集合S中y的最小值和最大值；F(x,y)函數給點(x,y)處賦值，1 代表存在障礙物，0 代表無障礙物。F函數的含義是當x= index（或者y= index）時，相應地在x方向（或者y方向上）進行填充。

對每個子地圖進行填充后，將子地圖合并得到最終處理后的地圖。S計算公式為：

整個環境優化過程如圖4 所示。

圖4 環境優化過程

2.2 全局規劃算法改進

在負反饋的基礎上，為進一步提高搜索效率，本文采用了無效路徑（沒有到達目標點的路徑）上的負反饋方法對基本蟻群算法進行改進，減少算法在無效區域的運算，同時也保留了最差有效路徑負反饋的方法，減少陷入局部最優的情況。此外，為了減少算法的迭代次數，更快地找到最短路徑，對于信息素和揮發系數等都進行優化。

2.2.1 改進的負反饋蟻群算法

公式（3）中Στkij(t)表示本次迭代中第k只螞蟻在本次循環中留在節點i和j之間的信息素，這里采用Dorigom 提出的Ant-Cycle 模型，如式（9）所示：

式中：Q為信息素強度；Lk為螞蟻k在本次循環中所走過路徑的總長度；pk(begin,end)為螞蟻k在本次循環中從起點到終點所走過的路徑。

在文獻[7]中只有最差路徑上的負信息素，在這里增加未到達目標點路徑上的負信息素。公式（3）中的Δτaddij表示每一代螞蟻釋放的負反饋激素，具體更新方式如下：

式中：Cw表示最差路徑的長度；dk表示未到達目標點的路徑長度；N為常量。當一次迭代完成后，會更新φij(t)矩陣，更新區域為：在整個最差路徑和無效路徑的后部分上釋放負信息素。

2.2.2 改進的信息素分布方法

本文中對初始化地圖上的信息素分布采取不均勻[18-19]的分布方式，如圖5 所示。連接目標點和起始點，為了蟻群算法能夠更快收斂得出結果的同時，也希望其能夠在障礙物途中找出最短的路徑。所以在距離起始和終點連線距離的dij2 的外部信息素濃度只有中間區域信息素濃度的2 3，有利于蟻群提高搜索速度，快速找到最優路線。

圖5 信息素起始分布

同時為了防止負反饋導致某條路徑信息素過高而另外的某條路徑上信息素又過低，導致螞蟻過于集中某條道路或者完全不走某條道路，將每個區域的信息素設置一個上下限度[1，8]，擴大螞蟻的搜索能力，避免陷入局部最優解。

2.2.3 改進的啟發函數

傳統蟻群算法的啟發函數都是在利用當前和下一步之間的距離倒數，或者當前位置和終點之間的距離倒數。這種函數前面的收斂性要么不強，使得蟻群尋找的路徑冗長，要么后面一種收斂性過強，過于引導螞蟻選擇終點方向，從而出現錯誤選擇而導致路線冗長。改進后的啟發函數如下：

式中：dij為當前和下一步選擇之間的距離；dis為當前位置和最終點之間的距離。將它們兩個相減是為了權衡下一步的選擇，使其能盡量避免陷入局部最優，增加蟻群算法的全局性。

2.2.4 增加的結束判斷函數

在一般算法中是讓蟻群固定地循環固定次數，但是蟻群已經找到最優路徑而循環還沒結束，因此在本文中設置了結束判斷，即當螞蟻已經找到了最優路徑的情況下結束算法的運行。在算法中每一輪循環都會尋找出一個此輪最優的路徑，根據這個機制結合實驗，提出了當某個最優路徑連續出現預設循環輪數K的1 10 次，默認為已經找到最優路徑，退出循環。

2.2.5 動態視野域

視野域是指螞蟻在網格地圖中被網格地圖能夠看到的范圍大小[20]。自然界中螞蟻部落中有工蟻和兵蟻兩種螞蟻，工蟻根據信息素行走的同時也會跟隨兵蟻，在前期工蟻已經找到食物和當前最短的路徑時，會出動強壯的兵蟻前往。設定兵蟻能夠使用嗅覺的同時也會使用它們的視覺，此時工蟻依舊在它們認為的最短路徑中行走于起始點和終點之間（視作可以看見的一條線路），而兵蟻在嗅著信息素往前走的同時也觀察視野里行走的螞蟻，它們會朝向當前視野中最遠的螞蟻位置直線走去，而工蟻會跟隨。兵蟻只有當視覺觀察到轉角（即當和最近障礙物的距離為移動機器人的半徑時）或者走到上次視野觀察最遠的位置螞蟻相遇，利用觸角交換信息后，才會暫時停下來繼續觀察下一步。視野域如圖6 所示。

圖6 視野域

圖6 中的粗線為初步規劃路線，黑色框為障礙物，工蟻的視野域規劃為圖中虛線線段，最后新生成的路線如圖中的細線段。

視野域的原理是：將先前規劃好的路線分為若干個節點，螞蟻從一個節點出發時視野域是全局的，但為加快程序運行速度，采取工蟻會優先向目標點方向望去（圖6 中為右下角）。看到視野里最遠的螞蟻時會直接向該螞蟻的位置走去，直至遇到轉角（即當和最近障礙物的距離為移動機器人的半徑時）或者走到觀察最遠的位置螞蟻相遇時，生成一個新的節點和新的路線并記錄，刪除走過的路徑。重新循環該方法直至走到終點。

式中：nextpot是下一個選擇節點集合；A為路線節點集合；B是視野范圍內的節點。從交集當中選出距離最遠的。

采用動態視野域的思想主要目的是優化路線且減少運算時間，使得移動機器人更快地到達終點。其原理是移動機器人在行走過程中設定有最大和最小速度，以及加減加速度，而過多的轉彎會使機器人頻繁地加減速度，浪費不必要的時間。

2.3 局部規劃算法

2.3.1 改進動態窗口算法

本文中，在Matlab 上仿真，設置地圖每隔0.2 s 更新一次，檢測地圖規劃路線上是否出現新的障礙物。當檢測可以正常通過的位置遇到動態障礙物或者出現多個靜態障礙物時，本文采取的是以當前節點為起點，重新規劃路線和DWA 局部路徑規劃兩種方法結合來處理。當路線上出現多個靜態障礙物時，從當前起點重新規劃最優路線。

式中：heading(v,w)、dist(v,w)、v(v,w)和high(v,w)分別為偏向目標方向、距離最近障礙物距離、當前速度和距離規劃路線距離；Vα、Vβ、Vγ、Vδ代表權重因子。

自適應窗口法能夠解決動態障礙物和單個靜態障礙物的問題，改進部分使其能夠快速尋找到最短的路徑。機器人在相鄰的節點運動過程中，通過自適應動態窗口算法[21]動態調節探索窗口大小得到所有可行行走方向區域后，在可行區域內，機器人通過式（14）中不同評價因素共同來確定最優前進方向。

自適應動態窗口算法關注的是在具有復雜障礙物的地圖環境中行走時，過快速度和角速度容易發生危險，但是過慢會浪費時間，因此提出了自適應的速度和角速度，使其在障礙物較少的時候以較高的速度和較低的角速度行走，在障礙物較多的地方以較低的速度和較高的角速度行走，使其在障礙物多時行走的路線更加平滑。但是速度不是瞬間變化的，而是有加速度和角加速度的，所以隨著周圍環境的變化，下一時刻規定了最大的線速度和角速度。

式（15）是機器人的運動學約束，Vk表示機器人底盤實際上能夠跑到的最大最小線速度和角速度。式（16）是機器人的動力學約束，Vb表示機器人底盤從當前時刻vc在經過時間段dt后能夠達到的最大最小線速度和角速度范圍，其中Vv代表線速度加減速度，Vw代表角速度加減速度。式（17）是機器人在環境障礙物的約束，Pnull、Pall是障礙物占柵格的比率，V(t+ 1)和W(t+ 1)代表下一時刻所走區域的最大最小線速度和角速度，Kv表示線速度調整參數，Kw表示角速度調整參數，主要是為了使其能夠以較低的速度和較高的角速度通過障礙物較多的區域。

2.3.2 DWA 算法與重新規劃結合

在實驗仿真行走時，會出現多個靜態障礙物擋住行走路線規劃，如圖7 所示。而此時采用DWA 算法進行局部規劃，在障礙物中行走時，由于自適應速度，行走較慢；而以當前起點重新采取全局規劃，可以使得行走時間更短，轉彎節點更少。

圖7 靜態障礙物

判斷應該采取DWA 算法還是重新規劃的標準為：

式中：η、μ是權重參數；obs 是障礙物數量；Δx是第一個在線上到最后一個在線上的靜態障礙物之間的距離；Pfull、Pall是障礙物占柵格的比率。通過實驗測試表明，將η、μ設置為6 和4 的時候，當temp 為3.1，最佳；當temp小于3.1 時，采取DWA 算法；當temp 大于3.1 時，采取重新規劃。

3 消融實驗仿真與結果分析

為了驗證環境優化算法改進的有效性，本文采用控制單一變量法進行多組對比實驗，分別在簡單環境下和復雜環境下進行對比試驗，簡單環境為20×20 柵格圖，復雜環境為40×40 柵格圖。同時在本文中，對于算法的靜態實驗和動態實驗進行分開對比，對于靜態實驗，主要是對比算法步數、路徑距離和收斂速度，對于動態實驗，主要是研究路徑距離。

3.1 實驗環境及參數設置

實驗環境為：WIN10 系統，CPU 為E5-2697，128 GB運行內存，Matlab 2016a。算法中使用的部分參數見表1，行走時間包括直線行走時間和固定轉彎時間。

表1 主要參數表

為了驗證環境優化和算法優化對提高目標物自主尋跡效率的有效性，本文采用傳統常見的AS-N 算法（負反饋蟻群算法）、多步蟻長算法和本文改進的蟻群算法在2 組不同環境下進行對比測試實驗，簡單環境為20×20 柵格圖，復雜環境為40×40 柵格圖。

3.2 改進蟻群算法實驗對比

簡單環境下， AS-N、AS-N+填充、多步步長+填充、改進算法+填充對比結果如圖8～圖11 所示，不同算法的最小路徑長度隨迭代次數的變化曲線如圖12 所示。

圖8 AS-N

圖9 AS-N+填充（一）

圖10 多步步長+填充（一）

圖11 改進算法+填充（一）

圖12 迭代圖（一）

比較圖8 和圖9 可知，在未填充的地圖環境下，算法找到的最優路徑都會陷入陷阱中，并最后出現局部收斂的情況，這種情況既增大了路徑長度，也增大了計算量和行走時間。在填充后的環境圖10 中，AS-N 算法找到的最優路徑都相對短，幾乎杜絕了陷入凹形障礙物的情況。從圖12 可以看出，填充后的最小路徑都相對較小，收斂速度都相對較快。而從表2 簡單環境實驗結果中可以詳細地看到，改進的算法即使在填充后相同的環境，轉角數量和行走時間都少于前者。

表2 簡單環境實驗結果

由圖9～圖11 比較可知，在簡單環境下，本文改進的算法與AS-N 算法找到的最小路徑長度相近，這是由于在相對簡單的地圖環境下，每代“螞蟻”幾乎都能順利找到目標點；AS-N 算法和多步步長算法在這種情況下夠用，這是由于本文改進算法在此類環境下對于減小最小路徑長度的效果并不明顯，但改進后的算法在行走時間上相比前兩種算法較短。

但從圖13～圖15 復雜環境下不同算法實驗結果中可以明顯看出，在復雜的環境下，AS-N 算法和多步步長算法已經開始不適用了，最終收斂的路徑并不是全局最優解。同時通過圖16 和表3 可以看出，改進后的算法收斂路徑變短、收斂速度提高且行走時間減少明顯，算法運行時間也變短。

表3 復雜環境實驗結果

圖13 AS-N+填充（二）

圖14 多步步長+填充（二）

圖15 改進算法+填充（二）

圖16 迭代圖（二）

綜上所述，在地圖環境較為復雜時，AS-N 算法雖然能夠找到最優解，但會有很大可能陷入局部最優，即陷入凹形障礙物中；經過環境優化后，AS-N 算法和多步步長算法便很容易找到最優解，且不會陷入凹形障礙物中，同時也減少了收斂所需的迭代次數，有效地提高了算法運行的效率和精度；在地圖環境極其復雜的情況下，單靠AS-N 算法和多步步長算法已經很難找到最優解，且收斂速度慢。但本文改進算法能夠降低螞蟻前往無效區域探索的概率，使得算法集中在有效區域運算，從而找到的路徑相對較短。最后，再通過環境的優化和改進算法的雙重作用，最終取得了較好效果，解決了前文提出的各種問題。

3.3 改進結合動態算法實驗分析

對于動態地圖實驗的仿真，為了便于觀察和對比，對40×40 地圖模型進行放大10 倍，在此基礎上添加了如圖7 中間的靜態和移動障礙物，障礙物模型為邊長為1的正方形方向指向正上，速度為0.1 m/s，在放大過后的模型上顯示為1 m/s 和在地圖中顯示為10 的正方形，其余參數也對應放大10 倍。其運行結果如圖17、圖18所示。

圖17 改進DWA 算法

圖18 DWA 算法

從圖17、圖18 可以看出：對于中間的一個稍空曠地帶的動態障礙物，改進DWA 算法的移動軌跡規劃能更加靠近移動障礙物和全局路徑來行走，生成的路徑更優，而在距離目的地較近的兩個障礙物，仍采取DWA算法會出現繞遠路的情況，重新規劃反而更加快捷。最后從表4 可以看出，改進算法的路徑長度更短、行走時間更少。

表4 改進DWA 算法與DWA 算法實驗結果對比

4 結 語

本文通過消融仿真實驗，在改進的蟻群算法中，首先利用AS-N 算法進行了環境優化前后的對比實驗，主要通過收斂所需的迭代次數（收斂速度）和收斂路徑的長短這兩個指標來進行性能分析，驗證了填充的有效性；然后將改進AS-N 算法和動態視野域結合起來，進一步優化步長選擇并更好地加快收斂，在簡單和復雜地圖下分別與AS-N 和多步步長算法比較。實驗還證明改進算法在復雜環境中效果明顯，能有效解決諸如收斂速度慢、盲目性搜索、局部最優、步長選擇等復雜環境下的快速路徑規劃問題。最后采用改進的DWA 算法與重新規劃結合，使其能夠快速規避行走過程中新出現的動態障礙物或者多個靜態障礙物。整體實驗表明：采用該規劃算法可使路徑長度減少了11%，轉角和行走時間減少了45.4%和32.3%。所以，該算法能為智能機器人在動態環境的自主規劃與導航提供一種可行的解決辦法。

注：本文通訊作者為周霖。