Post on 19-Jan-2016
description
Robot Modeling and the Forward Kinematic Solution
ME 3230
R. R. Lindeke
Looking Closely at the T0n Matrix
At the end of our discussion of “Robot Mapping” we found that the T0
n matrix related the end of the arm frame (n) to its base (0) – Thus it must contain information related to
the several joints of the robot It is a 4x4 matrix populated by complex
equations for both position and orientation (POSE)
Looking Closely at the T0n Matrix
To get at the equation set, we will choose to add a series of coordinate frames to each of the joint positions
The frame attached to the 1st joint is labeled 0 – the base frame! – while joint two has Frame 1, etc.
The last Frame is the end or n Frame
Looking Closely at the T0n Matrix
As we have seen earlier, we can define a HTM (T(i-1)
i) to the transformation between any two SO3 based frames
Thus we will find that the T0n is
given by a product formed by:1 2
0 0 1 1n n
nT T T T g gL g
Looking Closely at the T0n Matrix
For simplicity, we will redefine each of the of these transforms (T(i-1)
i) as Ai
Then, for a typical 3 DOF robot Arm, T0
n = A1*A2*A3
While for a full functioned 6 DOF robot (arm and wrist) would be: T0
n = A1*A2*A3*A4*A5*A6
A1 to A3 ‘explain’ the arm joint effect while A4 to A6 explain the wrist joint effects
Looking At The Frame To Frame Arrangements – Building A Modeling Basis
When we move from one frame to another, we will encounter: Two translations (in a controlled sense) Two rotations (also in a controlled sense)
A rotation and translation WRT the Zi-1
These are called the Joint Parameters
A rotation and translation WRT the Xi
These are called the Link Parameters
A model of the Joint Parameters
NOTE!!!
A model of the Link Parameters
ai or
Talking Specifics – Joint Parameters
i is an angle measured about the Zi-1 axis from Xi-1 to Xi and is a variable for a revolute joint – its fixed for a Prismatic Joint
di is a distance measured from the origin of Frame(i-1) to the intersection of Zi-1 and Xi and is a variable for a prismatic joint – its fixed for a Revolute Joint
Talking Specifics – Link Parameters
ai (or li) is the Link length and measures the distance from the intersection of Zi-1 to the origin of Framei measured along Xi
i is the Twist angle which measures the angle from Zi-1 to Zi as measured about Xi
Both of these parameters are fixed in value regardless of the joint type. A Further note: Fixed does not mean zero
degrees or zero length just that they don’t change
Very Important to note:
Two Design Axioms prevail in this modeling approach Axiom DH1: The Axis Xi must be designed to be
perpendicular to Zi-1
Axiom DH2: The Axis Xi must be designed to intersect Zi-1
Thus, within reason we can design the structure of the coordinate frames to simplify the math (they are under our control!)
Returning to the 4 ‘Frame-Pair’ Operators:
1st is which is an operation of pure rotation about Z or:
2nd is d which is a translation along Z or:
os 0 0
0 0
0 0 1 0
0 0 0 1
C Sin
Sin Cos
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
d
Returning to the 4 Frame Operators:
3rd is a Translation Along X or:
4th is which is a pure Rotation about X or:
1 0 0
0 1 0 0
0 0 1 0
0 0 0 1
a
1 0 0 0
0 0
0 0
0 0 0 1
Cos Sin
Sin Cos
The Overall Effect is:
os 0 0 1 0 0 0 1 0 0 1 0 0 0
0 0 0 1 0 0 0 1 0 0 0 0
0 0 1 0 0 0 1 0 0 1 0 0 0
0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1
C Sin a
Sin Cos Cos Sin
d Sin Cos
Simplifying this Matrix Product:
0
0 0 0 1
i i i i i i i
i i i i i i i
i i i
C S C S S a C
S C C C S a S
S C d
This matrix is the general transformation relating each and every of the frame pairs
along a robot structure
So, Since We Can Control the Building of this Set Of Frames, What Are The Rules?
We will employ a method called the Denavit-Hartenberg Method
It is a Step-by-Step approach for modeling each of the frames from the initial (or 0) frame all the way to the n (or end) frame
This modeling technique makes each joint axis (either rotation or extension) the Z-axis of the appropriate frame (Z0 to Zn-1).
The Joint motion, thus, is taken W.R.T. the Zi-1 axis of the frame pair making up the specific transformation matrix under design
The D-H Modeling Rules:1) Locate & Label the Joint Axes: Z0 to Zn-1
2) Establish the Base Frame. Set Base Origin anywhere on the Z0 axis. Choose X0 and Y0 conveniently and to form a right hand frame.
3) Locate the origin Oi where the common normal to Zi-1 and Zi intersects Zi. If Zi intersects Zi-1 locate Oi at this intersection. If Zi-1 and Zi are parallel, locate Oi at Joint i+1
The D-H Modeling Rules:
4) Establish Xi along the common normal between Zi-1 and Zi through Oi, or in the direction Normal to the plane Zi-1 – Zi if these axes intersect
5) Establish Yi to form a right hand system Set i = i+1, if i<n loop back to step 3
(Repeat Steps 3 to 5 for I = 1 to I = n-1)
6) Establish the End-Effector (n) frame: OnXnYnZn. Assuming the n-th joint is revolute, set kn = a along the direction Zn-1. Establish the origin On conveniently along Zn, typically mounting point of gripper or tool. Set jn = o in the direction of gripper closure (opening) and set in = n such that n = oxa. Note if tool is not a simple gripper, set Xn and Yn conveniently to form a right hand frame.
The D-H Modeling Rules:
7) Create a table of “Link” parameters: 1) i as angle about Zi-1 between X’s
2) di as distance along Zi-1
3) i as angle about Xi between Z’s
4) ai as distance along Xi
8) Form HTM matrices A1, A2, … An by substituting , d, and a into the general model
9) Form T0n = A1*A2*…*An
Some Issues to remember:
If you have parallel Z axes, the X axis of the second frame runs perpendicularly between them
When working on a revolute joint, the model will be simpler if the two X directions are in alignment at “Kinematic Home” – ie. Our model’s starting point
To achieve this kinematic home, rotate the model about moveable axes (Zi-1’s) to align X’s
Kinematic Home is not particularly critical for prismatic joints
An ideal model will have n+1 frames However, additional frames may be necessary –
these are considered ‘Dummy’ frames since they won’t contain joint axes
Applying D-H to a General Case:
General Case: Considering Link i
Connects Frames: i-1 and I and includes Joint i
This information allowed us to ‘Build’ The L.P. (link parameter) Table as seen here
Frames Link Var d a S C S C
i -1 to i i R + 37 17.5 u 47.8 u 17.8 0.306 .952 S( + 37) C( + 37)
Leads to an Ai Matrix:
( 37 ) .952 ( 37 ) .306 ( 37 ) 47.8* ( 37 )
( 37 ) .952 ( 37 ) .306 ( 37 ) 47.8 ( 37 )
0 0.306 .952 17.5
0 0 0 1
C S S C
S C C S
Frame Skeleton for Prismatic Hand Robot
LP Table:
Frames Link Var d a S C S C
0 1 1 R 10 0 -90 -1 0 S1 C1
1 2 2 R 2-1 6 0 0 1 S2 C2
2 3 3 R 30.5 0 90 1 0 S3 C3
3 4 4 P 0 d4 + 4.25 0 0 0 1 S4 C4
4 n 5 R 51 0 0 0 1 S5 C5
Depends on Location of n(end)-frame!
Leading to 5 Ai Matrices
1
1 0 1 0
1 0 1 0
0 1 0 0
0 0 0 1
C S
S CA
3
3 0 3 0
3 0 3 0
0 1 0 .5
0 0 0 1
C S
S CA
2
2 2 0 6 2
2 2 0 6 2
0 0 1 1
0 0 0 1
C S C
S C SA
44
1 0 0 0
0 1 0 0
0 0 1 4.25
0 0 0 1
Ad
#5 is:
5
5 5 0 0
5 5 0 0
0 0 1 1
0 0 0 1
C S
S CA
Now, Lets Form the FKS: T0
n = A1*A2*A3*A4*A5
I’ll use a software: Mathematica
This value is called the Hand Span and depends on the Frame Skeleton we developed
Solving for FKS
Here we have a special case – two of the Joints are a “planer arm” revolute model – i.e. parallel, consecutive revolute joints
These are contained in the A2 and A3 Matrices These should be pre-multiplied using a
trigonometric tool that recognizes the sum of angle cases ((Full)Simplify in mathematica)
Basically then: T0n = A1*{A2A3}*A4*A5
Finalizing the FKS – perform a physical verification
Physical verification means to plug known angles into the variables and compute the Ai’s and FKS against the Frame Skeleton
Another? 6dof Articulating Arm – (The Figure Contains Frame Skelton)
LP Table
Frames
Link
Var d a S C S C
0 1 1 R 1 0 0 90 -1 0 S1 C1
1 2 2 R 2 0 a2 0 0 1 S2 C2
2 3 3 R 3 0 a3 0 0 1 S3 C3
3 4 4 R 4 0 a4 -90 -1 0 S4 C4
4 5 5 R 5 0 0 90 1 0 S5 C5
5 6 6 R 6* d6 0 0 0 1 S6 C6
* With End Frame in Better Kinematic Home. Here, as shown, it would be (6 - 90), which is a problem!
A Matrices
1
1 0 1 0
1 0 1 0
0 1 0 0
0 0 0 1
C S
S CA
2
22
2 2 0 2
2 2 0 2
0 0 1 0
0 0 0 1
C S a C
S C a SA
3
33
3 3 0 3
3 3 0 3
0 0 1 0
0 0 0 1
C S a C
S C a SA
4
44
4 0 4 4
4 0 4 4
0 1 0 0
0 0 0 1
C S a C
S C a SA
A Matrices, cont.
5
5 0 5 0
5 0 5 0
0 1 0 0
0 0 0 1
C S
S CA
66
6 6 0 0
6 6 0 0
0 0 1
0 0 0 1
C S
S CA
d
At Better Kinematic Home!
Leads To:
FKS of:
0 1 2 3 4 5 6nT A A A A A A
PreProcessg g g g g
After Preprocessing A2-A3-A4, with (Full)Simplify function, the FKS must be
reordered as A1-A234-A5-A6 and Simplified
Solving for FKS
Pre-process {A2*A3*A4} with Full Simplify
They are the “planer arm” issue as in the previous robot model
Then Form: A1* {A2*A3*A4}*A5*A6
Simplify for FKS!
Simplifies to(in the expected Tabular Form):
nx = C1·(C5·C6·C234 - S6·S234) - S1·S5·C6ny = C1·S5·C6 + S1·(C5·C6·C234 - S6·S234)nz = S6·C234 + C5·C6·S234
ox = S1·S5·S6 - C1·(C5·S6·C234 + C6·S234)oy = - C1·S5·S6 - S1·(C5·S6·C234 + C6·S234)oz = C6·C234 - C5·S6·S234
ax = C1·S5·C234 + S1·C5ay = S1·S5·C234 - C1·C5az = S5·S234
dx = C1·(C234·(d6·S5 + a4) + a3·C23 + a2·C2) + d6·S1·C5dy = S1·(C234·(d6·S5 + a4) + a3·C23 + a2·C2) - d6·C1·C5dz = S234·(d6·S5 + a4) + a3·S23 + a2·S2
Verify the Model
Again, substitute known’s (typically 0 or 0 units) for the variable kinematic variables
Check each individual A matrix against your robot frame skeleton sketch for physical agreement
Check the simplified FKS against the Frame skeleton for appropriateness
After Substitution: nx = C1·(C5·C6·C234 - S6·S234) - S1·S5·C6 = 1(1-0) – 0 = 1 ny = C1·S5·C6 + S1·(C5·C6·C234 - S6·S234) = 0+ 0(1 – 0) = 0 nz = S6·C234 + C5·C6·S234 = 0 + 0 = 0
ox = S1·S5·S6 - C1·(C5·S6·C234 + C6·S234) = 0 – 1(0 + 0) = 0 oy = - C1·S5·S6 - S1·(C5·S6·C234 + C6·S234) = -0 – 0(0 + 0) = 0 oz = C6·C234 - C5·S6·S234 = 1 – 0 = 1
ax = C1·S5·C234 + S1·C5 = 0 + 0 = 0 ay = S1·S5·C234 - C1·C5 = 0 – 1 = -1 az = S5·S234 = 0
dx = C1·(C234·(d6·S5 + a4) + a3·C23 + a2·C2) + d6·S1·C5= 1*(1(0 + a4) + a3 + a2) + 0 = a4 + a3 + a2
dy = S1·(C234·(d6·S5 + a4) + a3·C23 + a2·C2) - d6·C1·C5= 0(1(0 + a4) + a3 + a2) – d6 = -d6
dz = S234·(d6·S5 + a4) + a3·S23 + a2·S2= 0(0 + a4) + 0 + 0 = 0