robot modeling and the forward kinematic solution
DESCRIPTION
Robot Modeling and the Forward Kinematic Solution. ME 3230 R. R. Lindeke. Looking Closely at the T 0 n Matrix. At the end of our discussion of “Robot Mapping” we found that the T 0 n matrix related the end of the arm frame (n) to its base (0) – - PowerPoint PPT PresentationTRANSCRIPT
![Page 1: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/1.jpg)
Robot Modeling and the Forward Kinematic Solution
ME 3230
R. R. Lindeke
![Page 2: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/2.jpg)
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)
![Page 3: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/3.jpg)
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
![Page 4: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/4.jpg)
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
![Page 5: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/5.jpg)
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
![Page 6: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/6.jpg)
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
![Page 7: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/7.jpg)
A model of the Joint Parameters
NOTE!!!
![Page 8: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/8.jpg)
A model of the Link Parameters
ai or
![Page 9: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/9.jpg)
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
![Page 10: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/10.jpg)
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
![Page 11: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/11.jpg)
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!)
![Page 12: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/12.jpg)
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
![Page 13: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/13.jpg)
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
![Page 14: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/14.jpg)
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
![Page 15: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/15.jpg)
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
![Page 16: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/16.jpg)
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
![Page 17: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/17.jpg)
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
![Page 18: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/18.jpg)
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.
![Page 19: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/19.jpg)
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
![Page 20: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/20.jpg)
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
![Page 21: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/21.jpg)
Applying D-H to a General Case:
![Page 22: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/22.jpg)
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)
![Page 23: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/23.jpg)
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
![Page 24: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/24.jpg)
Frame Skeleton for Prismatic Hand Robot
![Page 25: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/25.jpg)
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!
![Page 26: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/26.jpg)
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
![Page 27: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/27.jpg)
#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
![Page 28: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/28.jpg)
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
![Page 29: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/29.jpg)
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
![Page 30: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/30.jpg)
Another? 6dof Articulating Arm – (The Figure Contains Frame Skelton)
![Page 31: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/31.jpg)
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!
![Page 32: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/32.jpg)
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
![Page 33: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/33.jpg)
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!
![Page 34: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/34.jpg)
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
![Page 35: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/35.jpg)
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!
![Page 36: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/36.jpg)
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
![Page 37: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/37.jpg)
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
![Page 38: Robot Modeling and the Forward Kinematic Solution](https://reader034.vdocuments.mx/reader034/viewer/2022051116/56814e4e550346895dbbddc8/html5/thumbnails/38.jpg)
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