robotic kinematics_inverse kinematic solution
TRANSCRIPT
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
1/54
11/24/2010ME 3230Page 1
Robotic Kinematics the InverseKinematic Solution
ME 3230
Kinematics and MechatronicsDr. R. Lindeke
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
2/54
11/24/2010ME 3230Page 2
FKS vs. IKS
In FKS we built a tool for finding end frame geometryfrom Given Joint data:
In IKS we need Joint models from given End PointGeometry:
Joint
Space
CartesianSpace
Joint
SpaceCartesianSpace
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
3/54
11/24/2010ME 3230Page 3
So this IKS Problem is Nasty(in Mathematics this isconsidered a Hard Modeling Issue)
It a more difficult problem because:
The Equation set is Over-Specified:
12 equations in 6 unknowns
Space can be 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
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
4/54
11/24/2010ME 3230Page 4
But the IKS is VERY Useful some Usesof IKS include:
Building Workspace Maps
Allow Off-Line Programming solutions
The IKS allows the engineer to equate Workspacecapabilities with Programming realities to assure thatexecution is feasible(as done in ROBCAD & IGRIP)
The IKS Aids in Workplace Design and OperationalSimulations
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
5/54
11/24/2010ME 3230Page 5
Doing a Pure IKS solution: the UR Manipulator
X0
Y0
Z0
Z1
X1
Y1 Y2
X2
Z2
X0
Y0
Z0
Z1
X1
Y1 Y2
X2
Z2
Same Origin
Point!
UR Frame Skeleton (as DH Suggest!)
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
6/54
11/24/2010ME 3230Page 6
LP Table and Ais
1
1 0 1 0
1 0 1 00 1 0 0
0 0 0 1
S C
C SA
! -
Frames Link Var U d a E SE CE S U CU
0 1 1 R U + 90 0 0 90 1 0 C1 -S1
1 2 2 P 0 d2 + cl2 0 0 1 0 1 0
2
2 2
1 0 0 0
0 1 0 00 0 1
0 0 0 1
Ad cl
! -
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
7/54
11/24/2010ME 3230Page 7
FKS is A1*A2:
2 2
1 0 1 0 1 0 0 0
1 0 1 0 0 1 0 0
0 1 0 0 0 0 10 0 0 1 0 0 0 1
S
S
d cl
y
- -
2 2
2 2
1 0 1 1 ( )
1 0 1 1 ( )0 1 0 0
0 0 0 1
S C C d cl
C S S d cl
-
g
g
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
8/54
11/24/2010ME 3230Page 8
Forming The IKS:
2 2
2 2
1 0 1 1 ( )
1 0 1 1 ( )
0 1 0 0
0 0 0 1
S d cl
S S d cl
-
0 0 0 1
x x x x
y y y y
z z z z
n o a d
n o a d
n o a d
-
In the Inverse Problem, The RHS MATRIX is completely known(perhaps from a robot mapped solution)! And we use these values tofind a solution to the joint equations that populate the LHS MATRIX
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
9/54
11/24/2010ME 3230Page 9
Forming The IKS:
Examining these two matrices n, o, a and d are givens in the inverse sense!!!
(But typically we want to build generalmodels leaving these terms
unspecified) Term (1, 4) & (2,4) on both sides allow us to find an equation for
U:
(1,4): C1*(d2+cl2) = dx (2,4): S1*(d
2+cl
2) = d
y
Form a ratio to build Tan(U): S1/C1 = dy/ dx Tan U = dy/dx U = Atan2(dx, dy)
If 2 Matrices areEqual then EACHand EVERY term isalso Uniquely equalas well!
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
10/54
11/24/2010ME 3230Page 10
Forming The IKS:
After U is found, back substituteand solve for d
2
: (1,4): C1*(d2+cl2) = dx Isolating d2: d2 = [dx/CosU1] - cl2
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
11/54
11/24/2010ME 3230Page 11
Alternative Method doing a pure inverse approach
Form A1-1 then pre-multiply both side by this
inverse
Leads to: A2 = A1-1
*T0n
given
2 2
1 0 0 0 1 1 0 0
0 1 0 0 0 0 1 0
0 0 1 1 1 0 0
0 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 d
n o a d
d cl C S n o a d
! y - - -
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
12/54
11/24/2010ME 3230Page 12
Simplifying & Solving:
Selecting and Equating (1,4) 0 = -S1*dx +C1*dy Solving: S1*d
x= C1*d
y Tan(U) = (S1/C1) = (dy/dx) U = Atan2(dx, dy) the same as before
Selecting and Equating (3,4) -- after backsubstituting U solution
d2 + cl2 = C1*dx + S1*dy d2 = C1*dx + S1*dy - cl2
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
13/54
11/24/2010ME 3230Page 13
Performing IKS For Industrial Robots: (a moreinvolved problem)
First lets consider the concept of the Spherical WristSimplification:
All Wrist joint Zs intersect at a point The n Frame is offset from this Zs intersection point at a
distance dn (the hand span) along the a vector of thedesired solution (3rd column of desired orientation sub-matrix!) which is the z direction of the 3rd wrist joint
This follows the DH Algorithm design tools as we havelearned them!
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
14/54
11/24/2010ME 3230Page 14
Performing IKS with a Spherical Wrist
We can now separate the POSE effects:
ARM joints
Joints 1 to 3 in a full function manipulator (without redundantjoints)
They function to maneuver the spherical wrist to a targetPOSITION related to the desired target POSE
WRIST Joints Joints 4 to 6 in a full functioning spherical wrist
Wrist Joints function as a primary tool to ORIENT the end frameas required by the desired target POSE
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
15/54
11/24/2010ME 3230Page 15
Performing IKS: Focus on Positioning
We will define a point(called the WRISTCENTER) where all 3 zs intersect as:
Pc = [Px, Py, Pz] We find that this position is exactly:
Pc = dtarget- dn*a
Px
= dtarget,x
- dn*a
x Py = dtarget,y - dn*ay Pz = dtarget,z - dn*az
Note: dn is the so calledHandspan (a CONSTANT)
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
16/54
11/24/2010ME 3230Page 16
Treating the Arm Types (as separaterobot entities):
Cartesian (2 types) Cantilevered
Gantry
Cylindrical
Spherical (w/o d2 offset)
2 Link Articulating (w/o d2 offset)
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
17/54
11/24/2010ME 3230Page 17
Cantilevered Cartesian Robot
P-P-P
Configuration
J1
J3
J2
X0
Y0
Z0
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
18/54
11/24/2010ME 3230Page 18
Gantry Cartesian Robot
P-P-P
ConfigurationZ0X0
Y0
J1
J2
J3
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
19/54
11/24/2010ME 3230Page 19
Cylindrical Robot
Doing IKS given a
value for: X, Y and Z of End
Compute U, Z and R
R
X0Y0
Z0
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
20/54
11/24/2010ME 3230Page 20
Spherical Robot
Developing a IKS
(model): Given Xe, Ye, & Ze Compute U, J & R
R
J
X0
Y0
Z0
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
21/54
11/24/2010ME 3230Page 21
2-Link Articulating Arm Manipulator
U3
U1
U2
L2
L1
Z0
X0Y0
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
22/54
11/24/2010ME 3230Page 22
Focusing on the ARM Manipulatorsin terms of P
c
:
Prismatic: q1 = d1= Pz (its along Z0!) cl1 q
2= d2 = P
xor P
y- cl
2 q3 = d3= Py or Px - cl3
Cylindrical: U1 = Atan2(Px, Py) d2 = Pz cl2 d3 = Px/C1 cl3 {or +(Px
2 + Py2).5 cl3}
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
23/54
11/24/2010ME 3230Page 23
Focusing on the ARM Manipulatorsin terms of P
c
:
Spherical: U1 = Atan2(Px, Py)
U2 = Atan2((Px2 + Py2).5 , Pz) D3 = (Px
2 + Py2 + Pz
2).5 cl3
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
24/54
11/24/2010ME 3230Page 24
Focusing on the ARM Manipulatorsin terms of P
c
:
Articulating: U1 = Atan2(Px, Py)
U3 = Atan2(D, s(1 D2).5)
Where D =
U2 = J - E J is: Atan2((Px2 + Py2).5, Pz)
E is:
2 2 2 2 22 32 32
x y z P P P a a
a a
2
2 3
2 2 2 2 2
2 3
sintan2( )cos
2 1tan2
x y z
A
a a DA
P P P a a
E E !
s
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
25/54
11/24/2010ME 3230Page 25
Focusing on the ARM Manipulatorsin terms of P
c
:
U2 =
Where D =
2
2 3
2 2 2 2 22 3
2 1
tan2 x y z
a a D
A P P P a a
s
2 2 2 2 22 32 32
x y z P P P a a
a a
Atan2((Px2 + Py
2).5, Pz) -
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
26/54
11/24/2010ME 3230Page 26
One Further Complication MustBeConsidered:
This is called the d2 offset problem
A d2 offset is a problem that states that the nth frame
has a non-zero offset along the Y0 axis as observed in
the solution of the T0n with all joints at home (like the 5 dof articulating robots in our lab!)
This leads to two solutions for U1 theSo-Called Shoulder Left and Shoulder Rightsolutions
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
27/54
11/24/2010ME 3230Page 27
Defining the d2
Offset issue
X0, X1
Y0, Z1
Z0 d2The ARM
The
Wrist
Ypc
Xpc
Zpc
Here: The ARM might contain a prismatic joint (as in the Stanford Arm discussed
in text) or it might be the a2 & a3 links in an Articulating Arm as it rotates out of plane
A d2 offset means that there are two places where U1 can be placed to touch a givenpoint (and note, when U1 is at Home, the wrist centeris not on the X0 axis!)
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
28/54
11/24/2010ME 3230Page 28
Lets look at this Device From the Top a planview of the structure projected to the X0Y0 plane
Pc'(Px, Py)
X0
Y0
Z1
Z1'
X1
d2
d2
a2'
a3'
R'
X1'
E
U11
F1
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
29/54
11/24/2010ME 3230Page 29
Solving For U1:
We will have a Choice of two poses for U1:
1 1 1
1
.52 2 2
2 2
1
1 tan2( , )
tan2 ,pc pc
pc pc
A X Y
A X Y d d
U J E
U
!
!
2 1 1
.52 2 2
2 2
1 180
180 tan2 ,
tan2 ,pc pc
pc pc
A X Y d d
A X Y
U E J! r !
r
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
30/54
11/24/2010ME 3230Page 30
This device (like the S110 robots in lab) is called aHard Arm Solution
We have two U1s These lead to two U2s (Spherical)
One for Shoulder Right & one for Shoulder Left
Or four U2s and U3s in the Articulating Arm: Shoulder Right Elbow Up & Down
Shoulder Left Elbow Up & Down
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
31/54
11/24/2010ME 3230Page 31
Now, lets look at Orientation IKSs
Orientation IKS again relies on separation ofjoint effects
We (now) know the first 3 joints controlpositions they would have been solved by using the appropriate set of
equations developed above
The last three (wrist joints) will control theachievement of our desired Orientation
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
32/54
11/24/2010ME 3230Page 32
These Ideas Lead to an Orientation Model for aDevice that is Given by:
This model separates Arm Joint and WristJointContribution to the desired Target
Orientation Note: target orientation is a given for the
IKS model!
3 6
0 3 given R R R!g
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
33/54
11/24/2010ME 3230Page 33
Focusing on Orientation Issues
Lets begin by considering EulerAngles(they are a model that isalmost identical to a fullfunctioning Spherical Wrist as
defined using the D-H algorithm!):
Step 1, Form a Product:
Rz1*Ry2*Rz3
This product becomes R36 inthe model on the previous slide
1
2
3
cos sin 0
sin cos 0
0 0 1
cos 0 sin
0 1 0
sin 0 cos
cos sin 0
sin cos 0
0 0 1
z
y
z
R
R
R
J J
J J
U U
U U
] ]
] ]
! - ! -
! -
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
34/54
11/24/2010ME 3230Page 34
Euler Wrist Simplified:
C C C S S C C S S C C S
S C C C S S C S C C S S
S C S S C
J U ] J ] J U ] J ] J U
J U ] J ] J U ] J ] J U
U ] U ] U
- this matrix, which contains the joint control angles, is then set equal to a U
matrix prepared by multiplying the inverse of the ARM joint orientation sub-
matrices and the Desired (given) target orientation sub-matrix:
130 x x x
y y y
z z z given
n o a
U n o a
n o a
! -
g
NOTE: R03 is
Manipulatordependent!
(Inverse required a
Transpose)
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
35/54
11/24/2010ME 3230Page 35
Continuing: to define the U-Matrix
11 12 13
21 22 23
31 32 33
11 21 31 11 21 31 11 21 31
12 22 32 12 22 32 12 22 32
13 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 U
U U U
U U U
n n n o o o a a a
n n n o o o a a a
n n n o o o a a a
! -
-
Rij are terms from the product of the first 3 Ais (rotational sub-matrix)
ni, oi & ai are from given target orientation
we develop our models in a general way to allow computation ofspecific angles for specific cases
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
36/54
11/24/2010ME 3230Page 36
Now Set:LHS (euler angles) = RHS (U-Matrix)
C C C S S C C S S C C S
S C C C S S C S C C S S
S C S S C
J U ] J ] J U ] J ] J U
J U ] J ] J U ] J ] J UU ] U ] U
! -
11 12 13
21 22 23
31 32 33
U U U
U U U
U U U
-
U as defined on theprevious slide! (a
function of arm-joint
POSE and Desired End-
Orientation)
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
37/54
11/24/2010ME 3230Page 37
Solving for Individual Orientation Angles (1stsolve forUthemiddle one):
Selecting (3,3) CU = U33
With CU we know SU = s(1 - C2U).5
Hence: U = Atan2(U33, s(1-U332).5)
NOTE: leads to 2 solutions for U!
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
38/54
11/24/2010ME 3230Page 38
Re-examining the Matrices:
To solve for J: Select terms: (1,3) & (2,3) CJSU = U13
SJSU = U23 Dividing the 2nd by the 1st: SJ/CJ = U23/U13 Tan(J) = U23/U13 J = Atan2(U13, U23)
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
39/54
11/24/2010ME 3230Page 39
Continuing our Solution:
To solve for ]: Select terms: (3,1) &(3,2)
-SUC] = U31 SUS] = U32 (and dividing this by previous) Tan(]) = U32/-U31 (note sign migrates with
term!)] = Atan2(-U31, U32)
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
40/54
11/24/2010ME 3230Page 40
Summarizing:
U = Atan2(U33, s(1-U332).5)
J = Atan2(U13, U23)
] = Atan2(-U31, U32)
Uij
s as defined on the earlier slide!
and
U is manipulator and desired orientation
dependent
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
41/54
11/24/2010ME 3230Page 41
Let consider a Spherical Wrist:
Z3
X3
Y3
Z4
X4
Y4
Z5
X5
Y5
Z6
X6
Y6 Here drawn in
Good Kinematic
Home for
attachment to anArticulating Arm
Same OriginPoint
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
42/54
11/24/2010ME 3230Page 42
IKSing the Spherical Wrist
Frames Link Var 5 d a E
3 4 4 R U4 0 0 -90
4 5 5 R U5 0 0 +90
5 6 6 R U6 d6 0 0
4
4 0 4 5 0 5 6 6 0
4 0 4 ; 5 5 0 5 ; 6 6 6 0
0 1 0 0 1 0 0 0 1
C S C S C S
R S C R S C R S C
! ! ! - - -
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
43/54
11/24/2010ME 3230Page 43
Writing The Solution:
11 12 13
21 22 23
31 32 33
4 0 4 5 0 5 6 6 0
4 0 4 5 0 5 6 6 0
0 1 0 0 1 0 0 0 1
C S C S C S
S C S C S C
U U U
U U UU U U
y y !
- - -
-
Uijs as defined on the earlier slide!
and
U is manipulator and desired orientation
dependent
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
44/54
11/24/2010ME 3230Page 44
Lets Solve (here I try the Pure Inverse Technique):
11 12 13
21 22 23
31 32 33
5 0 5 6 6 0
5 0 5 6 6 0
0 1 0 0 0 1
4 4 0
0 0 1
4 4 0
C S C S
S C S C
C S U U U
U U U
S C U U U
y !
- -
- -
Note:
R4s
Inverse
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
45/54
11/24/2010ME 3230Page 45
Simplifying
11 21 12 22 13 23
31 32 33
21 11 22 12 23 13
5 6 5 6 5
5 6 5 6 5
6 6 0
4 4 4 4 4 4
4 4 4 4 4 4
C C C S S
S C S S C
S C
C U S U C U S U C U S U
U U U
C U S U C U S U C U S U
! -
-
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
46/54
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
47/54
11/24/2010ME 3230Page 47
Solving for U5 & U6 For U5: Select(1,3) & (2,3) terms
S5 = C4U13 + S4U23 C5 = U33 Tan(
U5) = S
5/C
5= (C
4U
13+ S
4U
23)/U
33 U5 = Atan2(U33, C4U13 + S4U23)
For U6: Select(3,1) & (3, 2) terms S6 = C4U21 S4U11 C6 = C4U22 S4U12 Tan(U6) = S6/C6 = ([C4U21 S4U11]/[C4U22
S4U12])
U6 = Atan2 ([C4U22 S4U12], [C4U21 S4U11])
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
48/54
11/24/2010ME 3230Page 48
Using the Pure Inversing:
We removed ambiguity from the solution
We were able to solve for joints In Order
Without noting we see the obviousrelationship between Spherical wristandEuler Orientation!
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
49/54
11/24/2010ME 3230Page 49
Summarizing:
U4 = Atan2(U13, U23)
U5 = Atan2(U33, C4U13 + S4U23)
U6 = Atan2 ([C4U22 S4U12], [C4U21 S4U11])
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
50/54
11/24/2010ME 3230Page 50
Lets Try One:
Cylindrical Robot w/ Spherical Wrist
Given would be a Target matrix from Robot Mapping!(its an IKS after all!)
The d3constant is 400mm; the d6 offset(the HandSpan) is 150 mm.
U1 = Atan2((dx ax*150),(dy-ay*150)) d2 = (dz az*150)
d3 = ((dx ax*150)2+p(dy-ay*150)
2).5 - 400
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
51/54
11/24/2010ME 3230Page 51
The Frame Skeleton:
X
ZF0
F1 Z
X
X
F2 Z
Z
X
F6
F2.5
Z
X
F4
XF3
Z
F5
Z
X
X
Z
Note Dummy Frame to account forOrientation problem
with Spherical Wrist might not be needed if we had set
wrist kinematic home for CylindricalMachine!
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
52/54
11/24/2010ME 3230Page 52
Solving for U:
1
1 1 0 0 0 1 0 0 1 0 1 01 1 0 1 0 0 1 0 0 1 0 0
0 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
! y y y y - - - - - -
NOTE: We needed a Dummy Frame to account for the
Orientation issue at the end of the Arm (as drawn) this
becomes a part of the arm space not the wrist space!
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
53/54
11/24/2010ME 3230Page 53
Simplifying:
11 12 13
21 22 23
31 32 33
1 1 1 1 1 1
1 1 1 1 1 1
x z x z x z
y y y
x z x z x z
U U U C n
Sn
Co
So
Ca
Sa
U U U n o a
U U U S n C n S o C n S a C a
! - -
-
8/8/2019 Robotic Kinematics_Inverse Kinematic Solution
54/54
11/24/2010ME 3230Page 54
Subbing Uijs Into Spherical Wrist JointModels:
U4 = Atan2(U13, U23)= Atan2((C1ax + S1az), ay)
U5 = Atan2(U33, C4U13 + S4U23)= Atan2{ (S1ax-C1az) , [C4(C1ax+S1az) + S4*ay]}
U6 = Atan2 ([C4U21 - S4U11], [C4U22 - S4U12])= Atan2{[C4*ny - S4(C1nx+S1nz)],
[C4*oy - S4(C1ox+S1oz)]}