Robot Modeling and the Forward Kinematic Solution
description
Transcript of Robot Modeling and the Forward Kinematic Solution
![Page 1: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/1.jpg)
Robot Modeling and the Forward Kinematic Solution
ME 4135Lecture Series 4
![Page 2: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/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://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/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://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/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
![Page 5: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/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://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/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://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/7.jpg)
A model of the Joint Parameters
NOTE!!!
![Page 8: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/8.jpg)
A model of the Link Parameters
ai or
![Page 9: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/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://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/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://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/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://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/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 00 0
0 0 1 00 0 0 1
C SinSin Cos
1 0 0 00 1 0 00 0 10 0 0 1
d
![Page 13: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/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 00 1 0 00 0 1 00 0 0 1
a
1 0 0 00 00 00 0 0 1
Cos SinSin Cos
![Page 14: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/14.jpg)
The Overall Effect is:
os 0 0 1 0 0 0 1 0 0 1 0 0 00 0 0 1 0 0 0 1 0 0 0 0
0 0 1 0 0 0 1 0 0 1 0 0 00 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1
C Sin aSin Cos Cos Sin
d Sin Cos
![Page 15: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/15.jpg)
Simplifying this Matrix Product:
00 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 CS 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://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/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://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/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. Then Repeat Steps 3 to 5 for i = 1 through i = n-1:
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://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/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
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://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/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’s2) di as distance along Zi-1 3) i as angle about Xi between Z’s4) 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://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/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://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/21.jpg)
Applying D-H to a General Case:
Case 1: Like a part of a biological arm
Case 2: Like a part of a R-P manipulator
And Note:
![Page 22: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/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 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://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/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.50 0 0 1
C S S CS C C S
![Page 24: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/24.jpg)
Lindeke’s Folly – The “Plastic Handed” Robot
![Page 25: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/25.jpg)
Frame Skeleton for Prismatic Hand Robot
![Page 26: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/26.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-frame!
![Page 27: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/27.jpg)
Leading to 5 Ai Matrices
1
1 0 1 01 0 1 00 1 0 00 0 0 1
C SS C
A
3
3 0 3 03 0 3 00 1 0 .50 0 0 1
C SS C
A
2
2 2 0 6 22 2 0 6 20 0 1 10 0 0 1
C S CS C S
A
44
1 0 0 00 1 0 00 0 1 4.250 0 0 1
Ad
![Page 28: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/28.jpg)
#5 is:
5
5 5 0 05 5 0 00 0 1 10 0 0 1
C SS C
A
Now, Lets Form the FKS: T0
n = A1*A2*A3*A4*A5 Best to use a software: MathCad,
Mathematica, etc.
This value is called the Hand Span and depends on the Frame Skeleton we developed
![Page 29: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/29.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 or MathCad Simplify for A2*A3 then Evaluate for A1*{A2*A3}*A4*A5)
Basically then: T0n = A1*{A2A3}*A4*A5
![Page 30: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/30.jpg)
Extracts from Math Cad Analyses:
![Page 31: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/31.jpg)
Continuing
![Page 32: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/32.jpg)
Continuing
![Page 33: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/33.jpg)
In Tabular Form: Nx: C23C1C5 - S1S5
Ny: C1S5 + C23C5S1
Nz: -S23C5
Ox: -C23C1S5 - S1C5
Oy: C1C5 - C23S1S5
Oz: S23S5
Ax: S23C1
Ay: S1S23
Az: C23
Dx: S23C1 + 6C1C2 + 0.5S1 + S23C1(d4+4.25) Dy: S23S1 + 6S1C2 - 0.5C1 + S23S1(d4+4.25) Dz: C23 - 6S2 + C23(d4+4.25)
![Page 34: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/34.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 35: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/35.jpg)
Physical Verification in MathCad:
![Page 36: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/36.jpg)
Physical Verification of FKS:
![Page 37: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/37.jpg)
Another? 6dof Articulating Arm – (The Figure Contains Frame Skelton)
![Page 38: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/38.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 could be (6 - 90), which is a problem!
![Page 39: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/39.jpg)
A Matrices
1
1 0 1 01 0 1 00 1 0 00 0 0 1
C SS C
A
2
22
2 2 0 22 2 0 20 0 1 00 0 0 1
C S a CS C a S
A
3
33
3 3 0 33 3 0 30 0 1 00 0 0 1
C S a CS C a S
A
4
44
4 0 4 44 0 4 40 1 0 00 0 0 1
C S a CS C a S
A
![Page 40: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/40.jpg)
A Matrices, cont.
5
5 0 5 05 0 5 00 1 0 00 0 0 1
C SS C
A
66
6 6 0 06 6 0 00 0 10 0 0 1
C SS C
Ad
![Page 41: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/41.jpg)
Leads To:
FKS of:
0 1 2 3 4 5 6nT A A A A A A
PreProcess After Preprocessing A2-A3-A4, to collect
angular effect, the FKS must be reordered as A1-A234-A5-A6 and Simplified
![Page 42: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/42.jpg)
Solving for FKS
Pre-process {A2*A3*A4} with Evaluate Simplify
They are the planer arm issue as in the previous robot model
Then Form: A1* {A2*A3*A4}*A5*A6
Evaluate for FKS!
![Page 43: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/43.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 44: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/44.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 45: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/45.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 so here is (.2+1.5+1.5 = 3.2 m)
dy = S1·(C234·(d6·S5 + a4) + a3·C23 + a2·C2) - d6·C1·C5= 0(1(0 + a4) + a3 + a2) – d6 = -d6 so here is -.25 m
dz = S234·(d6·S5 + a4) + a3·S23 + a2·S2= 0(0 + a4) + 0 + 0 = 0
![Page 46: Robot Modeling and the Forward Kinematic Solution](https://reader036.fdocuments.us/reader036/viewer/2022062310/5681672c550346895ddbcf0b/html5/thumbnails/46.jpg)
And Physical Verification w/ Math Cad: