李華忠，但唐仁，唐強平

（深圳信息職業(yè)技術(shù)學(xué)院軟件工程系,廣東 深圳 518172 ）

自適應(yīng)雙向快速密集樹避碰運動規(guī)劃算法

李華忠，但唐仁，唐強平

（深圳信息職業(yè)技術(shù)學(xué)院軟件工程系,廣東 深圳 518172 ）

針對采樣運動規(guī)劃算法效率低，尤其在處理高維空間和復(fù)雜障礙環(huán)境等問題時，嚴重依賴于所選采樣參數(shù)和碰撞檢測距離等，提出了一種自適應(yīng)雙向快速密集樹(ABiRDT)避碰運動規(guī)劃方法。首先，深入研究了ABiRDT 算法的基礎(chǔ)理論和實現(xiàn)方法，可適應(yīng)調(diào)整碰撞檢測距離參數(shù)和隨機采樣擴展步長；其次，重點研究了本算法所采用的C-空間加權(quán)均勻采樣、最近鄰位形查找和基于混合包圍盒的并行離散碰撞檢測等關(guān)鍵自適應(yīng)策略；最后，通過三維可視化計算機仿真驗證了本文提出算法的有效性。

運動規(guī)劃；快速密集樹；自適應(yīng)算法；基于采樣技術(shù)；離散碰撞檢測；位形空間

自Lozano-Perez提出位形空間(Configuration Space，C-空間)概念，從而可將各類運動規(guī)劃問題轉(zhuǎn)化成在C-空間中尋找點的無碰路徑問題以來,基于采樣的規(guī)劃技術(shù)已被廣泛應(yīng)用于高自由度智能機器人避碰和操控等領(lǐng)域[1-4]，成為國內(nèi)外普遍關(guān)注的研究熱點。該方法的顯著優(yōu)點是基于隨機采樣只需近似構(gòu)造自由位形空間 ，從而避免了采用骨架網(wǎng)格和柵格分解[5]等方法需完整和精確構(gòu)造全部有效C-空間模型，以尋找精確解析解所帶來的PSPACE難題，規(guī)避了高自由度所引起的存儲空間和計算量呈指數(shù)爆炸增長的NP問題[6]。最典型的兩類基于采樣的運動規(guī)劃方法為：快速隨機搜索樹法（Rapidly exploring Random Tree：RRT）[7-8]和概率地圖法（Probabilistic Roadmap：PRM）[9]。它們均利用采樣技術(shù)在C-空間中構(gòu)造樹或圖，然后迭代細化，直到找到連接初始位形和目標(biāo)位形的無碰撞路徑解。該算法的效率，尤其在處理高維空間和復(fù)雜障礙環(huán)境等復(fù)雜問題時，嚴重依賴于所選的采樣參數(shù)和離散碰撞檢測距離參數(shù)。為了解決這些問題，本文提出一種自適應(yīng)雙向快速密集樹搜索算法，從而可通過自適應(yīng)調(diào)節(jié)碰撞檢測參數(shù)和擴展步長以尋找避碰運動規(guī)劃的精確解。

1 相關(guān)研究

