运动学分析

2 高速并联坐标测量机的运动学分析

2.1 引 言

本章主要介绍该并联坐标测量机机构的组成及其特点，利用自由度公式计算了该机构的自由度。对五个驱动支链分别进行了运动学分析，求出五个驱动杆的反解模型，并给出了定平台上虎克转角的数学模型以及驱动杆与动平台之间的速度映射关系。

2.2 坐标测量机结构分析

该坐标测量机是一种新型的五自由度并联机构——4-UPS/UPU机构，动平台可以实现三维移动和两维转动。该并联机构由定平台、动平台以及连接定、动平台的分支等组成。其特征在于：定平台通过四个结构完全相同的驱动分支UPS（虎克铰—移动副—球铰）分支和另一个驱动分支UPU（虎克铰—移动副—虎克铰）分支，与动平台相连接。该机构通过改变五个驱动杆的杆长值，使动平台可在一定范围内实现不同的位置和姿态。

本结构类似Stewart平台，属于闭环机构，自由度计算可采用Kutzbach Grubler公式。

M=6⨯(n-g-1)+∑fi （2.1）

i=1g

式中：M——表示机构自由度数

n——表示构件总数

fi——第i个运动副的相对自由度数

g——运动副总数

坐标测量机机构简图如图2.1所示，本机构中n=12，g=15，∑fi=29，且无复合铰

i=1g

链、虚约束、局部自由度等特殊情况。则

M=6⨯(n-g-1)+∑fi=6⨯(12-15-1)+29=5

i=1g

即本机构的自由度数为5个，与其输入数相等，因此，该机构能够实现确定的运动。

图2.1 并联坐标测量机机构简图 Fig. 2.1 Mechanism diagram of PCMM

2.3 坐标测量机运动学分析

2.3.1 坐标系建立

为方便求解五自由度平台空间位置关系，研究平台的运动规律，首先建立定、动两坐标系。定坐标系OA-XAYAZA（下文以定系{A}来叙述）固定于定平台上，定平台上五个虎克铰不是均匀布置的，其中四个虎克铰位于半径720mm的圆上，另外一个虎克铰中心位于半径780mm处。定平台上虎克铰分布示意图如图2.2所示。第1个虎克铰链点U1 位于定系{A}的YA坐标轴方向，U2、U3、U4、U5均匀布置，间隔θ=π/4分布，半径

ZA方向平行于U5U2U3、U5几何中心上，为rA=720mm，定系{A}的原点OA位于U2、U4、

方向，XA轴垂直定平台向下。动坐标系OB-XBYBZB（下文以动系{B}来叙述）的原点OB位于动平台几何中心，其Z轴通过U1'铰链点（为了计算说明方便，后文将用S1代替U1'），

XB轴垂直动平台向下。

图2.2 定平台上虎克铰分布示意图

Fig. 2.2 Diagram of Hooke joint on stationary platform

2.3.2 坐标转换矩阵

在此引入坐标转换矩阵的概念，是为了阐述空间任意点在一个坐标系到另一个坐标系的关系，以更好的说明空间刚体的姿态。采用Z—Y—X欧拉角表示动系{B}相对于定系{A}的姿态，则动系{B}的当前姿态可用3次有序转动来描述，先绕ZA转α角，再绕Y'轴转β角，最后绕X''转γ角，则动系{B}相对于定系{A}的旋转变换矩阵以欧拉角的形式可以表示为：

⎡cα

⎢sαA

R(α,β,γ)=R(Z,α)R(Y,β)R(X,γ)=B⎢

⎢⎣0

⎡cαcβ=⎢⎢sαcβ⎢⎣-sβ

-sαcα0

0⎤⎡cβ

⎢00⎥⎥⎢1⎥⎦⎢⎣-sβ

0sβ⎤⎡10

⎢0cγ10⎥⎥⎢

0cβ⎥⎦⎢⎣0sγ

0⎤

-sγ⎥⎥ cγ⎥⎦

cαsβsγ-sαcγsαsβsγ+cαcγ

cβsγ

cαsβcγ+sαsγ⎤

sαsβcγ-cαsγ⎥⎥ （2.2）

⎥cβcγ⎦

则动系{B}相对于定系{A}的变换矩阵以欧拉角形式表示为

⎡cαcβ

