基于改進BIT*算法的機器人路徑規劃研究

摘 要： 針對BIT*存在小樣本下路徑規劃成功率低、冗余大樣本下路徑規劃效率有待提高的問題，提出基于樣本增量生成和概率隨機幾何圖的BIT*-SP算法，設計了生成樣本和選擇樣本的啟發式函數。實驗結果表明，BIT*-SP算法在小樣本下路徑規劃成功率大幅度提高，且能更快找到初始解；在大樣本下能用更短的時間找到一條優秀的路徑，規劃速度顯著提升。該算法魯棒性高，在簡單及復雜環境中都能適用，性能高效。

關鍵詞： 路徑規劃； 移動機器人； BIT*； 采樣算法； 隨機幾何圖

中圖分類號： TP242.6"" 文獻標志碼： A

文章編號： 1001-3695（2022）01-010-0059-05

doi：10.19734/j.issn.1001-3695.2021.06.0214

Research on robot path planning based on improved BIT*

Zhang Boquan， Liu Jiadong

（College of Computer， Guangdong University of Technology， Guangzhou 510006， China）

Abstract： Aiming at the problems that BIT* has low success rate of path planning under small samples and the efficiency of path planning under redundant large samples is not high enough，this paper proposed BIT*-SP algorithm.Also，this paper designed the heuristic functions of generating samples and selecting samples.The experimental results show that BIT*-SP algorithm greatly improves the success rate of path planning under small samples，and can find the initial solution faster.In the case of large samples，it can find an excellent path in a shorter time，and the planning speed is significantly improved.The algorithm that can be applied in both simple and complex environments has high robustness and high performance.

Key words： path planning； mobile robot； BIT*； sampling-based； random geometric graph

0 引言

路徑規劃是移動機器人領域最基礎、最重要的研究課題之一。常用的路徑規劃方法主要包括基于圖的搜索和基于采樣的近似搜索兩類。

以Dijkstra、A*等為代表的圖搜索算法理論基礎可靠，在同一分辨率下總能得到最優解，但其也存在相關缺點：a）先驗分辨率難以選擇，當分辨率過低時，路徑質量往往不佳，當分辨率過高時，計算成本以指數級增長，特別是高維空間問題的處理需要更長的時間；b）實時性差，本類規劃需要基于具有先驗信息的地圖進行，當環境發生變化時，往往需要重新建圖和規劃，效率較低。針對上述問題，已進行相關研究。例如，LPA*重用已有的路徑信息進行搜索，能夠快速規劃出一條新路徑，環境適應能力強［1］。目前A*算法仍是主流的圖搜索算法之一，文獻［2］對球形機器人的運動學進行分析，提出了一種改進的混合A*算法，能生成符合運動學約束的可行路徑。文獻［3］提出了雙時效A*算法，它在起點和終點同時使用時效A*進行路徑規劃，降低了規劃時間與成本，規劃路徑也更加平滑。

RRT（rapidly-exploring random tree）和PRM（probabilistic roadmap）等采樣近似搜索算法采用任意時間（anytime）［4，5］思想進行漸進尋優，以快速找到一個可行解，然后進行改進。該思想也常用于ATD*［6］等的圖搜索。采樣搜索不需要完全近似空間，并且可將樣本的約束判斷延后甚至規避，因而計算效率高、實時性強。但路徑質量往往依賴于采樣數，樣本數量過低會引發路徑成本高甚至路徑規劃失敗。同時，隨機采樣的無序性往往導致路徑之初偏離目標，花費大量時間用于探索空間。

采樣近似搜索主要通過兩種方法生成路徑：a）以PRM為代表的連接圖法，圖中的每個樣本點均與周圍樣本點建立連接以構成顯式的隨機幾何圖（random geometric graph，RRG）［7，8］，該算法實現簡單，目前主要被應用于三維空間的路徑規劃，文獻［9］改進了PRM，它將邊界點作為確定樣本點建立最優可行區域，使無人機航跡的規劃時間以及路徑成本都有所下降；b）以RRT系列為代表的生成樹法，樹中的每個樣本點可以連接周圍的多個樣本點作為子節點，但只能連接一個樣本點作為父節點。

根據樣本處理流程，也有不同算法用于生成路徑樹，可分為多批單點采樣、單批多點采樣以及多批多點采樣。