T.Sartini等學(xué)者提出了一種分辨率自適應(yīng)RRT算法[10]。該算法利用分辨率自適應(yīng)策略增加概率運動規(guī)劃效率，進而提出自適應(yīng)選取擴展步長大小的方法，從而提高了RRT規(guī)劃器處理狹窄通道問題的效率。但該算法必須依賴于靠人工精心設(shè)置的初始和最后搜索步長等參數(shù)值。R.Bohlin和L.Kavraki提出了延遲PRM算法[11]。該算法最初先假設(shè)C-空間是完全避碰的，以便在預(yù)處理階段可以快速地構(gòu)建地圖，將邊的有效性驗證轉(zhuǎn)移到查詢階段，從而延遲對 表示的精化處理。該算法屬于單次查詢PRM法，通過減少采樣階段碰撞檢測預(yù)處理過程，加快規(guī)劃速度，其性能依賴于初始節(jié)點數(shù)的大小。L.Tapia等學(xué)者提出了構(gòu)造PRM的智能自適應(yīng)采樣方法[12]。該方法避免了在基于機器學(xué)習(xí)的混合PRM[13]和特征敏感的運動規(guī)劃框架[14]等方法中經(jīng)常所需的手動參數(shù)調(diào)整。它演示了如何在規(guī)劃過程中結(jié)合拓撲和采樣自適應(yīng)技術(shù)，以便學(xué)習(xí)構(gòu)建地圖的最佳采樣策略。然而，該方法需要學(xué)習(xí)采樣策略的訓(xùn)練集，因太復(fù)雜，必須人工設(shè)置基本路線圖的幾個參數(shù)。S.R.Lindemann和S.M.LaValle綜述了應(yīng)用于運動規(guī)劃算法的啟發(fā)式參數(shù)調(diào)節(jié)技術(shù)。I.Sucan 和L.Kavraki提出了基于采樣的單查詢運動規(guī)劃實現(xiàn)方法?，F(xiàn)有研究表明基于隨機采樣的運動規(guī)劃方法，盡管在概率完備性、路徑解的近似優(yōu)化性、采樣策略、查詢方式和計算復(fù)雜度等方面具有顯著優(yōu)越特性，但仍存在可重復(fù)性差、參數(shù)自適應(yīng)調(diào)整和算法局部性等許多有待解決的問題。例如，參數(shù)自適應(yīng)調(diào)整問題，包括概率閾值的確定、總采樣點數(shù)目的估計、隨機步長的選擇、離散碰撞檢測距離參數(shù)等等，將可能影響規(guī)劃的收斂速度、優(yōu)化程度,甚至決定規(guī)劃的成功與否[15-16]。

2 ABiRDT算法的基礎(chǔ)理論

本文提出的自適應(yīng)雙向快速密集樹算法ABiRDT (qs,qg)的流程圖如圖1所示。CBiRDT對象通過生長兩棵快速密集樹，一個從初始位形qs開始，另一個從目標(biāo)位形qg開始，遞增地構(gòu)造每棵搜索樹，自適應(yīng)地逐步提高分辨率。

圖1 ABiRDT算法流程圖Fig.1 Flow chart of ABiRDT algorithm

其中，qs和qg分別表示機器人的初始位形和目標(biāo)位形。CheckPathValid(Path )調(diào)用碰撞檢測算法來驗證路徑的有效性，若發(fā)生碰撞會自適應(yīng)調(diào)用SetColPara()方法將碰撞檢測參數(shù)rcol減半，以便于成功通過狹窄通道或障礙物密集空間。該算法的核心在于自適應(yīng)雙向快速密集樹動態(tài)擴展方法ABiRDTDExtend(qs,qg)的設(shè)計與實現(xiàn)，其算法流程如圖2所示。在ABiRDTDExtend(qs,qg)算法的搜索循環(huán)中主要采用RDTExtend (T,qrand，qnew)和RDTConnect (T,q)方法擴展搜索樹，通過局部自適應(yīng)調(diào)整存儲在搜索樹每個節(jié)點的擴展參數(shù)rext，能夠處理狹窄通道運動規(guī)劃問題。rext初始值為|cg-cs|，若擴展失敗，則該值減半；若擴展成功，則該值加倍，從而對環(huán)境中的障礙物做出條件反射，自適應(yīng)調(diào)整擴大或減小擴展步長，在障礙物較少的空間加快擴展速度，在狹窄通道或障礙物較多的地方，采用較小擴展步長，確保不與障礙物發(fā)生碰撞。

圖2 ABiRDTDExtend 算法流程圖Fig.2 Flow chart of ABiRDTDExtend algorithm

其中，快速密集樹擴展RDTExtend方法的基本思路是：首先，確定在邊e上與隨機采樣位形qrand的最近鄰位形qNN，然后通過考慮邊e的局部擴展步長rext，將擴展生成的新位形qnew，限制在rext以內(nèi)（如圖3所示）。

圖3 RDTExtend擴展方法示意圖Fig.3 Schematic diagram of RDTExtend expansion method

圖中藍色qrand情況表示，當(dāng)隨機采樣位形qrand位于擴展步長rext以內(nèi)，則qnew=qrand；綠色qrand情況表示，當(dāng)隨機采樣位形qrand位于擴展步長rext以外，則需要裁減為：。

