黃夢濤，李智偉

西安科技大學 電氣與控制工程學院，西安 710600

路徑規(guī)劃是實現(xiàn)移動機器人自動導航的一個關鍵環(huán)節(jié)。路徑規(guī)劃是按照給定的地圖和目標位置，規(guī)劃出一條使機器人抵達目標位置的路徑，它通過統(tǒng)籌全局信息，制定高效、安全的移動策略[1]。

路徑規(guī)劃可以按照規(guī)劃范圍劃分為全局、局部路徑規(guī)劃，環(huán)境信息完全已知時，才能實現(xiàn)全局路徑規(guī)劃[2]。全局路徑規(guī)劃算法可以分為傳統(tǒng)方法和仿生智能方法，傳統(tǒng)方法中最典型的有A*算法、快速擴展隨機樹（rapidexploration random tree，RRT）算法等；仿生智能方法有遺傳算法（genetic algorithm，GA）、蟻群算法（ant colony algorithm，ACA）、粒子群算法（particle swarm algorithm，PSO）等[3]。

傳統(tǒng)方法中，A*算法因為能在較短時間里有效地求解出最短路徑，被廣泛應用于各個領域，然而，A*算法的啟發(fā)式函數(shù)構造簡單，算法在執(zhí)行過程中會出現(xiàn)較多冗余節(jié)點，耗費時間較長[4-5]。RRT算法普適性強，不僅可以工作在二維地圖，也適用于高維空間，因此更多地應用于機械臂的路徑規(guī)劃，但是RRT算法得到的路徑質量不高，折轉次數(shù)多，解的最優(yōu)性較差，工程應用時往往使用的是與勢場法結合后的改進RRT算法。

仿生智能方法中，遺傳算法從多個初始點開始操作，而不是從某一個單獨點開始，很大程度上避免了搜索過程中過早收斂于局部極值，因此更有可能求得全局極值，但同時也使得計算速度較慢全局路徑搜索的效率相對較低，算法迭代進化時會產生一些沒有意義的種群，不適合實時路徑規(guī)劃。蟻群算法采用正反饋機制，使搜索過程不斷收斂，而且蟻群算法具有分布式計算能力，可以在全局多點同時搜索最優(yōu)路徑，因此計算效率較高，但是由于蟻群算法初期信息素的缺乏，容易陷入局部最優(yōu)，雖然蟻群算法初期收斂速度快，但在后續(xù)容易發(fā)生路徑迂回和死鎖[6]。粒子群算法的收斂速度快，需要設置的參數(shù)較少，簡單易行，但也容易陷入局部最優(yōu)。

1 文獻綜述

針對上述幾種路徑規(guī)劃算法的缺陷，當前有眾多科研工作者提出了改進方法。文獻[7]提出了一種改進的A*算法，通過引入環(huán)境障礙物比率K，量化了環(huán)境中的障礙物信息，根據(jù)K值調節(jié)啟發(fā)函數(shù)的權重，提高了算法效率和靈活性，另外，基于Floyd算法思想設計了一種路徑節(jié)點優(yōu)化算法，剔除了冗余節(jié)點，提高了路徑平滑度，但是隨著啟發(fā)函數(shù)的權重變化，路徑的最優(yōu)性會受到影響，K值越大，搜索速度越快，路徑的最優(yōu)性就越差，所以應該加入約束條件來限制K值的增長；文獻[8]針對傳統(tǒng)RRT算法內存消耗大的問題提出了一種基于簡化地圖的區(qū)域采樣RRT*（simplified map-based regional sampling RRT*，SMRS-RRT*）算法，首先簡化柵格地圖，再用PRM算法尋找最優(yōu)路徑作為引導路徑，通過智能采樣因子擴大引導路徑得到智能采樣區(qū)域，在其中迭代搜索找到一條代價小且無碰撞的路徑，最后結合最小轉彎半徑約束和B樣條曲線優(yōu)化路徑，但是這種簡化地圖的方法比較簡單，僅通過障礙物占用的柵格數(shù)量與設定的閾值比較，剔除占用柵格少的一部分障礙物，因此無法保證最終生成路徑的安全性；文獻[9]針對遺傳算法在初始化種群時計算方法的不足，提出利用SPS（surrounding point set）算法在障礙物周圍生成點來產生初始路徑，提高算法快速生成初始種群的能力，增加平滑、刪除算子，刪除不必要的路徑節(jié)點，使路徑更平滑，最后結合小生境法保持種群多樣性，避免出現(xiàn)早熟現(xiàn)象，但是刪除算子只能作用于路徑節(jié)點初次與障礙物連接的地方，每處障礙物至多刪除一個多余節(jié)點，因此對路徑的收斂速度和平滑性提升不大；文獻[10]提出了一種改進的蟻群算法，將螞蟻搜索方向擴展為16方向24鄰域，結合向量夾角的思想設計出新的啟發(fā)信息計算方法，引入轉移概率控制參數(shù)δ調控算法搜索范圍，提高了路徑尋優(yōu)效果和搜索效率，雖然文中說明了δ=0時算法類似于貪心算法，δ=1時算法采用輪盤賭策略，但是沒有說明δ的具體取值方法，而且蟻群算法容易陷入局部最優(yōu)的問題沒有解決；文獻[11]提出了一種任意時刻啟發(fā)式搜索ARA*算法，它首先在一個較短時間內找到一條非最優(yōu)可行路徑，然后在接下來的時間中盡可能地優(yōu)化，如果時間足夠多的話，可以找到一條最優(yōu)的可行路徑，ARA*算法會重復使用之前的搜索結果，這使得它比其他的任意時間規(guī)劃算法更加高效，但是算法效率提升不明顯[11]。

