王 濤， 朱愛東， 陳金兵, 孫中杰

(北京理工大學(xué)自動(dòng)化學(xué)院， 北京 10081)

引言

高速機(jī)器人機(jī)械臂目前已廣泛應(yīng)用在電子、輕工、食品和醫(yī)藥等生產(chǎn)行業(yè)中[1-2]。機(jī)械臂按其機(jī)械結(jié)構(gòu)可以分為并聯(lián)機(jī)械臂和串聯(lián)機(jī)械臂兩類。串聯(lián)機(jī)械臂以串聯(lián)機(jī)構(gòu)為其機(jī)構(gòu)原形，一般由基座、腰部(或肩部)、大臂、小臂、腕部和手部組成并以串聯(lián)方式連接。并聯(lián)機(jī)械臂以并聯(lián)機(jī)構(gòu)為其機(jī)構(gòu)原形，通常由基座、臂部以及連接件組成[3]。并聯(lián)機(jī)械臂的所有驅(qū)動(dòng)機(jī)構(gòu)都可以安裝在基座上或基座附近[4]。并聯(lián)機(jī)械臂采用閉環(huán)結(jié)構(gòu)，相對(duì)串聯(lián)機(jī)械臂而言具有一些獨(dú)特的性能，如剛度大、對(duì)稱性好、承載能力強(qiáng)等[5-6]。氣動(dòng)驅(qū)動(dòng)型并聯(lián)機(jī)械臂具有成本低、動(dòng)作可靠、能源清潔等優(yōu)點(diǎn)。

本研究結(jié)合三自由度并聯(lián)機(jī)械手，由其機(jī)械結(jié)構(gòu)得到了其運(yùn)動(dòng)學(xué)的正解與逆解。接著以二自由度并聯(lián)機(jī)械手為例，實(shí)驗(yàn)驗(yàn)證了協(xié)調(diào)控制算法。

1 三自由度機(jī)械臂的結(jié)構(gòu)布局與坐標(biāo)系建立

如圖1a 所示，結(jié)構(gòu)主要包括1基座，2氣缸，3活動(dòng)底座，4萬向節(jié)，5連接球鉸。3個(gè)萬向節(jié)將氣缸與基座連接起來，在基座上形成1個(gè)等邊三角形，萬向節(jié)可使氣缸實(shí)現(xiàn)旋轉(zhuǎn)與俯仰運(yùn)動(dòng)；3個(gè)氣缸作為機(jī)械臂的驅(qū)動(dòng)副，協(xié)同工作使活動(dòng)底座運(yùn)動(dòng)；3個(gè)連接球鉸在活動(dòng)底座上形成1個(gè)等邊三角形。

圖1 三自由度機(jī)械臂

如圖1b所示，建立OXYZ坐標(biāo)系，O位于基座上萬向節(jié)形成的等邊三角形的中心；OXY平面位于基座平面上；Z軸垂直于基座；OY軸垂直于等邊三角形的某邊，設(shè)為A′B′?；蠴點(diǎn)到3個(gè)萬向節(jié)距離設(shè)為m、活動(dòng)底座上中心點(diǎn)到3個(gè)頂點(diǎn)的距離設(shè)為n、3個(gè)驅(qū)動(dòng)副原長為L0。

2 三自由度機(jī)械臂的運(yùn)動(dòng)學(xué)分析

2.1 位置正解

已知機(jī)構(gòu)的各個(gè)連接關(guān)節(jié)的角度或位置求解出機(jī)構(gòu)末端位姿即為運(yùn)動(dòng)學(xué)的正解[7]。由三自由度機(jī)械臂的布局特點(diǎn)知：

(1)

c(0,-(m-n),0)

S(x,y,z)

式中，m為基座上等邊三角形頂點(diǎn)到其中心的距離；n為活動(dòng)底座上等邊三角形到其中心的距離。令w=m-n,由距離公式可得：

由式(2)可解出S的坐標(biāo)如下：

(3)

式中，L0為氣缸原長；La,Lb,Lc為3個(gè)氣缸的伸長量。

2.2 位置逆解

