基于脊線模態(tài)法和RRT算法的蛇形機械臂避障控制研究 ①

王 軫 常 健 李 斌 王 聰 劉 春 張良全

(*中國科學院沈陽自動化研究所機器人學國家重點實驗室 沈陽 110016)(**中國科學院機器人與智能制造創(chuàng)新研究院 沈陽 110169)(***中國科學院大學 北京 100049)(****東北大學信息科學與工程學院 沈陽 110819)

(收稿日期：2019-11-13)

0 引 言

隨著科學技術的進步，新一代的仿生機器人將遇到一系列的挑戰(zhàn)[1]。替代人在惡劣環(huán)境下工作，仿生機器人表現(xiàn)出了良好的應用前景[2]?，F(xiàn)今越來越復雜的工作環(huán)境，如太空探索、資源勘探、管道檢查、水下工作等，對仿生機器人的工作內容有了更高的要求。目前，研究者通過模仿蛇的生理機構，研究出具有高自由度的蛇形機械臂。為了突出蛇形機械臂的高靈活性，降低機械臂主體的重量，多采用繩索驅動[3-4]。與傳統(tǒng)機械臂相比，蛇形機械臂對復雜的工作環(huán)境具有更強的避障能力和適應性。

蛇形機械臂屬于多冗余機器人，其逆運動學求解和軌跡規(guī)劃具有一定難度，被國內外學者廣泛研究。目前運動學求逆通常采用雅可比迭代法[5-7]，但是對多冗余機械臂，存在求解雅可比偽逆困難、計算量巨大的問題。王俊剛等人[8]針對多冗余蛇形機械臂提出了一種末端跟隨運動的逆解算法。根據末端的運動，依次計算出其他關節(jié)的運動。與雅可比迭代法相比，末端跟隨算法計算量得到了降低，但它只適用于基座能夠平移的蛇形機械臂。董云等人[9]將遺傳算法與解析法結合提出了計算量小、適應性強的逆解算法，但是其針對的是平面三自由度冗余機械臂，對于三維空間高自由度機械臂無法快速獲得最優(yōu)解。相比于傳統(tǒng)求解方法，脊線模態(tài)法可以得到機械臂末端滿足期望位姿的脊線，同時反映機械臂的宏觀幾何構型，具有效率高、計算簡單的優(yōu)點，且對末端姿態(tài)有精確的定義，適用于非閉環(huán)構型的冗余機械臂的逆解求解。Chirikjian等人[10-11]提出了使用“脊線”對多冗余機械臂的運動學進行研究，用空間脊線來表示機械臂的幾何特征。Fahimi等人[12]改進了Chirikjian脊線模型的模態(tài)參數方程組，提高了脊線模態(tài)法的效率。

而對于機械臂的軌跡規(guī)劃問題，祁若龍等人[13]將軌跡規(guī)劃轉化成多目標優(yōu)化求解問題，并用遺傳算法求解。但是隨著自由度的增加，遺傳算法的待求解參數數量增加，碰撞檢測的計算成本也會增加，計算時間隨之成倍增加。Zhou等人[14]利用蟻群算法規(guī)劃出機械臂末端的無碰撞最短路徑，但是對于更復雜的障礙布局易出現(xiàn)無解的情況。此外，軌跡規(guī)劃的方法還有人工勢場法[15]、快速擴展隨機樹(rapidly-exploring random tree， RRT)算法[16]、強化學習算法[17]等。其中，RRT算法是基于采樣查詢的規(guī)劃方法，通過隨機采樣，不斷查詢，最終生成一條不發(fā)生碰撞的路徑，同時避免了對空間的模型建立，具有無需精確的數學模型、適用高維空間等優(yōu)點，尤為適用于多冗余機械臂高維空間的軌跡規(guī)劃。