現(xiàn)階段針對路徑規(guī)劃算法的改進集中于執(zhí)行效率和路徑的平滑程度這兩方面，也就是說，在路徑規(guī)劃算法應用于實際工程之前，需要著力于提升算法的實時性和安全性。因此，本文在ARA*算法的基礎上做出了改良，使路徑規(guī)劃的效率提升更顯著，另外還提出了一種自適應選擇節(jié)點間連接方式的策略，兼顧了4連接和8連接的優(yōu)點，降低了安全風險的同時，縮短了路徑長度。

2 ARA*路徑規(guī)劃算法介紹及分析

ARA*（anytime repairing A*）算法是A*算法的一種改進算法：是在WA*（weighted A*）算法[12]基礎上做了進一步改動，著重于解決實際的工程問題，優(yōu)先考慮算法的執(zhí)行速度，并且算法的可移植性也得到了明顯的改善，本章對ARA*算法做簡要介紹。

ARA*算法在代價估計函數(shù)f(s)中加入膨脹因子ε。

其中，s表示當前節(jié)點，g(s)是從起點到當前節(jié)點的實際代價，h(s)是從當前節(jié)點到終點的代價估計值，f(s)則是從起點到終點的代價。算法運行時，膨脹因子ε從設定初值開始逐漸減小，在時間允許的范圍內εmin=1，如果時間不夠充足ε會盡可能地接近1，那么最后只能求得路徑的次優(yōu)解，也就是說路徑的次優(yōu)性由ε決定，且路徑的長度不大于最優(yōu)路徑的ε倍[13]。

圖1是A*算法得到的路徑，圖2是在同樣的柵格地圖中ε值不同時ARA*算法規(guī)劃出的路徑。通過對比圖1和圖2（c）得出結論，ε=1時路徑長度最短，而且此時ARA*算法等價于A*算法，路徑是最優(yōu)的。

圖1 A*算法得到的路徑Fig.1 Path obtained by A*algorithm

圖2 ε值不同時ARA*算法得到的路徑Fig.2 Path obtained by ARA*algorithm with different ε

ARA*算法用三種列表存放不同狀態(tài)的節(jié)點：

（1）OPEN列表：存儲具有局部不一致狀態(tài)的節(jié)點。

（2）CLOSED列表：存儲已經(jīng)擴展過的節(jié)點。

（3）INCONS列表：當存儲在CLOSED列表的節(jié)點g(s)值又降低時放入該列表中。

ARA*算法的局部不一致狀態(tài)[14]：如果一個節(jié)點的g(s)值在它下一次被擴展之前降低，就認為該節(jié)點處于局部不一致狀態(tài)，針對任意節(jié)點s'，假設節(jié)點s是它的最優(yōu)前繼節(jié)點，則它們應滿足公式（2）：

公式（2）中c(s,s')是從節(jié)點s到s'的代價，假如等式右側值減小，如公式（3）所示：

g(s)值的降低意味著節(jié)點s的g(s)值和其后繼若干節(jié)點s'的g(s')值之間并不滿足公式（2）的等價關系，這就是當前節(jié)點與它的前繼節(jié)點之間存在的局部不一致。

