劉艷梨 吳洪濤 李 耀 王若冰 徐媛媛 陳 柏

(1.南京航空航天大學(xué)機(jī)電學(xué)院， 南京 210016； 2.江蘇安全技術(shù)職業(yè)學(xué)院機(jī)械工程系， 徐州 221011；3.南通理工學(xué)院機(jī)械工程學(xué)院， 南通 226002)

0 引言

現(xiàn)有的求解6-UPS并聯(lián)機(jī)器人正向運(yùn)動(dòng)學(xué)方法[1]可概括為多項(xiàng)式、添加傳感器、數(shù)值迭代3類方法，這些方法不具備實(shí)時(shí)反饋的能力。多項(xiàng)式方法可以通過采用對(duì)偶四元數(shù)方法[2]、特殊的幾何性質(zhì)[3]、Gr?bner基算法[4-5]、區(qū)間分析[6]和代數(shù)消元[7-8]等方法求解非線性方程組，化簡得到一元40、20、17、14次的代數(shù)方程，最終得到所有可能解，包括無理復(fù)根和增根的所有解，運(yùn)動(dòng)學(xué)正解多解對(duì)應(yīng)多種裝配模式，且計(jì)算時(shí)間長，給實(shí)時(shí)反饋控制帶來困難。文獻(xiàn)[9]采用方位特征集方法得到了全解析算法，解決了多解容易產(chǎn)生增根、出現(xiàn)失根問題。傳感器方法通過添加4個(gè)[10]、3個(gè)[11]、2個(gè)[12]甚至1個(gè)傳感器[13-14]來輔助計(jì)算正向運(yùn)動(dòng)學(xué)，能夠得到較好結(jié)果，但是，添加傳感器不但增加了硬件成本，還引入傳感器測(cè)量誤差等問題。文獻(xiàn)[13]采用傳感器方法得到唯一位姿，但是推導(dǎo)過程繁瑣，計(jì)算時(shí)間較長。數(shù)值迭代方法關(guān)鍵是選取一個(gè)合適的初始值，如果初值選取得當(dāng)，使用牛頓-拉夫森迭代方法可以得到唯一解[15-18]，否則會(huì)影響系統(tǒng)的魯棒性，同時(shí)無法得到所有的裝配模式。文獻(xiàn)[19]采用四元數(shù)得到了正向運(yùn)動(dòng)學(xué)的快速數(shù)值解，消除了姿態(tài)矩陣的數(shù)學(xué)奇異性，是一個(gè)較好的數(shù)值方法。文獻(xiàn)[20]研究了冗余驅(qū)動(dòng)的數(shù)值解，消除了算法中位置和姿態(tài)的數(shù)學(xué)奇異性。文獻(xiàn)[21]采用齊次坐標(biāo)變換求解反向運(yùn)動(dòng)學(xué)。文獻(xiàn)[22]采用由點(diǎn)和方位矢量形成的自然坐標(biāo)法來描述多體系統(tǒng)的位置，而沒有使用角度坐標(biāo)。文獻(xiàn)[23]利用鏈路的自然坐標(biāo)來描述相鄰鏈路的相對(duì)運(yùn)動(dòng)，這些自然坐標(biāo)僅描述機(jī)構(gòu)與所選擇的相對(duì)系統(tǒng)之間的關(guān)系，而不管機(jī)構(gòu)的運(yùn)動(dòng)學(xué)結(jié)構(gòu)。文獻(xiàn)[24]通過選擇合適的點(diǎn)和方位矢量形成的自然坐標(biāo)描述多體系統(tǒng)，建立了一個(gè)合適的模型。文獻(xiàn)[25]提出了用自然坐標(biāo)表示的致動(dòng)器的驅(qū)動(dòng)力公式。采用自然坐標(biāo)方法描述系統(tǒng)運(yùn)動(dòng)學(xué)，可有利于建立二次或線性的運(yùn)動(dòng)學(xué)方程組，能避免三角函數(shù)等超越函數(shù)計(jì)算。

本文將并聯(lián)機(jī)器人運(yùn)動(dòng)參數(shù)選為若干點(diǎn)的坐標(biāo)，選擇平面平臺(tái)型6-UPS并聯(lián)機(jī)器人動(dòng)平臺(tái)平面中2個(gè)動(dòng)坐標(biāo)軸端點(diǎn)和坐標(biāo)軸原點(diǎn)為代表點(diǎn)，將并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)表示為3個(gè)代表點(diǎn)坐標(biāo)的函數(shù)形式，建立9個(gè)一次和二次多項(xiàng)式方程，進(jìn)行消元處理，得到6個(gè)二次多項(xiàng)式方程，在此基礎(chǔ)上，改進(jìn)牛頓-拉夫森迭代算法，最終求解得到唯一一組位置和姿態(tài)參數(shù)。