結合脊線模態(tài)法和RRT的控制算法的優(yōu)點，本文提出適合于復雜作業(yè)空間的蛇形機械臂避障的算法。算法主要利用脊線模態(tài)法實現(xiàn)多冗余蛇形機械臂的逆運動學求解，并在此基礎之上進行關節(jié)自適應調整，最后利用RRT算法進行軌跡生成。

本文首先對蛇形機械臂進行結構特性設計及運動模型的建立，其次利用脊線模態(tài)法實現(xiàn)機械臂的運動學逆解以及研究避障軌跡規(guī)劃，最后在Matlab環(huán)境下進行仿真分析。

1 機械臂結構設計及運動學模型建立

1.1 結構設計

蛇形機械臂采用蜿蜒型[18]如圖1所示。整體機械臂由若干段剛性臂桿構成，各臂桿之間由關節(jié)連接，具有很好的靈活性及剛度。

圖1 蜿蜒型機器人

蛇形機械臂的驅動方式為繩索驅動，即驅動器電機與機械臂本體分離，驅動器通過繩索進行驅動力的傳遞。繩索驅動有效地減少了機械臂本體的質量與體積。蛇形機械臂的整體結構[19]如圖2所示，機械臂關節(jié)為萬向節(jié)類型，單個萬向節(jié)關節(jié)具有2個自由度，整個機械臂通過8個關節(jié)模塊串聯(lián)在一起?；姍C和絲杠，電機帶動絲杠旋轉拉動繩索。

圖2 蛇形機械臂整體結構圖

1.2 運動學模型建立

蛇形機械臂單個關節(jié)通過3根繩索驅動，整個機械臂共有24根繩索。繩子在底端圓盤的分布如圖3所示。

圖3 繩索圓盤分布

圖2中關節(jié)的序號從右向左依次為1、2、3、4、5、6、7、8，圓盤半徑為r。根據右手定則，在圖3中建立的坐標系，第i個關節(jié)對應的3根繩索的坐標矩陣為

(1)

對單個關節(jié)進行分析，關節(jié)角度變化與繩索變化的示意圖如圖4所示。l1h1、l2h2、l3h3即表示3根繩索，萬向節(jié)關節(jié)的2個角度與3根繩索的長度是相對應的。

圖4 關節(jié)角度與繩長示意圖

通過坐標變換法由角度變化得到繩索變化。根據圖4利用改進型D-H法建立單個關節(jié)的坐標系，如圖5所示。

萬向節(jié)中心到關節(jié)的上平面和下平面的距離均設為l，對于第i個關節(jié)旋轉的2個角度為θzi和θyi，相對應的變換矩陣為

(2)

通過式(1)求得關節(jié)上平面3點h1、h2、h3相對自身坐標系的坐標，然后根據變換矩陣，求出相對圖5中基坐標系的坐標。

(3)

圖5 單關節(jié)坐標系

通過兩點距離求出當前角度狀態(tài)下l1h1、l2h2、l3h3的距離。其他關節(jié)繩索的長度同理可得。同時蛇形機械臂的運動是存在耦合的。例如1號關節(jié)對應的3根繩索長度變化是所有關節(jié)對這3根繩索長度變化之和。所以最終給定16個角度，則24根繩長變化如式(4)所示，其中j為圖3中的繩索序號。

(4)

2 機械臂的逆運動學

蛇形機械臂屬于多冗余機械臂，本文首先采用脊線模態(tài)法確定蛇形機械臂的空間脊線，其次通過二分法將各個關節(jié)擬合在空間脊線上，最后通過幾何法計算出各個關節(jié)的角度，實現(xiàn)逆解。

2.1 脊線模態(tài)法

脊線模態(tài)法基于微分幾何，用來確定機械臂的空間脊線，如圖6所示，其函數表達式為

(5)

其中，s表示脊線歸一化的長度參數，s∈[0, 1]；L為脊線的實際長度；u(σ,t)為脊線在σ處的單位化的切線向量。其中，脊線的起始坐標就是x(0,t)，終點坐標為x(1,t)。脊線上的點對應的切向量用式(6)表示為