在已經(jīng)構建好柵格地圖中，ARA*算法會從起點Sstart開始擴展，把與Sstart相鄰的8個子節(jié)點加入OPEN列表，再根據(jù)公式（1）規(guī)定的啟發(fā)式搜索規(guī)則從OPEN列表里搜索f(s)最小的節(jié)點作為下一個被擴展的節(jié)點，然后對下一個被擴展的節(jié)點執(zhí)行同樣的步驟，以此類推。隨著路徑的推進，有些節(jié)點的g(s)會降低，如果該節(jié)點已經(jīng)存儲在OPEN列表里，而且它是當前被擴展節(jié)點的子節(jié)點，則需要對它的g(s)重新計算。ARA*算法的執(zhí)行流程如圖3所示。

圖3 ARA*主流程Fig.3 Mainstream of ARA*algorithm

圖4表示主流程中節(jié)點的更新過程。首先，從OPEN列表中檢索f(s)值最小的節(jié)點s，把s從OPEN列表中移除，再將它存儲到CLOSED列表中，表示s已經(jīng)被擴展過，如果s的子節(jié)點s'中某些節(jié)點的g(s')值比擴展前更低，當這些節(jié)點s'不在CLOSED列表里，則將它們存儲到OPEN列表；否則，將它們存儲到INCONS列表。

圖4 ImprovePath（）流程Fig.4 ImporvePath（）process

在ARA*算法的運行過程中，隨著路徑從起點到終點的推進，會不斷地在OPEN列表中搜索f(s)值最小的節(jié)點作為下一個被擴展的節(jié)點，每向前推進一個節(jié)點，就要從當前被擴展節(jié)點的周圍至多8個子節(jié)點中搜索f(s)最小值，因此，加快搜索f(s)最小值的速度，ARA*路徑規(guī)劃的效率就會提高。

3 改進的ARA*路徑規(guī)劃算法

本文在ARA*算法中引入二叉排序樹存儲代價估計函數(shù)f(s)的計算結果，提高ARA*算法的執(zhí)行效率；通過制定自適應節(jié)點選擇策略，降低機器人與障礙物碰撞風險。

3.1 在ARA*算法中引入二叉排序樹

傳統(tǒng)ARA*算法的OPEN列表采用的是順序存儲結構，如圖5所示。順序存儲結構的優(yōu)點是：邏輯上緊鄰的存儲單元在物理位置上也是緊鄰的，可以節(jié)省內存空間，并且可以實現(xiàn)隨機存取[15]。但是，順序存儲結構的插入或刪除運算不方便，除了結尾位置，在其余任何位置上實現(xiàn)插入或刪除都不得不移動大量的數(shù)據(jù)元素，效率較低。為了加快在OPEN列表中搜索f(s)最小值的速度，需要找到一種數(shù)據(jù)結構，它既可以有較高的插入和刪除效率，并且具備較高的查找效率。

圖5 OPEN列表的改進Fig.5 Improvement of OPEN list

樹是一種應用廣泛的非線性數(shù)據(jù)結構。二叉樹是每個節(jié)點最多有兩個子樹的有序樹，它結合了數(shù)組和鏈表的優(yōu)點，插入、刪除和查找的速度很快[16]。二叉排序樹是二叉樹的一種特殊情況，如圖5所示。

數(shù)據(jù)的最小值存放在整棵樹最后一層的最左端，搜索最小值時，只需要遍歷整棵樹的左子樹，也就是說遍歷全部節(jié)點的一半就能找到最小值。因此，二叉排序樹更適合存儲ARA*算法中的路徑節(jié)點數(shù)據(jù)。

傳統(tǒng)ARA*算法在OPEN列表中查找最小值時，用需要查找的數(shù)據(jù)與線性表中各個數(shù)據(jù)元素逐個比較，直到成功或遍歷結束。假設OPEN列表數(shù)據(jù)長度為n，那么查找第i個數(shù)據(jù)元素時需要進行n-i+1次比較，即Ci=n-i+1。又假設查找任意一個數(shù)據(jù)元素的概率都相同，即Pi=1/n，那么順序查找算法的平均查找長度表示為公式（4）：

假設一棵滿二叉排序樹的總節(jié)點數(shù)是n，高度是h，根節(jié)點的高度是1，n與h的關系滿足n=2h-1。那么對于高度為h，總節(jié)點數(shù)是n的滿二叉排序樹，查找成功時的平均查找長度為公式（5）：

將n=2h-1代入公式（5）計算得到公式（6）：

公式（4）和公式（6）的曲線如圖6所示，順序存儲結構的平均查找長度隨著數(shù)據(jù)元素的個數(shù)呈線性增長，而二叉排序樹的平均查找長度按照對數(shù)函數(shù)的規(guī)律增長，其增長速率遠遠小于順序存儲結構。因此，二叉排序樹更適合存儲ARA*算法的OPEN列表中的數(shù)據(jù)。