已知機(jī)構(gòu)的末端位姿推導(dǎo)出各個(gè)連接關(guān)節(jié)的角度或位置即為運(yùn)動(dòng)學(xué)逆解。已知活動(dòng)端坐標(biāo)S(x,y,z)，求各缸伸長量La,Lb,Lc，由式(2)可得下式：

解之可得下式：

2.3 速度模型

動(dòng)平臺(tái)中心S的運(yùn)動(dòng)速度如式(6),則根據(jù)逆解求出三個(gè)氣缸的運(yùn)動(dòng)速度如式(7):

(6)

(7)

將式(5)代入式(7)得:

(8)

3 三自由度機(jī)械臂運(yùn)動(dòng)空間分析

3.1 結(jié)構(gòu)約束

1) 萬向節(jié)轉(zhuǎn)角約束

設(shè)β1,β2,β3分別為aO與aS，bO與bS，cO與cS的夾角。由余弦定理可得下式：

(9)

假設(shè)β1,β2,β3的最小夾角分別是β1min,β2min，β3min，最大夾角分別是β1max,β2max，β3max,則機(jī)械臂的夾角約束為：

(10)

2) 機(jī)械臂的長度約束

設(shè)T為氣缸的行程，則易知機(jī)械臂長度約束滿足下式：

(11)

3.2 奇異位置分析

(12)

其中，

當(dāng)J1非奇異時(shí)，即可得到雅可比矩陣J的表達(dá)式，但其表達(dá)式較為復(fù)雜，這里僅給出簡化的表達(dá)式，如式(14)所示:

(14)

當(dāng)雅可比矩陣行列式值為0時(shí)，并聯(lián)機(jī)械臂出現(xiàn)奇異位形。對(duì)式(14)而言，當(dāng)矩陣J1不可逆時(shí)，機(jī)械臂出現(xiàn)正向運(yùn)動(dòng)學(xué)奇異。從數(shù)學(xué)上求解方程det(J1)=0并找出所有正向奇異位形點(diǎn)是很困難的，可從機(jī)械結(jié)構(gòu)的幾何性質(zhì)上直觀找出所有奇異位形點(diǎn)。

當(dāng)3氣缸共面，即z=0時(shí)，會(huì)出現(xiàn)奇異構(gòu)型，但從機(jī)械角度分析不會(huì)出現(xiàn)這種情況。

4 二自由度機(jī)械臂協(xié)調(diào)運(yùn)動(dòng)控制實(shí)驗(yàn)

以并聯(lián)機(jī)械手末端動(dòng)平臺(tái)直線運(yùn)動(dòng)為例進(jìn)行雙氣缸的協(xié)調(diào)運(yùn)動(dòng)控制，為了使動(dòng)平臺(tái)受到的2個(gè)氣缸的力平衡且保持水平姿態(tài)，需要使2個(gè)氣缸的位移時(shí)刻滿足運(yùn)動(dòng)學(xué)約束關(guān)系，基于此研究了雙氣缸協(xié)調(diào)運(yùn)動(dòng)控制算法[8]，實(shí)驗(yàn)裝置如圖2所示。

1.電/氣比例閥 2.直線導(dǎo)軌 3.軸承滑塊 4.氣缸 5.位移傳感器 6.輔助伸縮桿 7.輔助桿連接件 8.動(dòng)平臺(tái) 9.固定彩筆 10.定平臺(tái)

本研究使用兩個(gè)低摩擦氣缸作為運(yùn)動(dòng)器件，并采用流量型電/氣比例伺服閥作為驅(qū)動(dòng)單元。使用STM32F103作為控制器，PC即作為上位機(jī)進(jìn)行發(fā)送控制指令和顯示相關(guān)信息。氣缸的運(yùn)動(dòng)控制過程為首先嵌入式控制器根據(jù)上位機(jī)接收的目標(biāo)信號(hào)與傳感器信號(hào)進(jìn)行比較并通過控制算法輸出相應(yīng)控制量，控制量經(jīng)過D/A轉(zhuǎn)換和信號(hào)放大驅(qū)動(dòng)電/氣比例伺服閥，使氣缸按照設(shè)定的目標(biāo)任務(wù)運(yùn)動(dòng)，通過實(shí)時(shí)采集氣缸的運(yùn)動(dòng)狀態(tài)和實(shí)時(shí)控制實(shí)現(xiàn)氣缸的運(yùn)動(dòng)控制。