μ=[x′(s)y′(s)z′(s)]

=[sinKcosTcosKcosTsinT]

(6)

圖6 空間脊線

K和T的定義如圖7所示。定義空間脊線的起始點為坐標原點，所以空間脊線上的任一點可以表示為

(7)

由此，通過確定K與T得到空間脊線的整體構

圖7 K與T角度的定義

型。K與T可表示為

K(s,t)=a1sin(2πs)+a2(1-cos(2πs))

(8)

式(8)中a1、a2、a3為模態(tài)協(xié)同參數，[b1k,b1t]=[K(0,t),T(0,t)]為起始位置的2個角度，[b2k,b2t]=[K(1,t),T(1,t)]為脊線末端的2個角度。b1k、b1t、b2k、b2t是根據期望目標確定的。得到合適的模態(tài)協(xié)同參數就可以確定K與T。本文采用數值解析法來求解模態(tài)協(xié)同參數。在s=1情況下將式(7)對t求導，整理得：

(9)

(10)

其中：

J31=0

J32=0

根據式(9)建立迭代公式：

am+1=am+αJ-1(am)[xd-xm]

(11)

其中，α為控制收斂率的常數，m為迭代次數，J-1(am)為式(10)中3×3的模態(tài)雅可比矩陣的逆矩陣，xd為脊線末端期望位置，xm為當前求解的位置。收斂系數α和迭代總次數根據經驗選擇，直到期望位置與當前求解的位置的誤差滿足精度要求，則得出合適的模態(tài)協(xié)同參數。模態(tài)協(xié)同參數確定之后，即得到一條末端滿足期望位姿的空間脊線。

2.2 關節(jié)擬合

得到滿足期望目標的空間曲線之后，通過關節(jié)擬合，將蛇形機械臂的各個關節(jié)點盡可能地擬合到空間曲線上。本蛇形機械臂有8個關節(jié)，起點為坐標原點，終點即為所求空間脊線的終點，需要在曲線上擬合7個點。同時保證相鄰兩點之間的距離為單個臂桿的長度，本文采用二分法從終點倒推，依次查找其他關節(jié)點的空間坐標，流程圖如圖8所示。

圖8 二分法算法流程圖

通過二分法計算得出蛇形機械臂所有關節(jié)點的空間坐標，之后利用坐標變換法依次求出每個關節(jié)對應的2個角度值，將所有角度帶入式(4)獲得24根繩索的長度。

3 機械臂的避障軌跡規(guī)劃

在各類非結構化的工作環(huán)境中，要使蛇形機械臂從初始位置到達期望位置，機械臂需要具有良好的避障能力。根據脊線模態(tài)法得到一條期望的空間脊線。蛇形機械臂的軌跡規(guī)劃問題就可以轉化為從初始位置的脊線生成到期望脊線的軌跡，同時充分考慮障礙的影響。以下對不同位置的障礙展開研究。

3.1 障礙在脊線上的避障策略

本文用圓球來表示障礙，設定障礙圓球的半徑為r0。對于碰撞檢測，脊線上存在某點到障礙圓球的圓心的距離小于r0，則說明碰撞。利用脊線模態(tài)法生成的空間脊線上存在障礙，則可以利用蛇形機械臂高自由度的特性，通過旋轉部分關節(jié)，從而繞過障礙，形成一條新的空間脊線。

如圖9所示，當障礙半徑較小時，通過計算，只需要調整2個關節(jié)即可繞過障礙。即以關節(jié)5的起點、關節(jié)6的終點連線為旋轉軸，對兩關節(jié)進行旋轉，直到關節(jié)曲線避過障礙。圖中障礙存在這個脊線上，脊線模態(tài)法初始生成的空間脊線與障礙發(fā)生碰撞，障礙上方的脊線表示調整2個關節(jié)之后的脊線。該脊線與其他未發(fā)生碰撞的脊線構成了一個新的繞過障礙的空間脊線。調整2個關節(jié)，計算簡便，同時各個關節(jié)角變化量小，但對于障礙半徑較大的情況，旋轉2個關節(jié)無法避開障礙，則可以選擇調整4個關節(jié)，以此類推。