圖6 平均查找長度Fig.6 Average search length

3.2 自適應節(jié)點連接方式選擇策略

環(huán)境地圖一般有三種描述方式：柵格地圖、幾何地圖和拓撲地圖[17]。在柵格地圖中，為了將不同的柵格節(jié)點聯(lián)系起來，需要制定節(jié)點間的連接規(guī)則，有兩種典型的連接方式：4連接和8連接，8連接可能使移動機器人恰巧途經(jīng)障礙物柵格的頂點，這在實際情況中很危險，但如果只使用4連接，在沒有障礙的區(qū)域中，過多的折轉會增加路徑的距離。本文提出了一種自適應的節(jié)點間連接方式選擇策略，在地圖中的不同區(qū)域采取更適用于當前局部環(huán)境的連接方式。

在啟發(fā)式函數(shù)中，為了計算當前節(jié)點到目標節(jié)點的距離，首先需要確定節(jié)點間的連接方式。本文提出的自適應節(jié)點間連接方式選擇策略，兼顧了4連接和8連接的優(yōu)點，當路徑經(jīng)過障礙物的相鄰節(jié)點發(fā)生折轉時，為了不穿過障礙物柵格的頂點，采用4連接；當路徑的鄰域內沒有障礙物時切換為8連接，因為沒有障礙物時8連接的路徑長度更短，折轉次數(shù)少，路徑更平滑。

在改進后的ARA*算法中，計算柵格地圖中任意兩個節(jié)點(ai,aj)和(bi,bj)的h(s)值時，忽略節(jié)點之間的障礙物，結合使用兩種距離估計的方法：曼哈頓距離和對角距離。曼哈頓距離的計算方法如公式（7）所示：

對角距離的計算方法如公式（8）所示：

4 仿真實現(xiàn)與結果分析

本章設計了兩組仿真對比實驗，驗證改進ARA*算法的快速性和有效性：

實驗1對比傳統(tǒng)ARA*算法和改進ARA*算法。

實驗2對比蟻群算法、粒子群算法和改進ARA*算法。

為了探究地圖規(guī)模對算法結果的影響程度，對三種規(guī)模的柵格地圖進行仿真：50 m×50 m、100 m×100 m和200 m×200 m。柵格地圖中的柵格節(jié)點有兩種形式：有障礙物柵格和無障礙物柵格，機器人經(jīng)過無障礙物柵格的代價權值為1，柵格地圖中的障礙物柵格隨機分布。實驗平臺參數(shù)：Win10操作系統(tǒng)；Intel i5-8300H CPU；16 GB DDR4，仿真程序用Python3.7和MATLAB2015b實現(xiàn)。

4.1 傳統(tǒng)ARA*算法和改進ARA*算法對比

改進前后的ARA*算法在50 m×50 m、100 m×100 m和200 m×200 m三種規(guī)模地圖上的仿真結果分別如圖7、圖8、圖9所示。

圖7 50 m×50 m仿真結果Fig.7 50 m×50 m simulation results

圖8 100 m×100 m仿真結果Fig.8 100 m×100 m simulation results

圖9 200 m×200 m仿真結果Fig.9 200 m×200 m simulation results

在圖7、圖8、圖9中（a）是改進前4連接ARA*算法的結果，（b）是改進前8連接ARA*算法的結果，（c）是改進后的算法結果。比較（a）、（b）可以看出，傳統(tǒng)的4連接ARA*算法的轉折次數(shù)明顯多于8連接，路徑長度更長，但是4連接的路徑不會從障礙物柵格的頂點穿過，保持了機器人與障礙物之間的距離，因此比8連接更安全。（c）相比（a）和（b）折轉次數(shù)更少，而且在障礙物處折轉時避開了障礙物節(jié)點的頂點，兼顧了4連接和8連接的優(yōu)點，既降低了安全風險，又不會明顯增加路徑長度。改進前后ARA*算法的搜索時間、路徑長度和折轉次數(shù)如表1所示。

表1 ARA*與改進的ARA*仿真結果Table 1 Simulation results of ARA*and improved ARA*algorithm

從表1中可以看出，在保證柵格地圖規(guī)模與障礙物設置完全相同的情況下，改進的ARA*算法搜索時間相比于傳統(tǒng)的ARA*算法有了明顯的縮短；路徑長度介于4連接和8連接之間，但明顯小于4連接的路徑長度；折轉次數(shù)與8連接相近，遠遠小于4連接。

