东南大学远程教育
机器人原理及应用
第 五 讲
主讲教师：王兴松
东南大学机械系
Email:xswang@seu.edu.cn,Tel:025-3802606
机械手的运动
Movement of Robotics
2.1 机械手运动的表示方法
2.2 手爪位置和关节变量的关系
2.3 雅可比矩阵
2.4 手爪力和关节驱动力的关系
2.5 机械手运动方程式的求解
Robotics运动
2.1 机械手运动的表示方法
2.1.1 机械手的结构
Robotics运动
2.1 机械手运动的表示方法
2.1.2 机械手的机构和运动学
回转关节
棱柱关节
关节变量
手爪姿态
运动学
Robotics运动
2.1 机械手运动的表示方法
2.1.2 机械手的机构和运动学
手爪位置 r;关节变量 θ
有：
写为,运动学方程式。
??
?
??
??
??
?
??
??
2
1,
?
?θ
y
xr
)c o s (c o s 21211 ??? ??? LLx
)s i n(s i n 21211 ??? ??? LLy
)(θr f?
Robotics运动
2.1 机械手运动的表示方法
2.1.2 机械手的机构和运动学
正运动学与逆运动学
r)θ (1?? f
??? ??2
c o ss i nt a nt a n
221
2211
1 ???
?
???
?
????
??
?
?? ??
?
??
LL
L
x
y
???
?
???
? ????? ?
21
2
2
2
1
22
1
2
)(c os
LL
LLyx?
Robotics运动
2.1 机械手运动的表示方法
2.1.3 运动学、静力学、动力学的关系
手爪力 F与
关节驱动力静态时
的关系,静力学
Robotics运动
2.1 机械手运动的表示方法
2.1.3 运动学、静力学、动力学的关系
驱动力矩与关节位置
关节速度、关节加
速度的关系 动力学
Robotics运动
2.1 机械手运动的表示方法
2.1.3 运动学、静力学、动力学的关系
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.1 手爪位置和姿态的表示方法
Σ B 基坐标系
Σ E 手爪坐标系
BpE∈R 3x1:手爪坐标系
原点在基坐标中的
位置向量
BRE ∈R 3x3:坐标变换
矩阵
? ?zByBxBEB eeeR,,?
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.2 姿态变换矩阵
同一点 P在两个坐标系
中的坐标：
假设：
可写为：
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
y
B
x
B
B
y
A
x
A
A
p
p
p
p pp,
pe ATxAxB p ?
pe ATyAyB p ?
pRp AABB ?
?
?
?
?
?
?
?
?
? T
y
A
T
x
A
A
B
e
eR
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.2 姿态变换矩阵
BRA,姿态坐标变换阵
有如下性质：
??
单位矩阵）(
1 0
0 1
)(
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
y
AT
y
AAT
y
A
y
AT
x
AAT
x
A
y
AA
T
y
A
T
x
A
T
A
B
A
B
eeee
eeee
ee
e
e
RR
x
x
x
TABAB )()( 1 RR ??
pRpRp BTABBABA )()( 1 ?? ?
pRp BBAA ? ?? yAATABBA eeRR x )( ??
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.3 齐次变换
两个坐标系中位姿关系,
上式称为 齐次变换矩阵
EBpEEBpB ppRp ??
???
?
???
??
???
?
???
?
11
p
E
E
Bp
B p
Tp
33
??
???
?
???
?? R
10
pRT
T
E
B
E
B
E
B
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.3 齐次变换
对二自由度机械手
???
?
???
??
???
?
???
?
11
p
E
E
Bp
B p
Tp
???
?
???
??
10
pRT
T
E
B
E
B
E
B
???
?
???
??
???
?
???
?
11
1
1
pBp
B p
Tp ??
?
?
???
??
10
pRT
11
1 T
BB
B
???
?
???
??
???
?
???
?
11
2
2
1
1
pp pTp ??
?
?
???
??
10
pRT
2121
2
1
T
???
?
???
??
?????? 11 2
2
p
E
E
P pTp
???
?
???
??
10
pRT
121
T
EE
E
EBEB TTTT 2211?
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.3 齐次变换
利用上式的确步骤：
1）建立连杆坐标系，并用
连杆长度和关节变量，
求相邻坐标系的位姿关系
2）求相邻坐标系的齐次变换
矩阵；
3）利用上式求总变换
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.3 齐次变换
?????? ?????????
11
11
11 C
,
0
0
S
SCBB Rp
??????
??
???????
22
22
2
11
2
1
C
,
0 S
SCL Rp
?????????????? 1 0
0 1,
0
222
EE
L Rp
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.3 齐次变换
?
?
?
?
?
?
?
?
?
? ?
?
1 0 0
0
0
11
11
1 CS
SC
B T
?
?
?
?
?
?
?
?
?
? ?
?
1 0 0
0
22
122
2
1 CS
LSC
T
?
?
?
?
?
?
?
?
?
?
?
1 0 0
0 1 0
0 1 2
2
L
ET
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.3 齐次变换
?
?
?
?
?
?
?
?
?
?
??
??
?
?
?
?
?
?
?
?
?
?
?
?????
?????
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
? ?
?
100
100
)(L
)(L
1 0 0
0 1 0
0 1
1 0 0
0
1 0 0
0
0
122111212
122111212
112121221212121
112121221212121
2
22
122
11
11
SLSLCS
CLCLSC
SLSCCSCCSSSCCS
CLSSCCCSSCSSCC
L
CS
LSC
CS
SC
E
B
T
Robotics运动
2.3 雅可比矩阵
2.3.1雅可比矩阵的定义
机器人正运动学方程：,这里
其中：
n>m:冗余机器人
)(θf?r
? ?
? ? 121
1
21
,,
,,
?
?
??????
??????
nT
n
mT
mrrr
R
Rr
???θ
),,2,1(),,( 2111 mjfr n ????????? ???
Robotics运动
2.3 雅可比矩阵
2.3.1雅可比矩阵的定义
J,雅可比矩阵
θ ?? Jr ?
nm
n
mm
n
T
ff
ff
f ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
?
?
?
?
?
? RJ
??
??
1
1
1
1
1
)(
??
θ
θ
θdd Jr ?
Robotics运动
2.3 雅可比矩阵
2.3.1雅可比矩阵的定义
例：两自由度机械手的雅可比矩阵
12211 CLCLx ??
12211 SLSLy ??
)s i n();c os (;s i n;c os 211221121111 ?????? ?????? SCSC
122
2
12211
1
,SLxSLSLx ????????? ??
122
2
12211
1
,CLyCLCLy ??????? ??
?????? ?
????
12212211
12212211
SLCLCL
SLSLSLJ
Robotics运动
2.3 雅可比矩阵
2.3.2 关节速度和手爪速度的几何学关系
? ? 1221,??? RJJJJ i
??????
??
?????? ?
???
122
122
2
12211
12211
1,CL
SL
CLCL
SLSL JJ
2211 ?? ??? JJr ??
Robotics运动
2.3 雅可比矩阵
2.3.2 关节速度和手爪速度的几何学关系
1
12211
12211
12211
12211
1,
01
10
)2/c o s ()2/s i n (
)2/s i n ()2/c o s (
J
p
??
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
??
?
?
?
?
? ?
CLCL
SLSL
SLSL
CLCL
E
??
??
2
122
122
122
122
2,
01
10
)2/c o s ()2/s i n (
)2/s i n ()2/c o s (
J
p
??
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
??
?
?
?
?
? ?
CL
SL
SL
CL
E
??
??
Robotics运动
2.4 手爪力和关节驱动力的关系
2.4.1 虚功原理
0?? BBAA xFxF ??
?????? BBAA LxLx ??,
0)( ?? ??BBAA LFLF
0?? BBAA LFLF
A
B
A
B FL
LF ??
Robotics运动
2.4 手爪力和关节驱动力的关系
2.4.2 机械手静力学关系式的推导
? ? 11,,??? mTmrr Rr ??? ?
手爪的虚位移
? ? 11,,??? nTn R????? ?θ
手爪的虚位移
? ? 11,,,??? mTmff RF ?
手爪力
? ? 11,,,??? nTm Rτ ?? ?
关节驱动力
Robotics运动
2.4 手爪力和关节驱动力的关系
2.4.2 机械手静力学关系式的推导
rFθτW ??? TT )( ???
0)( ??? rFθτ ?? TT
θJr ?? ?
0)( ?? θJFτ ?TT
0?? JFτ TT
FJτ T?
Robotics运动
2.4 手爪力和关节驱动力的关系
2.4.2 机械手静力学关系式的推导
?
?
?
?
?
? ???
?
?
?
?
?
?
?
???
?
01
22
12212211
12212211
L
LL
CLCLCL
SLSLSL
J
?
?
?
?
?
? ?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
??
0
0
0
00
1
2
12
2
2
2
12
y
y
B
T
B
x
xx
A
T
A
fL
fL
LL
FJ
fL
fLf
L
LL
FJ
?
?
Robotics运动
2.5 机械手运动方程式的求解
2.5.1 惯性矩
Fxm ???
????? rx ?
r
NF ?
Nmr ????2
NI ????
2mrI ?
Robotics运动
2.5 机械手运动方程式的求解
2.5.1 惯性矩
绕一端旋转惯性矩
绕重心旋转惯性矩
dVdm ??
dVrd m rdI 22 ???
?? ?? dVrdII 2?
2
0
3
0
2
3
1
3 ML
x
L
MdxxI LL ?
??
?
??
??? ? ?
2
2/
0
32/
0
2
3
1
322 ML
x
L
MdxxI LL
C ???
?
??
??? ? ?
Robotics运动
2.5 机械手运动方程式的求解
2.5.2 牛顿 -欧拉方程式
CCm Fv ??
NCC ??? )( ωIωωI ?
Robotics运动
2.5 机械手运动方程式的求解
2.5.2 牛顿 -欧拉方程式
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
0
0
0
0
0
0
0
,0
0
??? ????
?
II
I ωωωI
?
?
?
?
?
?
?
?
?
?
?
?
?? c o s
0
0
cmgL
N
??? ?? co sCm g LI ??
2CC mLII ??
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
???????
?
?
???
?
?
?
q
L
q
L
dt
d
PKL ??
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
Lagrange方程
T:系统动能；
qi:广义坐标； Qi:对应于广义坐标的广义力
当主动力为势力时,方程变为：
L:Lagrange函数
j
jj
Q
q
T
q
T
dt
d ?
?
??
?
?
?
0?
?
??
?
?
?
?
?
?
?
?
?
?
jj q
L
q
L
dt
d
? VTL ??
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
当主动力中有非势力时,
Qj:为非势的广义力
当含有粘性阻尼时，方程变为：
,Φ,瑞利耗三散函数
j
jj
Q
q
L
q
L
dt
d ?
?
??
?
?
?
?
?
?
?
?
?
?
?
j
jjj
Q
qq
L
q
L
dt
d ?
?
???
?
??
?
?
?
?
?
?
?
?
?
?
??
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
例：图示为振动系统方程
1。动能
2。势能
)(21 222211 xmxmT ?? ??
? ?2122211 )(21 xxcxcV ???
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
3。耗散函数
4。拉格朗日函数
? ?2122211 )(21 xxx ??? ???? ??
? ?? ?2222122121222211 2
2
1)(
2
1 xcxxcxccxmxm
VTL
??????
??
??
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
?
?
?
?
??
??
c o s,
s i n
2
1
,s i n,
2
1
2
2
c
c
c
m g L
L
I
L
m g LIL
m g LPIK
??
?
?
?
?
?
??
??
?
?
?
?
?? ?? c o sCm g LθI ??
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
2
111111 2
1
2
1 ????
CC
T
C ImK ?? pp
1111 SgLmP C?
2
2122222 )(2
1
2
1 ?? ???? ???
CC
T
C ImK pp
)( 12211212 SLSLgmP C??
? ?TC iyC ixCi pp,?p
是第 i个连杆质量
中心的位置向量
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
111 CLpL xC ?
111 SLpL yC ?
122112 CLCLpL CxC ??
122111 SLSLpL CxC ??
212111 ???? CCTC L?pp
)(2)( 212122122122212122 ?????? ???????? ????? CLLLL CCCTC pp
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
2121 PPKKL ????
τθgθθcθθM ??? )(),()( ?????
??
?
??
??
??
?
??
??
??
?
??
??
2
1
2
1
2221
1211 )(,),(,)(
g
g
c
c
MM
MM θgθθcθM ???
212122212121111 )2( CCCC ICLLLLmILmM ??????
222122212 )( CCC ICLLLmM ???
2112 MM ? 222222 CC ILmM ??
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
)2( 212222121 ??? ??? ??? SLLmc C
2122122 ??SLLmc C?
)( 1221121111 CLCLgmCgLmg CC ???
122122 CgLmg C?
θθM ??)(
),( θθc ???
)(θg
惯性力；
离心力；
重力项
机器人原理及应用
第 五 讲
主讲教师：王兴松
东南大学机械系
Email:xswang@seu.edu.cn,Tel:025-3802606
机械手的运动
Movement of Robotics
2.1 机械手运动的表示方法
2.2 手爪位置和关节变量的关系
2.3 雅可比矩阵
2.4 手爪力和关节驱动力的关系
2.5 机械手运动方程式的求解
Robotics运动
2.1 机械手运动的表示方法
2.1.1 机械手的结构
Robotics运动
2.1 机械手运动的表示方法
2.1.2 机械手的机构和运动学
回转关节
棱柱关节
关节变量
手爪姿态
运动学
Robotics运动
2.1 机械手运动的表示方法
2.1.2 机械手的机构和运动学
手爪位置 r;关节变量 θ
有：
写为,运动学方程式。
??
?
??
??
??
?
??
??
2
1,
?
?θ
y
xr
)c o s (c o s 21211 ??? ??? LLx
)s i n(s i n 21211 ??? ??? LLy
)(θr f?
Robotics运动
2.1 机械手运动的表示方法
2.1.2 机械手的机构和运动学
正运动学与逆运动学
r)θ (1?? f
??? ??2
c o ss i nt a nt a n
221
2211
1 ???
?
???
?
????
??
?
?? ??
?
??
LL
L
x
y
???
?
???
? ????? ?
21
2
2
2
1
22
1
2
)(c os
LL
LLyx?
Robotics运动
2.1 机械手运动的表示方法
2.1.3 运动学、静力学、动力学的关系
手爪力 F与
关节驱动力静态时
的关系,静力学
Robotics运动
2.1 机械手运动的表示方法
2.1.3 运动学、静力学、动力学的关系
驱动力矩与关节位置
关节速度、关节加
速度的关系 动力学
Robotics运动
2.1 机械手运动的表示方法
2.1.3 运动学、静力学、动力学的关系
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.1 手爪位置和姿态的表示方法
Σ B 基坐标系
Σ E 手爪坐标系
BpE∈R 3x1:手爪坐标系
原点在基坐标中的
位置向量
BRE ∈R 3x3:坐标变换
矩阵
? ?zByBxBEB eeeR,,?
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.2 姿态变换矩阵
同一点 P在两个坐标系
中的坐标：
假设：
可写为：
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
y
B
x
B
B
y
A
x
A
A
p
p
p
p pp,
pe ATxAxB p ?
pe ATyAyB p ?
pRp AABB ?
?
?
?
?
?
?
?
?
? T
y
A
T
x
A
A
B
e
eR
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.2 姿态变换矩阵
BRA,姿态坐标变换阵
有如下性质：
??
单位矩阵）(
1 0
0 1
)(
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
y
AT
y
AAT
y
A
y
AT
x
AAT
x
A
y
AA
T
y
A
T
x
A
T
A
B
A
B
eeee
eeee
ee
e
e
RR
x
x
x
TABAB )()( 1 RR ??
pRpRp BTABBABA )()( 1 ?? ?
pRp BBAA ? ?? yAATABBA eeRR x )( ??
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.3 齐次变换
两个坐标系中位姿关系,
上式称为 齐次变换矩阵
EBpEEBpB ppRp ??
???
?
???
??
???
?
???
?
11
p
E
E
Bp
B p
Tp
33
??
???
?
???
?? R
10
pRT
T
E
B
E
B
E
B
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.3 齐次变换
对二自由度机械手
???
?
???
??
???
?
???
?
11
p
E
E
Bp
B p
Tp
???
?
???
??
10
pRT
T
E
B
E
B
E
B
???
?
???
??
???
?
???
?
11
1
1
pBp
B p
Tp ??
?
?
???
??
10
pRT
11
1 T
BB
B
???
?
???
??
???
?
???
?
11
2
2
1
1
pp pTp ??
?
?
???
??
10
pRT
2121
2
1
T
???
?
???
??
?????? 11 2
2
p
E
E
P pTp
???
?
???
??
10
pRT
121
T
EE
E
EBEB TTTT 2211?
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.3 齐次变换
利用上式的确步骤：
1）建立连杆坐标系，并用
连杆长度和关节变量，
求相邻坐标系的位姿关系
2）求相邻坐标系的齐次变换
矩阵；
3）利用上式求总变换
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.3 齐次变换
?????? ?????????
11
11
11 C
,
0
0
S
SCBB Rp
??????
??
???????
22
22
2
11
2
1
C
,
0 S
SCL Rp
?????????????? 1 0
0 1,
0
222
EE
L Rp
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.3 齐次变换
?
?
?
?
?
?
?
?
?
? ?
?
1 0 0
0
0
11
11
1 CS
SC
B T
?
?
?
?
?
?
?
?
?
? ?
?
1 0 0
0
22
122
2
1 CS
LSC
T
?
?
?
?
?
?
?
?
?
?
?
1 0 0
0 1 0
0 1 2
2
L
ET
Robotics运动
2.2 手爪位置和关节变量的关系
2.2.3 齐次变换
?
?
?
?
?
?
?
?
?
?
??
??
?
?
?
?
?
?
?
?
?
?
?
?????
?????
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
? ?
?
100
100
)(L
)(L
1 0 0
0 1 0
0 1
1 0 0
0
1 0 0
0
0
122111212
122111212
112121221212121
112121221212121
2
22
122
11
11
SLSLCS
CLCLSC
SLSCCSCCSSSCCS
CLSSCCCSSCSSCC
L
CS
LSC
CS
SC
E
B
T
Robotics运动
2.3 雅可比矩阵
2.3.1雅可比矩阵的定义
机器人正运动学方程：,这里
其中：
n>m:冗余机器人
)(θf?r
? ?
? ? 121
1
21
,,
,,
?
?
??????
??????
nT
n
mT
mrrr
R
Rr
???θ
),,2,1(),,( 2111 mjfr n ????????? ???
Robotics运动
2.3 雅可比矩阵
2.3.1雅可比矩阵的定义
J,雅可比矩阵
θ ?? Jr ?
nm
n
mm
n
T
ff
ff
f ?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
???
?
?
?
?
?
? RJ
??
??
1
1
1
1
1
)(
??
θ
θ
θdd Jr ?
Robotics运动
2.3 雅可比矩阵
2.3.1雅可比矩阵的定义
例：两自由度机械手的雅可比矩阵
12211 CLCLx ??
12211 SLSLy ??
)s i n();c os (;s i n;c os 211221121111 ?????? ?????? SCSC
122
2
12211
1
,SLxSLSLx ????????? ??
122
2
12211
1
,CLyCLCLy ??????? ??
?????? ?
????
12212211
12212211
SLCLCL
SLSLSLJ
Robotics运动
2.3 雅可比矩阵
2.3.2 关节速度和手爪速度的几何学关系
? ? 1221,??? RJJJJ i
??????
??
?????? ?
???
122
122
2
12211
12211
1,CL
SL
CLCL
SLSL JJ
2211 ?? ??? JJr ??
Robotics运动
2.3 雅可比矩阵
2.3.2 关节速度和手爪速度的几何学关系
1
12211
12211
12211
12211
1,
01
10
)2/c o s ()2/s i n (
)2/s i n ()2/c o s (
J
p
??
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
??
?
?
?
?
? ?
CLCL
SLSL
SLSL
CLCL
E
??
??
2
122
122
122
122
2,
01
10
)2/c o s ()2/s i n (
)2/s i n ()2/c o s (
J
p
??
?
?
?
?
? ?
?
?
?
?
?
?
?
?
?
?
?
?
? ?
??
?
?
?
?
? ?
CL
SL
SL
CL
E
??
??
Robotics运动
2.4 手爪力和关节驱动力的关系
2.4.1 虚功原理
0?? BBAA xFxF ??
?????? BBAA LxLx ??,
0)( ?? ??BBAA LFLF
0?? BBAA LFLF
A
B
A
B FL
LF ??
Robotics运动
2.4 手爪力和关节驱动力的关系
2.4.2 机械手静力学关系式的推导
? ? 11,,??? mTmrr Rr ??? ?
手爪的虚位移
? ? 11,,??? nTn R????? ?θ
手爪的虚位移
? ? 11,,,??? mTmff RF ?
手爪力
? ? 11,,,??? nTm Rτ ?? ?
关节驱动力
Robotics运动
2.4 手爪力和关节驱动力的关系
2.4.2 机械手静力学关系式的推导
rFθτW ??? TT )( ???
0)( ??? rFθτ ?? TT
θJr ?? ?
0)( ?? θJFτ ?TT
0?? JFτ TT
FJτ T?
Robotics运动
2.4 手爪力和关节驱动力的关系
2.4.2 机械手静力学关系式的推导
?
?
?
?
?
? ???
?
?
?
?
?
?
?
???
?
01
22
12212211
12212211
L
LL
CLCLCL
SLSLSL
J
?
?
?
?
?
? ?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
?
?
??
0
0
0
00
1
2
12
2
2
2
12
y
y
B
T
B
x
xx
A
T
A
fL
fL
LL
FJ
fL
fLf
L
LL
FJ
?
?
Robotics运动
2.5 机械手运动方程式的求解
2.5.1 惯性矩
Fxm ???
????? rx ?
r
NF ?
Nmr ????2
NI ????
2mrI ?
Robotics运动
2.5 机械手运动方程式的求解
2.5.1 惯性矩
绕一端旋转惯性矩
绕重心旋转惯性矩
dVdm ??
dVrd m rdI 22 ???
?? ?? dVrdII 2?
2
0
3
0
2
3
1
3 ML
x
L
MdxxI LL ?
??
?
??
??? ? ?
2
2/
0
32/
0
2
3
1
322 ML
x
L
MdxxI LL
C ???
?
??
??? ? ?
Robotics运动
2.5 机械手运动方程式的求解
2.5.2 牛顿 -欧拉方程式
CCm Fv ??
NCC ??? )( ωIωωI ?
Robotics运动
2.5 机械手运动方程式的求解
2.5.2 牛顿 -欧拉方程式
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?
?
?
?
?
?
0
0
0
0
0
0
0
,0
0
??? ????
?
II
I ωωωI
?
?
?
?
?
?
?
?
?
?
?
?
?? c o s
0
0
cmgL
N
??? ?? co sCm g LI ??
2CC mLII ??
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
???????
?
?
???
?
?
?
q
L
q
L
dt
d
PKL ??
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
Lagrange方程
T:系统动能；
qi:广义坐标； Qi:对应于广义坐标的广义力
当主动力为势力时,方程变为：
L:Lagrange函数
j
jj
Q
q
T
q
T
dt
d ?
?
??
?
?
?
0?
?
??
?
?
?
?
?
?
?
?
?
?
jj q
L
q
L
dt
d
? VTL ??
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
当主动力中有非势力时,
Qj:为非势的广义力
当含有粘性阻尼时，方程变为：
,Φ,瑞利耗三散函数
j
jj
Q
q
L
q
L
dt
d ?
?
??
?
?
?
?
?
?
?
?
?
?
?
j
jjj
Q
L
q
L
dt
d ?
?
???
?
??
?
?
?
?
?
?
?
?
?
?
??
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
例：图示为振动系统方程
1。动能
2。势能
)(21 222211 xmxmT ?? ??
? ?2122211 )(21 xxcxcV ???
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
3。耗散函数
4。拉格朗日函数
? ?2122211 )(21 xxx ??? ???? ??
? ?? ?2222122121222211 2
2
1)(
2
1 xcxxcxccxmxm
VTL
??????
??
??
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
?
?
?
?
??
??
c o s,
s i n
2
1
,s i n,
2
1
2
2
c
c
c
m g L
L
I
L
m g LIL
m g LPIK
??
?
?
?
?
?
??
??
?
?
?
?
?? ?? c o sCm g LθI ??
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
2
111111 2
1
2
1 ????
CC
T
C ImK ?? pp
1111 SgLmP C?
2
2122222 )(2
1
2
1 ?? ???? ???
CC
T
C ImK pp
)( 12211212 SLSLgmP C??
? ?TC iyC ixCi pp,?p
是第 i个连杆质量
中心的位置向量
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
111 CLpL xC ?
111 SLpL yC ?
122112 CLCLpL CxC ??
122111 SLSLpL CxC ??
212111 ???? CCTC L?pp
)(2)( 212122122122212122 ?????? ???????? ????? CLLLL CCCTC pp
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
2121 PPKKL ????
τθgθθcθθM ??? )(),()( ?????
??
?
??
??
??
?
??
??
??
?
??
??
2
1
2
1
2221
1211 )(,),(,)(
g
g
c
c
MM
MM θgθθcθM ???
212122212121111 )2( CCCC ICLLLLmILmM ??????
222122212 )( CCC ICLLLmM ???
2112 MM ? 222222 CC ILmM ??
Robotics运动
2.5 机械手运动方程式的求解
2.5.3 拉格郎日运动方程式
)2( 212222121 ??? ??? ??? SLLmc C
2122122 ??SLLmc C?
)( 1221121111 CLCLgmCgLmg CC ???
122122 CgLmg C?
θθM ??)(
),( θθc ???
)(θg
惯性力；
离心力；
重力项
