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

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

關(guān)鍵詞： 路徑規(guī)劃； 移動機器人； 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 引言

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

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

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

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

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

FMT*［10］采用單批多點采樣生成樹。該方法的搜索不依賴于采樣順序，但容易產(chǎn)生冗余擴展導(dǎo)致性能較低。針對此問題，文獻［11］提出了HBFMT*算法對樣本點進行篩選并賦予權(quán)重，減少冗余探索，使其具有更快的收斂速度。文獻［12］提出的DS-FMT*算法為每個擬擴展樣本產(chǎn)生一個有利于擴展的候選方向，并與實際探索方向?qū)Ρ龋瑥亩鴽Q定是否對該樣本優(yōu)先擴展。

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

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

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

2 仿真實驗與分析

2.1 實驗環(huán)境

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

2.2 簡單環(huán)境下采樣及路徑規(guī)劃實驗

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

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

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

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

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

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

2.3 復(fù)雜環(huán)境下路徑規(guī)劃實驗

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

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

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

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

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

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

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

3 結(jié)束語

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

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

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

參考文獻：

［1］Koenig S，Likhachev M，F(xiàn)urcy 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］高民東，張雅妮，朱凌云.應(yīng)用于機器人路徑規(guī)劃的雙向時效A*算法［J］.計算機應(yīng)用研究，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，F(xiàn)erguson D，Gordon G，et al.Anytime search in dynamic graphs［J］.Artificial Intelligence，2008，172（14）：1613-1643.

［5］郭亞軍，魯漢榕.任意時間算法的性能描述［J］.武漢交通科技大學(xué)學(xué)報，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算法的無人機航跡規(guī)劃［J］.傳感器與微系統(tǒng)，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］吳錚，陳彥杰，何炳蔚，等.基于方向選擇的移動機器人路徑規(guī)劃方法［J］.計算機集成制造系統(tǒng)，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］劉亞秋，趙漢琛，劉勛，等.一種基于改進的快速擴展隨機樹的工業(yè)機器人路徑避障規(guī)劃算法［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*算法的機器人路徑規(guī)劃［J/OL］.河南理工大學(xué)學(xué)報：自然科學(xué)版. （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］鄒宇星.基于關(guān)節(jié)空間的采摘機器人機械臂實時避障算法研究［D］.長沙：中南林業(yè)科技大學(xué)，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.