⎢sαcβ

A⎢BTZYX(α,β,γ)=⎢-sβ

⎢⎣0

cαsβsγ-sαcγsαsβsγ+cαcγ

cβsγ0

cαsβcγ+sαsγsαsβcγ-cαsγ

cβcγ0

XB⎤YB⎥⎥ （2.3） ZB⎥⎥1⎦

其中，APBO=[XBYBZB]为动系{B}原点OB在定系{A}的位置坐标

T

(α,β,γ)为动系{B}在定系{A}的姿态欧拉角。

2.3.3 反解分析

由图2.1可知，在机构定动平台上已经建立了定系{A}和动系{B}。由动平台的位姿求位置反解，则动平台的位姿(XB,YB,ZB,α,β,γ)已知条件，设R为动平台姿态方向余弦矩阵，APBO=[XBYB

ZB]为动系{B}原点在定系{A}中的位置矢量。根据给定机构的各

T

个结构尺寸，利用几何关系，可求出动、定平台各个UPS、UPU分支铰链点在各自坐标系中的值。

定平台上五个铰链点Ui(i=1,2, 5)在{A}中坐标可表示为

A

PUi=

[

A

XUi

A

YUi

A

ZUi

] （2.4）

T

即

⎧APU1=0

⎪A

⎪PU2=[0⎪A

⎨PU3=[0⎪AP=[0⎪U4A⎪⎩PU5=[0

[

rU1yrAcθ

rAsθ]

]

T

TTT

-rAcθ-rAcθrAcθ

rAsθ]

-rAsθ]

-rAsθ]

T

动平台上五个铰链点均匀布置，间隔角度为φ=2π/5，则Si(i=1,2, 5)在动系{B}中的坐标表示为

B

PSi=

[

B

XSi

B

YSi

B

ZSi

]

T

（2.5）

即

⎧BPS1=[rs1x⎪B

⎪PS2=[0⎪B

⎨PS3=[0⎪BP=[0⎪S4B⎪⎩PS5=[0

0rs1z]rBsφ

T

T

T

rBs(2φ)-rBc(2φ)]

-rBcφ]

rBs(4φ)-rBc(4φ)]

rBs(3φ)-rBc(3φ)]

TT

动平台上铰链点Si(i=1,2, 5)在定系{A}中的位置矢量为

A

PSi=

[

A

XSi

A

YSi

A

ZSi

]

T

（2.6）

则可以得到5个驱动杆长在定系{A}中的矢量Li=(AXSi-AXUi,AYSi-AYUi,AZSi-AZUi)T，即

Li=APSi-APUi=RBPSi+APBO-APUi （2.7）

其中，i=1,2, 5。

可用Z-Y-X欧拉角(α,β,γ)表示方向余弦矩阵R，由于本机构约束γ=90︒，则R简化如下：

⎡cαcβ

AR=BRZ-Y-X(α,β,γ=90︒)=⎢⎢sαcβ

⎢⎣-sβ

cαsβsαsβcβ

sα⎤

-cα⎥⎥ （2.8） 0⎥⎦

根据5个驱动杆的动平台铰链点在定系{A}下的坐标和已知的定平台上虎克铰点的坐标，即可求得各驱动杆长度为

li=Li=

A

PSi-APUi=(AXSi-AXUi)2+(AYSi-AYUi)2+(AZSi-AZUi)2 （2.9）

其中，i=1,2, 5。

由上式，可以将测量机工作空间中规划好的动平台中心点的位姿，转化为关节空间中的驱动杆杆长，从而实现对测头位姿的控制。 2.3.4 驱动支链的分析

采用单开链分析方法对驱动链进行运动学分析并建立运动学模型，即将机构划分成5个单开链（1个UPU，4个UPS支链）。每一个驱动链以定平台坐标系原点作为起始点，到动平台相对应的球铰中心或虎克铰中心结束，支链包括一个虎克铰，一根驱动杆和一个球铰（或虎克铰）。