FMT*［10］采用單批多點采樣生成樹。該方法的搜索不依賴于采樣順序，但容易產生冗余擴展導致性能較低。針對此問題，文獻［11］提出了HBFMT*算法對樣本點進行篩選并賦予權重，減少冗余探索，使其具有更快的收斂速度。文獻［12］提出的DS-FMT*算法為每個擬擴展樣本產生一個有利于擴展的候選方向，并與實際探索方向對比，從而決定是否對該樣本優先擴展。

RRT則采用多批單點采樣，但每次只采取并且處理一個樣本點。RRT*［13］通過重新布線使得生成樹路徑總能收斂于最優。RRT-connect［14］則通過分別維護從起點和目標點生成的兩棵雙向樹，快速找到一條可行路徑，但這條路徑成本往往很高，一般不是最優解。Informed RRT*［15］在找到初始路徑后，通過知情采樣［16］限制采樣范圍，使得路徑更快收斂到最優解。文獻［17］提出了擴展策略和自適應步長策略，結合Dijkstra 算法和避免回歸機制，改進了RRT，有效地避免了陷入局部極小值，提高了收斂速度。文獻［18］基于貪心算法和改變搜索對象，改進了Informed RRT*，有效地縮短了規劃時間，同時減少了路徑成本。

BIT*［19，20］綜合FMT*和RRT的生成路徑樹流程，進行多批次多樣本的采樣。它采用知情采樣，通過構建漸趨密集的隱式RRG進行增量搜索，生成漸進最優的顯式樹。同時，考慮當前成本和未來成本，構建啟發式函數評價該樣本點，延遲甚至規避一些樣本點的約束性判斷。基于上述處理，BIT*能以更短的時間收斂至最優解，總體上優于現有的采樣漸進最優算法［19］。文獻［21］提出了ABIT*算法，采用高級圖搜索技術，利用膨脹和截斷因子，能更快地找到一條初始路徑并用剩余時間進行改進。文獻［22］提出了實時避障的BIT*，并應用于采摘機器人上，有效實現了實時避障功能。

研究發現，BIT*仍存在小樣本下路徑規劃成功率偏低、在大樣本下規劃效率仍待提高的問題。因此，針對上述問題，本文提出以下兩點改進：a）樣本增量生成（sample incremental generation，SIG）采樣策略的改進；b）概率RGG（probabilistic random geometric graph，PRGG）構圖策略的改進。

2 仿真實驗與分析

2.1 實驗環境

為了驗證基于SIG和PRGG的BIT*-SP算法的性能，根據機器人路徑規劃實踐，本文抽象了兩類實驗環境，抽象的實驗環境如圖2所示。其中圖2（a）屬于簡單環境，但有一條狹長的過道。在這種環境中，采樣算法在小樣本下成功率普遍偏低；圖2（b）屬于復雜環境，是復雜真實環境高精細度的建模。其不僅過道狹長，而且具有許多障礙物。

2.2 簡單環境下采樣及路徑規劃實驗

圖3是在簡單環境下，SIG采樣法和隨機采樣法成功率對比圖。每組實驗均進行了1 000次測試。結果顯示，SIG采樣法達到可靠的成功率所需的樣本量大幅減少。在樣本量m=200處甚至能達到99%，比隨機采樣高出了87%。這主要得益于SIG采樣法使樣本點分布得更加均勻，便于獲取更多的全局信息。

圖4是FMT*、BIT*和BIT*-SP在簡單環境下的初始解比較圖。由于RRT系列為單點采樣，搜索并不獨立于采樣順序，增加樣本數僅會找到初始解或者改善最終解，并不會影響初始解的質量，故被排除在本次實驗對象之外。在樣本量m=100時，三種算法的路徑規劃結果都不理想。其余各組實驗均取1 000次成功規劃后的結果平均值。實驗結果表明，FMT*比其他兩種算法平均多耗費了3.39倍的規劃時間，這主要是因為它進行了更多的冗余擴展來探索空間。但也得益于此，它在低樣本下路徑成本要比其他兩種算法低3.22%～5.96%，隨著樣本數的增加，它的收斂速度明顯不及其他兩者，這主要是因為在選擇擴展點時，FMT*的啟發式函數并未考慮到未來成本，導致選擇的擴展點不一定最優。與BIT*相比，BIT*-SP的計算時間更快，時間成本平均減少了17.65%，且隨著樣本數增大，優勢更加明顯，這主要是因為概率RGG過濾了許多對規劃路徑幫助不大的樣本點。同時，在低樣本下初始解的路徑成本也更低，平均少了3.64%，這是由于在低樣本下，樣本均勻分布有助于平滑路徑。

