6-UPS并聯(lián)機器人快速正向運動學(xué)研究

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

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

0 引言

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

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

1 6-UPS并聯(lián)機器人運動學(xué)基礎(chǔ)

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

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

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

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

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

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

P2、P3和P1存在如下關(guān)系：P2=P1+R·x，P3=P1+R·y。動平臺動坐標(biāo)系Oaαβγ的α、β、γ軸對應(yīng)的矢量記為：α=(αx,αy,αz)T，β=(βx,βy,βz)T，γ=(γx,γy,γz)T。則矢量α、β、γ用代表點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。采用代表點表示，動平臺位置在靜坐標(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)機器人位置矢量方程構(gòu)建

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

(5)

其中

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

bk=(bkx,bky,bkz)T

式中Lk——第k個連桿桿長

ek——第k個連桿單位矢量

ak——動坐標(biāo)系Oaαβγ中動平臺各個頂點Ak坐標(biāo)值

bk——靜坐標(biāo)系Obxyz中靜平臺各個頂點Bk坐標(biāo)值

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

(6)

其中

令W為動平臺在動坐標(biāo)系中的位置矢量，W=(Wx,Wy,Wz)T，則存在P=RW，結(jié)合R的正交性，同時成立：W=RTP。將ak、bk、P、R和W代入式(5)，兩邊與自身進行矢量點乘，得到6個桿長平方標(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個未知數(shù)，各個未知數(shù)之間由動平臺的位置和姿態(tài)各個參數(shù)聯(lián)系起來，且9個未知數(shù)的系數(shù)由平臺結(jié)構(gòu)參數(shù)和桿長參數(shù)決定。

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

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

3.1 運動學(xué)模型構(gòu)造

由式(7)可得，通過對Pp、Px、Py、Wx、Wy、αx、αy、βx、βy9個未知數(shù)進行移項、整理，可解得

(8)

其中

(9)

將9個未知數(shù)的代表點表達式代入式(8)后可得6個一次或二次多項式方程組

(10)

(11)

(12)

(13)

(14)

(15)

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

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

可得9個未知數(shù)的代表點約束方程

(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)最高次為一次多項式，本文先求出x3、y2、y3對x1、y1、z1、x2、z2、z3的線性表示，再代回剩余的6個方程，可得到式(10)、(13)、(14)、(16)～(18)的6個二次多項式方程，將其記為

(19)

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

將式(19)代表的6個方程統(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 正解運動學(xué)方程數(shù)值迭代求解

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

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

因此可得

(23)

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

(24)

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

(25)

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

(26)

(27)

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

(28)

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

4 數(shù)值算例驗證

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

(1)正確性

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

① 給定反解條件。6-UPS并聯(lián)機器人結(jié)構(gòu)參數(shù)θa=0.286 18π、θb=0.046 988π、ra=0.849 864、rb=0.849 864，由此可以確定6-UPS并聯(lián)機器人動、靜平臺的頂點坐標(biāo)ai、bi(i=1，2，…，6)。角度單位為rad，長度單位為m。設(shè)定6-UPS并聯(lián)機器人預(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計算可得另外兩個代表點坐標(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，通過反解計算得到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ù)課題組研制的物理樣機，其構(gòu)型選擇的電缸行程為0.5 m，6條支腿的平衡位置均在1.36 m處，即6條支腿的運動范圍均在(1.360.25) m范圍內(nèi)，即：關(guān)節(jié)空間范圍為Li∈[1.11,1.61]，i=1,2,…,6，進而可以驗證Pt、Rt選取正確。

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

假設(shè)6-UPS并聯(lián)機器人動平臺從初始位姿：P0=(0.5,0.5,2)T，R0=I3×3出發(fā)，運動到反解給定的位姿：Pt、Rt。那么，對應(yīng)的始末位姿代表點坐標(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次迭代計算后得到的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完全一致，驗證了所提算法是正確的，機構(gòu)位姿示意如圖3所示。

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

(2) 高效性

與旋轉(zhuǎn)矩陣方法的計算進行對比，即可驗證所提算法的高效性。所提算法和旋轉(zhuǎn)矩陣方法均在以下條件下進行計算，計算機硬件為：Win 8.1/Intel Core i5 2.4 GHz/8 GB RAM/MatlabR2016b，代表點初始值取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èi)任意選取25組數(shù)據(jù)。首先，25組初始位置姿態(tài)P0i、R0i均取為P0、R0，其具體數(shù)值為：P0=(0.5,0.5,2)T，R0=I3×3，轉(zhuǎn)換成代表點表示為：X0=(0.5,0.5,2,1.5,2,2)T。其次，針對表2中給定的25組數(shù)據(jù)，其每一組數(shù)據(jù)均是從初始位姿：P0、R0開始，分別代入所提算法，重復(fù)(1)的反解、正解過程并進行計算，動平臺最終分別運動到表2中給定的25組終止位置姿態(tài)，即25組Pti、Rti處。經(jīng)過計算可知，25組數(shù)據(jù)計算結(jié)果均正確且收斂，同時也得到每一組數(shù)據(jù)計算所需時間，并將其與旋轉(zhuǎn)矩陣方法計算反解、正解過程所需時間進行對比。每一組位姿在Matlab中計算100次并取平均數(shù)值即為本次計算時間，結(jié)果如表2所示。

表2 兩種算法計算時間對比Tab.2 Consumption time comparison of two algorithms

5 結(jié)論

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

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

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