在驱动链上建立坐标系，如图2.3所示。Ui-x0y0z0坐标系（见图2.2）建立在虎克铰链Ui中心上，x0轴方向竖直向下，z0轴沿着铰链的固定转动轴线，y0轴沿着铰链的另一个转动轴线。将Ui-x0y0z0坐标系首先绕z轴旋转角度θ1i，然后绕y轴旋转角度θ2i，则坐标系Ui-x0y0z0转换为坐标系Ui-x1y1z1；再把坐标系Ui-x1y1z1沿x1轴运动li，会得到坐标系Ui-x2y2z2，铰链中心点在坐标系Ui-x2y2z2中的坐标是(000)。

T

图2.3 驱动链分析坐标系建立

Fig. 2.3 Coordinate system of the driving limb

坐标系Ui-x0y0z0相对于定系{A}的变换矩阵为

⎡⎢100xi⎤A0cφ

i-sφiy⎥i0

T=⎢⎢⎢

0sφicφiz⎥⎥ i⎣00

1⎥⎦

坐标系Ui-x1y1z1相对于坐标系Ui-x0y0z0的变换矩阵为

0T=⎡⎢R(z,θ1i)R(y,θ2i)0⎤1

⎣01⎥⎦

⎡cθ1i

-sθ1i0⎤

其中，R(z,θ1i)=⎢⎢sθ1i

cθ1i0⎥⎢⎣0

1⎥ ⎥⎦

⎡cθ2i

0sθ2i⎤

R(y,θ⎥2i)=⎢⎢0

10⎢⎣-sθ2icθ⎥ 02i⎥⎦

坐标系Ui-x2y2z2相对于坐标系Ui-x1y1z1的变换矩阵为

2.10） 2.11）

（ （

⎡1⎢01⎢2T=⎢0

⎢⎣000li⎤100⎥⎥ （2.12） 010⎥

⎥

001⎦

由式（2.10）、（2.11）、（2.12）可以得到坐标系Ui-x2y2z2相对于定系{A}的变换矩阵为

⎡10

⎢0cφ

iAA01

T2T=⎢2T=0T1

⎢0sφi⎢

⎣00cθ1icθ2i⎡

⎢cφsθcθ+sφsθ

i2i

=⎢i1i2i

⎢sφisθ1icθ2i-cφisθ2i⎢

0⎣

0-sφicφi0-sθ1icφicθ1isφicθ1i

xi⎤⎡10

⎢01yi⎥R(z,θ)R(y,θ)0⎡⎤1i2i⎥⎢

⎢⎥zi⎥⎣01⎦⎢00⎥⎢1⎦⎣00

cθ1isθ2i

cφisθ1isθ2i-sφicθ2isφisθ1isθ2i+cφicθ2i

0li⎤

00⎥⎥ 10⎥

⎥01⎦

⎤

lisθ1icθ2icφi+lisθ2isφi+yi⎥⎥ lisθ1icθ2isφi-lisθ2icφi+zi⎥

⎥

1⎦

licθ1icθ2i+xi

（2.13）

由式（2.13）得到铰链中心（铰链中心为球铰或虎克铰中心）在定系{A}下的坐标矢量为

licθ1icθ2i+xi⎡⎤

⎢lsθcθcφ+lsθsφ+y⎥

