基于改進式RRT*的手術(shù)機器人在線避障運動規(guī)劃

胡孟慧，王懷震，梁鳳強，孟超

(1.山東大學(xué) 臨床醫(yī)學(xué)院，山東 濟南 250011;2.山東新一代信息產(chǎn)業(yè)技術(shù)研究院有限公司,山東 濟南 250011;3.臨沂供電公司，山東 臨沂 276003;4.臨沂正信工程勘察設(shè)計有限公司，山東 臨沂 276005 ）

0 引言

運動規(guī)劃是機器人研究的基本問題，針對移動機器人的路徑規(guī)劃方法已經(jīng)非常豐富，目前該類方法主要有Dijkstra 算法、A*算法、隨機采樣法（PRM）、快速隨機搜索樹（RRT）、人工勢場法、智能算法等。但是，針對機械臂的多輸入多輸出、非線性、強耦合的高維復(fù)雜系統(tǒng)，上述方法很難得到應(yīng)用。為保證機械臂安全工作，應(yīng)該滿足其在線避障要求。因此，機械臂在線避障運動規(guī)劃是機械臂控制的一個關(guān)鍵問題。

為了將移動機器人路徑規(guī)劃方法應(yīng)用到機械臂系統(tǒng)上，學(xué)者們對這些規(guī)劃方法提出了相應(yīng)改進。祁若龍等[1]對理想軌跡進行分段描述，將空間機械臂軌跡規(guī)劃問題轉(zhuǎn)變?yōu)槎嗄繕?biāo)優(yōu)化求解問題，建立適應(yīng)度評價函數(shù)，用遺傳算法進行求解。理論上該算法可以推廣到任意自由度和任意多的障礙物，但是參數(shù)量增加導(dǎo)致計算成本增加，計算時間也隨之成倍增長。杜爽等[2]使用雙向RRT 算法的規(guī)劃方法，得到仿人機器人雙腿穩(wěn)定位形和抓手位姿序列，實現(xiàn)了機器人的全身運動規(guī)劃。楊一波等[3]對人工勢場法進行了改進，對障礙物建立排斥勢場，對目標(biāo)建立引力勢場，針對其容易陷入局部極小點的問題，引入了與目標(biāo)點相關(guān)的因子，能夠避開障礙物到達目標(biāo)點。劉成菊等[4]對RRT進行了改進，引入了引力場的概念，使得隨機樹的生長向終點偏移，引入路徑緩存方法和動態(tài)擴展隨機樹的方法，適合于移動障礙物環(huán)境中。賈慶軒等[5]利用運動學(xué)方法將空間障礙物轉(zhuǎn)換到了關(guān)節(jié)空間，再根據(jù)連桿與障礙物的碰撞條件，確定出了機械臂的自由運動空間，利用A*算法在自由運動空間進行路徑搜索，實現(xiàn)了無碰撞路徑規(guī)劃。何兆楚等[6]將RRT 和人工勢場法結(jié)合起來，利用人工勢場法進行局部路徑規(guī)劃，一旦陷入局部極小值，就使用RRT 選擇臨時目標(biāo)點，使其跳出局部最小值，能夠?qū)崿F(xiàn)避障，最終到達目標(biāo)點，但其只考慮了機械臂末端的避障，未曾考慮機械臂桿件的避障。這些改進方法都是開環(huán)的，不能應(yīng)對動態(tài)環(huán)境，國外有學(xué)者關(guān)于實時避障做了一些研究[7-8]，可以適應(yīng)于動態(tài)環(huán)境，但其主要針對的是移動機器人，關(guān)于機械臂實時或在線避障的內(nèi)容較少。

本文對RRT*進行改進并應(yīng)用到機械臂空間，根據(jù)目標(biāo)位置變化更新搜索路徑形成閉環(huán)，實現(xiàn)在線避障運動規(guī)劃。

1 機械臂在線避障運動規(guī)劃

在學(xué)者們改進的基礎(chǔ)上，結(jié)合機械臂系統(tǒng)高維度的特點，本文對RRT*算法進行了改進使其適用于機械臂在線避障運動規(guī)劃。為降低運動學(xué)逆解所帶來的時間損耗，機械臂運動規(guī)劃一般在關(guān)節(jié)空間內(nèi)進行。而目標(biāo)點通常是基于笛卡爾空間的，通過逆解轉(zhuǎn)換到關(guān)節(jié)空間。

在線避障運動規(guī)劃包括兩個階段：預(yù)規(guī)劃和重規(guī)劃。預(yù)規(guī)劃階段從起始點開始搜索，同時將搜索到的路徑點送至機械臂控制器，直至搜索到目標(biāo)點進入重規(guī)劃階段。重規(guī)劃階段在包含目標(biāo)節(jié)點的路徑節(jié)點附近采樣以優(yōu)化路徑，利用攝像機或其它設(shè)備監(jiān)控目標(biāo)點，當(dāng)目標(biāo)點發(fā)生移動時，以當(dāng)前機械臂位置作為起始點重新規(guī)劃，實現(xiàn)閉環(huán)控制，直至機械臂移動到目標(biāo)節(jié)點。