圖4 RDTExtend算法流程圖Fig.4 flow chart of RDTExtend algorithm

圖5 RDTConnect算法流程圖Fig.5 Flow chart of RDTConnect algorithm

bool RDTExtend(T,qrand，qnew)算法流程圖如圖4所示。其中的快速密集樹連接bool RDTConnect (T,q) 方法的流程圖如圖5所示。

圖6 RDT Connect方法rext減半示意圖Fig.6 Diagram of RDT Connect method rext to halve

該快速密集樹連接方法RDTConnect (T,q)主要用于構(gòu)建搜索樹的新邊。如果在位形之間的直線段路徑是避碰的，則新邊被添加到RDT中，且與邊e對應(yīng)的擴展步長rext加倍。否則，在從到q路徑上最大有效位形的邊將被添加到樹中，且將擴展步長rext減半。該方法的擴展步長減半的示意圖如圖6所示。

3 關(guān)鍵自適應(yīng)策略

在ABiRDT算法中采用的關(guān)鍵自適應(yīng)策略包括：C-空間加權(quán)均勻采樣、最近鄰位形查找和離散碰撞檢測等。

3.1 C-空間加權(quán)均勻采樣

RandConfig()函數(shù)主要處理C-空間隨機采樣，本文采用加權(quán)采樣方法，生成均勻采樣，以消除C-空間不同維度在工作空間對機器人影響的差異性。為了在C-空間路徑兩個位形q1和q2之間進行加權(quán)均勻采樣，其單位步長矢量計算公式如(1)式：

在(2)式中，權(quán)重系數(shù)wi可通過求解從機器人第i個關(guān)節(jié)開始的所有運動鏈K的最大距離。對運動鏈K，屬于K所有關(guān)節(jié)k∈K 在工作空間的最大擴展是通過查找在相應(yīng)表面Sk上兩點qci和qcj之間最大距離和獲得，其計算公式如(3)式：

因此，在兩個位形q1和q2之間的路徑上可均勻產(chǎn)生個中間采樣位形qt，其計算公式如（4）式：

若將wi設(shè)置為機器人工作空間位移的最大上限，則簡化計算，從而避免依賴人工設(shè)置歸一化C-空間維度參數(shù)，提高C-空間采樣的自適應(yīng)性。

3.2 最近鄰位形查找

自適應(yīng)雙向快速密集樹（ABiRDT）規(guī)劃器在自由C-空間Cfree中采用直線路徑段構(gòu)造搜索樹，從而避免了采用基于RRT的運動規(guī)劃所依賴的采樣參數(shù)，其與基于RRT的方法相比較，不需要將中間采樣添加到樹結(jié)構(gòu)中，且可能存在長邊，從而使得本算法所構(gòu)造的樹比RRT的樹要稀疏很多。通常查找最近鄰位形NNeighborOnEdge的計算時間與頂點樹呈線性關(guān)系。頂點數(shù)量的增加可提供運動規(guī)劃求解質(zhì)量，但也會增加計算時間。本文為補償這種損失將頂點插入一個為搜索最近鄰位形而建立的Kd-樹數(shù)據(jù)結(jié)構(gòu)中。P個位形點的Kd-樹的構(gòu)造過程如下。初始時，將位形點對x軸進行排序，則中值點將p∈P 分成兩個集合，這取決于位形點處在通過點p的垂線的哪一側(cè)；對其中的每一側(cè)再根據(jù)y軸進行分類，選取中值點，通過該中值的水平線將位形點再次分割為上下兩部分；在下一輪遞歸中，再次使用垂線、水平線，如此重復(fù)。通過循環(huán)進行n維坐標(biāo)分割，相同的思路應(yīng)用于Rn,將Kd-樹擴展到運動規(guī)劃的拓撲空間，構(gòu)造P點Kd-樹所需時間為O(nplg p)。設(shè)KDTree root為Kd-樹對象，在Kd-樹中尋找最近鄰位形的Node NNeighborOnEdge ()算法流程如圖7示。

圖7 查找最近鄰位形算法流程圖Fig.7 Flow chart of NNeighborOnEdge algorithm