1 6-UPS并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)基礎(chǔ)

1.1 結(jié)構(gòu)

6-UPS并聯(lián)機(jī)器人及其坐標(biāo)系如圖1所示。該機(jī)構(gòu)包括動(dòng)、靜上下2個(gè)平臺(tái)和6條結(jié)構(gòu)一致支腿組成，各個(gè)支腿和動(dòng)平臺(tái)之間通過球鉸S連接，和靜平臺(tái)之間通過虎克鉸U連接，支腿上有移動(dòng)副P，通過對(duì)P施加驅(qū)動(dòng)，來保證整個(gè)平臺(tái)的期望運(yùn)動(dòng)。該機(jī)構(gòu)屬于平面平臺(tái)型，即動(dòng)、靜平臺(tái)的6個(gè)球鉸S和6個(gè)虎克鉸U中心分別布置在兩個(gè)平面之中。為了方便分析，選擇絕對(duì)靜坐標(biāo)系Obxyz與靜平臺(tái)固連，相對(duì)動(dòng)坐標(biāo)系Oaαβγ與動(dòng)平臺(tái)固連，其中，Oa、Ob分別是動(dòng)、靜平臺(tái)的外接圓圓心；z、γ軸分別垂直于各自所在平面。動(dòng)、靜平臺(tái)6對(duì)頂點(diǎn)Ai、Bi(i=1,2，…，6)分別循環(huán)對(duì)稱布置在一個(gè)平面圓周上，如圖2所示。

圖1 6-UPS并聯(lián)機(jī)器人簡圖Fig.1 Structure diagram of 6-UPS parallel robot

圖2 6-UPS并聯(lián)機(jī)器人的頂點(diǎn)布置示意圖Fig.2 Vertices arrangement schematic of 6-UPS parallel robot

選擇3點(diǎn)P1、P2、P3為代表點(diǎn)，分別位于動(dòng)坐標(biāo)系原點(diǎn)Oa、α軸端點(diǎn)和β軸的端點(diǎn)處，如圖1所示。用P1=(x1,y1,z1)T、P2=(x2,y2,z2)T、P3=(x3,y3,z3)T分別表示3個(gè)代表點(diǎn)P1、P2、P3在靜坐標(biāo)系Obxyz中的位置坐標(biāo)，于是正向運(yùn)動(dòng)學(xué)模型中含有9個(gè)待求變量。

1.2 基于代表點(diǎn)坐標(biāo)的位置P和姿態(tài)R

P2、P3和P1存在如下關(guān)系：P2=P1+R·x，P3=P1+R·y。動(dòng)平臺(tái)動(dòng)坐標(biāo)系Oaαβγ的α、β、γ軸對(duì)應(yīng)的矢量記為：α=(αx,αy,αz)T，β=(βx,βy,βz)T，γ=(γx,γy,γz)T。則矢量α、β、γ用代表點(diǎn)P1、P2、P3坐標(biāo)表示后可得

α=P2-P1=(x2,y2,z2)T-(x1,y1,z1)T= ((x2-x1),(y2-y1),(z2-z1))T

(1)

同理可得

β=((x3-x1),(y3-y1),(z3-z1))T

(2)

由于γ垂直于α和β所決定的平面，所以存在

(3)

靜坐標(biāo)系基矢量為：x=(1,0,0)T；y=(0,1,0)T；z=(0,0,1)T。采用代表點(diǎn)表示，動(dòng)平臺(tái)位置在靜坐標(biāo)系中的位置矢量為P=P1=(x1,y1,z1)T；姿態(tài)矩陣R表示為

(4)

根據(jù)式(1)～(3)可得αx=x2-x1、αy=y2-y1、αz=z2-z1、βx=x3-x1、βy=y3-y1、βz=z3-z1、γx=y1z2-y1z3-y2z1+y2z3+y3z1-y3z2、γy=-x1z2+x1z3+x2z1-x2z3-x3z1+x3z2、γz=x1y2-x1y3-x2y1+x2y3+x3y1-x3y2。

2 6-UPS并聯(lián)機(jī)器人位置矢量方程構(gòu)建

Lkek=P+Rak-bk(k=1,2，…,6)

(5)

其中

ak=(akα,akβ,akγ)T

bk=(bkx,bky,bkz)T

式中Lk——第k個(gè)連桿桿長

ek——第k個(gè)連桿單位矢量

