王 凌,張 印,謝雨鑫,蒲 偉
(四川大學(xué) 空天科學(xué)與工程學(xué)院,四川 成都 610065)
軟體機(jī)械臂相比于傳統(tǒng)的剛性超冗余機(jī)械臂,具有剛度小、質(zhì)量輕、柔性化等特點(diǎn),靈活性和環(huán)境適應(yīng)能力更強(qiáng)[1]。在深海探測(cè)、醫(yī)療手術(shù)、航空航天、工業(yè)生產(chǎn)等領(lǐng)域應(yīng)用潛力巨大[2-4]。
近年來,國內(nèi)外研究者圍繞軟體臂的設(shè)計(jì)、建模、控制和避障開展了諸多研究。例如,Shafti 等[5]提出了一種新的卡爾曼濾波算法,將姿態(tài)估計(jì)和避障相結(jié)合,實(shí)現(xiàn)了多段連續(xù)體機(jī)械臂的避障運(yùn)動(dòng)。Xiao 等提出了一種在不確定環(huán)境中抓取目標(biāo)物體的在線運(yùn)動(dòng)規(guī)劃算法,并將此算法運(yùn)用在研制的平面連續(xù)體機(jī)器人上。Ataka 等[6]提出了一種基于多段連續(xù)機(jī)械臂的實(shí)時(shí)路徑規(guī)劃算法,并在研制的單節(jié)連續(xù)機(jī)器人上進(jìn)行了實(shí)驗(yàn)驗(yàn)證。
但是由于軟體臂具有無限自由度,不同構(gòu)型間切換時(shí)在運(yùn)動(dòng)學(xué)上存在耦合。比如,從正運(yùn)動(dòng)學(xué)來看,當(dāng)求解柔性臂正向運(yùn)動(dòng)學(xué)時(shí),柔性臂每段的參考坐標(biāo)系會(huì)隨之改變。反之,從逆運(yùn)動(dòng)學(xué)來看,當(dāng)已知坐標(biāo)點(diǎn)反求柔性臂關(guān)節(jié)參數(shù)信息時(shí),前一段柔性臂會(huì)跟隨后一段進(jìn)行一系列的運(yùn)動(dòng)。這種運(yùn)動(dòng)學(xué)上的耦合關(guān)系是軟體機(jī)械臂在復(fù)雜環(huán)境下避障面臨的巨大挑戰(zhàn)。因?yàn)闄C(jī)械臂要繞過狹窄非結(jié)構(gòu)化障礙物,要求柔性臂有良好的柔順性。提出一種分段順序建模和控制的方法,在構(gòu)建運(yùn)動(dòng)學(xué)模型時(shí),我們將軟體臂本體的一大段視為若干小段,然后從根部到前端順序驅(qū)動(dòng)控制每一小段變形。利用這種仿藤蔓生長(zhǎng)原理,實(shí)現(xiàn)軟體機(jī)械臂在復(fù)雜的環(huán)境中軌跡規(guī)劃。
主要貢獻(xiàn)如下:
(1)提出了一種順序控制方法,通過順序控制軟體機(jī)械臂的運(yùn)動(dòng)來避免運(yùn)動(dòng)學(xué)耦合。
(2)建立了軟體機(jī)械臂的正逆運(yùn)動(dòng)學(xué)模型,并在仿真環(huán)境中驗(yàn)證了模型的正確性。
(3)構(gòu)建了改進(jìn)型快速探索隨機(jī)樹路徑規(guī)劃算法,提高路徑規(guī)劃算法的成功率、減少算法耗時(shí)。
受大自然中藤蔓植物生長(zhǎng)過程的啟發(fā),提出了一種軟體機(jī)械臂的順序控制思想。這種控制思想的優(yōu)點(diǎn)是在避障時(shí),柔性臂能夠仿藤蔓生長(zhǎng)的過程,按照順序生長(zhǎng)運(yùn)動(dòng)。避免柔性機(jī)械臂每段之間存在耦合的問題,在非結(jié)構(gòu)化復(fù)雜環(huán)境中能更好地完成避障任務(wù)。每段軟體臂不僅有彎曲和偏轉(zhuǎn)方向的自由度,并且有一個(gè)伸長(zhǎng)方向的自由度,如圖1 所示。
圖1 為軟體臂的順序控制思想:每段軟體臂依次進(jìn)行運(yùn)動(dòng),當(dāng)?shù)谝欢芜\(yùn)動(dòng)時(shí)其余兩段將收縮在第一段的裝置內(nèi),當(dāng)?shù)谝欢瓮瓿蛇\(yùn)動(dòng)后,其余兩段再依次運(yùn)動(dòng)。
圖1 順序控制思想示意圖
建立軟體機(jī)械臂的運(yùn)動(dòng)學(xué)模型是對(duì)實(shí)現(xiàn)在復(fù)雜環(huán)境中避障的重要前提。由于軟體機(jī)械臂相比于傳統(tǒng)的剛性機(jī)械臂沒有自由度和關(guān)節(jié)等概念,針對(duì)傳統(tǒng)剛性機(jī)械臂的建模方法不能直接應(yīng)用。為此,有研究人員分別利用分段常曲率方法[7-8]、Cosserat 桿理論對(duì)軟體機(jī)械臂進(jìn)行運(yùn)動(dòng)學(xué)建模分析。采用分段常曲率方法對(duì)軟體機(jī)械臂進(jìn)行運(yùn)動(dòng)學(xué)分析。引入關(guān)節(jié)空間變量(θ,φ),兩個(gè)參數(shù)分別對(duì)應(yīng)每一段柔性機(jī)械臂的彎曲角和偏轉(zhuǎn)角,k表示每段柔性臂彎曲曲率。機(jī)械臂的每一段由三個(gè)氣動(dòng)驅(qū)動(dòng)器所驅(qū)動(dòng)i為驅(qū)動(dòng)器的編號(hào)。在運(yùn)動(dòng)學(xué)模型中如圖2(b)所示,正運(yùn)動(dòng)學(xué)關(guān)系為驅(qū)動(dòng)空間到關(guān)節(jié)空間的映射和關(guān)節(jié)空間到工作空間的映射。逆運(yùn)動(dòng)學(xué)關(guān)系為工作空間到關(guān)節(jié)空間的映射和關(guān)節(jié)空間到驅(qū)動(dòng)空間的映射。
圖2(a)軟體臂在空間中的參數(shù),其中θ為軟體臂的彎曲角度,φ為軟體臂在空間中繞Z軸的偏轉(zhuǎn)角度,r為曲率半徑,軟體機(jī)械臂驅(qū)動(dòng)器長(zhǎng)度、彎曲角度、偏轉(zhuǎn)角度示意圖l0i表示每一段軟體臂的三個(gè)驅(qū)動(dòng)腔體的長(zhǎng)度,d為中心點(diǎn)O到氣腔表面距離。(b)為運(yùn)動(dòng)學(xué)映射關(guān)系。
圖2 軟體臂參數(shù)信息
從機(jī)械臂的兩種狀態(tài)來分析其運(yùn)動(dòng)學(xué)關(guān)系。初始狀態(tài)時(shí),整個(gè)軟體機(jī)械臂收縮在一起,我們認(rèn)為此時(shí)臂每段的長(zhǎng)度為零。伸長(zhǎng)狀態(tài)時(shí),每段軟體臂依次先伸長(zhǎng)再進(jìn)行彎曲,伸長(zhǎng)量為l,彎曲角度為θ。本研究中,整個(gè)軟體機(jī)械臂由三段組成,每段的運(yùn)動(dòng)學(xué)建模思路完全相同,為說明運(yùn)動(dòng)學(xué)的分析方法,下面以單段軟體臂為例進(jìn)行詳細(xì)的正運(yùn)動(dòng)學(xué)分析。軟體機(jī)械臂單段的簡(jiǎn)化幾何模型如圖2(a)所示,θ表示軟體臂的彎曲角度,l表示軟體臂的長(zhǎng)度,φ表示在空間中軟體臂與Z軸的偏轉(zhuǎn)角度。
軟體機(jī)械臂關(guān)節(jié)空間到工作空間的映射關(guān)系可由齊次變換矩陣T表示,T表示基座坐標(biāo)系至末端坐標(biāo)系的變換。代表從上一個(gè)基坐標(biāo)系原點(diǎn)到下一段坐標(biāo)系原點(diǎn)的平移。此變換方式為先繞Z軸旋轉(zhuǎn)φ,再繞Y軸旋轉(zhuǎn)θ,最后再繞Z旋轉(zhuǎn)-φ。設(shè)定彎曲角θ,θ?(0,π),設(shè)定偏轉(zhuǎn)角度φ,φ?(0,2π),li為軟體臂各段的長(zhǎng)度,i?(1,2,3)。根據(jù)分段常曲率模型,可求得齊次變換矩陣為:
根據(jù)單段軟體臂分析方法可求得三段式軟體機(jī)械臂關(guān)節(jié)空間至工作空間的映射關(guān)系。由基座標(biāo)系O0至第三段末端坐標(biāo)系O3的齊次變換矩陣表示。基座標(biāo)系到最末端坐標(biāo)系可以由基座標(biāo)系0 到第一段末端1 的齊次變換矩陣;坐標(biāo)系1 到坐標(biāo)系2 的齊次變換矩陣;坐標(biāo)系2 到坐標(biāo)系3 的齊次變換矩陣三部分相乘得到,其中:
如圖2(c)所示,軟體臂的第一段的驅(qū)動(dòng)空間與關(guān)節(jié)空間的映射關(guān)系如式(6)-(8)所示:
驅(qū)動(dòng)器長(zhǎng)度與彎曲角、偏轉(zhuǎn)角之間的關(guān)系為:
為建立起機(jī)械臂從工作空間到關(guān)節(jié)空間的映射,需要構(gòu)建軟體臂的逆運(yùn)動(dòng)學(xué)模型。通過求解齊次變換矩陣中非線性方程組實(shí)現(xiàn)工作空間到關(guān)節(jié)空間的映射,但具有一定挑戰(zhàn)性。為了提高求解可靠性,降低求解難度,在求解逆運(yùn)動(dòng)學(xué)方程時(shí)設(shè)定約束條件來簡(jiǎn)化求解逆運(yùn)動(dòng)學(xué)方程難度,從而避免在求解逆運(yùn)動(dòng)學(xué)過程中可能會(huì)產(chǎn)生奇異解甚至無解的情況。假設(shè)在求解軟體臂逆運(yùn)動(dòng)學(xué)參數(shù)時(shí),每段長(zhǎng)度固定不變,只用考慮彎曲和偏轉(zhuǎn)兩個(gè)變量。約束條件和求解如公式10-13。
為驗(yàn)證運(yùn)動(dòng)學(xué)模型的正確性,在Matlab 仿真環(huán)境中設(shè)定了系列目標(biāo)點(diǎn)位置,然后讓機(jī)械臂每段處于已伸長(zhǎng)完成的狀態(tài),從初始位置(0,0,0)運(yùn)動(dòng)到指定目標(biāo)點(diǎn)位置(123,259.4,421)。通過逆運(yùn)動(dòng)學(xué)求解各段彎曲角為(0.63rad,0.63rad,0.63rad),各段偏轉(zhuǎn)角為(0.45rad,3.58rad,0.45rad)。利用逆運(yùn)動(dòng)學(xué)求解軟體臂運(yùn)動(dòng)到目標(biāo)點(diǎn)仿真,結(jié)果見圖3(a),圖中顯示軟體臂成功運(yùn)動(dòng)到指定點(diǎn)。運(yùn)動(dòng)到指定點(diǎn)所運(yùn)動(dòng)過程中各段驅(qū)動(dòng)器長(zhǎng)度的變化量,如圖3(b)所示,從圖能夠發(fā)現(xiàn)各驅(qū)動(dòng)器的長(zhǎng)度變化量之間是解耦合關(guān)系。
圖3 逆運(yùn)動(dòng)學(xué)模型驗(yàn)證
快速探索隨機(jī)樹算法(Rapidly Exploring Random Trees,RRT)是一種基于隨機(jī)采樣的路徑規(guī)劃算法。該算法的工作原理是在空間中從起點(diǎn)向終點(diǎn)進(jìn)行采樣,將滿足要求的采樣點(diǎn)依次連接形成一條像樹枝的路徑。RRT 算法在無人機(jī)、智能車避障領(lǐng)域中有著廣泛的應(yīng)用。傳統(tǒng)的RRT 算法在空間中隨機(jī)采樣,存在采樣數(shù)據(jù)點(diǎn)過多、隨機(jī)性較大和整體算法耗時(shí)長(zhǎng)等問題。針對(duì)這些問題,提出了一種RRT 算法的優(yōu)化方法。
因?yàn)镽RT 算法在空間中尋找采樣點(diǎn)完全隨機(jī),所以在尋找路徑時(shí)會(huì)存在耗時(shí)較高和導(dǎo)向性不穩(wěn)定等缺點(diǎn)。因此,在采樣時(shí)利用一種基于概率的采樣方法。假設(shè)關(guān)系如下:
如式(14)所示,設(shè)定概率Pt,按照概率均勻隨機(jī)分布,在采樣前獲得一個(gè)概率值P,如果P小于Pt,則采樣點(diǎn)為目標(biāo)點(diǎn),路徑則向目標(biāo)點(diǎn)方向生長(zhǎng)。如果大于Pt,將空間中隨機(jī)產(chǎn)生的點(diǎn)作為采樣點(diǎn)。
RRT 算法在生成節(jié)點(diǎn)時(shí)存在一定隨機(jī)性,生成的最終路徑不連續(xù)。此外,起點(diǎn)距離終點(diǎn)太遠(yuǎn)容易導(dǎo)致路徑規(guī)劃失敗。因此,為提高算法的成功率,將路徑找尋任務(wù)分為若干子任務(wù)。首先,在尋找路徑時(shí),對(duì)整條路徑信息進(jìn)行預(yù)先細(xì)分。然后,在仿真環(huán)境中依次設(shè)置“分段終點(diǎn)”。最后,RRT 算法在尋找路徑時(shí)將根據(jù)預(yù)先設(shè)置的“分段終點(diǎn)”依次到達(dá)目標(biāo)點(diǎn)位。這種方法能夠有效地提高路徑尋找的成功率,漸少生成路徑的隨機(jī)性。
仿真使用的計(jì)算機(jī)軟硬件配置為:IntelR CoreTM i5 6300HQ,內(nèi)存(RAM)16GB,顯卡為Nvidia GeForce GTX 960M,MATLAB 使用版本為2016b。
在本次仿真實(shí)驗(yàn)中,設(shè)置搜索步長(zhǎng)為6,最高迭代次數(shù)為7000,實(shí)驗(yàn)空間環(huán)境設(shè)置為[800,800,1000]路徑規(guī)劃的起點(diǎn)設(shè)置為[0,0,0],目標(biāo)終點(diǎn)設(shè)置為[500,.600,150],實(shí)驗(yàn)中的分段終點(diǎn)為[400,320,100],[420,410,250]。路徑規(guī)劃實(shí)驗(yàn),如圖4 所示。其中長(zhǎng)方體表示障礙物,在復(fù)雜環(huán)境中生成的路徑如圖4 所示,由圖可見該路徑成功避開障礙物。
圖4 路徑尋找實(shí)驗(yàn)
為驗(yàn)證軟體臂自生長(zhǎng)工作模式的避障效果,進(jìn)行了軟體臂在非結(jié)構(gòu)化復(fù)雜環(huán)境中的避障實(shí)驗(yàn)。為模擬軟體臂在工作時(shí)需要穿越狹小空間的情況在仿真環(huán)境中設(shè)置長(zhǎng)方體障礙物布局如圖4 所示。為了保證機(jī)械臂能夠成功避開障礙物,到達(dá)目標(biāo)點(diǎn)位,首先將上一章節(jié)提前規(guī)劃完成的路徑信息進(jìn)行分段,確定軟體臂段數(shù)。其次,利用2.2 節(jié)所提出的自生長(zhǎng)模式逆運(yùn)動(dòng)學(xué),使每一段運(yùn)動(dòng)到指定位置。通過每一段的生長(zhǎng)運(yùn)動(dòng)使機(jī)械臂到達(dá)終點(diǎn)。如圖5 所示,根據(jù)起點(diǎn)距離終點(diǎn)的長(zhǎng)度信息,在實(shí)驗(yàn)中只需要軟體臂前面兩大段。表1 為避障實(shí)驗(yàn)中各段的關(guān)節(jié)信息。這種工作模式的優(yōu)點(diǎn)在于可根據(jù)任務(wù)要求決定使用具體的軟體臂段數(shù),每一段之間獨(dú)立控制不存在耦合情況。
圖5 自生長(zhǎng)工作模式下避障實(shí)驗(yàn)
表1 避障實(shí)驗(yàn)中各段參數(shù)信息
針對(duì)軟體機(jī)械臂提出一種順序建模和控制方法,使機(jī)械臂能夠模仿藤蔓生長(zhǎng)的過程,每段先伸長(zhǎng)后彎曲,避免機(jī)械臂運(yùn)動(dòng)學(xué)上的強(qiáng)耦合,從而實(shí)現(xiàn)在非結(jié)構(gòu)化環(huán)境中避障。具體地,利用改進(jìn)型RRT 算法在非結(jié)構(gòu)化障礙物空間中尋找一條路。軟體機(jī)械臂將按照此路徑,分段順序運(yùn)動(dòng)進(jìn)而實(shí)現(xiàn)避障。在仿真環(huán)境中實(shí)現(xiàn)了運(yùn)動(dòng)學(xué)分析及避障算法的驗(yàn)證。
由于軟體臂在伸長(zhǎng)過程中是采取的先伸長(zhǎng)后彎曲的控制策略,在今后的工作中我們希望能夠在伸長(zhǎng)的同時(shí)實(shí)現(xiàn)彎曲,使得整個(gè)軟體機(jī)械臂運(yùn)動(dòng)地更加柔順,在未來的工作里我們將搭建實(shí)物軟體臂來進(jìn)行仿真和實(shí)驗(yàn)的相互驗(yàn)證。