PUMA机器人大作业1坐标系建立：
坐标系可以简化为：
2 D-H 参数表：
PUMA 机器人的杆件参数:
1d 0.6604m =，2d 0.14909m =，4d 0.43307m =，6d 0.05625m =，2a 0.4318m =，3a 0.02032m =
3 正运动学推导
由式1
11111111100
00
1i
i i i i i i i i i i i i i i i i i i c s a s c c c s d s T s s c s c d c θθθαθαααθαθααα-----------⎡⎤
⎢⎥--⎢⎥=
⎢⎥
matlab学好了有什么用⎢
⎥⎣⎦
可得： 1
11
10
1000000100001c s s c T -⎡⎤⎢⎥⎢⎥=⎢⎥
⎢⎥⎣⎦，2
2212220
0001000001c s d T s c -⎡⎤⎢⎥⎢⎥=⎢⎥
--⎢
⎥⎣⎦，3
32332300000100001c s a s c T -⎡⎤
⎢⎥⎢⎥=⎢⎥
⎢⎥⎣⎦44343
4440001000
01c s a d T s c -⎡⎤⎢⎥⎢
⎥=⎢⎥
--⎢⎥⎣⎦，
5
545550000100000
01c s T s c -⎡⎤⎢⎥-⎢⎥=⎢⎥
⎢⎥⎣⎦，6
65666000010000001c s T s c -⎡⎤⎢⎥⎢⎥=⎢⎥
--⎢⎥⎣⎦
0123456123456
T T T T T T T =
机械手变换矩阵
60
1x x x x y
y y y z z z z n o a p n o a p T n o a p ⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦
23654164123651654164123654164123651654164123654642365
()()()x y z n c c c c c s s c s c s c c c s s s c s n c c c c s s s s s c s s c c s c s c c n s c c c s s c c s =--++=----=--- 23654164123651654164123654164123651645164123654642365()()()x y z o c s c c c c s c s s s c s c s s c c s o c s c c s c s s s s s s s s c c c c c o s s c c s s c c s =-++-+=-+++-=++ 2354123515412354123515412352354
x y z a c s c c s c c s s s a c s c s s c s s s c a c c s s c =---=--+=-+ 3231221423121323122142312142332322
x y z p a c c a c c d s c d s p a c s a c s d s s d c p d c a s a s =+--=+-+=---
4 逆运动学推导
1．求1θ
用逆变换01
1T -左乘方程00123456123456T T T T T T T =两边： 0
10123451623456T T T T T T T -=
1111160000001000010
1x
x x x y
y y y z
z z z c s n o a p s c n o a p T n o a p ⎡⎤⎡⎤⎢⎥⎢⎥-⎢⎥⎢⎥=⎢⎥⎢⎥⎢
⎥⎢⎥⎣⎦⎣⎦
得112x y s p c p d -+=
三角代换 cos x p ρφ=，sin y p ρφ=
式中，ρ=tan 2(,)x y a p p φ=
得到1θ的解
12a tan 2(,)a tan 2(,y x p p d θ=-
2．求3θ
矩阵方程两端的元素（1，4）和（2，4）分别对应相等
1132322423
42332322x y z c p s p a c a c d s p d c a s a s +=+-⎧⎨
-=++⎩
平方和为：
4333d s a c k +=
其中 2222222
2423
2
2x y z p p p d d a a k a ++----=
解得：334a tan 2(,)a tan 2(,a d k θ=-  3．求2θ
在矩阵方程00123456123456T T T T T T T =两边左乘逆变换01
3T -。 0
1034536456T T T T T -=
123123232312312323233611200
10
1x
x x x y
y y y z
z z z c c s c s a c n o a p c s s s c a s n o a p T s c d n o a p --⎡⎤⎡⎤⎢⎥⎢⎥---⎢⎥⎢⎥=⎢⎥⎢⎥--⎢⎥⎢⎥⎣⎦⎣⎦
方程两边的元素（1，4）和（3，4）分别对应相等，得
123123233231231232323
40
0x y z x y z c c p s c p s p a a c c s p s s p c p a s d +---=⎧⎨
++-+=⎩ 联立，得23s 和23c
()()()()()()()()23411233232211233112342322
11x y z x y z x y z x y z a s d c p s p p a c a s p c p s p a c a c p s p p a s d c p c p s p ⎧-+-+=⎪⎪++⎪
⎨
+++-⎪=⎪++⎪⎩
23s 和23c 表达式的分母相等，且为正，于是
()()()()()()23232341123323311234a tan 2,x y z x y z a s d c p s p p a c a a c a c p s p p a s d θθθ⎡⎤=+=-+-++++-⎣⎦
根据解1θ和3θ的四种可能组合，可以得到相应的四种可能值23θ，于是可得到2θ的四种可能解
2233θθθ=-
式中2θ取与3θ相对应的值。  4．求4θ
令两边元素（1，3）和（2，3）分别对应相等，则可得
1231232345
1145x y z x y c c a s c a s a c s s a c a s s +-=-⎧⎨
-+=⎩
只要50s ≠，便可求出4θ
()41112312323a tan 2,x y x y z s a c a c c a s c a s a θ=-+-+
当50s =时，机械手处于奇异形位。  5．求5θ
10454656
T T T T -=
142314
1423142343422443412314412314
234342244346123123
23324
00
10
1x
x x x y y y y z z z z c c c s s s c c c s s c c c a d s c a n o a p s c c s c s s c c c s s c s a d c s a n o a p T c s s s c s a d n o a p +---+-⎡⎤⎡⎤
⎢⎥⎢⎥-+--++⎢⎥⎢⎥=⎢⎥⎢⎥---+⎢
⎥⎢
⎥⎣
⎦⎣⎦
根据矩阵两边元素（1，3）和（2，3）分别对应相等，可得
()()2341423141423145
123231235z x y x y z a s c a c c c s s a s c c c s s a c s a s s a c c -+--=⎧⎪⎨
---=⎪⎩
()()()
523414231414231412323123a tan 2,z x y x y z a s c a c c c s s a s c c c s a c s a s s a c θ=-+-----
6．求6θ
105566T T T -=

版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。