表1展示了目前主流的幾種算法在簡單環境下的最終解測試結果，選取具有代表性的樣本數目闡述。圖5是相關算法在500（1 000）樣本點下的規劃仿真圖。由于RRT-connect目的是快速找到一個可行解，該算法不隨樣本量的增加而改善已有解，故本次實驗并未進行對比。BIT*系列的增量采樣數取初始樣本的一半，每組實驗進行500輪迭代，其他算法取BIT*系列總處理樣本數的均值（已假設不重復取樣），在表中括號內表示。各算法盡量保證參數（步長、搜索范圍等）在相同的情況下進行多次實驗，并取1 000次成功規劃的數據的均值。

實驗表明，BIT*系列性能總體優于其他算法。這是因為BIT*系列使用了強指導性的啟發式函數批處理樣本點，不僅考慮了當前成本，也考慮了未來成本，使其能快速往目標點逼近，找到初始解。同時使用了Informed采樣法限制采樣空間，使其收斂速度大幅度上升。其中，BIT*-SP收斂至最終解的速度始終比BIT*快，平均快了27.27%。實驗結果也表明，在同等樣本量下，BIT*-SP難以收斂到與BIT*同等精度的路徑成本，但實際差距并不大，最多只有 1.16%，且隨著樣本量增大，差距越來越小。這主要由于BIT*-SP概率性過濾了擬擴展點中距離過近的樣本點，這些點往往對找到或改善路徑的幫助較小，但忽略這些點使得路徑難以得到更精細的改善，故而導致路徑成本收斂較慢。

在固定樣本量下，FMT*與RRT系列的對比充分展示了批處理樣本的優勢，FMT*的樣本處理速度是RRT*的2.23～7.57倍，這是因為FMT*的搜索獨立于采樣順序，一次迭代能進行多個樣本的擴展，不用煩瑣地切換流程，提升了計算效率。路徑成本差距在1%以下，可以忽略不計。

Informed RRT*與 RRT*的對比則充分展示了知情采樣加速路徑收斂的作用。它們的計算時間差別不大，但Informed RRT*的路徑成本收斂速度遠快于RRT*。知情采樣將當前最優成本作為長軸，起始點與目標點的直線距離作為短軸，構建了一個橢圓空間限制采樣范圍，并通過不斷改善當前最優成本縮小橢圓范圍，極大程度地加速了路徑收斂過程。

2.3 復雜環境下路徑規劃實驗

在復雜環境下的實驗對比了目前主流的一些采樣近似搜索算法，本次實驗將其他因素的影響都抽象成計算時間與成功率以及計算時間與路徑成本的對比。其他配置與上一小節相同。實驗仿真結果如圖6所示。

圖6顯示，相比于BIT*，BIT*-SP擴展的邊數量更少，長度更長。兩者又比Informed RRT*、RRT*以及FMT*少了許多探索空間的冗余擴展。而RRT-connect通過雙向生長樹指導探索，擴展的節點數目最少，但大部分情況下路徑搜索成本會比其他算法高。

圖7（a）（b）是具體的實驗結果比較，與文獻［19，20］的實驗結果基本一致。圖7（a）展示了各算法在某時間段內成功規劃的概率，圖7（b）展示了經過50 s的計算迭代，各算法的路徑成本比較。

RRT-connect仍然是眾多采樣算法中最快找到解的算法，有70.8%的概率能在1 s內找到可行解，并保證能在3 s內找到解，然而其路徑成本相比其他算法高許多，而且并非漸進最優。該算法的目的在于快速找到一條可行路徑，利用雙向樹探索使其能在規劃期間采樣和擴展非常少的節點，故計算時間非常快，但對空間的不充分探索往往導致過高的路徑成本。