ak——?jiǎng)幼鴺?biāo)系Oaαβγ中動(dòng)平臺(tái)各個(gè)頂點(diǎn)Ak坐標(biāo)值

bk——靜坐標(biāo)系Obxyz中靜平臺(tái)各個(gè)頂點(diǎn)Bk坐標(biāo)值

圖1所示機(jī)構(gòu)，因動(dòng)、靜平臺(tái)均為平面布置，所以ak、bk的γ軸和z軸分量均為0，即akγ=bkz=0，也即ak=(akα,akβ,0)T，bk=(bkx,bky,0)T，可見動(dòng)、靜平臺(tái)的頂點(diǎn)坐標(biāo)也可以通過4個(gè)變量ra、rb、θa、θb來表示，各點(diǎn)坐標(biāo)進(jìn)一步表示如下

(6)

其中

令W為動(dòng)平臺(tái)在動(dòng)坐標(biāo)系中的位置矢量，W=(Wx,Wy,Wz)T，則存在P=RW，結(jié)合R的正交性，同時(shí)成立：W=RTP。將ak、bk、P、R和W代入式(5)，兩邊與自身進(jìn)行矢量點(diǎn)乘，得到6個(gè)桿長平方標(biāo)量方程式如下(此處省略下標(biāo)k)

(7)

其中Px=P·xPy=P·yWx=P·αWy=P·β

αx=α·xαy=α·yβx=β·xβy=β·y

式中Pp——位置矢量P模長平方

Px——P在x方向上的投影

Py——P在y方向上的投影

Wx——P在α方向上的投影

Wy——P在β方向上的投影

αx——α在x方向上的投影

αy——α在y方向上的投影

βx——β在x方向上的投影

βy——β在y方向上的投影

式中，含Pp、Px、Py、Wx、Wy、αx、αy、βx、βy等9個(gè)未知數(shù)，各個(gè)未知數(shù)之間由動(dòng)平臺(tái)的位置和姿態(tài)各個(gè)參數(shù)聯(lián)系起來，且9個(gè)未知數(shù)的系數(shù)由平臺(tái)結(jié)構(gòu)參數(shù)和桿長參數(shù)決定。

3 正向運(yùn)動(dòng)學(xué)模型構(gòu)造并求解

正向運(yùn)動(dòng)學(xué)問題就是已知機(jī)構(gòu)的6個(gè)桿長輸入，求解末端運(yùn)動(dòng)平臺(tái)的位置矢量P和姿態(tài)矩陣R。此處首先將正向運(yùn)動(dòng)學(xué)問題轉(zhuǎn)換為構(gòu)造和求解6個(gè)二次多項(xiàng)式方程問題。

3.1 運(yùn)動(dòng)學(xué)模型構(gòu)造

由式(7)可得，通過對(duì)Pp、Px、Py、Wx、Wy、αx、αy、βx、βy9個(gè)未知數(shù)進(jìn)行移項(xiàng)、整理，可解得

(8)

其中

(9)

將9個(gè)未知數(shù)的代表點(diǎn)表達(dá)式代入式(8)后可得6個(gè)一次或二次多項(xiàng)式方程組

(10)

(11)

(12)

(13)

(14)

(15)

又由α和β的正交性和歸一性

α·β=0α·α=1β·β=1

可得9個(gè)未知數(shù)的代表點(diǎn)約束方程

(x2-x1)(x3-x1)+(y2-y1)(y3-y1)+ (z2-z1)(z3-z1)=0

(16)

(x2-x1)2+(y2-y1)2+(z2-z1)2=1

(17)

(x3-x1)2+(y3-y1)2+(z3-z1)2=1

(18)

觀察式(11)、(12)、(15)最高次為一次多項(xiàng)式，本文先求出x3、y2、y3對(duì)x1、y1、z1、x2、z2、z3的線性表示，再代回剩余的6個(gè)方程，可得到式(10)、(13)、(14)、(16)～(18)的6個(gè)二次多項(xiàng)式方程，將其記為

(19)

其中X為待求未知數(shù)，X=(x1,y1,z1,x2,z2,z3)，Ci是由6-UPS并聯(lián)機(jī)器人結(jié)構(gòu)參數(shù)和6條支腿長決定；Mi由6-UPS并聯(lián)機(jī)器人結(jié)構(gòu)參數(shù)所決定；Qi(i=1,2,…,6)是二次項(xiàng)系數(shù)矩陣，是由系統(tǒng)結(jié)構(gòu)參數(shù)中3個(gè)參數(shù)ra、θa、θb所決定的6×6維對(duì)稱矩陣，Qi的具體形式為

