机械手动力学公式

（1）位姿方程推导
在D-H 坐标下，两坐标系的转换矩阵为
1
0100
001
ij
ij ij ij
ij ij ij ij
ij ij S l C S C l S C A -=
- 其中，ij ij ij ij S C θθsin ,cos ==;
i —表示开链序号，i =1,2,3；
j —表示开链中连杆序号，j =1,2;
ij
ij A 1
-—表示坐标系X ij Y ij 在坐标系X ij-1Y ij-1中的变换矩阵；
θ—表示坐标系X ij Y ij 与X ij-1Y ij-1两者之间X 轴之间的夹角。

在开链1中，由此可得：
100
0100001
111111
111111111
1110
S l C S C l S C A -=
，
1
01000012
12121212121212
1211
S l C S C l S C A -=
，
1
01000011
1112111212111211111112111212111211121111101210
S l S l C S C l C l S C A A A ++-=
⨯=++++++，
同理可得，
1
0001000021
2122212222
2122
212121222122222122
21222121202220
S l S l C S C l C l S C A A A ++-=
⨯=++++++，
1
0100
0031
3132313232
3132
313131323132323132
313231
3130
3230
S l S l C S C l C l S C A A A ++-=
⨯=++++++。

设三个开链中，开链2、3相对于开链1的变换矩阵为
10
0001000
100
010
010
i i i y x T =
，则 =
⨯=2220
2010
2210
A T A 1
000100
0020
212122212222
2122
21202121222122222122
21y S l S l C S x C l C l S C ++++-++++++，
=
⨯=323030103210
A T A 1
01000030
313132313232
313231303131323132323132
31y S l S l C S x C l C l S C ++++-++++++
其中，
（x i0,y i0）为开链2、3基坐标在开链1中的坐标。

设C 点在基坐标x 10y 10z 10中的坐标为（x c ,y c ,0,1），根据坐标变换有
1
0c c
y x =1
000
1
0001
000321022101210⨯=⨯=⨯A A A 由上式可解得：
x c =l 12C 11+12+l 11C 11=l 22C 21+22+l 21C 21+x 20= l 32C 31+32+l 31C 31+x 30 （2.1） y c = l 12S 11+12+l 11S 11=l 22S 21+22+l 21S 21+y 20= l 32S 31+32+l 31S 31+y 30 （2.2） 设P 点在坐标系x 12y 12z 12中的坐标为（l pc ,0,0,1），则可得
1
0011210
pc p p p l A z y x ⨯
=，解得：
x p =（l pc +l 12）C 11+12+l 11C 11 （2.3） y p =（l pc +l 12）S 11+12+l 11S 11 （2.4） 联立（2.1）（2.2）式可解得
C 11+12= f （3121,θθ）, C 11= f （3121,θθ）。

（2.5） 将（2.5）带入式（2.3）（2.4）解得：
x p =f （3121,θθ）
y p =f （3121,θθ），同时可解出22321211,,,θθθθ关于3121,θθ的表达式。

基于拉格朗日方程，L=K —P ，其中K 为系统动能，
对于开链（1），有K 1=⎥⎦
⎤
⎢⎣⎡+⎪⎭⎫ ⎝⎛++∙∙∙
212122122111221111212121v m I I θθθ，
2
122
122
121212∙
∙
+=y x v l v 的质心速度，为杆，
,5.05.0121211
11121212111112⎩⎨
⎧+=+=S l S l y C l C l x 对于开链（2），有K 2=⎥⎦
⎤
⎢⎣⎡+⎪⎭⎫ ⎝⎛++∙∙∙
222222222212222121212121v m I I θθθ，
2
222
222
222222∙
∙
+=y x v l v 的质心速度，为杆，
,5.05.0222211
11222222212122⎩⎨
⎧+=+=S l S l y C l C l x K 3=⎥⎦
⎤⎢⎣⎡+⎪⎭⎫ ⎝⎛++∙∙∙232322322313223131212121v m I I θθθ,
2
322
322
323232∙
∙
+=y x v l v 的质心速度，为杆
,5.05.032
3231313232
32313132⎩⎨
⎧+=+=S l S l y C l C l x 杆l pc 的动能K 4=
2
12
pc pc v m ，系统总势能P=0，带入拉氏方程有 L=K —P=K =K 1+K 2+K 3+K 4
可推出31
313121
2121)()(θθθθ∂∂-
⎪⎩
⎪
⎨⎧
∂∂∂=∂∂-⎪⎩
⎪⎨⎧
∂∂∂=∙∙
L L t T L L t T ，。