BIT*-SP有8.6%概率能在2 s內找到初始解并保證在5 s內必定找到初始解，且所得的初始路徑成本最低，但收斂至最優路徑的速度較慢，優勢與劣勢的原因已在2.2節中具體闡述，這里不再贅述。BIT*僅次于BIT*-SP，它幾乎不能在2 s內找到初始解，但保證能在6 s內找到解。它的初始解成本比BIT*-SP高但經過一段時間的迭代，能更快地收斂至理論上的最優解。

其次是Informed RRT*和RRT*，保證能在8 s內找到初始解。它們幾乎同時找到初始解，這是因為它們在找到初始解前的流程是相同的。Informed RRT*經過迭代，路徑成本略優于BIT*，但RRT*卻收斂得非常慢，可見知情采樣法對路徑的收斂有非常明顯的效果。

值得一提的是，FMT*在復雜環境下，不管在時間成本還是路徑成本上，表現都遜色于其他算法，性能反而不如RRT系列。主要是因為該算法在復雜環境下需要大量樣本點保證成功率，處理大量樣本意味著需要進行大量的障礙碰撞檢測，這造成了巨大的運算量。BIT*系列改進了該缺點，通過啟發式函數篩選，只對有希望找到或改善路徑的節點進行障礙碰撞檢測。

3 結束語

文獻［19］的實驗結果表明，BIT*算法幾乎優于現有的所有采樣漸進算法，而BIT*-SP作為BIT*的改進，在簡單環境下成功規劃所需樣本量減少了70%，且時間成本平均少了27.27%。復雜環境下，在某個時間段內找到初始解的概率平均高了9.9%，且時間成本平均減少了18.2%，

本文下一步將針對以下方向進行研究：a）文獻［19，22］分別實現了BIT*在高維空間的規劃以及實時避障規劃，理論上BIT*-SP也應該完全適用于高維空間以及實時規劃，但仍缺少重要的實驗數據論證，這將是日后的工作；b）尋找BIT*-SP的更多應用環境也是今后的重要工作之一。

最后需要強調的是，針對其特點，應該靈活地使用BIT*-SP，比如在高維空間且大樣本的情況下，隨機采樣本身具有足夠高的成功率使SIG采樣法效果甚微，且會產生大量的計算成本，反而拖累算法，應該停用。再比如為了更快地收斂至理論上的最優路徑，應該在迭代至穩定解后停止使用PRGG，以求更精細的樣本點擴展。盡管在實際應用中，快速找到一條成本較低的路徑更具有實際意義。

參考文獻：

［1］Koenig S，Likhachev M，Furcy D.Lifelong planning A*［J］.Artificial Intelligence，2004，155（1-2）：93-146.

［2］Zhang Ziang，Wan Yixu，Wang You，et al.Improved hybrid A* path planning method for spherical mobile robot based on pendulum［J］.International Journal of Advanced Robotic Systems，2021，18（1）：1-14.

［3］高民東，張雅妮，朱凌云.應用于機器人路徑規劃的雙向時效A*算法［J］.計算機應用研究，2019，36（3）：159-162，167. （Gao Mindong，Zhang Yani，Zhu Lingyun.Bidirectional time-efficient A* algorithm for robot path planning［J］.Application Research of Computers，2019，36（3）：159-162，167.）

［4］Likhachev M，Ferguson D，Gordon G，et al.Anytime search in dynamic graphs［J］.Artificial Intelligence，2008，172（14）：1613-1643.

［5］郭亞軍，魯漢榕.任意時間算法的性能描述［J］.武漢交通科技大學學報，2000，24（5）：562-565. （Guo Yajun，Lu Hanrong.Perfor-mance profiles of anytime algorithms［J］.Journal of Wuhan University of Technology，2000，24（5）：562-565.）

［6］Aine S，Likhachev M.Anytime truncated D*：anytime replanning with truncation［C］//Proc of the 6th Annual Symposium on Combinatorial Search.2013：2-10.

［7］Penrose M.Random geometric graphs［M］.Oxford：Oxford University Press，2003.

［8］Dall J，Christensen M.Random geometric graphs［J］.Physical Review E，2002，66（1）：016121.

［9］譚建豪，肖友倫，劉力銘，等.改進PRM算法的無人機航跡規劃［J］.傳感器與微系統，2020，39（1）：44-47. （Tan Jianhao，Xiao Youlun，Liu Liming，et al.Improved PRM algorithm for path planning of UAV［J］.Transducer and Microsystem Technologies，2020，39（1）：44-47.）