圖9 障礙在關節(jié)曲線上

關節(jié)調整分為3種情況進行研究。(1)障礙靠近期望終點，那么在這個期望位姿的空間脊線是無法調整的。在滿足要求的情況下，可以更換終點的姿態(tài)，生成新的關節(jié)曲線。(2)障礙靠近起始點，則只能對2個關節(jié)進行調整。(3)其他情況，首先進行2個關節(jié)調整，如果對2個關節(jié)調整后無法避開障礙，則進行4個關節(jié)的調整。流程圖如圖10所示，圖中n為機械臂的關節(jié)總數。通過分析距離障礙圓心最近的關節(jié)序號的大小進行分類討論。

圖10 關節(jié)調整流程圖

調整4個關節(jié)的主要思路是先將中間關節(jié)調整成能夠擬合一條弧線，之后進行旋轉避障。5個關節(jié)點設為L1、L2、L3、L4和L5。首先選擇L1、L3、L5所在的平面建立坐標系，L1、L5點的絕對位置始終保持不變。設單個連桿長度為L，先構造以L1、L3、L5為頂點，兩邊為2L的等腰三角形如圖11所示，同時求出當前坐標系相對基坐標系的變換矩陣。

根據等腰三角形的性質求得L3的坐標，之后進行循環(huán)操作，即L3沿著z軸負方向減小一個步長。直至L3L5的距離等于L2L4的距離，即保證這5個關節(jié)點在弧線上，如圖12所示。

圖11 L1、L3、L5構成等腰三角形

圖12 壓縮、擬合圓弧

對L2、L3、L43點進行初步調整之后，以L1和L5連線為旋轉軸，其他3點繞旋轉軸進行旋轉，直到關節(jié)線避開障礙。最后得到5個關節(jié)點的絕對坐標，與其他關節(jié)點形成新的空間脊線，如圖13所示。原始脊線被調整后生成無碰撞的空間脊線。

圖13 調整4個關節(jié)避障

3.2 障礙在其他位置的避障策略

通過上節(jié)的分析，解決了期望目標空間脊線上存在障礙的問題。而對于其他位置的障礙，即機械臂從初始位置到達最終位置的過程中存在的障礙，本文采取RRT算法生成繞過障礙的路徑軌跡。RRT算法中初始角度矩陣與期望角度矩陣維度為16。算法在十六維的空間中快速探尋出無碰撞的軌跡。蛇形機械臂RRT算法的流程圖如圖14所示。

圖14 RRT算法流程圖

4 仿真分析

通過Matlab對蛇形機械臂進行仿真，主要包括角度變化與繩長變化映射關系、脊線模態(tài)算法、二分法關節(jié)擬合效果以及蛇形機械臂軌跡規(guī)劃，進而驗證這些方法的有效性。

對單個關節(jié)進行研究，分析角度變化與繩長變化的關系，根據前文圖2所示，以1號關節(jié)為例，兩個旋轉角度為θz和θy。圓盤半徑r為6 cm，萬向節(jié)中心到上下端面的距離l為5 cm。兩個角度變化的范圍均為[-60 °,60 °]。單個角度變化影響的繩長變化如圖15所示。從圖15中上圖可以看出，關節(jié)1在θy為0的情況下θz變化對3根繩索的影響，根據圖3,2號繩索與3號繩索位置關于y軸對稱，關節(jié)繞z軸旋轉，2號繩索與3號繩索長度變化是相同的，θz變化過程中1號繩索伸長，2號、3號繩索縮短。從圖15中下圖可以看出，關節(jié)1在θz為0的情況下θy變化對3根繩索的影響，1號繩索在y軸的負方向上，整體變化不大，2號繩索逐漸伸長，3號繩索逐漸縮短。仿真計算分析符合實際情況。

