Slide Series 5: IKS for Robots
description
Transcript of Slide Series 5: IKS for Robots
SLIDE SERIES 5:IKS FOR ROBOTS
ME 4135Robotics & ControlR. Lindeke, Ph. D.
FKS vs. IKS
In FKS we built a tool for finding end frame geometry from Given Joint data:
In IKS we need Joint models from given End Point Geometry:
Joint Space
Cartesian Space
Joint Space
Cartesian Space
So this IKS is Nasty (as we know!)
It a more difficult problem because: The Equation set is “Over-Specified”
12 equations in 6 unknowns Space is “Under-Specified”
Planer devices with more joints than 2 The Solution set can contain
Redundancies Multiple solutions
The Solution Sets may be un-defined Unreachable in 1 or many joints
Uses of IKS
Builds Workspace
Allows “Off-Line Programming” solutions
Thus, compares Workspace capabilities with Programming realities to assure that execution is feasible
Aids in Workplace design and operation simulations
Doing a Pure IKS solution: the R Manipulator
X0
Y0
Z0
Z1
X1
Y1 Y2
X2
Z2
R Frame Skeleton (as last time)
LP Table and Ai’s
1
1 0 1 01 0 1 00 1 0 00 0 0 1
S CC S
A
Frames Link Var d a S C S C
0 → 1 1 R + 90 0 0 90 1 0 C1 -S1
1 → 2 2 P 0 d2 + cl2 0 0 1 0 1 0
22 2
1 0 0 00 1 0 00 0 10 0 0 1
Ad cl
FKS is A1*A2:
2 2
1 0 1 0 1 0 0 01 0 1 0 0 1 0 00 1 0 0 0 0 10 0 0 1 0 0 0 1
S CC S
d cl
2 2
2 2
1 0 1 1 ( )1 0 1 1 ( )0 1 0 00 0 0 1
S C C d clC S S d cl
Forming The IKS:
2 2
2 2
1 0 1 1 ( )1 0 1 1 ( )0 1 0 00 0 0 1
S C C d clC S S d cl
0 0 0 1
x x x x
y y y y
z z z z
n o a dn o a dn o a d
Forming The IKS:
Examining these two equation (n, o, a and d are ‘givens’ in an inverse sense!!!):
Term (1, 4) & (2,4) both side allow us to find an equation for :
(1,4): C1*(d2+cl2) = dx (2,4): S1*(d2+cl2) = dy
Form a ratio to build Tan(): S1/C1 = dy/ dx Tan = dy/dx = Atan2(dx, dy)
Forming The IKS:
After is found, back substitute and solve for d2:
(1,4): C1*(d2+cl2) = dx
Isolating d2: d2 = [dx/C1] - cl2
Alternative Method – doing a pure inverse approach Form A1
-1 then pre-multiply both side by this ‘inverse’
Leads to: A2 = A1-1*T0
ngiven
2 2
1 0 0 0 1 1 0 00 1 0 0 0 0 1 00 0 1 1 1 0 00 0 0 1 0 0 0 1 0 0 0 1
x x x x
y y y y
z z z z
S C n o a dn o a d
d cl C S n o a d
Simplify RHS means:
1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1
0 0 0 1
x y x y x y x y
z z z z
x y x y x y x y
S n C n S o C o S a C a S d C d
n o a d
C n S n C o S o C a S a C d S d
2 2
1 0 0 00 1 0 00 0 10 0 0 1
d cl
Solving:
Selecting and Equating (1,4) 0 = -S1*dx + C1*dy
Solving: S1*dx = C1*dy
Tan() = (S1/C1) = (dy/dx) = Atan2(dx, dy)
Selecting and Equating (3,4) -- after back substituting solution
d2 + cl2 = C1*dx + S1*dy
d2 = C1*dx + S1*dy - cl2
Performing IKS For Industrial Robots:
First lets consider the previously defined Spherical Wrist simplification All Wrist joint Z’s intersect at a point The nth Frame is offset from this
intersection by a distance dn along the a vector of the desired solution (3rd column of desired orientation sub-matrix)
This result is as expected by following the DH Algorithm!
Performing IKS
We can now separate the effects of the ARM joints Joints 1 to 3 in a full function manipulator
(without redundant joints) They function to position the spherical wrist at a
target POSITION related to the desired target POSE
… From the WRIST Joints Joints 4 to 6 in a full functioning spherical wrist Wrist Joints function as the primary tool to
ORIENT the end frame as required by the desired target POSE
Performing IKS: Focus on Positioning
We will define a point (the WRIST CENTER) as: Pc = [Px, Py, Pz] Here we define Pc = dtarget - dn*a
Px = dtarget,x - dn*ax
Py = dtarget,y - dn*ay
Pz = dtarget,z - dn*az
Focusing on the ARM Manipulators in terms of Pc:
Prismatic: q1 = Pz (its along Z0!) – cl1 q2 = Px or Py - cl2 q3 = Py or Px - cl3
Cylindrical: 1 = Atan2(Px, Py) d2 = Pz – cl2 d3 = Px*C1 – cl3 or +(Px
2 + Py2).5 – cl3
Focusing on the ARM Manipulators in terms of Pc:
Spherical: 1 = Atan2(Px, Py) 2 = Atan2( (Px
2 + Py2).5 , Pz)
D3 = (Px2 + Py
2 + Pz2).5 – cl3
Focusing on the ARM Manipulators in terms of Pc: Articulating:
1 = Atan2(Px, Py) 3 = Atan2(D, (1 – D2).5)
Where D =
2 = - is: Atan2((Px2 + Py2).5, Pz)
is:
2 2 2 2 22 3
2 32x y zP P P a a
a a
22 3
2 2 2 2 22 3
sintan2( )cos
2 1tan2
x y z
A
a a DA
P P P a a
One Further Complication:
This can be considered the d2 offset problem
A d2 offset is a design issue that states that the nth frame has a non-zero offset along the Y0 axis as observed with all joints at home, in the solution of the T0
n This leads to two solutions for 1
the So-Called Shoulder Left and Shoulder Right solutions
Defining the d2 Offset issue
X0, X1
Y0, Z1
Z0 d2
The ARMThe Wrist
Ypc
Xpc
Zpc
Here: ‘The ARM’ might be a prismatic joint as in the Stanford Arm or it might be a2 & a3 links in an Articulating Arm and rotates out of plane
A d2 offset means that there are two places where 1 can be placed to touch a given point (and note, when 1 is at Home, the wrist center is not on the X0 axis!)
Lets look at this Device “From the Top”
Pc'(Px, Py)
X0
Y0
Z1
Z1'
X1
d2
d2
a2'
a3'
R'
X1'
aQ11
F1
Solving For 1
We will have a Choice (of two) poses for :
1 1 1
1
.52 2 22 2
11 tan 2( , )
tan 2 ,
pc pc
pc pc
A X Y
A X Y d d
2 1 1
.52 2 22 2
1 180
180 tan2 ,
tan 2 ,
pc pc
pc pc
A X Y d d
A X Y
In this so-called “Hard Arm” We have two 1’s These lead to two 2’s (Spherical) Or to four 2’s and 3’s in the
Articulating Arm Shoulder Right Elbow Up & Down Shoulder Left Elbow Up & Down
The Orientation Model
Evolves from:
Separates Arm Joint and Wrist Joint Contribution to the desiredTarget (given) orientation
3 60 3 givenR R R
Focusing on Orientation Issues Lets begin by
considering Euler Angles (they are a model that is almost identical to a full functioning Spherical Wrist):
Form Product: Rz1*Ry2*Rz3 This becomes R3
6
1
2
3
cos sin 0sin cos 00 0 1
cos 0 sin0 1 0sin 0 cos
cos sin 0sin cos 00 0 1
z
y
z
R
R
R
Euler Wrist Simplified:C C C S S C C S S C C SS C C C S S C S C C S S
S C S S C
130
x x x
y y y
z z z given
n o aR n o a
n o a
And this matrix is equal to a U matrix prepared by multiplying the inverse of the ARM joint orientation matrices inverse and the Desired (given) target orientation
NOTE: R03 is
Manipulator Arm dependent!
Simplifying the RHS: (our so-called U Matrix)
30
130
11 12 1321 22 2331 32 33
11 21 3112 22 3213 23 33
(this is a transpose!)
R R RR R R R
R R R
R R RR R R R
R R R
Continuing:
11 12 1321 22 2331 32 33
11 21 31 11 21 31 11 21 3112 22 32 12 22 32 12 22 3213 23 33 13 23 33 13 23 33
x y z x y z x y z
x y z x y z x y z
x y z x y z x y z
U U UU U UU U U
n R n R n R o R o R o R a R a R a Rn R n R n R o R o R o R a R a R a Rn R n R n R o R o R o R a R a R a R
Finally:
63
C C C S S C C S S C C SR S C C C S S C S C C S S
S C S S C
LHS
130
11 12 13* 21 22 23
31 32 33
U U UR R U U U
U U U
givenRHS
Solving for Individual Orientation Angles (1st ):
Selecting (3,3)→ C = U33
With C we “know” S = (1-C2).5
Hence: = Atan2(U33, (1-U332).5
NOTE: 2 solutions for !
Re-examining the Matrices: To solve for : Select terms: (1,3) &
(2,3) CS = U13 SS = U23 Dividing the 2nd by the 1st: S /C =
U23/U13 Tan() = U23/U13 = Atan2(U13, U23)
Continuing our Solution:
To solve for : Select terms: (3,1) & (3,2) -SC = U31 SS = U32 Tan() = U32/-U31 = Atan2(-U31, U32)
Summarizing:
= Atan2(U33, (1-U332).5
= Atan2(U13, U23)
= Atan2(-U31, U32)
Let do a (true) Spherical Wrist:
Z3
X3
Y3
Z4
X4
Y4
Z5
X5
Y5
Z6
X6
Y6
IKSing the Spherical WristFrames Link Var d a
3 → 4 4 R 4 0 0 -90
4 → 5 5 R 5 0 0 +90
5 → 6 6 R 6 d6 0 0
4
4 0 4 5 0 5 6 6 04 0 4 ; 5 5 0 5 ; 6 6 6 00 1 0 0 1 0 0 0 1
C S C S C S
R S C R S C R S C
Writing The Solution:
11 12 13
21 22 23
31 32 33
4 0 4 5 0 5 6 6 04 0 4 5 0 5 6 6 00 1 0 0 1 0 0 0 1
C S C S C SS C S C S C
U U UU U UU U U
Lets See By Pure Inverse Technique:
11 12 13
21 22 23
31 32 33
5 0 5 6 6 05 0 5 6 6 00 1 0 0 0 1
4 4 00 0 14 4 0
C S C SS C S C
C S U U UU U U
S C U U U
Simplifying
11 21 12 22 13 23
31 32 33
21 11 22 12 23 13
5 6 5 6 55 6 5 6 56 6 0
4 4 4 4 4 4
4 4 4 4 4 4
C C C S SS C S S CS C
C U S U C U S U C U S UU U U
C U S U C U S U C U S U
Solving:
Examination here lets select (3,3) both sides:
0 = C4U23 – S4U13
S4U13 = C4U23
Tan(4) = S4/C4 = U23/U13
4 = Atan2(U13, U23) With the given desired orientation
and values (from the arm joints) we have a value for 4 the RHS is completely known
Solving for 5 & 6
For 5: Select (1,3) & (2,3) terms S5 = C4U13 + S4U23
C5 = U33
Tan(5) = S5/C5 = (C4U13 + S4U23)/U33
5 = Atan2(U33, C4U13 + S4U23)
For 6: Select (3,1) & (3, 2) S6 = C4U21 – S4U11
C6 = C4U22 – S4U12
Tan(6) = S6/C6 = ([C4U21 – S4U11],[C4U22 – S4U12]) 6 = Atan2 ([C4U21 – S4U11], [C4U22 – S4U12])
Summarizing:
4 = Atan2(U13, U23)
5 = Atan2(U33, C4U13 + S4U23)
6 = Atan2 ([C4U21 – S4U11], [C4U22 – S4U12])
Lets Try One:
Cylindrical Robot w/ Spherical Wrist Given a Target matrix (it’s an IKS
after all!) The d3 “constant” is 400mm; the d6
offset (call it the ‘Hand Span’) is 150 mm.
1 = Atan2((dx – ax*150),(dy-ay*150)) d2 = (dz – az*150) d3 = [(dx – ax*150)2,(dy-ay*150)2].5 -
400
The Frame Skeleton:
X
ZF0
F1 Z
X
X
F2 Z
Z
X
F6
F2.5
Z
X
F4
XF3
Z
F5
ZX
X
Z
Note “Dummy” Frame to account for Orientation problem with Spherical Wrist modeled earlier
Solving for U:
1 1 0 0 0 1 0 0 1 0 1 01 1 0 1 0 0 1 0 0 1 0 00 0 1 0 1 0 0 1 0 0 0 1
x x x
y y y
z z z
C S n o aU S C n o a
n o a
NOTE: We needed a “Dummy Frame” to account for the Orientation issue at the end of the Arm
Simplifying:
11 12 13
21 22 23
31 32 33
1 1 1 1 1 11 1 1 1 1 1
x z x z x z
x z x z x z
y y y
U U U C n S n C o S o C a S aU U U S n C n S o C n S a C aU U U n o a
Subbing Uij’s Into Spherical Wrist Joint Models:
4 = Atan2(U13, U23)= Atan2((C1ax + S1az), (S1ax-C1az))
5 = Atan2(U33, C4U13 + S4U23)= Atan2{ay, [C4(C1ax+S1az) + S4 (S1ax-C1az)]}
6 = Atan2 ([C4U21 - S4U11], [C4U22 - S4U12]) = Atan2{[C4(S1nx-C1nz) - S4(C1nx+S1nz)], [C4(S1ox-C1oz) - S4(C1ox+S1oz)]}