4R机构 矩阵法求解运动学
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
1..建立D-H 坐标系。
D-H 坐标（后置）
杆号 转角变量
θi
连杆扭角αi
连杆间距d i 连杆长度a i 1 1θ -2π
0.5 0 2 2θ 2
π
0.5 0 3 3θ -2π
0.5 0 4 4
θ
2
π
0.5 0 5
0 0
0.5
2.计算T i i−1
T
i i−1=[
R
i i−1P 00
0i
];
R
=[Cθi Sθi
−Sθi Cαi Sαi Sθi
Cθi Cαi −Cθi Sαi 0
Sαi Cαi
]i i−1
;
P =(a i Cθi
a i Sθi d i );T =[Cθi −Sθi Cαi Sθi Cθi Cαi Sαi Sθi a i Cθi
−Cθi Sαi a i Sθi
Sαi 00Cαi d i 01
]i
i−1.即：
T =[Cθ1−Sθ1Cα1Sθ1Cθ1Cα1Sα1Sθ1a 1Cθ1−Cθ1Sα1a 1Sθ10
Sα100Cα1d 101
]10
即：T =[Cθ10Sθ10−Sθ10Cθ100 −10 0
0 0.50 1]10
∴ T =[Cθ20Sθ20Sθ20−Cθ200 10 0 0 0.50 1]21； T =[Cθ30Sθ30−Sθ30Cθ30
0 −10 0
0 0.5
0 1
]32；
T =[Cθ40Sθ40Sθ40−Cθ400 10 0
0 0.50 1
]43； T =[1 00 10 00 0 0 00 0 1 0.5 0 1]t 4
3. 任意设定各关节变量，计算T T 0
（解运动学正问题）；
设定θ1=θ2=θ3=θ4=π3⁄ ,则：
T =T 10∙T 21∙T 32∙T 43∙T t 4
t
=[0.500000.86000−0.866000.500000 −10 0 0 0.50 1]∙[0.500000.860000.86600−0.500000 10 0 0 0.50 1]∙[0.500000.86000−0.866000.500000 −10 0
0 0.5
0 1]∙
[0.500000.860000.86600−0.500000 10 0
0 0.5
0 1
] . [1 00 10 0
0 0 0 00 0 1 0.5 0 1]
=[−0.6875 −0.6495−0.3247 −0.1250
−0.3247
−0.7036 0.9374 1.0312
−0.6495 0.75000 0
−0.1250
1.0625 0
1.0000
]
4. 利用Paul 反变换法求解各关节变量.
∵ T =T 10∙T 21∙T 32∙T 43∙T t 4
t 0
在式的两边同时乘T −110
得：T −1∙10
T =T 21∙T 32∙T 43∙T t 4t 0
………………………………………⑴
由T 10得T −110=[C θ1S θ1
0000−10.5−S θ1C θ1
00
0001
]
∴⑴式左边为 T −1∙10T t 0=[C θ1S θ1
0000−10.5−S θ1C θ1
00
0001]∙[−0.6875 −0.6495−0.3247 −0.1250
−0.3247 −0.7036
0.9374 1.0312
−0.6495 0.7500
0 0
−0.1250 1.0625
0 1.0000
]=
[
−0.6875C 1−0.3247S 1−0.6495C 1−0.1250S 1
0.6495−0.7500−0.3247C 1+0.9374S 1
−0.7036C 1+1.0312S 1
0.1250
−1.06250.6875S 1−0.3247C 10.6495S 1−0.1250C 1
00
0.3247S 1+0.9374C 1
0.7036S 1+1.0312C 1
1
]
⑴ 式右边为：
T 21∙T 32∙T 43∙T t 4
=[n x o x n y o y a x p x a y p y n z o z 00a z p z
01
] 其中： n x =C θ2C θ3C θ4−S θ2S θ4 ; n y =S θ2C θ3C θ4+C θ2S θ4 ; n z =S θ3C θ4 o x =−S θ3C θ2 ; o y =−S θ3S θ2 ; o z =C θ3
a x =C θ2C θ3S θ4+C θ4S θ2 ; a y =S θ2C θ3S θ4−C θ2C θ4 ; a z =S θ3S θ4 p x =0.5C θ2C θ3S θ4+0.5C θ4S θ2−0.5S θ3C θ2+0.5S θ2 ; p y =0.5S θ2C θ3S θ4−0.5C θ4C θ2−0.5S θ3S θ2−0.5C θ2 ; p z =0.5S θ3S θ4+0.5C θ3+0.5 由⑴式两边对应元素相等得
S θ3S θ4=0.3247S 1+0.9374C 1 …………………………………………………………………⑵ 0.5S θ3S θ4+0.5C θ3+0.5=0.7036S 1+1.0312C 1 ……………………………………………⑶ C θ3=0.6495S 1−0.1250C 1 ……………………………………………………………………⑷ 解得θ1≈π3⁄ ，将θ1≈π3⁄代入⑷得θ3=π3⁄ 将θ1，θ3代入⑵得θ4=π3⁄
又∵−S θ3S θ2=−0.7500 …………………………………………………………………………⑸ 解得θ2=π3⁄