4.1 兩機(jī)械臂協(xié)調(diào)控制算法

首先明確并聯(lián)機(jī)構(gòu)的運(yùn)動(dòng)目標(biāo)，根據(jù)運(yùn)動(dòng)目標(biāo)進(jìn)行軌跡規(guī)劃確定運(yùn)動(dòng)路徑?；诓⒙?lián)機(jī)械手的運(yùn)動(dòng)學(xué)分析和支鏈間的約束關(guān)系，分別對(duì)每個(gè)氣缸進(jìn)行運(yùn)動(dòng)學(xué)逆解，得到并聯(lián)機(jī)械手動(dòng)平臺(tái)在對(duì)應(yīng)運(yùn)動(dòng)軌跡上的每個(gè)氣缸的期望位移，并在對(duì)應(yīng)的位姿下對(duì)每個(gè)氣缸進(jìn)行控制，各個(gè)氣缸控制器根據(jù)期望位移與實(shí)際位移的的誤差來驅(qū)動(dòng)[9]。

考慮到并聯(lián)雙氣缸運(yùn)動(dòng)過程中的相互影響，應(yīng)采用以下協(xié)調(diào)控制方法：通過2個(gè)氣缸位移的誤差進(jìn)行對(duì)比，按照一定的比例對(duì)2個(gè)氣缸的控制量進(jìn)行補(bǔ)償[10-11]。在雙氣缸的控制器中相互含有對(duì)方的信息，實(shí)現(xiàn)了2個(gè)氣缸控制量之間的通訊，從而使得2個(gè)氣缸的位移誤差之差變小，2個(gè)氣缸的運(yùn)動(dòng)趨于同步，實(shí)現(xiàn)2個(gè)氣缸的協(xié)調(diào)運(yùn)動(dòng)。

SL和SR是左右2個(gè)氣缸的期位移一輸入，fL,fR是左右2個(gè)位移傳感器檢測的氣缸位移，氣缸控制器的輸入為2個(gè)氣缸的位移誤差分別是eL和eR,輸出分別是ωL和ωR;協(xié)調(diào)控制器的輸入為eL和eR,輸出為σL和σR；控制系統(tǒng)的控制變量輸出為τL和τR協(xié)調(diào)控制器兩個(gè)氣缸誤差分配式如下：

(15)

(16)

式(9)和式(10)中sgn(eL·eR)是符號(hào)參數(shù),其表達(dá)式如式(11)。Kc是氣缸運(yùn)動(dòng)的協(xié)調(diào)比例系數(shù),是氣缸的協(xié)調(diào)加權(quán)系數(shù)，取值范圍為[0,1]。

(17)

由上可得并聯(lián)機(jī)械手中兩個(gè)氣缸的協(xié)調(diào)運(yùn)動(dòng)控制輸出為:

(X=L,R)

(18)

根據(jù)以上分析過程可得到雙氣缸協(xié)調(diào)控制原理圖，如圖3所示。

4.2 正方形軌跡實(shí)驗(yàn)

為了驗(yàn)證協(xié)調(diào)控制算法的有效性，我們讓并聯(lián)機(jī)械手沿正方形軌跡進(jìn)行運(yùn)動(dòng)，機(jī)械手末端動(dòng)平臺(tái)運(yùn)動(dòng)速度為120 mm/s，通過運(yùn)動(dòng)學(xué)正解得出動(dòng)平臺(tái)中心點(diǎn)的運(yùn)動(dòng)軌跡如圖4所示，圖5～圖8為對(duì)應(yīng)AB，BC，CD，DA軌跡段實(shí)跡軌跡與期望軌跡的偏差。

圖3 并聯(lián)機(jī)械手協(xié)調(diào)控制原理圖

圖4 方形期望運(yùn)動(dòng)軌跡與實(shí)際運(yùn)動(dòng)軌跡

圖5 AB段實(shí)際軌跡與期望軌跡偏差曲線圖

