机器人控制技术课件Chapter 4

General Mechanism with 6 d.o.f.
Number of solutions < 16
PUMA 560
PUMA 560
8 Solutions
θ4 θ5 θ6
θ4 + 180 − θ5 θ6 + 180ο
ο
Solvability
A manipulator is solvable if ALL the sets of solutions can be determined. 6 d.o.f. open-chain mechanisms are “now” solvable.
Closed Form Solutions
{6} {0}
0 6
T
B W
P0
T
0 6
T (θ 1 , θ 2 , . . . , θ 6 ) = T
B W
Solutions: • Algebraic • Geometric
Geometric Solutions
y0 D C −θ2 l1 γ βB θ1 (x0 ,y0) First θ2: l2 2 2 2 2 l1 + l2 + 2 l1 l2 cos θ 2 = x 0 + y 0 θ2 E x0 =>
LM x OP x = M y P = f (q) MNα PQ
2 12
LM x OP LM l c + l c OP x = M yP = M l s + l s P MNα PQ MNθ + θ + θ PQ
1 1 1 1 2 12 1 2 3
cos θ 1 + θ 2 = c12
b
g
Given q
123
I F cα JJ = GG sα JJ GG 0 K H0
1
0 0
− sα 0 cα 0 0 0
0 0 1 0
x0
0
I y J JJ 0 J 1K
cos(θ 1 + θ 2 + θ 3 ) = cos α 0 sin(θ 1 + θ 2 + θ 3 ) = sin α 0
For θ1 and θ2: l1 c1 + l 2 c12 = x 0
2 1
2 2 ( x 0 + y 0 ) − (l12 + l22 ) cos θ 2 = 2l1l2
α0
A
θ 2 and - θ 2
2 2 2 2 l22 = l12 + ( x 0 + y 0 ) − 2 l1 x 0 + y 0 cos γ θ1:
cos γ =
θ3:
x + y +l −l
2 0 2 0 2 2 2l1 x 0 + y 0
− s123 c123 0 0
x3
(x0,y0); α0
cα 0 0 l1 c1 + l2 c12 sα 0 0 l1 s1 + l2 s12 = 0 1 0 0 0 1
0 3
Fc GG s T= GG 0 H0
123
123
I JJ JJ K
F GG GG H
− sα 0 cα 0 0 0
0 0 1 0
Algebraic Solutions
α0
l1 c1 + l2 c12 = x 0 l1 s1 + l2 s12 = y 0
θ3
{0}
(x0,y0)
For θ1:
1
l2 θ 2 l1 θ1
r = k +k
2 1 2 2
( k1 , k 2 )
− − − − −− → tan γ = k 2 / k1
a unique x
Inverse Kinematics
{B} {W}
B W
T = T (θ 1 , θ 2 , θ 3 , θ 4 , θ 5 , θ 6 )
0 6
or
Given
LM X OP = f (Θ) X = NX Q
P R
Inverse Problem
B (W
T or X) find Θ
Weighted Joint distance moving smaller joints
Number of Solutions
It depends on • Number of Joints • Link Parameters e.g. 6-revolute-joint manipulator if all a i ≠ 0 Number solutions < 16 if a1 = a3 = a5 = 0 Number solutions < 4 • Range of Motion
Rg S T
LMc g − s g OP LMx OP MMs g + c g PP = MM y PP N g Q Nz Q
1 1 1 2
0
1 1
1 2
0
3
0
2 1
2 2 2 2 2 + g 2 + g3 = x 0 + y 0 + z 0 = r02 g3 = z 0
g i = g i ( c 2 , s 2 , f1 , f 2 , f 3 ) ( k1 s2 − k 2 c 2 )sα 1 + k 4 = z 0
Inverse Kinematics
Finding
Θ= f
−1
(X)
or Solving
0 6
T (θ 1 , θ 2 , θ 3 , θ 4 , θ 5 , θ 6 ) = T
B W
(
12 equations 6 unknowns
)
Existence of Solutions
y3
y0 l2 l1 x0
Dextrous Workspace
Multiplicity of Solutions
A
B1
l2
B2
B Selection of a solution
Criterion: Joint distance
l1
C1 = Θ ( B1) − Θ ( A ) C2 = Θ ( B 2 ) − Θ ( A )
0 1
Pieper’s Solution
{3} {2}
2
3
{0} {1}
1P
0P
P
P
{4}
3
LM a OP − sα . d PP P=M MM cα . d P N 1 Q
3 3 4 3 4
2
T(θ 3 ) . 3 P P=
2 3
2
LM f (θ )OP f (θ ) PP P=M MM f (θ )P N 1 Q
ki = ki ( f1 , f 2 , f3 ) → θ 2
if ki are known
( k1 c2 + k 2 s 2 )2 a1 + k3 = r02
Pieper’s Solution
{3} {2}
2
3
{0} {1}
1P
0P
P
P
{4}
( k1 c 2 + k 2 s 2 )2 a1 + k 3 = r02
x0 y0 0 1
I JJ JJ K
solution if 2 2 2 2 (l1 − l2 ) ≤ x 0 + y 0 ≤ (l1 + l2 )
l1- l2 l1
l2
Joint Limits
0 ≤ θ 1 ≤ 180
o o
0 ≤ θ 2 ≤ 180
Workspace
• Reachable Workspace • Dextrous Workspace
U⇒θ V W
+θ2 +θ3 = α0
l1 s1 + l 2 s12 = y 0 Solution if (x0, y0) is in the workspace
2 2 2 ( x 0 + y 0 ) − (l12 + l2 ) −1 ≤ cos θ 2 = 2l1l2
≤1
θ 2 = A tan 2( ± 1 − cos 2 θ 2 , cos θ 2 ) =>
3 2
with u = tan
θ3
2
For θ4, θ5, and θ6
0 6
0 6
R(Θ ) ≡ R0
3 4
0 2 4 R(Θ ) = 1 R(θ 1 ). 21 R(θ 2 ). 3 R(θ 3 ). 43 R(θ 4 ). 5 R(θ 5 ). 65 R(θ 6 )
R(θ 4 )= R|θ 4 = 0 . RZ (θ 4 )
θi di
εi ≡ 1− εi
Direct Kinematics Given q = ( q 1
0 n
R S T
q2
... q n ) T
T = 0T ( q) or x = f ( q) (Geometric Model) n
Inverse Kinematics
θ3 y0 l1 l3 l2 θ 2 θ1 x 0 α
Chapter 4 Inverse Kinematics
Direct Kinematics f
q•
(forward kinematics)
•x
Joint Space (dimensions n)
LMq OP MMq. PP q=M P MM . PP MNq. PQ
1 2 n
Task Space (dimensions m)