第二章机电传动系统建模方法

4X1的列向量
A p
1
0
A B
R
0
0
A
pB0 1
B p
1
A
pAB
RB
p
A
pB0
1 1
可简化为： A pABTB p
4X4的方阵 设为： ABT — — 齐次变换。称ABT为齐次变换矩阵。
点P的齐次坐标
ABT的特点：
坐标{B}相对于{A}的 旋转矩阵（3X3）
ABT
0
A B
R
旋转矩阵
坐标系Oxyz为三维空间的固定坐标系， Ouvw为固连在运动杆件上
的动坐标系。空间某点P在Oxyz和Ouvw系中的坐标可表示为：
Poxyz [px py pz ]T Pouvw [pu pv pw ]T
Oxyz和Ouvw系重合
Ouvw系相对Oxyz系旋转
P点在Ouvw系中可表示为：Pouvw puiu pv jv pwkw
0 0 1 2
0
-1
2
1
1 0 3 3
0 1
sin
2
cos 3
位姿坐标变换/一般变换
{B}坐标系的原点在{A} 坐标系中的坐标
A p BAR B p A pB0
齐次变换矩阵
因为在一般变换过程中，复合变换方程为：
A pABRB pApB0 — — 非齐次的。
上式的一种等价的变换形式：
s
Ji
+
1 s2
JL
1
s+Bi
+
1 i2
BL
i
Ti
-
1 i
TL
ωL
=
J
1 es+Be
i
Ti
-
1 i
TL
J
e
=mL
L0 2π
2
Je
d2θi (t) dt2
=Ti
(t)-Be
dθi (t) dt
Jes2θi (s)=Ti (s)-Besθi (s)
θi
(s)=
s
Ti (s) Jes+Be
三个基本旋转矩阵：
Ouvw绕Ox轴旋转α角，则有：
Poxyz Rx, Pouvw
1 Rx, ix iu 0
0
0
cos sin
0
sin
cos
同理，
cos 0 sin
Ry,
0
1
0
-sin 0 cos
cos -sin 0
Rz,
sin
cos
0
0 0 1
R Rz, Ry, Rx, RT Ru, Rv, Rw,
L(s)=
L0 2π
Ti (s)
s J es+Be
θL(s)=
1 Kri2
iTi
(s)-
1+
1 Kri2
JLs+BL Jis+Bi s2
Jis2 +Bis + i2Ji +JL
TL (s) s2 + i2Bi +BL
s
ωL
(s)=
1 Kri2
iTi
(s)-
r3s3
r3c3
r4 s 4 r4c4
3 4
2r2s2
2
r2c
2
记为
（5）建立加速度方程
C D
以杆2的角速度和角加速度为输入，将速度方程对时间求导：
r3s3
r3c3
r4 s 4 r4c4
3 4
2r2
s2
2 r2c
22r2c2 32r3c3 42r4c4 2 22r2s2 32r3s3 42r4s4
解：（1）
1 Poxyz 0
0
0 cos sin
0 1 1 0 0 1 1
sin
2
0
0
-1 2 3
cos 3 0 1 0 3 2
（2）
cos
Poxyz
sin
-sin cos
0 1 0 0
0 cos
0 1 0
sin
2
1
-1 0 1 0 0 0
0 0
0 1 3 -1 2 1
0
0
A
pB0 1
坐标{B}的原点在 {A}坐标系中的坐 标。（3X1）
标示符，“1”位置； “0”方向
即：ABT描述了坐标系 {B}相对于{A}的位置和方位。
齐次变换矩阵 ABT的另一种变形：
ABT
0
A B
R
0
0
A
pB0 1
注意左乘平 移变换矩阵 的意义！
I3x3
0 0 0
A
pB0
BAR
利用点积运算可求得点P在Oxyz系中的坐标：
px Pouvw ix (puiu pviv pwkw )ix py Pouvw jy (puiu pviv pwkw ) jy pz Pouvw kz (puiu pviv pwkw )kz
矩阵形式：ppxy
iu
ix
iu jy
)d12
m2d22
2m2d1d2
cos2
1
(m2d
2 2
m2d1d2
cos2
K2
1 2
m2v22
1 2
m2
d1212
d22
12 212 22
2d1d2 cos2
12 12
P2 m2gd1 cos1 m2gd2 cos1 2
K K1 K2
P P1 P2
L K P
L
1 2
(m1
m2 )d1212
1 2
m2d22 (12
212
22 )
x
R2
o
R3
3
2 R1 y
平面四杆机构
R4
4
Ri 0
将每个连杆表示为一个位移矢量，该矢量由连杆的一端指向另一端。
建模举例
（1）定义连杆矢量 （2）建立封闭矢量方程
R2 R3 R1 R4
（3）建立矢量投影方程 r2c2 r3c3 r1c1 r4c4 r2s2 r3s3 r1s1 r4s4
2. D-H法建立运动学模型
该方法最早由丹纳维特（Denavit）和哈顿贝格（Hartenberg）提出， 在多刚体系统（如机器人）的位置和姿态建模计算中得到广泛应用。
坐标系的建立
为了研究机构各连杆之间的位移关系，可以在每个连杆上固定一个坐 标系，通过坐标系之间的变换，描述相邻连杆之间的关系。
坐标系的建立规则：
机电系统建模与仿真
第二章 机电传动系统建模方法
第一节 机构的数学建模
一、机构的运动学建模
机构运动学研究机构的位置、速度、加速度变量之间的关系，不考 虑产生这一运动的力。机构动力学研究机构的运动与产生这些运动的力 和力矩之间的关系。机构运动学和机构动力学建模和仿真分析是实现机 构运动控制的基础。
本节主要介绍如何用闭环矢量法和D-H法建立系统的运动学模型。
1+
1 Kri2
Jis2 +Bis
TL(s)
JLs+BL Jis+Bi s+ i2Ji +JL s+ i2Bi +BL
ωL (s)=
Ti
(s)-
1 i
TL
(s)
i2Ji +JL s+ i2Bi +BL
i
Fi
d dt
L qi
L qi
i=1,2,L,n
K1
1 2
m1v12
1 2
m1d1212
y2
x3
y1
l2
C
q2
y0
l1
x1
B
A
q1
x0
cosq1 -sinq1 0 l1cosq1
ABT
0
A B
R
0
0
A
pB0 1
sinq1 0 0
cosq1 0 0
0 1 0
l1sinq1
0
1
cos q2 sin q2 0 l2 cos q2
CBT
0
B C
R
0
0
B
pC 1
0
m2d1d2 cos2 (12 12 ) (m1 m2 )gd1 cos1 m2gd2 cos(1 2 )
L
1
(m1
m2 )gd1 sin1
m2 gd 2
sin(1
2 )
L
1
(m1
m2 )d121
m2d22 (1
2 )
m2d1d2
cos2 (21
2 )
d dt
L
1
(m1
m2
sin q2 0 0
cos q2 0 0
0
l2
sin
q2
1 0
0
1
ACT ABT BCT
cq1 -sq1 0 l1cq1 cq2 -sq2 0 l2cq2
sq1
cq1
0
l1sq1
sq2
cq2
0
l2sq2
0 0 1 0 0 0 1 0
0
0
0
1
0
00
1
cq1cq1-sq1sq2 -cq1sq2 -sq1cq2 0 l2cq1cq2 -l2sq1sq2 +l1cq1
cβ 0 sβ 0
Ty,
0
-sβ
1 0
0 cβ
0 0
0
0
0
1
1 0 0 dx
Ttran
0 0
1 0
0 1
d
y
d
z
0 0 0 1
齐次变换矩阵相乘
已知三维空间中的三个坐标系{A}、{B}、{C}；{B}相对于{A}的描述为ABT， {C}相对于{B}的描述为BCT，则对于空间任一点p，有：
Ti
(t)=Ji
Байду номын сангаас
d2θi (t) dt2
+Bi
dθi (t) dt
+T(t)
iT(t)=J L
d2θL (t) dt2
+BL
dθL (t) dt
+TL
(t)
Ti (s)=Jis2θi (s)+Bisθi (s)+T(s) iT(s)=J Ls2θL (s)+BLsθL (s)+TL (s)
θL =
sq1cq2 +cq1sq2
-sq1sq2 +cq1cq2
0
l2sq1cq2
+l2cq1sq2
+l2sq1
0
0
1
0
0
0
0
1
xp =l1cq1+l2c(q1+q2 )
yp
=l1sq1
+l2s(q1+q2
)
xp =l1cq1+l2c(q1+q2 )
yp
=l1sq1+l2s(q1
+q2
)
xp =-l1sq1-l2s(q1+q2 )q1-l2s(q1+q2 )q2 yp =l1cq1+l2c(q1+q2 )q1+l2c(q1+q2 )q2
机构运动学的应用（以机器人为例） 已知杆件几何参数和关节变量，求末端执行器相对与参考坐标系的 位置和姿态。（运动学正问题） 已知杆件机构参数，给定末端执行器相对于参考坐标系的位置和姿 态，确定关节变量的大小。（运动学逆问题）
1.闭环矢量法
基本原理与步骤
分析机构各个杆件之间的矢量关系，根据封闭的矢量为零的原理 建立机构的位置方程，再通过微分（求导）建立速度、加速度方程。
变换顺序右 左乘 乘“ “(( 从 从左 右到 到右 左” ”))： ：运 运动 动相 相对 对于 于运 固动 定坐 坐标 标系 系而 而言 言的 的。 。
y2
x3
y1
l2
C
q2
y0
l1
x1
B
A
q1
x0
ABT Trans(l1cq1,l1sq1,0) Rot(z,q1) BCT Trans(l2cq2,l2sq2,0) Rot(z,q2 )
0
0 1 0 sin cos 3 0 0 1 0 1 0 3 2
（3）
cos
Poxyz
0
-sin
0 0
0
1
-1 0
0 sin cos
1
0
sin
0 cos 0
1 0 -1 0 1 0 1 0 0 0 0 0 0 1 0
-sin 0 1 0
cos 0 0 cos 0 1 0 sin
pz
iu
kz
jv ix jv jy jv kz
kw kw
ix jy
pu
pu
Poxyz
RPouvw
kw
kz
pu
旋转矩阵：
R
iu iu
ix jy
iu
kz
Pouvw QPoxyz
jv ix jv jy jv kz
kw kw
ix jy
kw
kz
R为正交矩阵！
Q R1 RT
1 0 0 0
0 1
平移变换矩阵 旋转变换矩阵
ABT
0
BAR 0
0
A
pB0
I3x3
1 0 0 0
A
pB0
BAR
1 0 0 0
0
1
基本齐次变换矩阵：
1 0 0 0
Tx,
0 0
cα sα
-sα cα
0 0
0 0 0 1
cθ -sθ 0 0
Tz,
sθ 0
cθ 0
0 1
0 0
0
0 0 1
x
R2
o
R3
3
2 R1 y
平面四杆机构
（4）建立速度方程
取 1 0
2r2s2 3r3s3 4r4s4 2r2c2 3r3c3 4r4c4
R4
4
若以杆2的角速度为输入，则有： 3r3s3 4r4s4 2r2s2 3r3c3 4r4c4 2r2c2
矩阵形式
3r3s3 4r4s4 2r2s2 3r3c3 4r4c4 2r2c2
A B
若A的逆存在，则有 A1B
仿真模型
上述数学模型的求解需要进行微分运算。在计算机上进行微分运算 比较困难，因此，仿真计算一般通过对加速度的积分运算，获得速度和 位置输出。
加速度 dt 速度 dt 位移
运动输入
运动学方程
dt
dt
dt
dt
闭环矢量法的运动学仿真框图
特点：不能得到位置和速度的解析解，但可通过数值积分运算求得杆 件的速度及位置的数值解。建模难度小、计算收敛性好，主要用于闭 式链机构的运动学分析。
B pBCT C p A pABT B pABT BCTC p ACTC p ACTABT BCT 复合变换（{C}相对于{A}的描述）
这种变换的另一种解释： 坐标系{C}是这样得到的：最初{C}与{A}重合，首先相对于{A}作运动
ABT，到达{B}，然后相对于{B}作运动 BCT，到达最终{C}。
P1 m1gy1 m1gd1 cos1
K2
1 2
m2v22, P2
mgy2
v22 x22 y22
x2 d1 sin1 d2 sin(1 2 ) y2 d1 cos1 d2 cos(1 2 ) x2 d1 cos11 d2 cos(1 2 )(1 2 ) y2 d1 sin11 d2 sin(1 2 )(1 2 )