圖15 單關節(jié)角度繩長變化圖

通過例子對脊線模態(tài)法和二分法關節(jié)擬合進行分析。單個臂桿的長度為20 cm，根據脊線模態(tài)法，假設起始位置的兩個角度[K(0,t),T(0,t)]=[π/2, 0]切線方向平行于x軸，脊線末端的兩個角度[K(1,t),T(1,t)]=[0, π/6],末端的位置設為[60， 80， 80]，根據2.1節(jié)的計算步驟，得到a1為0.40、a2為0.13、a3為0.20。計算生成一條空間脊線，如圖16所示。曲線為脊線模態(tài)法生成的空間脊線。通過二分法進行關節(jié)擬合，每個折線段表示單個臂桿。

圖16 脊線模態(tài)法及關節(jié)擬合

對存在脊線上的障礙和其他位置的障礙進行分析，通過調整關節(jié)和RRT算法生成使末端到達期望位姿且無碰撞的軌跡，如圖17所示。一簇脊線從右下到左上是算法生成的軌跡。兩個圓球代表兩種不同類型的障礙。對于存在初始期望脊線上的障礙，脊線調整4個關節(jié)生成繞過障礙的空間脊線，之后利用RRT算法生成圖中的軌跡，使蛇形機械臂到達期望的目標。

圖17 蛇形機械臂軌跡規(guī)劃

從數值上分析圖17中的軌跡無碰撞發(fā)生。圖中兩個障礙的半徑設定為10 cm,蛇形機械臂從當前軌跡運動到下一個軌跡的時間為0.1 s,則兩障礙圓心到每個軌跡的最短距離變化如圖18所示。實曲線為存在脊線上的障礙圓心到機械臂的最短距離，從初始位置到最終位置，距離呈減小趨勢。調整關節(jié)時考慮的是理想情況，不發(fā)生碰撞就停止調整

圖18 兩障礙到所有軌跡的最短距離

關節(jié)，脊線上存在某點在障礙的臨界位置，所以最終軌跡機械臂到圓心的距離逼近10 cm。虛曲線為過程中障礙圓心到機械臂的最短距離，數值先減小后增加。只要滿足虛曲線每點對應的值大于10 cm即表明無碰撞發(fā)生，算法生成的路徑中有一段靠近障礙，如圖17所示，因此虛曲線存在一部分距離接近于10 cm。

對圖17中機械臂運動中繩長變化進行分析。通過式(4)計算得出在蛇形機械臂運動過程中每條脊線對應的繩索長度，24條繩索長度變化如圖19所示。圖17中每條脊線末端兩條線段變化較小，即代表關節(jié)7、關節(jié)8角度變化較小，對應圖19中繩索19～24長度變化較小。

圖19 繩長變化

5 結 論

本文以蛇形機械臂為研究對象，研究得到機械臂各角度變化到繩索長度的函數關系。提出了一種基于脊線模態(tài)法和RRT算法的避障軌跡規(guī)劃算法。采用脊線模態(tài)法得到滿足期望位姿的空間脊線，通過二分法對空間脊線進行關節(jié)擬合。在脊線模態(tài)法的基礎之上主要對存在于脊線上的障礙進行分析，通過調整關節(jié)得到不經過障礙的空間脊線，然后利用RRT算法生成無碰撞的軌跡。最后，通過Matlab仿真分析了單關節(jié)角度繩長的變化關系，并驗證了基于脊線模態(tài)法與RRT的避障控制算法的可行性。在未來的研究中，將進一步改進蛇形機械臂的避障軌跡規(guī)劃算法，優(yōu)化調整關節(jié)的算法，探尋最優(yōu)路徑，降低蛇形機械臂的工作代價。