3.3 離散碰撞檢測

由于連續(xù)碰撞檢測(Continuous Collision Detection,CCD)算法難以滿足運動規(guī)劃的實時性要求，本文實現(xiàn)的碰撞檢測算法isCollision()主要基于采樣的離散碰撞檢測(Discrete Collision Detection,DCD)思想[17-18]。DCD算法存在的兩大主要缺陷：（一）存在遺漏路徑碰撞的情況，特別是對狹窄通道和障礙物密集的環(huán)境，若采樣步長太大，則DCD算法無法正確地檢測出路徑中存在的碰撞；（二）存在刺穿現(xiàn)象，當(dāng)DCD步長過大時，機器人和障礙物之間已經(jīng)發(fā)生了一定深度的刺穿才被檢測到路徑已發(fā)生碰撞。為了克服DCD存在的問題，ABiRDT中通過自適應(yīng)調(diào)整碰撞檢測參數(shù)rcol和擴展步長rext加以避免。針對運動規(guī)劃對碰撞檢測實時性和精確性的要求，本文采用基于混合包圍盒的并行碰撞檢測算法，利用方向包圍盒(OBB)的緊密性和球狀包圍盒(Sphere)計算簡單的優(yōu)點來構(gòu)造機器人和環(huán)境的混合包圍體層次樹結(jié)構(gòu)，快速排除不相交的對象，并利用OpenMP模型并行遍歷混合包圍體層次樹，從而加快碰撞檢測速度。該算法的流程圖如圖8所示。本文研究的混合包圍盒之間的相交檢測主要包括：兩球狀包圍盒間，兩OBB間以及球狀包圍盒和OBB的相交測試。

圖8 基于混合包圍盒的并行碰撞檢測算法流程圖Fig.8 flow chart of collision detection algorithm based on Parallel Hybrid bounding box

3.3.1 兩球狀包圍盒間的相交檢測

只需要比較兩球狀包圍盒球心之間的距離是否大于兩球半徑之和，其公式如(5)所示(采用平方形式)。

其中，o1和o2、1r和r2分別為兩球球心和半徑。若(5)式成立，則它們之間不相交，否則相交。

3.3.2兩球狀包圍盒間的相交檢測

如圖9所示，假設(shè)兩個OBB包圍盒A和B，其邊長的一半分別為ai和bi，單位軸向量分別為Ai和Bi(i=1,2,3)。A和B兩中心間的距離為TAB，LSB是平行于分離軸的單位向量。rA和rB分別為A和B的三個半徑在LSB方向上投影之合。只有比較|TAB·LSB|與(rA+rB)之和就能判斷兩OBB間是否相交，即是：

其中，LSB為15條分離軸中的一條。若前者大于后者，則兩個OBB不相交；否則繼續(xù)計算A和B在其他14個分離軸上的投影，并利用(6)式進行比較，直到15個分離軸的投影均不滿足(6)式，則可判定A和B相交[8]。

3.3.3 兩球狀包圍盒間的相交檢測

設(shè)OBB包圍盒為A，球狀包圍盒為B，可以將球狀包圍盒和OBB的相交檢測看成兩個OBB間相交檢測的特例。用球狀包圍盒B的半徑r代替bi，可得類似的判定公式如(7)式：

其中，LSB為OBB包圍盒三個分離軸中的一條。若前者大于后者，則球狀包圍盒和OBB不相交；否則繼續(xù)計算A和B在其他兩個分離軸上的投影，并利用（7）式進行比較，直到三個分離軸的投影均不滿足（7）式，則可判定A和B相交。

圖9 兩OBB間的相交檢測Fig.9 two intersection judgment between two OBBs

4 可視化仿真