1.1 預(yù)規(guī)劃

預(yù)規(guī)劃的主要任務(wù)是對整個自由空間進行探索，使得節(jié)點布滿解空間，同時找到目標(biāo)點。Algorithm1 給出了預(yù)規(guī)劃的偽代碼，此算法在RRT*的基礎(chǔ)上做了改進。在此過程中，機械臂始終處于運動狀態(tài)，當(dāng)機械臂qa運動至與根節(jié)點q0之間的距離小于設(shè)定閾值時，從規(guī)劃的路徑中選取前k個節(jié)點輸入給機械臂控制器（17 行），控制機械臂按照這k個節(jié)點進行運動，將根節(jié)點q0更新為（14—16 行），保證了在線運動。

Algorithm2 給出了規(guī)劃包含k個節(jié)點路徑的偽代碼。算法使用成本函數(shù)式（1）選擇節(jié)點，當(dāng)路徑規(guī)劃進行到葉節(jié)點時，標(biāo)記該節(jié)點并返回規(guī)劃路徑（3—5 行）。

其中，cost(qi)為起始點到qi的成本，H(qi)為qi到目標(biāo)點的成本，計算方法如式（2）所示。當(dāng)qi標(biāo)記時，表明qi已經(jīng)訪問過，當(dāng)通過重新布線或添加新節(jié)點將未標(biāo)記節(jié)點添加為其子節(jié)點時，其祖先節(jié)點都有機會再次訪問，即所有祖先節(jié)點去除標(biāo)記。當(dāng)規(guī)劃路徑比最佳路徑更接近目標(biāo)點時，則更新最佳路徑（6 行）。

1.1.1qrand的選取和qnew的計算

qrand采用帶有目標(biāo)偏向的隨機采樣，如式（3）所示，qnew計算方式如式（4）所示。該方法以概率α向目標(biāo)點方向采樣，概率1—α在自由空間內(nèi)隨機采樣，利于搜索樹向未知空間探索，又保證朝目標(biāo)點進行搜索，兼顧了隨機性和搜索目標(biāo)的快速性[9]。α的設(shè)置根據(jù)障礙物確定，當(dāng)障礙物較少時，α設(shè)為較大值，障礙物較多時，α設(shè)為較小值。

其中，Pr∈[0,1]是一個隨機數(shù)，α為自定義常數(shù)，qrand=[θ1,θ2,...,θ6]為關(guān)節(jié)空間內(nèi)6 個關(guān)節(jié)角組成的點。

1.1.2 成本計算

路徑的總成本為節(jié)點之間的距離總和，一般用歐式距離來定義[10]。關(guān)節(jié)空間的歐式距離定義為式（5），笛卡爾空間位置和姿態(tài)的歐式距離定義為式（6）和式（7），r，p，y分別為繞z，y，x軸的轉(zhuǎn)角。

對機械臂來講，兩組相差較小的關(guān)節(jié)角對應(yīng)的末端位姿可能相差甚遠，另外由于逆解的多解性，兩組完全不同的關(guān)節(jié)角對應(yīng)的末端位姿也可能完全一致，因此可用三者的加權(quán)值來定義距離成本，如式（8）所示。

1.2 重規(guī)劃

由于目標(biāo)位置的變化可能會導(dǎo)致規(guī)劃好的路徑不再適用，所以需要對路徑進行重新搜索和優(yōu)化[11]。Algorithm3 給出了重規(guī)劃的偽代碼描述。4—5 行在路徑周圍進行采樣和重新布線，如果找到更優(yōu)的路徑則更新path（6 行）。7—10 行與Algorithm1 對應(yīng)代碼作用相同。當(dāng)目標(biāo)點移動導(dǎo)致選擇的目標(biāo)點與障礙物發(fā)生碰撞時，需重新選擇目標(biāo)點（11—12 行），以機械臂當(dāng)前點作為起始點重新規(guī)劃，形成閉環(huán)在線控制，直至機械臂到達目標(biāo)點。

1.2.1 擴展與重新布線

RRT*-smart[12]在二維空間擴展與重新布線示意圖如圖1 所示，對比二維空間，機械臂空間也有類似的性質(zhì)。擴展采樣方式如式（9）所示，該方法以概率α在以信標(biāo)qbeacon為圓心，r為半徑的圓內(nèi)隨機采樣（左圖），概率1—α在自由空間內(nèi)隨機采樣。信標(biāo)qbeacon是路徑的節(jié)點，包含路徑的“拐點”信息，因此在信標(biāo)周圍采樣能優(yōu)化這些“拐彎”處的路徑。右圖展示了如何重新布線優(yōu)化路徑，根據(jù)三角形定理“兩邊之和大于第三邊”可知去掉中間點會使得成本更小，據(jù)此對整條路徑重新布線以優(yōu)化路徑。

圖1 擴展與重新布線

1.2.2 查找臨近點

預(yù)規(guī)劃階段對全局進行搜索，存儲了大量的節(jié)點，優(yōu)化路徑時需要進行查找，查找速度決定算法的運行速度，因此需要合適的存儲結(jié)構(gòu)減少查找時間。本文使用KD 樹[13]（k-dimension tree）存儲節(jié)點，KD 樹是一種對K 維空間中的實例進行存儲以便對其進行快速檢索的數(shù)據(jù)結(jié)構(gòu)。