i⎥1)T=A001)T=⎢i1i2iii2ii （2.14） 2T(0⎢lisθ1icθ2isφi-lisθ2icφi+zi⎥

⎢⎥

1⎣⎦

(XSiYSiZSi

由式（2.14）得到虎克铰的两个转角的数学模型为

(AYSi-AYUi)cφi+(AZSi-AZUi)sφi

θ1i=arctan （2.15） A

XSi-AXUi(AYSi-AYUi)sφi-(AZSi-AZUi)cφi

θ2i=arcsin （2.16）

li

其中：i=1,2, 5，φi为虎克铰的安装角度。

(AXUi(AXSi

A

YUi

A

ZUi)为铰链安装点Ui在定坐标系{A}下的位置坐标值；

A

YSi

A

ZSi)为铰链安装点Si在定坐标系{A}下的位置坐标值。

定义杆长Li在定系{A}下的三个坐标轴分量为lxi、lyi、lzi，则

⎧lxi=AXSi-AXUi⎪AA

⎨lyi=YSi-YUi （2.17） ⎪l=AZ-AZ

SiUi⎩zi

将式（2.17）代入（2.15）、（2.16），得

θ1i=arctan

lyicφi+lzisφi

lxi

lyisφi-lzicφi

li

（2.18）

θ2i=arcsin

（2.19）

将式（2.18）、（2.19）两端对时间t求导

(l yicφi+l zisφi)lxi-(lyicφi+lzisφi)l xi

θ1i= （2.20） 2

lxi+(lyicφi+lzisφi)2

=θ2i

(l yisφi-l zicφi)li-(lyisφi-lzicφi)l i

lil-(lyisφi-lzicφi)

2i

2

（2.21）

2.3.5 速度雅克比矩阵

并联坐标测量机的驱动杆与动平台之间的速度映射关系，可以通过雅克比矩阵来进行描述。雅克比矩阵是分析和优化测量机性能强有力的工具，通过对雅克比矩阵进行奇异值分解，可以对机构的结构参数进行优化，避免奇异位形的出现。

如图2.4所示，在定系{A}中，vBO为动平台中心点OB的线速度，ωB为动平台的角速度，ri为铰链点Si相对于的OB矢径，vSi为铰点Si的速度，ni为杆Li的单位方向矢量，

l i为杆Li的杆长变化速率，即驱动杆驱动速率，其中i=1,2, ,5。

图2.4 运动速度分析示意图

Fig. 2.4 Velocity diagram of the mechanism

驱动杆分支与动平台连接的铰链点Si的速度vSi可表示如下

vSi=vBO+ωB×r （2.22）

杆Li的驱动速度l i可表示为

l i=vSi·ni=niT

[

⎡v⎤

(ri⨯ni)T⎢BO⎥ （2.23）

⎣ωB⎦

]

对于全部5个驱动支链，有

T

⎡l ⎤⎡n11

⎢ ⎥⎢T⎢l2⎥⎢n2

T⎢l 3⎥=⎢n3

⎢ ⎥⎢T⎢l4⎥⎢n4⎢l ⎥⎢nT⎣5⎦⎣5

(r1⨯n1)T⎤

⎥

(r2⨯n2)T⎥

⎡vBO⎤

(r3⨯n3)T⎥⎢⎥ （2.24）

⎥⎣ωB⎦

(r4⨯n4)T⎥(r5⨯n5)T⎥⎦

=[J]⎡vBO⎤ （2.25） 简记：L⎢ω⎥

⎣B⎦

[]

=l l 其中，L12

T

⎡n1⎢T⎢n2

[J]=⎢n3T

⎢T⎢n4⎢nT⎣5

[][

l 3l 4Tl 5

]

(r1⨯n1)T⎤

⎥

(r2⨯n2)T⎥

n2⎡n1

T⎥=(r3⨯n3)⎢r⨯nr⨯n

2⎣112T⎥(r4⨯n4)⎥

(r5⨯n5)T⎥⎦

n3r3⨯n3

n4r4⨯n4

n5⎤

（2.26） ⎥r5⨯n5⎦

T

雅克比矩阵[J]∈R5⨯6称为并联测量机的速度传递矩阵，表示动平台六维速度与关节驱动速度矢量之间的映射关系。

为保证工作空间和关系空间的一一对应关系，需推导出5×5的速度传递矩阵。 需要注意的是表示平台的姿态参数的欧拉角α，β，γ对时间的导数不是平台的角速度ωx，ωy，ωz，但它们之间存在着一定的关系。

用欧拉角表示的旋转变换矩阵为式（2.8）。 绕任一轴K=kxi+kyj+kzk旋转变换矩阵为

⎡r11r12

R(K,θ)=⎢⎢r21r22

⎢⎣r31r32

r13⎤

r23⎥⎥ （2.27） r33⎥⎦

其中，r11=kxkx(1-cosθ)+cosθ

r12=kykx(1-cosθ)-kzsinθ r13=kzkx(1-cosθ)+kysinθ r21=kxky(1-cosθ)+kzsinθ r22=kyky(1-cosθ)+cosθ r23=kzky(1-cosθ)-kxsinθ r31=kxkz(1-cosθ)-kysinθ r32=kykz(1-cosθ)+kxsinθ

r33=kzkz(1-cosθ)+cosθ

而任一微分转动变换总是可以看成是绕K轴旋转δθ角的微分转动。将上式中的θ用微量δθ代替，由于sinδθ=δθ，cosδθ=1，1-cosδθ=0，则式（2.22）可变为

⎡1

⎢

R(K,δθ)=⎢kzδθ

⎢-kyδθ⎣

-kzδθ1kxδθ

kyδθ⎤

⎥

-kxδθ⎥ （2.28） 1⎥⎦

(t)和微分为求刚体运动的角速度运动矢量ω，先计算旋转变换矩阵R(t)的导数R

∆R，即

R(t+∆t)-R(t)∆R(t) = （2.29） R=limlim∆t∆t∆t→0∆t→0

将R(t+∆t)看成是R(t)经过旋转变换得来的，即

R(t+∆t)=R(K,δθ)R(t) （2.30）

式（2.25）中，微分转角δθ是在∆t时间间隔内绕轴K转动的角度，因此

∆R(t)=R(t+∆t)-R(t)=[R(K,δθ)-I3]R(t)=ΛR(t) （2.31）

⎡0

⎢

其中，Λ=R(K,δθ)-I3=⎢kzδθ

⎢-kyδθ⎣所以，

-kzδθ0kxδθ

kyδθ⎤

⎥

-kxδθ⎥，I3为三维单位矩阵。 0⎥⎦

kθ ⎤⎡0-kzθy

⎥R(t) （2.32） (t)=⎢kθR0-kθx⎥⎢z

kθ ⎢-kyθ0⎥x⎣⎦

将式（2.27）写成如下形式

(t)=S(ω)R(t) （2.33） R

上式中角速度算子矩阵S(ω)为

⎡0⎢S(ω)=⎢ωz

⎢-ωy⎣

而角速度矢量ω定义为 -ωz0ωx kθ ⎤ωy⎤⎡0-kzθy⎥⎢ ⎥ （2.34） -ωx⎥=⎢kzθ0-kxθ⎥ ⎢0⎥0⎥⎦⎣-kyθkxθ⎦

⎤⎡k⎤⎡ωx⎤⎡kxθx⎢⎥⎥=kθ =⎢k⎥θ =Kθ （2.35） ω=⎢ωyyy⎢⎥⎢⎥⎢⎥ ⎥⎣kz⎥⎢⎣ωz⎥⎦⎢⎦⎣kzθ⎦⎢

角速度矢量ω的模等于θ ，瞬时转轴K=kxi+kyj+kzk，方向余弦为(kx,ky,kz)。由 (t)=S(ω)R(t)，可得 于R

R+R R+R R （2.36a） ωx=R[1**********]3

R+R R+R R （2.36b） ωy=R[1**********]3

R+R R+R R （2.36c） ωz=R[1**********]3

根据上式和旋转变换矩阵R(t)的欧拉角表示，即可导出利用欧拉角转速表示的等效角速度矢量

⎤⎡ωx⎤⎡α⎢ω⎥=A⎢β ⎥ （2.37） y⎢⎥⎢⎥⎢⎢⎣ωz⎥⎦⎣γ ⎥⎦

由于γ=90︒，故

⎡ωx⎤ ⎤⎢ω⎥=A⎡α （2.38） ⎢β ⎥⎢y⎥⎣⎦⎢⎣ωz⎥⎦

⎡0-sinα⎤

式中，A=⎢0cosα⎥ ⎢⎥⎢0⎥⎣1⎦

[J]可以表示为[J]=[JvJw]

式中，[Jv]是[J]的前3列组成的5×3矩阵

[Jw]是[J]的后3列组成的5×3矩阵

由此，得到

T⎡n1⎢T⎢n2[J]=⎢n3T⎢T⎢n4⎢nT⎣5(r1,A⨯n1)TA⎤⎥(r2,A⨯n2)TA⎥(r3,A⨯n3)TA⎥ （2.39） ⎥(r4,A⨯n4)TA⎥(r5,A⨯n5)TA⎥⎦

速度雅克比矩阵[J]变成了一个5×5的方阵，可对其进行逆运算。

2.4 本章小结

本章主要研究内容：

（1）确定该并联坐标测量机为4-UPS/UPU机构，通过对该机构的结构分析，得出该机构为五自由度并联机构。

（2）确定了该机构的定坐标系、动坐标系以及它们之间的坐标转换矩阵。

（3）对机构的五个驱动支链进行运动学分析，得出五个驱动杆杆长的反解模型以及定平台上虎克转角的数学模型。

（4）导出该并联机构的速度雅克比矩阵。