將式(19)代表的6個(gè)方程統(tǒng)一表示為向量形式

(20)

其中F(X)=(f1(X),f2(X),…,f6(X))T

Q=(Q1,Q2,…,Q6)TM=(M1,M2,…,M6)TC=(C1,C2,…,C6)T

(21)

(22)

3.2 正解運(yùn)動(dòng)學(xué)方程數(shù)值迭代求解

對(duì)二次多項(xiàng)式方程組(20)求導(dǎo)，可得

現(xiàn)在考慮牛頓-拉夫森數(shù)值迭代算法，當(dāng)待求未知變量X取第k步數(shù)值，即：X=xk時(shí)，可得

因此可得

(23)

根據(jù)一般牛頓-拉夫森數(shù)值迭代算法

(24)

將式(23)代入X=xk時(shí)的方程組(20)可得

(25)

將式(25)代回式(24)一般牛頓-拉夫森數(shù)值迭代算法可得

(26)

(27)

由式(26)和式(27)聯(lián)合可得一類代表點(diǎn)表示的6-UPS并聯(lián)機(jī)器人的快速正向運(yùn)動(dòng)學(xué)迭代算法

(28)

據(jù)此，可解出唯一一組變量x1、y1、z1、x2、z2、z3的解，根據(jù)P=(x1,y1,z1)T和姿態(tài)矩陣R(式(4))可以求解出唯一的位置和姿態(tài)。

4 數(shù)值算例驗(yàn)證

采用數(shù)值算例來說明和驗(yàn)證前文方法的正確性、高效性。

(1)正確性

通過給定一組反解，即給定平臺(tái)位置姿態(tài)，計(jì)算對(duì)應(yīng)的代表點(diǎn)坐標(biāo)值。將給定的一組初始代表點(diǎn)坐標(biāo)值代入式(28)進(jìn)行運(yùn)動(dòng)學(xué)正解計(jì)算，如果能夠收斂、且收斂到反解對(duì)應(yīng)的代表點(diǎn)坐標(biāo)值，即可驗(yàn)證所提算法的正確性。

① 給定反解條件。6-UPS并聯(lián)機(jī)器人結(jié)構(gòu)參數(shù)θa=0.286 18π、θb=0.046 988π、ra=0.849 864、rb=0.849 864，由此可以確定6-UPS并聯(lián)機(jī)器人動(dòng)、靜平臺(tái)的頂點(diǎn)坐標(biāo)ai、bi(i=1，2，…，6)。角度單位為rad，長度單位為m。設(shè)定6-UPS并聯(lián)機(jī)器人預(yù)設(shè)狀態(tài)下位置矢量為Pt=(-0.2,-0.03,1.1)T，姿態(tài)矩陣Rt為

可得到P1=(-0.2,-0.03,1.1)T，通過P2=P1+Rt(1,0,0)T，P3=P1+Rt(0,1,0)T計(jì)算可得另外兩個(gè)代表點(diǎn)坐標(biāo)分別為：P2=(0.769 017，-0.194 971,1.283 82)T，P3=(-0.028 091 9，0.954 859,1.077 65)T，通過反解計(jì)算得到6條支腿長為：L1=1.516 92 m、L2=1.318 95 m、L3=1.268 81 m、L4=1.136 69 m、L5=1.257 04 m、L6=1.209 43 m。

根據(jù)課題組研制的物理樣機(jī)，其構(gòu)型選擇的電缸行程為0.5 m，6條支腿的平衡位置均在1.36 m處，即6條支腿的運(yùn)動(dòng)范圍均在(1.360.25) m范圍內(nèi)，即：關(guān)節(jié)空間范圍為Li∈[1.11,1.61]，i=1,2,…,6，進(jìn)而可以驗(yàn)證Pt、Rt選取正確。

② 求解正解。首先，將6-UPS并聯(lián)機(jī)器人結(jié)構(gòu)參數(shù)和腿長參數(shù)代入前述的C、M、Q，可得

假設(shè)6-UPS并聯(lián)機(jī)器人動(dòng)平臺(tái)從初始位姿：P0=(0.5,0.5,2)T，R0=I3×3出發(fā)，運(yùn)動(dòng)到反解給定的位姿：Pt、Rt。那么，對(duì)應(yīng)的始末位姿代表點(diǎn)坐標(biāo)值分別為：X0=(0.5,0.5,2,1.5,2,2)T、Xt=(-0.2,-0.03,1.1,0.769 017,1.283 82,1.077 65)T。

