王 凌，張 印，謝雨鑫，蒲 偉

（四川大學(xué) 空天科學(xué)與工程學(xué)院，四川 成都 610065）

0 引言

軟體機(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]。

近年來(lái)，國(guó)內(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)證。

但是由于軟體臂具有無(wú)限自由度，不同構(gòu)型間切換時(shí)在運(yùn)動(dòng)學(xué)上存在耦合。比如，從正運(yùn)動(dòng)學(xué)來(lái)看，當(dāng)求解柔性臂正向運(yùn)動(dòng)學(xué)時(shí)，柔性臂每段的參考坐標(biāo)系會(huì)隨之改變。反之，從逆運(yùn)動(dòng)學(xué)來(lái)看，當(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ī)械臂要繞過(guò)狹窄非結(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）提出了一種順序控制方法，通過(guò)順序控制軟體機(jī)械臂的運(yùn)動(dòng)來(lái)避免運(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í)。

1 順序控制思想

受大自然中藤蔓植物生長(zhǎng)過(guò)程的啟發(fā)，提出了一種軟體機(jī)械臂的順序控制思想。這種控制思想的優(yōu)點(diǎn)是在避障時(shí)，柔性臂能夠仿藤蔓生長(zhǎng)的過(guò)程，按照順序生長(zhǎng)運(yùn)動(dòng)。避免柔性機(jī)械臂每段之間存在耦合的問(wèn)題，在非結(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 順序控制思想示意圖

2 運(yùn)動(dòng)學(xué)建模

建立軟體機(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ù)信息

2.1 正運(yùn)動(dòng)學(xué)模型

從機(jī)械臂的兩種狀態(tài)來(lá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é)建模思路完全相同，為說(shuō)明運(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)系為：

2.2 逆運(yùn)動(dòng)學(xué)模型

為建立起機(jī)械臂從工作空間到關(guān)節(jié)空間的映射，需要構(gòu)建軟體臂的逆運(yùn)動(dòng)學(xué)模型。通過(guò)求解齊次變換矩陣中非線性方程組實(shí)現(xiàn)工作空間到關(guān)節(jié)空間的映射，但具有一定挑戰(zhàn)性。為了提高求解可靠性，降低求解難度，在求解逆運(yùn)動(dòng)學(xué)方程時(shí)設(shè)定約束條件來(lái)簡(jiǎn)化求解逆運(yùn)動(dòng)學(xué)方程難度，從而避免在求解逆運(yùn)動(dòng)學(xué)過(guò)程中可能會(huì)產(chǎn)生奇異解甚至無(wú)解的情況。假設(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）。通過(guò)逆運(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)過(guò)程中各段驅(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)證

3 避障

3.1 改進(jìn)型RRT 避障算法

快速探索隨機(jī)樹算法（Rapidly Exploring Random Trees，RRT）是一種基于隨機(jī)采樣的路徑規(guī)劃算法。該算法的工作原理是在空間中從起點(diǎn)向終點(diǎn)進(jìn)行采樣，將滿足要求的采樣點(diǎn)依次連接形成一條像樹枝的路徑。RRT 算法在無(wú)人機(jī)、智能車避障領(lǐng)域中有著廣泛的應(yīng)用。傳統(tǒng)的RRT 算法在空間中隨機(jī)采樣，存在采樣數(shù)據(jù)點(diǎn)過(guò)多、隨機(jī)性較大和整體算法耗時(shí)長(zhǎng)等問(wèn)題。針對(duì)這些問(wèn)題，提出了一種RRT 算法的優(yōu)化方法。

3.2 概率偏向目標(biāo)點(diǎn)

因?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)。

3.3 分段尋跡

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ī)性。

3.4 路徑尋找實(shí)驗(yàn)

仿真使用的計(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)

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)到指定位置。通過(guò)每一段的生長(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ù)信息

5 結(jié)語(yǔ)

針對(duì)軟體機(jī)械臂提出一種順序建模和控制方法，使機(jī)械臂能夠模仿藤蔓生長(zhǎng)的過(guò)程，每段先伸長(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)過(guò)程中是采取的先伸長(zhǎng)后彎曲的控制策略，在今后的工作中我們希望能夠在伸長(zhǎng)的同時(shí)實(shí)現(xiàn)彎曲，使得整個(gè)軟體機(jī)械臂運(yùn)動(dòng)地更加柔順，在未來(lái)的工作里我們將搭建實(shí)物軟體臂來(lái)進(jìn)行仿真和實(shí)驗(yàn)的相互驗(yàn)證。