圖6 BC段實(shí)際軌跡與期望軌跡偏差曲線

4.3 圓形軌跡實(shí)驗(yàn)

機(jī)械手末端動(dòng)平臺(tái)中心點(diǎn)的運(yùn)動(dòng)軌跡如圖9中圓形，機(jī)械手末端動(dòng)平臺(tái)運(yùn)動(dòng)速度為120 mm/s，通過運(yùn)動(dòng)學(xué)正解得出動(dòng)平臺(tái)中心點(diǎn)的運(yùn)動(dòng)軌跡如圖9所示，圖10為實(shí)跡軌跡與期望軌跡的偏差，即實(shí)際軌跡對(duì)應(yīng)θ角上點(diǎn)到圓心距離與期望圓半徑的差值。

圖7 CD段實(shí)際軌跡與期望軌跡偏差曲線

圖8 DA段實(shí)際軌跡與期望軌跡偏差曲線

圖9 圓形期望運(yùn)動(dòng)軌跡與實(shí)際運(yùn)動(dòng)軌跡

圖10 圓形實(shí)際軌跡與期望軌跡的偏差

通過對(duì)并聯(lián)機(jī)械手方形軌跡運(yùn)動(dòng)和圓形軌跡運(yùn)動(dòng)分析，機(jī)械手末端動(dòng)平臺(tái)最大運(yùn)動(dòng)位置偏差為4 mm，此運(yùn)動(dòng)誤差可能是由機(jī)械關(guān)節(jié)連接處存在微小間隙引起。

5 結(jié)論

本研究主要研究了一種新型并聯(lián)機(jī)器人的機(jī)械結(jié)構(gòu)設(shè)計(jì)，并分析了機(jī)械結(jié)構(gòu)的運(yùn)動(dòng)學(xué)正解、逆解與工作空間。針對(duì)并聯(lián)機(jī)構(gòu)的約束性，設(shè)計(jì)了一種協(xié)調(diào)控制算法，并在二自由度并聯(lián)機(jī)械臂進(jìn)行了畫圓與畫方的實(shí)驗(yàn)。

首先根據(jù)三自由度氣動(dòng)并聯(lián)機(jī)器人的機(jī)械結(jié)構(gòu)及運(yùn)動(dòng)特征得到了機(jī)械臂末端的運(yùn)動(dòng)學(xué)正解和逆解，并根據(jù)機(jī)械結(jié)構(gòu)的約束，對(duì)機(jī)械臂的運(yùn)動(dòng)空間進(jìn)行了分析；接著以二自由度并聯(lián)機(jī)械手為例，對(duì)并聯(lián)機(jī)構(gòu)的運(yùn)動(dòng)協(xié)調(diào)控制算法進(jìn)行了探究；最后通過有無協(xié)調(diào)控制算法的運(yùn)動(dòng)控制實(shí)驗(yàn)，以及圓形軌跡實(shí)驗(yàn)驗(yàn)證了本研究協(xié)調(diào)算法的有效性。機(jī)械結(jié)構(gòu)的設(shè)計(jì)與計(jì)算為實(shí)現(xiàn)三自由度并聯(lián)機(jī)械臂提供了依據(jù)，協(xié)調(diào)控制算法的驗(yàn)證也為后續(xù)設(shè)計(jì)三自由度并聯(lián)機(jī)械臂協(xié)調(diào)控制算法打下了基礎(chǔ)。

協(xié)調(diào)控制算法目前只是針對(duì)二自由度機(jī)械臂的協(xié)調(diào)控制，還沒有設(shè)計(jì)針對(duì)三自由度并聯(lián)機(jī)械臂的控制算法。三自由度并聯(lián)機(jī)械臂相比于二自由度并聯(lián)機(jī)械臂，末端運(yùn)動(dòng)自由度增加、機(jī)械臂間約束增多、運(yùn)動(dòng)空間增大，這些導(dǎo)致協(xié)調(diào)控制算法會(huì)更復(fù)雜，后續(xù)工作還需要針對(duì)三自由度并聯(lián)機(jī)械臂設(shè)計(jì)協(xié)調(diào)控制算法。