迭代過程如表1所示，經(jīng)過5次迭代計(jì)算后得到的X5=(-0.2,-0.03,1.1,0.769 017,1.283 82,1.077 65)T與反解給定的Xt=(-0.2,-0.03,1.1,0.769 017,1.283 82,1.077 65)T完全一致，驗(yàn)證了所提算法是正確的，機(jī)構(gòu)位姿示意如圖3所示。

本次計(jì)算中，所提方法迭代時(shí)間為0.20 ms，對(duì)比計(jì)算表明，采用傳統(tǒng)歐拉角方法描述的牛頓-拉夫森方法，則迭代次數(shù)8次、需要耗時(shí)2.67 ms，由此可見本文所提出的二次形方程組數(shù)值迭代方法的迭代次數(shù)少，用時(shí)少，收斂速度快，效率提高92.51%，至此，初步顯示出算法的高效性。

(2) 高效性

與旋轉(zhuǎn)矩陣方法的計(jì)算進(jìn)行對(duì)比，即可驗(yàn)證所提算法的高效性。所提算法和旋轉(zhuǎn)矩陣方法均在以下條件下進(jìn)行計(jì)算，計(jì)算機(jī)硬件為：Win 8.1/Intel Core i5 2.4 GHz/8 GB RAM/MatlabR2016b，代表點(diǎn)初始值取X0=(0.5,0.5,2,1.5,2,2)T，最大迭代次數(shù)均為20次，算法終止條件允許誤差‖Δx‖<10-8。

表1 迭代過程及其結(jié)果Tab.1 Iterative process and results

圖3 位置和姿態(tài)示意圖Fig.3 Schematic of position and orientation for 6-UPS parallel robot

為了進(jìn)一步驗(yàn)證所提算法的高計(jì)算效率，根據(jù)課題組研制的物理樣機(jī)，在其可達(dá)無奇異工作空間內(nèi)任意選取25組數(shù)據(jù)。首先，25組初始位置姿態(tài)P0i、R0i均取為P0、R0，其具體數(shù)值為：P0=(0.5,0.5,2)T，R0=I3×3，轉(zhuǎn)換成代表點(diǎn)表示為：X0=(0.5,0.5,2,1.5,2,2)T。其次，針對(duì)表2中給定的25組數(shù)據(jù)，其每一組數(shù)據(jù)均是從初始位姿：P0、R0開始，分別代入所提算法，重復(fù)(1)的反解、正解過程并進(jìn)行計(jì)算，動(dòng)平臺(tái)最終分別運(yùn)動(dòng)到表2中給定的25組終止位置姿態(tài)，即25組Pti、Rti處。經(jīng)過計(jì)算可知，25組數(shù)據(jù)計(jì)算結(jié)果均正確且收斂，同時(shí)也得到每一組數(shù)據(jù)計(jì)算所需時(shí)間，并將其與旋轉(zhuǎn)矩陣方法計(jì)算反解、正解過程所需時(shí)間進(jìn)行對(duì)比。每一組位姿在Matlab中計(jì)算100次并取平均數(shù)值即為本次計(jì)算時(shí)間，結(jié)果如表2所示。

表2 兩種算法計(jì)算時(shí)間對(duì)比Tab.2 Consumption time comparison of two algorithms

5 結(jié)論

(1)采用代表點(diǎn)描述可將并聯(lián)機(jī)器人正向運(yùn)動(dòng)學(xué)模型表達(dá)為一組二次方程組。該模型含有二次項(xiàng)、一次項(xiàng)和常數(shù)項(xiàng)，最高次項(xiàng)為二次，求導(dǎo)后得到的雅可比矩陣僅含有一次項(xiàng)和常數(shù)項(xiàng)，最高次項(xiàng)為一次，建立了簡單非線性的并聯(lián)機(jī)器人正向運(yùn)動(dòng)學(xué)模型。

(2)結(jié)合正運(yùn)動(dòng)學(xué)模型的特點(diǎn)，對(duì)一般的牛頓-拉夫森方法進(jìn)行改進(jìn)，改進(jìn)后的牛頓-拉夫森迭代方法不需要求解雅可比的逆矩陣；而且，在每一次迭代計(jì)算過程中，部分變量還原到原始二次形，計(jì)算過程中直接抵消了大部分計(jì)算，方程組與雅可比矩陣的更新僅需消耗極少的計(jì)算量，由此確保所提算法收斂速度快、計(jì)算效率高，并可得到唯一解，便于實(shí)時(shí)控制所需。

(3)通過對(duì)比傳統(tǒng)的旋轉(zhuǎn)矩陣方法，本文提出的二次方程組數(shù)值迭代方法的迭代次數(shù)少、用時(shí)少、收斂速度快，效率高。