為了驗證本文提出算法的有效性和正確性，作者進行了大量的仿真實驗，所有仿真實驗均在一臺配置Intel奔騰雙核G640處理器（主頻為2.8GHz）、4GB內(nèi)存，帶3MB三級緩存的計算機上進行。編譯工具為VS2008 C++。本文以Kuka機器人在復(fù)雜環(huán)境中避碰運動規(guī)劃為例，對本文提出的算法進行了三維可視化仿真驗證研究。首先，用三維造型繪制Kuka機器人、環(huán)境和障礙物等三維模型，并轉(zhuǎn)存為Open Inventor支持的iv格式；然后，通過Visual C++導(dǎo)入數(shù)據(jù)模型；最后，利用本文提出的算法實現(xiàn)自適應(yīng)雙向快速密集樹（ABiRDT）避碰運動規(guī)劃。圖10為運動規(guī)劃三維可視化場景。本運動規(guī)劃算法的平均執(zhí)行時間為62.4秒，平均碰撞檢測時間為56.4秒，平均DCD采樣大小為8.4。

圖10 基于 Open Inventor的機器人運動規(guī)劃可視化場景Fig.10 Thr diagram of baseing on Open Inventor

5 結(jié)論

本文提出了一種自適應(yīng)雙向快速密集樹避碰運動規(guī)劃算法，利用C-空間加權(quán)均勻采樣、最近鄰位形查找和離散碰撞檢測等策略，不必人工設(shè)置碰撞檢測參數(shù)和隨機采樣擴展步長，而是根據(jù)碰撞檢測情況，自適應(yīng)調(diào)整相關(guān)參數(shù)，從而更有效地尋找到避碰運動路徑，最后通過可視化仿真驗證了該算法的有效性。

（References）

[1]S.Chakravorty and S.Kumar.Generalized Sampling-Based Motion Planners[J].IEEE TRANSACTIONS ON SYSTEMS,MAN,AND CYBERNETICS—PART B:CYBERNETICS,2011,41(3):855-865.

[2]I.Sucan and L.Kavraki.On the implementation of singlequery sampling-based motion planners[C].//2010 IEEE International Conference on Robotics and Automation (ICRA),may.2010:2005-2011.

[3]N.Vahrenkamp,M.Do,T.Asfour and et al.Integrated Grasp and Motion Planning[C].2010 IEEE International Conference on Robotics and Automation Anchorage Convention District May 3-8,2010,Anchorage,Alaska,USA:2883-2888.

[4]肖國寶,嚴宣輝.一種動態(tài)不確定環(huán)境中機器人路徑規(guī)劃方法[J].計算機系統(tǒng)應(yīng)用,2012,21(4):92-98.XIAO Guo-Bao,YAN Xuan-Hui.Path Planning of Mobile Robot in Dynamic Nondeterministic Environments[J].Computer Systems &Applications,2012,21(4):92-98 (in Chinese).

[5]LI Huazhong,LIANG Yongsheng,WANG Meini,Dan Tangren.Design and Implementation of Improved RRT Algorithm for Collision Free Motion Planning of High-Dimensional Robot in Complex Environment[C]// ICCSNT 2012,2nd Int.Conference on Computer Science and Network Technology,CHANGCHUN,CHINA:IEEE,2012 :1391-1397.

[6]Fu,Yujian;Drager,Steven.Modeling and Verification of Humanoid Robot Task Coordination[C]// High-Assurance Systems Engineering (HASE),2014 IEEE 15th International Symposium on.Miami Beach,FL,USA:IEEE,2014.

[7]S.M.LaValle,Planning Algorithms.Cambridge,U.K.:Cambridge University Press,2006,available at http:// planning.cs.uiuc.edu/.

[8]李華忠，梁永生，但唐仁等.基于RRT的機器人避碰運動規(guī)劃算法研究[J].深圳信息職業(yè)技術(shù)學(xué)院學(xué)報,2012,10(3):20-27.LI Huazhong，LIANG Yongsheng,DAN Tangren,et al.Collision-free Motion Planning Algorithm Based on RRT for Robot[J].Journal of Shenzhen Institute of Information Technology,2012,10(3):20-27 (in Chinese).

[9]劉洋,章衛(wèi)國,李廣文.基于改進PRM算法的路徑規(guī)劃研究[J].計算機應(yīng)用研究,2012,29(1):104-106.LIU Yang,ZHANG Wei-guo,LI Guang-wen.Study on path planning based on improved PRM method[J].Application Research of Computers,2012,29(1):104-106 (in Chinese).