根據(jù)表2，ARA*算法改進前后的數(shù)據(jù)對比結果可知，改進的ARA*算法對地圖規(guī)模不敏感，搜索時間相比改進前平均縮短了43.62%；路徑長度相比4連接平均縮短了18%，與8連接的路徑長度相近；折轉次數(shù)相比4連接平均減少了75.2%，相比8連接平均增加了25.4%，對路徑平滑性影響不大。結合以上分析得出結論，改進的ARA*算法大幅縮短了搜索時間，并且算法規(guī)劃出的路徑更安全可靠。

表2 改進前后結果對比Table 2 Comparison of results before and after improvement

改進ARA*算法用來存放具有局部不一致狀態(tài)節(jié)點的OPEN列表，從傳統(tǒng)的順序存儲結構改為二叉排序樹存儲，加快了路徑節(jié)點更新時從OPEN列表中搜索f(s)最小值的速度。為了證明上述理論的正確性，在50 m×50 m的柵格地圖中，記錄隨著路徑節(jié)點數(shù)量累積單次搜索f(s)最小值的時間，結果如圖10所示。

圖10 單次搜索時間Fig.10 Single search time

對比圖6和圖10，因為平均查找長度與單次搜索時間呈正相關，所以單次搜索時間與平均查找長度變化規(guī)律相似，增長率數(shù)據(jù)如表3所示。

表3 單次搜索時間增長率Table 3 Single search time growth rate

由表3可知，改進后時間增長率明顯小于改進前，搜索OPEN列表中f(s)最小值的速度大幅提高，證實了用二叉排序樹代替順序存儲結構可以提高算法執(zhí)行效率。效率提升的根本原因是，在數(shù)據(jù)個數(shù)相同的情況下，二叉排序樹搜索最值的平均查找長度小于順序存儲結構，比如根據(jù)圖6，有100個數(shù)據(jù)元素時，順序存儲結構需要查詢50個數(shù)據(jù)元素，而二叉排序樹只需要查詢不到10個。

隨著數(shù)據(jù)個數(shù)越來越多，改進前平均查找長度呈線性增長，增長率不變，而改進后的曲線呈對數(shù)規(guī)律增長，增長率越來越小，可見地圖規(guī)模越大，改進ARA*算法執(zhí)行效率高的優(yōu)勢越明顯。

4.2 蟻群、粒子群算法和改進ARA*算法對比

本節(jié)通過仿真實驗，對比改進ARA*算法與蟻群算法、粒子群算法，進一步驗證改進ARA*算法的快速性和有效性。蟻群算法的參數(shù)設置如表4所示，粒子群算法的參數(shù)設置如表5所示，三種路徑規(guī)劃算法的仿真結果如圖11和表6所示。

表4 蟻群算法參數(shù)設置Table 4 Parameter setting of ACA

表5 粒子群算法參數(shù)設置Table 5 Parameter setting of PSO

圖11 50 m×50 m地圖三種算法的路徑Fig.11 Path of 3 algorithms on 50 m×50 m map

圖11和表6的仿真結果反映出，相比仿生智能算法，改進ARA*算法執(zhí)行速度更快。改進算法的路徑長度比粒子群算法多出11%，這是因為改進的ARA*算法為了保證算法的快速性，啟發(fā)函數(shù)中的膨脹因子ε>1，因此沒有計算出路徑的最優(yōu)解，但是多出的路徑長度仍可以接受。對于傳統(tǒng)蟻群算法，它的啟發(fā)信息較弱，在路徑搜索時轉折點較多，有時出現(xiàn)回環(huán)交叉，遠離目標行走，導致路徑長度增加。粒子群算法的路徑最短，折轉次數(shù)僅有1次，路徑最平滑，因為粒子群算法是概率型的全局路徑規(guī)劃算法，在迭代的過程中充滿更多可能性，搜索路徑過程中能覆蓋全局地圖的機會更大，因此更能夠得到全局最優(yōu)解[18]，但是算法運行時間過長，不適合解決實際工程問題。

表6 三種算法的仿真結果Table 6 Simulation results of 3 algorithms

5 結束語

由于傳統(tǒng)的ARA*算法不能滿足移動機器人對實時性和安全性的要求，本文利用二叉排序樹代替順序存儲結構，降低了節(jié)點更新時每次從OPEN列表中搜索最值的時間，優(yōu)化了路徑規(guī)劃時數(shù)據(jù)查找的效率，搜索時間縮短了43%；另外，提出了自適應的連接方式選擇方法，改善了了機器人移動的安全性。實驗結果證明，改進的ARA*算法為移動機器人路徑規(guī)劃問題提供了一種可行的解決方案，一定程度上推動了路徑規(guī)劃算法的發(fā)展。