算法在執(zhí)行過程中不斷擴展節(jié)點，將這些節(jié)點按照“左小右大”的原則動態(tài)添加進樹中。在查找近鄰點時，從根節(jié)點出發(fā)，遞歸地向下遍歷，當(dāng)目標(biāo)點當(dāng)前維小于切分點的坐標(biāo)值時，移動到左子樹，否則移動到右子節(jié)點，直到葉節(jié)點為止。KD 樹的引入將查找的時間復(fù)雜度從O(N)降低到了O(logN)。

2 仿真實驗

為了驗證本文提出的算法，利用MATLAB在三維空間內(nèi)進行兩組測試。設(shè)置7 個障礙物模型，初始點為{10,10,10}，目標(biāo)點為{90,90,90}，步長為5，最大迭代次數(shù)為2 000。最后在實際機械臂上對算法進行了驗證。運動平臺選用的計算機主頻為2.3 GHz，內(nèi)存為8 GB。

2.1 算法對比

針對規(guī)劃速度而言，RRT 系列算法的速度遠高于其它規(guī)劃算法，因此本文與RRT 系列算法進行對比。在初始點、目標(biāo)點、障礙物等變量相同的條件下，分別對RRT、RRT*和Algorithm1 進行了10 次對比，對比結(jié)果見表1，迭代次數(shù)、運行時間和路徑長度取成功平均值。

表1 算法性能比對

可以看出，針對7 個障礙物的復(fù)雜環(huán)境，三種算法都有著較高的成功率。RRT*由于對父節(jié)點的選擇做了改進，路徑長度小于RRT，但運行時間較長，迭代次數(shù)更多。Algorithm1在RRT*的基礎(chǔ)上改進采樣方式，同時將前k個節(jié)點送給機械臂控制器，使其路徑長度更短，成功率更高，迭代次數(shù)也最短，而運行時間與RRT 相比變化不大。

三種算法搜索結(jié)果如圖2 所示，可以看出，RRT 搜索軌跡不夠光滑，RRT*的搜索點幾乎布滿了整個空間，很多都是沒必要的，Algorithm1 以概率α向目標(biāo)點進行搜索，軌跡較為光滑，同時在自由空間也有一些搜索點，便于重規(guī)劃進行擴展和重新布線。

圖2 三種算法搜索結(jié)果比較

2.2 規(guī)劃對比

預(yù)規(guī)劃與重規(guī)劃的對比如圖3 所示，左圖是預(yù)規(guī)劃給出的路徑，對中間障礙物進行膨脹操作后與規(guī)劃路徑發(fā)生沖突，此時重規(guī)劃以當(dāng)前機械臂位置為起點重新規(guī)劃后面的路徑，如右圖所示，依然可以順利完成避障。表明該算法在環(huán)境發(fā)生變化時，能夠在線重新規(guī)劃后面的路徑，適應(yīng)動態(tài)環(huán)境以完成避障任務(wù)。

圖3 規(guī)劃對比

2.3 實物模擬

根據(jù)本文提出的算法，利用ROS（Robot Operating System）中的運動規(guī)劃庫MoveIt!控制Jaco2 機械臂進行避障運動規(guī)劃驗證。實際環(huán)境下避障過程如圖4 所示。圖4 中長方體紙箱為障礙物，黃色方塊位置為起始點，將其移動至障礙物的另一側(cè)，在規(guī)劃的同時，將得到的路徑點送至機械臂控制器，實現(xiàn)在線規(guī)劃任務(wù)。圖中可見本文算法能夠完成避障任務(wù)，驗證了仿真結(jié)果。

圖4 實際環(huán)境下避障過程

3 結(jié)語

本文從機械臂的作業(yè)需求出發(fā)，提出了一種在線避障規(guī)劃算法。規(guī)劃算法分為兩階段：預(yù)規(guī)劃和重規(guī)劃。

預(yù)規(guī)劃階段，在關(guān)節(jié)空間內(nèi)以概率α向目標(biāo)點搜索，對節(jié)點進行碰撞檢測以確定是否添加進搜索樹中，同時將規(guī)劃出的前k個節(jié)點送給機械臂控制器，實現(xiàn)規(guī)劃與運動并發(fā)進行。

重規(guī)劃階段，在路徑點附近采樣，對之前生成的路徑進行“剪枝”和“重連接”操作，優(yōu)化路徑，減少成本。當(dāng)目標(biāo)節(jié)點移動時，以機械臂當(dāng)前位置為起始點重新規(guī)劃路徑，能夠?qū)崿F(xiàn)動態(tài)避障和跟隨目標(biāo)。

在仿真環(huán)境中添加了7 個障礙物進行了算法驗證，證明該算法能夠在復(fù)雜環(huán)境中規(guī)劃出一條避障路徑，當(dāng)環(huán)境變化導(dǎo)致規(guī)劃路徑不可行時，算法能及時調(diào)整路徑以適應(yīng)新的環(huán)境，滿足機械臂在線避障要求。