[10]T.Sartini,M.Vendittelli,and G.Oriolo.A resolutionadaptive strategy for probabilistic motion planning[C].// Proceedings of the 5th Biannual World in Automation Congress,2002,(14):591-596.

[11]R.Bohlin and L.Kavraki.Path planning using lazyprm[C].IEEE International Conference on Robotics and Automation,San Francisco,2000,1:521-528.

[12]L.Tapia,S.Thomas,B.Boyd,and N.Amato.An unsupervised adaptive strategy for constructing probabilistic roadmaps[C].//2009.ICRA' 09 in Robotics and Automation.IEEE International Conference on,may 2009:4037-4044.

[13]H.Kurniawati and D.Hsu.Workspace-based connectivity oracle:An adaptive sampling strategy for prm planning[C].//Proceedings of the International Workshop on the Algorithmic Foundations of Robotics (WAFR).SpringerVerlag,2006.

[14]M.Morales,L.Tapia,R.Pearce,S.Rodriguez,and N.M.Amato.A machine learning approach for feature-sensitive motion planning[M].Algorithmic Foundations of Robotics VI,2004:361-376.

[15]S.R.Lindemann and S.M.LaValle.Current issues in sampling-based motion planning[C].//The Eleventh International Symposium in Robotics Research,P.Dario and R.Chatila,Eds.,2005:36-54.

[16]I.Sucan and L.Kavraki.On the implementation of singlequery sampling-based motion planners.//2010 IEEE International Conference on Robotics and Automation (ICRA),2010,pp:2005-2011.

[17]謝世富,馬立元,劉鵬遠等.虛擬環(huán)境下運動線纜碰撞檢測算法研究與實現(xiàn)[J].系統(tǒng)仿真學(xué)報,2013,25(8):1865-1870.XIE Shi-fu,MA Li-yuan,LIU Peng-yuan,et al.Research and Realization of Collision Detection Algorithm for Dynamic Cable in Virtual Environment[J].Journal of System Simulation,2013,25(8):1865-1870(in Chinese).

[18]姜曉路,劉淵.基于混合包圍盒的碰撞檢測算法優(yōu)化[J].計算機工程,2012,38(9):285-287.JIANG Xiao-lu,LIU Yuan.Optimization of Collision Detection Algorithm Based on Hybrid Bounding Box[J].Computer Engineering,2012,38(9):285-287(in Chinese).

Research on adaptive bi-directional rapidly exploring dense tree collision avoidance motion planning algorithm

LI Huazhong，DAN Tangren，TANG QiangPing
（Department of Software Engineering,Shenzhen Institute of Information Technology,Shenzhen 518172,P.R.China）

Due to algorithm efficiency of sampling-based motion planning is low,especially when dealing with highdimensional space and complex environmental obstacles,it is heavily dependent on the selected sampling parameters and collision detection distance,adaptive bidirectional balance rapidly exploring dense tree(ABiRDT) motion planning method has been proposed.First,basic theory and implementation method of the ABiRDT algorithm have been researched deeply,its including adaptive strategies include how to automatically adjust the collision detection distance parameter,random sampling expansion step;Secondly,key adaptive key adaptive strategies strategy used in the ABiRDT about C-space weighted uniform sampling,nearest neighbor configuration searching and hybrid bounding box based parallel discrete collision detection have been investigated strongly;Finally,effectiveness of the proposed algorithms have been verified by three-dimensional visualization computer simulation.

motion planning;rapidly exploring dense tree;adaptive algorithm;sampling-based techniques;discrete collision detection;configuration space

TP242.6

：A

1672-6332（2014）03-0024-07

【責(zé)任編輯：高潮】

2014-08-24

廣東省自然基金（項目編號：S2013010013779）；深圳基礎(chǔ)研究基金（項目編號：JC201006020820A，JCYJ20120615101640639）;廣東省高職教育信息技術(shù)類課題（xxjs-2013-1019）；院科技創(chuàng)新團隊項目（項目編號：CXTD2-002）；校級科研培育項目（項目編號：LG201405）

李華忠（1963-），男（漢），湖北洪湖人，副教授，博士。研究領(lǐng)域為機器人技術(shù)、人工智能、智能規(guī)劃。E-mail：lihz@sziit.com.cn