［10］Janson L，Schmerling E，Clark A.Fast marching tree：a fast marching sampling-based method for optimal motion planning in many dimensions［J］.The International Journal of Robotics Research，2015，34（7）：883-921.

［11］Gao Wenxiang，Tang Qing，Yao Jin，et al.Heuristic bidirectional fast marching tree for optimal motion planning［C］//Proc of the 3rd Asia-Pacific Conference on Intelligent Robot Systems.Piscataway，NJ：IEEE Press，2018：71-77.

［12］吳錚，陳彥杰，何炳蔚，等.基于方向選擇的移動機器人路徑規劃方法［J］.計算機集成制造系統，2021，27（3）：672-682. （Wu Zheng，Chen Yanjie，He Bingwei，et al.Direction selection-based algorithm for mobile robot path planning［J］.Computer Integrated Manufacturing Systems，2021，27（3）：672-682.）

［13］Karaman S，Walter M R，Perez A，et al.Anytime motion planning using the RRT*［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2011：1478-1483.

［14］Kuffner J J，Lavalle S M.RRT-connect：an efficient approach to single-query path planning［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2000：995-1001.

［15］Gammell J D，Srinivasa S S，Barfoot T D.Informed RRT*：optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic［C］//Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway，NJ：IEEE Press，2014：2997-3004.

［16］Gammell J D，Barfoot T D，Srinivasa S S.Informed sampling for asymptotically optimal path planning［J］.IEEE Trans on Robotics，2018，34（4）：966-984.

［17］劉亞秋，趙漢琛，劉勛，等.一種基于改進的快速擴展隨機樹的工業機器人路徑避障規劃算法［J］.信息與控制，2021，50（2）：235-246，256. （Liu Yaqiu，Zhao Hanchen，Liu Xun，et al.An improved RRT based obstacle avoidance path planning algorithm for industrial robot［J］.Information and Control，2021，50（2）：235-246，256.）

［18］代軍，李志明，李艷琴，等.基于改進informed RRT*算法的機器人路徑規劃［J/OL］.河南理工大學學報：自然科學版. （2021-06-08）［2021-07-23］.http：//kns.cnki.net/kcms/detail/41.1384.n.20210608.1446.004.html. （Dai Jun，Li Zhiming，Li Yanqin，et al.Roobot path planning based on improved Informed RRT* algorithm［J/OL］.Journal of Henan Polytechnic University：Natural Science.（2021-06-08）［2021-07-23］.http：//kns.cnki.net/kcms/detail/41.1384.n.20210608.1446.004.html.）

［19］Gammell J D，Srinivasa S S，Barfoot T D.Batch informed trees（BIT*）：sampling-based optimal planning via the heuristically guided search of implicit random geometric graphs［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2015：3067-3074.

［20］Gammell J D，Barfoot T D，Srinivasa S S.Batch informed trees（BIT*）：informed asymptotically optimal anytime search［J］.The International Journal of Robotics Research，2017，39（5）：543-567.

［21］Strub M P，Gammell J D.Advanced BIT*（ABIT*）：sampling-based planning with advanced graph-search techniques［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2020：130-136.

［22］鄒宇星.基于關節空間的采摘機器人機械臂實時避障算法研究［D］.長沙：中南林業科技大學，2018. （Zou Yuxing.Research on real-time obstacle avoidance algorithm of harvesting robot manipulator based on joint space［D］.Changsha：Central South University of Forestry and Technology，2018.）

［23］Akbaripour H，Masehian E.Semi-lazy probabilistic roadmap：a para-meter-tuned，resilient and robust path planning method for manipulator robots［J］.The International Journal of Advanced Manufacturing Technology，2017，89：1401-1430.

［24］Choudhury S，Gammell J D，Barfoot T D，et al.Regionally accelerated batch informed trees（RABIT*）：a framework to integrate local information into optimal path planning［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2016：4207-4214.

［25］Holston A C，Kim D H，Kim J H.Fast-BIT*：modified heuristic for sampling-based optimal planning with a faster first solution and convergence in implicit random geometric graphs［C］//Proc of IEEE International Conference on Robotics and Biomimetics.Piscataway，NJ：IEEE Press，2017：1892-1899.