Embed Size (px)
Citation preview
1
Robot Mechanics
Dr. Robert L. Williams II Mechanical Engineering
Ohio University
NotesBook™ Supplement for:
EE/ME 429/529 Mechanics and Control of Robotic Manipulators
© 2011 Dr. Bob Productions
oak.cats.ohiou.edu/~williar4
These notes supplement the EE/ME 429/529 NotesBookTM by Dr. Bob
This document presents supplemental notes to accompany the EE/ME 429/529 NotesBook™. The outline given in the Table of Contents on the next page dovetails with and augments the EE/ME 429/529 NotesBook™ outline and hence may appear to be incomplete here.
2
EE/ME 429/529 Supplement Table of Contents
CHAPTER 1. INTRODUCTION ............................................................................................................................................ 3
1.3 MOBILITY .......................................................................................................................................................................... 3 1.5 MATRICES ......................................................................................................................................................................... 8
CHAPTER 2. SERIAL MANIPULATOR KINEMATICS ........................................................................................................... 16
2.4 DENAVIT-HARTENBERG (DH) PARAMETERS – ADDITIONAL EXAMPLES ...................................................................... 16 2.5 FORWARD POSE KINEMATICS ....................................................................................................................................... 25
2.5.1 3D MATLAB Graphics for Animation ................................................................................................................. 25 2.5.2 Spatial 6-dof 6R PUMA Robot FPK Example .................................................................................................. 27
2.6 INVERSE POSE KINEMATICS .......................................................................................................................................... 31 2.8 VELOCITY KINEMATICS .................................................................................................................................................. 38
2.8.1 Velocity Introduction ............................................................................................................................................ 38 2.8.2 Velocity Equations Derivations........................................................................................................................... 40 2.8.3 Jacobian Matrices ............................................................................................................................................... 42 2.8.8 Velocity Kinematics Examples ........................................................................................................................... 46 2.8.9 Jacobian Matrix Expressed in Another Frame ................................................................................................. 47 2.8.10 Cartesian Transformation of Velocities and Wrenches ................................................................................ 48
2.9 ACCELERATION KINEMATICS ........................................................................................................................................ 52 2.10 ROBOT WORKSPACE ................................................................................................................................................... 57
CHAPTER 3. KINEMATICALLY‐REDUNDANT ROBOTS ...................................................................................................... 64
3.2 INVERSE VELOCITY (RESOLVED-RATE) SOLUTION ...................................................................................................... 64 3.2.1 Pseudoinverse-based .......................................................................................................................................... 64 3.2.3 Klein and Huang’s Algorithm .............................................................................................................................. 64 3.2.4 Singular Value Decomposition ........................................................................................................................... 65 3.2.5 Generalized Inverses ........................................................................................................................................... 67
CHAPTER 4. MANIPULATOR DYNAMICS ........................................................................................................................ 68
4.1 MASS DISTRIBUTION: INERTIA TENSOR ....................................................................................................................... 69 4.2 NEWTON-EULER RECURSIVE ALGORITHM .................................................................................................................... 70 4.3 LAGRANGE-EULER ENERGY METHOD .......................................................................................................................... 72 4.4 SIMPLE DYNAMICS EXAMPLE ........................................................................................................................................ 73 4.5 STRUCTURE OF MANIPULATOR DYNAMICS EQUATIONS ............................................................................................... 78 4.6 ROBOT DYNAMICS EXAMPLE ......................................................................................................................................... 79
CHAPTER 5. ROBOT CONTROL ARCHITECTURES ............................................................................................................. 83
5.1 INVERSE POSE CONTROL ARCHITECTURE .................................................................................................................... 83 5.2 INVERSE VELOCITY (RESOLVED-RATE) CONTROL ARCHITECTURE ............................................................................. 83 5.3 INVERSE DYNAMICS CONTROL ARCHITECTURE ............................................................................................................ 84 5.4 NASA LANGLEY TELEROBOTICS RESOLVED-RATE CONTROL ARCHITECTURE ......................................................... 85 5.5 NATURALLY-TRANSITIONING RATE-TO-FORCE CONTROLLER ARCHITECTURE .......................................................... 86 5.6 SINGLE JOINT CONTROL ................................................................................................................................................ 87
CHAPTER 6. PARALLEL ROBOTS .................................................................................................................................... 97
6.1 INTRODUCTION ............................................................................................................................................................... 97 6.2 PLANAR 3-RPR MANIPULATOR POSE KINEMATICS ..................................................................................................... 99 6.3 NEWTON-RAPHSON METHOD ...................................................................................................................................... 101 6.4 PARALLEL MANIPULATOR WORKSPACE ..................................................................................................................... 105 6.5 PLANAR 3-RPR MANIPULATOR VELOCITY KINEMATICS ............................................................................................ 106
3
Chapter 1. Introduction 1.3 Mobility
Mobility is the number of degrees-of-freedom (dof) which a robot or mechanism possesses. Structures have zero (or negative) dof, mechanisms have one dof, and robots have greater than one dof. Degrees-of-freedom (dof) – the minimum number of independent parameters required to fully specify the location of a device. For planar or spatial serial robots, to find the dof, one may simply count the number of single degree-of-freedom joints. This is also equal to the number of independent motors to control the robot. More formally, we can also use Kutzbach’s mobility equations for planar or spatial robots. Kutzbach's Planar Mobility Equation
1 23 1 2 1M N J J
Where: M is the mobility N is the total number of links, including ground J1 is the number of one-dof joints J2 is the number of two-dof joints
J1 – one-dof joints: revolute, prismatic J2 – two-dof joints: cam joint, gear joint, slotted-pin joint
revolute joint R – pin joint, turning pair prismatic joint P – sliding pair
In general J1 R and P joints are used much more extensively in practical robots than J2 joints.
In Kutzbach's planar mobility equation, links give degrees of freedom and joints constrain or remove degrees of freedom. Each moving link has 3-dof in the plane – but the ground link is counted and hence must be subtracted in the equation. One-dof joints thus remove 2-dof in planar motion and two-dof joints thus remove 1-dof in planar motion.
4 Planar robot mobility examples: 2R serial robot 3R serial robot 4R serial robot
RRPR serial robot five-bar parallel robot 3-RRR parallel robot
3 5 1 2(5) 1(0) 2dofM 3 8 1 2(9) 1(0) 3dofM
A general planar n-link serial robot has n+1 links (including the fixed ground link), connected by n active 1-dof joints:
3 1 1 2 1 0 3 2M n n n n n dof
This agrees with the previous statement, that we can just count the number of active joints to find the mobility. For parallel robots this does not apply, but we can still count the required motors.
5 Kutzbach's Spatial Mobility Equation
1 2 3 4 56 1 5 4 3 2 1M N J J J J J
Where: M is the mobility N is the total number of links, including ground J1 is the number of one-dof joints J2 is the number of two-dof joints J3 is the number of three-dof joints J4 is the number of four-dof joints J5 is the number of five-dof joints
J1 – one-dof joints: revolute, prismatic, screw J2 – two-dof joints: universal, cylindrical, gear joint J3 – three-dof joints: spherical J4 – four-dof joints: spherical in a slot J5 – five-dof joints: spatial cam
universal joint U spherical joint S In general J1 R and P, J2 U, and J3 S joints are used much more extensively in practical robots than the other named joints.
In Kutzbach's spatial mobility equation, links give degrees of freedom and joints constrain or remove degrees of freedom. Each moving link has 6-dof in 3D – but the ground link is counted and hence must be subtracted in the equation. One-dof joints remove 5-dof, two-dof joints remove 4-dof, three-dof joints remove 3-dof, and so on, in spatial motion.
6 Spatial robot mobility examples:
5R serial robot 6R PUMA serial robot
7R serial FTS robot RRPR Adept SCARA robot
8R serial ARMII robot Stewart platform
7 Human arm
A general spatial n-link serial robot has n+1 links (including the fixed ground link), connected by n active 1-dof joints:
6 1 1 5 4 0 3 0 2 0 1 0 6 5M n n n n n dof
This agrees with the previous statement, that we can just count the number of active joints to find the mobility. For parallel robots this does not apply, but we can still count the required motors. For some more unsolved mobility examples, please see Dr. Bob’s atlas of structures, mechanisms, and robots:
oak.cats.ohiou.edu/~williar4/PDF/MechanismAtlas.pdf MATLAB function to calculate mobility % Function for calculating planar mobility % Dr. Bob, EE/ME 429/529 function M = dof(N,J1,J2) M = 3*(N-1) - 2*J1 - 1*J2; Usage: mob = dof(4,4,0); % for 4-bar and slider-crank mechanisms Result: mob = 1
8
1.5 Matrices Matrix: an m x n array of numbers, where m is the number of rows and n in the number of columns.
11 12 1
21 22 2
1 2
n
n
m m mn
a a a
a a aA
a a a
Matrices can be used to simplify and standardize the solution of n linear equations in n unknowns (where m = n). In robotics matrices are used a lot, in pose (position and orientation) description, velocity, acceleration, and dynamics. Special Matrices
square matrix (m = n = 3) 11 12 13
21 22 23
31 32 33
a a a
A a a a
a a a
diagonal matrix 11
22
33
0 0
0 0
0 0
a
A a
a
identity matrix 3
1 0 0
0 1 0
0 0 1
I
transpose matrix 11 21 31
12 22 32
13 23 33
T
a a a
A a a a
a a a
(switch rows and columns)
symmetric matrix 11 12 13
12 22 23
13 23 33
T
a a a
A A a a a
a a a
column vector (3x1 matrix) 1
2
3
x
X x
x
row vector (1x3 matrix) 1 2 3
TX x x x
9 Matrix Addition add like terms and keep the results in place
a b e f a e b f
c d g h c g d h
Matrix Multiplication with Scalar multiply each term and keep the results in place
a b ka kbk
c d kc kd
Matrix Multiplication
C A B
In general, A B B A
The row and column indices must line up as follows:
( x ) ( x )( x )
C A B
m n m p p n
That is, in a matrix multiplication product, the number of columns p in the left-hand matrix must equal the number of rows p in the right-hand matrix. If this condition is not met, the matrix multiplication is undefined and cannot be done. The size of the resulting matrix [C] is from the number of rows m of the left-hand matrix and the number of columns n of the right-hand matrix: mxn.
Multiplication proceeds by multiplying like terms and adding them, along the rows of the left-hand matrix and down the columns of the right-hand matrix (use your index fingers from the left and right hands).
Example:
(2x1) (2x3)(3x1)
ga b c ag bh ci
C hd e f dg eh fi
i
Note the inner indices (p = 3) must match, as stated above, and the dimension of the result is dictated by the outer indices, i.e. mxn = 2x1.
10 Matrix Multiplication Examples
1 2 3
4 5 6A
7 8
9 8
7 6
B
7 8
1 2 39 8
4 5 67 6
7 18 21 8 16 18 46 42
28 45 42 32 40 36 115 108
C A B
(2x2) (2x3)(3x2)
7 8
1 2 39 8
4 5 67 6
7 32 14 40 21 48 39 54 69
9 32 18 40 27 48 41 58 75
7 24 14 30 21 36 31 44 57
D B A
(3x3) (3x2)(2x3)
11 Matrix Inversion Since we cannot divide by a matrix, we multiply by the matrix inverse instead. Given C A B , solve for [B]:
C A B
1 1A C A A B
I B
B
1B A C
Matrix [A] must be square (m = n) to invert.
1 1A A A A I
where [I] is the identity matrix, the matrix 1 (ones on the diagonal and zeros everywhere else). To calculate the matrix inverse:
1 adjoint( )AA
A
where: A determinant of [A]
adjoint( ) cofactor( )T
A A
cofactor(A) ( 1)i j
ij ija M
minor minor Mij is the determinant of the submatrix with row i and
column j removed. For another example, given C A B , solve for [A]
C A B
1 1C B A B B
A I
A
1A C B
In general the order of matrix multiplication and inversion is crucial and cannot be changed.
12 System of Linear Equations We can solve n linear equations in n unknowns with the help of a matrix. For n = 3:
11 1 12 2 13 3 1
21 1 22 2 23 3 2
31 1 32 2 33 3 3
a x a x a x b
a x a x a x b
a x a x a x b
Where aij are the nine known numerical equation coefficients, xi are the three unknowns, and bi are the three known right-hand-side terms. Using matrix multiplication backwards, this is written as A x b :
11 12 13 1 1
21 22 23 2 2
31 32 33 3 3
a a a x b
a a a x b
a a a x b
where:
11 12 13
21 22 23
31 32 33
a a a
A a a a
a a a
is the matrix of known numerical coefficients;
1
2
3
x
x x
x
is the vector of unknowns to be solved; and
1
2
3
b
b b
b
is the vector of known numerical right-hand-side terms.
There is a unique solution 1x A b
only if [A] has full rank. If not, 0A (the determinant of
coefficient matrix [A] is zero) and the inverse of matrix [A] is undefined (since it would require dividing by zero; in this case the rank is not full, it is less than 3, which means not all rows/columns of [A] are linearly independent). Actually, Gaussian Elimination is more robust and more computationally efficient than matrix inversion to solve the problem A x b for {x}.
13 Matrix Example – solve linear equations Solution of 2x2 coupled linear equations.
1 2
1 2
2 5
6 4 14
x x
x x
1
2
1 2 5
6 4 14
x
x
1 2
6 4A
1
2
xx
x
5
14b
1x A b
1 4 2 6 8A
The determinant of [A] is non-zero so there is a unique solution.
1 4 2 1/ 2 1/ 41
6 1 3/ 4 1/ 8A
A
check: 1 1
2
1 0
0 1A A A A I
1
2
1/ 2 1/ 4 5 1
3/ 4 1/ 8 14 2
x
x
answer.
Check this solution: substitute the answer {x} into the original equations A x b and make sure
the required original {b} results:
1 2 1 1(1) 2(2) 5
6 4 2 6(1) 4(2) 14
checks out
14 Same Matrix Examples in MATLAB %------------------------------- % Matrices.m - matrix examples % Dr. Bob, ME 301 %------------------------------- clear; clc; A1 = diag([1 2 3]) % 3x3 diagonal matrix A2 = eye(3) % 3x3 identity matrix A3 = [1 2;3 4]; % matrix addition A4 = [5 6;7 8]; Add = A3 + A4 k = 10; % matrix-scalar multiplication MultSca = k*A3 Trans = A4' % matrix transpose (swap rows,cols) A5 = [1 2 3;4 5 6]; % define two matrices A6 = [7 8;9 8;7 6]; A7 = A5*A6 % matrix-matrix multiplication A8 = A6*A5 A9 = [1 2;6 4]; % matrix for linear equations solution b = [5;14]; % define RHS vector dA9 = det(A9) % calculate determinant of A invA9 = inv(A9) % calculate the inverse of A x = invA9*b % solve linear equations x1 = x(1); % extract answers x2 = x(2); Check = A9*x % check answer – should be b xG = A9\b % Gaussian elimination is more efficient who % display the user-created variables whos % user-created variables with dimensions The first solution of the linear equations above uses the matrix inverse – actually, to solve linear equations, Gaussian Elimination is more efficient (more on this in the dynamics notes later) and more robust numerically; Gaussian elimination implementation is given in the third to the last line of the above m-file (with the back-slash).
15 Output of Matrices.m A1 = 1 0 0 0 2 0 0 0 3 A2 = 1 0 0 0 1 0 0 0 1 Add = 6 8 10 12 MultSca = 10 20 30 40 Trans = 5 7 6 8 A7 = 46 42 115 108 A8 = 39 54 69 41 58 75 31 44 57 dA9 = -8 invA9 = -0.5000 0.2500 0.7500 -0.1250 x = 1 2 Check = 5 14 xG = 1 2
16
Chapter 2. Serial Manipulator Kinematics 2.4 Denavit-Hartenberg (DH) Parameters – additional examples 1. 3-axis Spatial 3P Cartesian Robot
Workspace shown in hatched lines.
i 1i 1ia id i
1 0 0 d1 0
2 90 0 d2 90
3 90 0 d3 0
17 2. 3-axis Spatial PRP Cylindrical Robot (incomplete)
Workspace shown in hatched lines.
i 1i 1ia id i
1
2
3
18 3. 3-axis Spatial RRP Spherical Robot (incomplete)
Workspace shown in hatched lines.
i 1i 1ia id i
1
2
3
19 4. 4-axis Spatial 4R Articulated Robot (incomplete)
Workspace shown in hatched lines.
i 1i 1ia id i
1
2
3
4
20 5. 4-axis Spatial RRPR SCARA Robot (incomplete)
Workspace shown in hatched lines.
i 1i 1ia id i
1
2
3
4
21 6. 6-axis Spatial 6R Fanuc S10 Robot (incomplete)
i 1i 1ia id i
1 2 3 4 5 6
6. 6R Fanuc S10 Robot (continued) – coordinate frames
23
7. 7-axis Spatial 7R NASA Flight Telerobotic Servicer (FTS) Robot (incomplete)
i 1i 1ia id i
1
2
3
4
5
6
7
24
8. 8-axis Spatial 8R NASA AAI ARMII Robot (incomplete)
i 1i 1ia id i
1
2
3
4
5
6
7
8
25
2.5 Forward Pose Kinematics 2.5.1 3D MATLAB Graphics for Animation This section presents some basic MATLAB code to draw a 3D robot to the screen, for either snapshots or motion animation following Forward Pose Kinematics, or other robotics calculations to follow later in the 429/529 NotesBook. Specifically we will show how to draw a 3D stick-figure representation for the laboratory SCARA robot below.
Adept 550 SCARA 4-dof 3D Robot
MATLAB Code: xx1 = [0 0]; % link from {B} to {0} yy1 = [0 0]; zz1 = [0 L0]; xx2 = [0 L1*c1]; % link from {0} to {2} yy2 = [0 L1*s1]; zz2 = [L0 L0]; xx3 = [L1*c1 L1*c1+L2*c12]; % link from {2} to end of L2 yy3 = [L1*s1 L1*s1+L2*s12]; zz3 = [L0 L0]; xx4 = [L1*c1+L2*c12 L1*c1+L2*c12]; % link from end of L2 to {4} yy4 = [L1*s1+L2*s12 L1*s1+L2*s12]; zz4 = [L0 L0-d3]; figure; plot3(xx1,yy1,zz1,'b',xx2,yy2,zz2,'b',xx3,yy3,zz3,'b',xx4,yy4,zz4,'b'); This will generate the basic stick-figure on the screen; here are some add-ons:
Be sure to use axis(‘equal’) or axis(‘square’) with appropriate axis limits. Use grid to clarify the resulting image. Use 'linewidth',4 to make thicker lines. Draw thinner unit vectors to represent the origins and directions of {B}, {0}, and {4}. Use subplot for 4 views (3 planar projections and orthographic view) – see view(AZ,EL). Use X Y Z axis labels.
26
Here is what the simplified 3D 4-dof Adept 550 SCARA robot could look like in MATLAB:
This was generated using snapshot FPK for the inputs 1 2 3 4 15 25 0.300 35d
.
The thicker robot lines are drawn in blue ('b' in the code) and the {B}, {0}, and {4} axes use the convention r g b for the X Y Z unit vectors. If you want to use this SCARA graphics in a motion simulation, you must use pause(dt) within your motion loop. Otherwise the animation will flash by so fast you cannot see it. dt can be designed to approximately obtain real-time animations, though normal MATLAB and certainly MS Windows are not real-time. Use of on-screen robot graphics is very helpful in validating that your simulated snapshots and motion trajectories are working correctly.
27
2.5.2 Spatial 6-dof 6R PUMA Robot FPK Example Spatial 6R PUMA Robot Forward Pose Kinematics Symbolic Derivations
Given 1 2 3 4 5 6, , , , , , calculate 06T and B
HT .
i 1i 1ia id i
1 0 0 0 1
2 90 0 L0 2 90
3 0 L1 0 3 90
4 90 0 L2 4 5 90 0 0 5
6 90 0 0 6 90
28
Substitute each row of the DH parameters table into the equation for 1iiT
. Evaluate the angle offsets
also, i.e. use the trigonometric sum-of-angle formulas (see the 429/529 NotesBook) to simplify.
1
1 1 1 11
1 1 1 1
0
0 0 0 1
i i i
i i i i i i iii
i i i i i i i
c s a
s c c c s d sT
s s c s c d c
1 1
1 101
0 0
0 0
0 0 1 0
0 0 0 1
c s
s cT
2 2
012
2 2
0 0
0 0 1
0 0
0 0 0 1
s c
LT
c s
3 3 1
3 323
0
0 0
0 0 1 0
0 0 0 1
s c L
c sT
4 4
234
4 4
0 0
0 0 1
0 0
0 0 0 1
c s
LT
s c
5 5
45
5 5
0 0
0 0 1 0
0 0
0 0 0 1
c s
Ts c
6 6
56
6 6
0 0
0 0 1 0
0 0
0 0 0 1
s c
Tc s
0 0 1 2 3 4 56 1 1 2 2 3 3 4 4 5 5 6 6
0 0 36 3 1 2 3 6 4 5 6, , , ,
T T T T T T T
T T T
0 0 13 1 2 3 1 1 3 2 3, , ,T T T
Take advantage of parallel Z axes in consecutive frames {2} and {3} – use trigonometric sum-of-angles formulas.
1 1 23 23 1 2
1 1 003 1 2 3
23 23 1 2
1 23 1 23 1 0 1 1 1 2
1 23 1 23 1 0 1 1 1 203 1 2 3
23 23 1 2
0 0 0
0 0 0 0 1, ,
0 0 1 0 0
0 0 0 1 0 0 0 1
, ,0
0 0 0 1
c s c s L s
s c LT
s c L c
c c c s s L s L c s
s c s s c L c L s sT
s c L c
where we used the abbreviations:
23 2 3
23 2 3
cos
sin
c
s
29
3 3 4 56 4 5 6 4 4 5 5 6 6, ,T T T T
There are no consecutive parallel Z axes in frames {4}, {5}, and {6}. Therefore, no sum-of-angles formula simplification is possible.
4 6 4 5 6 4 6 4 5 6 4 5
5 6 5 6 5 236 4 5 6
4 6 4 5 6 4 6 4 5 6 4 5
0
, ,0
0 0 0 1
s c c c s s s c c c c s
s s s c c LT
c c s c s c s s c c s s
It is left to the reader to perform the symbolic multiplication 0 0 3
6 3 6T T T ; the result is complicated
and can easily be found by using symbolic math on the computer (e.g. the MATLAB Symbolic Toolbox). Alternatively, these two matrices can be evaluated and multiplied numerically, again using
0 0 36 3 6T T T .
Here we will present one aspect of this multiplication; due to the spherical wrist (wrist frames {4}, {5}, and {6} share a common origin), the position vector 0
6P is a function only of the first three
joint angles:
0 1 1 1 2 2 1 23
0 0 0 36 1 2 3 4 3 4 0 1 1 1 2 2 1 23
1 2 2 23
, ,
L s L c s L c s
P P T P L c L s s L s s
L c L c
Where constant vector 34 20 0
TP L . In this way the PUMA FPK symbolic expressions are
partially decoupled: the position vector 06P is only a function of 1 2 3, , , but the rotation matrix
06 R is a complicated function of all six joint angles.
Two additional, fixed transforms The basic Forward Pose Kinematics result is 0
6T . For this robot we need to control the {H}
frame. There is also a separate base frame {B}, different from {0}. The overall Forward Pose Kinematics Homogeneous Transformation Matrix is given conceptually below; the symbolic terms would be worse in complexity than 0
6T . The below overall transform can be evaluated numerically.
LB and LH are constants. The orientation of {B} is identical to {0} and the orientation of {H} is identical to {6} for all motion.
0 60 6 1 2 3 4 5 6, , , , ,B B
H B H HT T L T T L
0
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
BB
B
T LL
6
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
H HH
T LL
30
Note that 0B
BT L and 6H HT L are not evaluated by any row in the DH parameter table, since there
is no variable associated with these fixed homogeneous transformation matrices. Instead, they are determined by inspection, using the rotation matrix and position vector components of the basic homogeneous transformation matrix definition. PUMA Robot FPK Examples Given fixed robot lengths 0 1 21.0, 0.3, 1.5, 1.2, 0.5B HL L L L L (m).
1) Given 10 , 20 ,30 , 40 ,50 ,60 , calculate 0
6T and BHT .
0 0 36 3 1 2 3 6 4 5 6
0 60 6 1 2 3 4 5 6
, , , ,
, , , , ,B BH B H H
T T T
T T L T T L
06
0.023 0.637 0.771 1.358
0.030 0.771 0.636 0.544
0.999 0.008 0.036 2.181
0 0 0 1
T
0.023 0.637 0.771 1.744
0.030 0.771 0.636 0.862
0.999 0.008 0.036 3.163
0 0 0 1
BHT
2) Given 60 , 50 , 40 , 30 , 20 , 10 , calculate 0
6T and BHT .
06
0.638 0.699 0.322 0.915
0.437 0.015 0.899 2.184
0.634 0.715 0.296 0.964
0 0 0 1
T
0.638 0.699 0.322 1.076
0.437 0.015 0.899 2.634
0.634 0.715 0.296 1.816
0 0 0 1
BHT
31
2.6 Inverse Pose Kinematics Decoupled Inverse Pose Kinematics Solution - Pieper's Solution
Pieper proved that if a 6-dof robot has any three consecutive joint axes intersecting, there exists a closed-form (analytical) solution to the inverse position kinematics. The majority of industrial robots are in this category.
In particular, many robots have a spherical wrist, i.e. three wrist actuators that rotate about a axes intersecting in a common point. In this case, the position and orientation sub-problems may be decoupled. We solve for the first three joints first using the position vector input. Then we solve for the second three joints next using the given orientation, based on the orientation caused by the first three joints. The given position vector must point to the wrist point (the shared origin point of three consecutive wrist frames). We can always transform any end-effector or other tool vector vector to this origin point, since there are no more active joints beyond the last wrist joint. PUMA Example (without details):
Given 06T , calculate 1 2 3 4 5 6, , , , , .
06 1 2 3 4 5 6, , , , ,T f
0 06 1 2 3 4 5 6 6 1 2 30
6
, , , , , , ,
0 0 0 1
R PT
Joints 4, 5, and 6 cannot affect the translation of the wrist origin point. 1) Translational equations: Given 0
6P , calculate 1 2 3, , values (4 sets).
3 independent equations, 3 unknowns. 2) Rotational equations: Given 0
6 R , and now knowing 1 2 3, , , calculate 4 5 6, , values (2 sets).
3 independent equations, 6 dependent equations, 3 unknowns.
3 0 06 4 5 6 3 1 2 3 6 1 2 3 4 5 6, , , , , , , , ,
TR R R
4 sets of 1 2 3, , ; 2 sets of 4 5 6, , for each. Therefore, there are 8 overall solutions to the
inverse position problem for the PUMA. Some may lie outside joint ranges. Generally one would choose the closest solution to the previous position.
32
Peel-off homogeneous transformations matrices with unknowns to separate variables.
0 0 1 2 3 4 56 1 1 2 2 3 3 4 4 5 5 6 6T T T T T T T
LHS 0
6T is numbers
10 0 1 2 3 4 51 1 6 2 2 3 3 4 4 5 5 6 6T T T T T T T
can solve for 1 and 3
10 0 3 4 53 2 6 4 4 5 5 6 6T T T T T
can solve for 2 with 1 and 3 known
10 0 4 54 4 6 5 5 6 6T T T T
isolate and solve 4, 5, and 6
33
3. 8-axis Spatial 8R NASA AAI ARMII Robot
Inverse Pose Kinematics General Statement Given: Calculate: Since m = 6 (Cartesian dof) and n = 8 (joint dof) we have the kinematically-redundant underconstrained case. There are infinite solutions (multiple as well). There are some great ways to make use of this redundancy for performance optimization in addition to following commanded Cartesian translational and rotational velocity trajectories. For inverse pose purposes we will here simplify instead and lock out the redundancy so that m = n = 6; let us choose 3 5 0 for all motion to accomplish this. Then we
have a determined Inverse Pose Kinematics problem, still with multiple solutions (but finite).
34
The Forward Pose Kinematics relationship is:
So, the first step should be to simplify the equations as much as possible by calculating the required
08T to achieve the commanded B
HT :
The problem can be decoupled between the arm joints 1-4 and the wrist joints 5-8 since the ARMII has a spherical wrist (all 4 wrist joint Cartesian coordinate frames share the same origin). See the previous section that explained the Pieper results for the 6-axis PUMA robot. Now, we will further simplify by ignoring the wrist joints 6-8 (5 is already locked to zero) and solve the Inverse Pose Kinematics problem only for the arm joints 1,2, and 4. The full inverse solution is given in Williams0F
1. Inverse Pose Kinematics Symbolic Solution for Arm Joints Only, with 3 0
Reduced problem statement:
Given 5 5 5 5
TBP x y z , calculate 1 2 4, ,i
, for all possible solution sets i.
That is, with only three active joints, we can only specify three Cartesian objectives, in this case the XYZ location of the origin of {5} with respect to origin of {B} (and expressed in the basis of {B}). Note that
5 8B BP P due to the spherical wrist.
The equations to solve for this problem come from the Forward Pose Kinematics relationships for the ARMII robot, the translational portion only (further, with 3 0 ):
This equation yields (the derivation is left to student, use symbolic Forward Pose Kinematics):
5 1 3 2 5 24
5 5 1 3 2 5 24
5 3 2 5 24
B
B
x c d s d s
P y s d s d s
z d d c d c
where:
24 2 4
24 2 4
cos
sin
c
s
(Since 3 0 always, the Z axes of 2 and 4 are always parallel and we used the sum-of-angles trig
formulas.)
1 R.L. Williams II, Kinematic Equations for Control of the Redundant Eight-Degree-of-Freedom Advanced Research Manipulator II, NASA Technical Memorandum 4377, NASA Langley Research Center, Hampton, VA, July 1992.
35
Solution Process:
1. A ratio of the Y to X equations yields:
1 180 is also a valid solution
2. Since 1 is now known (two values), we can modify the Y and Z equations:
where: 5
1
5 B
yY s
Z z d
Isolate the 2 4 terms:
Square and add to eliminate 4:
The result is one equation in one unknown 2:
2 2cos sin 0E F G
where: 3
3
2 2 2 23 5
2
2
E Zd
F Yd
G Y Z d d
We can solve this equation for 2 by using the Tangent Half-Angle Substitution. We presented this back in the Inverse Pose Solution of the planar 3R robot; we solve for 2 (in that section, it was for 1). Solve for 2 by inverting the original Tangent Half-Angle definition:
36
Two 2 solutions result from the in the quadratic formula; both are correct (there are multiple solutions – elbow up and elbow down). To find 4, return to original (arranged) translational equations:
5 24 3 2
5 24 3 2
d s Y d s
d c Z d c
Find the unique 4 for each 2 value (use the quadrant-specific atan2 function in MATLAB): Solutions Summary The solution is now complete for the ARMII robot reduced inverse pose problem (translational joints only, plus 3 0 ).
There are multiple solutions: two values for 1; for each 1, there are two values for 2; for each valid 1 2, , there is a unique 4. So there are a total of four 1 2 4, , solution sets for this reduced
problem. We can show this with the PUMA model (it’s not the same robot, but it has similar joints when 3 0 ).
These four solution sets occur in a very special arrangement pattern, summarized in the table below.
i 1 2 3 4
1 1 12 0 4
2 1 22 0 –4
3 1 180 22 0 4
4 1 180 12 0 –4
In all numerical examples, you can check the results; plug all solution sets 1 2 4, , one at a
time into the Forward Pose solution and verify that all sets yield the same, commanded 5B P . You can
also calculate the associated 4B R – all should be different (why?).
37
8R ARMII Robot Translational Inverse Pose Kinematics Example Given 5
B P , calculate 1 2 4, , , 1, 2,3, 4i
i .
5
5 5
5
0.6572
0.1159
1.6952
B
x
P y
z
Answers (deg):
i 1 2 3 4
1 10 20 0 30
2 10 46.6 0 -30
3 190 -46.6 0 30
4 190 -20 0 -30
Check all solution sets via Forward Pose Kinematics to ensure all yield the correct, commanded 5B P .
38
2.8 Velocity Kinematics 2.8.1 Velocity Introduction 2.8.1.1 Translational Velocity Example
This example demonstrates the various indices in k ijV . A is a fixed reference frame.
Given: the absolute velocity of B is 2 @90in
s the absolute velocity of C is 4 @45
in
s
From the given information:
0
2
0
A ABV
; 2.83
2.83
0
A ACV
From the relative velocity equation:
A A AA A BC B CV V V
2.83
0.83
0
A A AB A AC C BV V V
; 2.95 @16in
s
39
Since velocity is a free vector, these same velocity vectors ( , ,A A BB C CV V V ) can be expressed in
frames {B} and {C} by using the rotation matrix:
k Ai k ij A jV R V (or just look at the components)
2
0
0
B ABV
2.83
2.83
0
B ACV
0.83
2.83
0
B BCV
1.41
1.41
0
C ABV
4
0
0
C ACV
2.58
1.43
0
C BCV
There are 27 permutations for A, B, C. So far, we have given 9 combinations. There are two more types:
1) k kj ii jV V e.g.
0
2
0
A BAV
(9 of these)
2) 0k iiV e.g.
0
0
0
A BBV
(9 of these)
40
2.8.2 Velocity Equations Derivations 2.8.2.2 Spatial 3P Cartesian manipulator
0 0X J L 1 2 3
TL d d d
Derive 0J : Partial derivatives definition
i
j
fJ
L
3
0 03 2
1
x
y
z
p d
f P p d
p d
1
0xp
d
2
0xp
d
3
1xp
d
1
0yp
d
2
1yp
d
3
0yp
d
1
1zp
d
2
0zp
d
3
0zp
d
0 03 0
0
0 0 1
0 1 0
1 0 0
J
0
1
2
3
0 0 1
0 1 0
1 0 0
x d
y d
z d
41
2.8.2.3 Spatial 3R PUMA robot (translational only)
The PUMA translational velocity equations are derived as follows, where W indicates the origin of the wrist frame {W} (recall the position of the wrist frame origin is a function of only the first three joint angles 1 2 3, , :
0
0 1 1 1 2 2 1 230
6 1 2 3 0 1 1 1 2 2 1 23
1 2 2 23
, ,
W
x L s L c s L c s
P y L c L s s L s s
z L c L c
00 1 1 1 1 2 1 1 1 2 2 2 1 23 1 2 1 23 2 3
00 1 1 1 1 2 1 1 1 2 2 2 1 23 1 2 1 23 2 3
01 2 2 2 23 2 3
W
W
W
x L c L s s L c c L s s L c c
y L s L c s L s c L c s L s c
z L s L s
0
0 1 1 1 2 2 1 23 1 1 2 2 1 23 2 1 23 1
0 1 1 1 2 2 1 23 1 1 2 2 1 23 2 1 23 2
1 2 2 23 2 23 3
0 0
0W
W
x L c L s s L s s L c c L c c L c c
y L s L c s L c s L s c L s c L s c
z L s L s L s
X J
Why is the (3,1) term of the Jacobian matrix zero? ( 1 cannot affect 0
Wz )
42
2.8.3 Jacobian Matrices
Four ways to derive the Jacobian matrix:
1) Time derivatives and relative angular velocity equation
2) Partial derivatives definition – closely related to the first method
f – vector of m Cartesian functions. This relationship holds only for translational part:
That is, no Cartesian orientation vector exists that we can differentiate to get the angular velocity vector.
43
3) Column as velocity due to joint i Physical interpretation of the Jacobian matrix: each column i is the Cartesian velocity
vector of the end-effector frame with respect to the base frame, due to joint i only, and with i factored
out.
0 0 0
1 2
| | |
| | |
k
kN N N N
J X X X
0
0
0
k
Nk iN ki
N i
VX
a) Revolute Joint Columns
where ˆ ˆk k ii i iZ R Z is the third column of k
i R and k ii k iN i NP R P where i i
NP is the translational
part of iNT .
Here is Jacobian matrix column i, for a revolute joint:
ˆˆ
ˆˆ
k k i ik ii i Nk i N
i k iki ii
R Z PZ PJ
R ZZ
Here is the Jacobian matrix for an all-revolute manipulator:
11
1
ˆ ˆˆ
ˆ ˆˆ
kk kk k i k Nk
i N N NNk
k kki N
Z P Z PZ PJ
Z ZZ
44
b) Prismatic Joint Columns
where ˆ ˆk k i
i i iZ R Z is the third column of ki R .
Here is Jacobian matrix column i, for a prismatic joint:
ˆ ˆ
0 0
k k ik i i i
i
Z R ZJ
4) Velocity recursion a'la Craig1F
2 Add translational and rotational velocities serially link-by-link from the base to the end-effector link. Revolute:
1 1 11 1 1
1 11 1
ˆi i i ii i i i i
i i i i ii i i i i
R Z
v R v P
Prismatic:
1 11
1 1 11 1 1 1
ˆ
i i ii i i
i i i i i ii i i i i i i
R
v R v P d Z
Start with 0 0
0 0 0v (if the robot base is fixed).
Use the velocity recursion equations, 0,1, 2, , 1i N . Then, factor out from ,N N
N Nv to get N J .
2 J.J. Craig, 2005, Introduction to Robotics: Mechanics and Control, 3rd edition, Pearson Prentice Hall, Upper Saddle River, NJ.
45
Derive 0J : Partial derivatives definition
For the planar 3R robot.
0J
0 03
x
y
pf P
p
3 3
1 1
ji i ij
j jj j
dp p p
t dt
i = x, y
1
xp
2
xp
3
xp
1
yp
2
yp
3
yp
0 0 0 00 0 1 23 1 2 3
0 03 1 2 3 Z
0J
46
2.8.8 Velocity Kinematics Examples 2.8.8.2 Spatial 3P Cartesian Manipulator Velocity Example
a) Forward Velocity Kinematics
0 0
0
1 3
2 2
3 1
0 0 1
0 1 0
1 0 0
X J L
x d d
y d d
z d d
b) Inverse Velocity Kinematics (analytical result)
10 0
0 0
1
2
3
0 0 1
0 1 0
1 0 0
L J X
d x z
d y y
d z x
c) Singularity Analysis
0 1 2 3 1J J J J no possible singularities
d) Static Force Analysis
0 0
0 0
1
2
3
0 0 1
0 1 0
1 0 0
T
x z
y y
z x
f J F
f F F
f F F
f F F
47
2.8.9 Jacobian Matrix Expressed in Another Frame Here the Jacobian matrix still relates the end-effector frame velocity with respect to the base frame. But simpler analytical expressions are possible for the Jacobian matrix, by choosing an intermediate frame to express the coordinates of the velocity vectors (different basis of expression).
00
0
0
0
kk
k
Rv v
R
k
kvJ
0
0vJ
0 0
0
0
0
k
k
k
RJ J
R
is not dependent on a frame (relative joint rates)
0 0
0
0
0
k
k
k
RJ J
R
Planar 3R Robot The Jacobian was derived in frame {0} (here we go to {3} instead of {H}).
1 1 2 12 2 120
1 1 2 12 2 12
0
0
1 1 1
L s L s L s
J L c L c L c
2 2 2 2
11 2 2 2 2
0
0
1 1 1
L s L s
J L L c L c
1 1
10 1 1
0
0
0 0 1
c s
R s c
1 2
21 2 2 2
0 0
0
1 1 1
L s
J L c L L
12 12
20 12 12
0
0
0 0 1
c s
R s c
1 23 2 3 2 3
31 23 2 3 2 3
0
0
1 1 1
L s L s L s
J L c L c L c
123 123
30 123 123
0
0
0 0 1
c s
R s c
3J agrees with that derived in the third method above.
48
2.8.10 Cartesian Transformation of Velocities and Wrenches Here we show how to move velocity and wrench vectors (both translational and rotational) from one point to another on a rotating rigid body. We can replace one vector with an equivalent vector acting at a different point. For example, to calculate the velocity (or wrench) at the wrist to produce a desired velocity (or wrench) at the hand. 2.8.10.1 Velocity Transformation Here we find the equivalent velocity at {A} corresponding to a given desired motion of {B}. For instance, {B} could be the hand frame {H} where we want motion and {A} would then be the last wrist frame {n} for which the Jacobian matrix is derived.
Basic equations:
AB A A B
B A
v v P
reversing: A A
A B B B B B B
A B
v v P v P
All vectors must be expressed in same frame, choose {A}.
0 0
0 00
A A AA BB B BA B
AA BB
R P RV V
R
A BAB VX T X
0
A A AB B BA
B V AB
R P RT
R
where:
0
0
0
z yA
B z x
y x
p p
P p p
p p
is the cross product matrix.
49
Velocity transformation example Find the equivalent velocity at A corresponding to a given motion of B.
Given: 0
1
1
0
B
BV
0
0
0
2
B
B
2.6
1.5
0
ABP
0.866 0.5 0
0.5 0.866 0
0 0 1
AB R
0 0 1.5 0.866 0.5 0 0 0 1.5
0 0 2.6 0.5 0.866 0 0 0 2.6
1.5 2.6 0 0 0 1 0 3 0
A AB BP R
A BAB VX T X
0
0
0.866 0.5 0 0 0 1.5 1 3.366
0.5 0.866 0 0 0 2.6 1 3.834
0 0 1 0 3 0 0 0
0 0 0 0.866 0.5 0 0 0
0 0 0 0.5 0.866 0 0 0
0 0 0 0 0 1 2 2
A
A
A
V
Check: A B A BV V r
0
ˆˆ ˆ1 1 0 1
1 0 0 2 1 6 5
0 3 0 0 0 0 0
B
A
i j k
V
50
2.8.10.2 Pseudostatic Wrench Transformation Here we find the equivalent wrench at {A} corresponding to a given wrench at {B}. For instance, {B} could be the hand frame {H} where we want the wrench to be exerted and {A} would then be the last wrist frame {n} for which the Jacobian matrix is derived.
Basic equations:
A B
AA B B B
F F
M M P F
more properly, k
BF
All vectors must be expressed in the same frame, choose {A}.
0AA BBA B
A A AA BB B B
RF F
M MP R R
A BAB WW T W
0ABA
B W A A AB B B
RT
P R R
Note: AB WT is a block-transpose of A
B VT ; i.e. A AB BP R is switched with 0.
There is a duality here: in the velocity transformation, the rotational term is unchanged, while in the wrench transformation, the translational term is unchanged.
51
Wrench transformation example Find the equivalent wrench at A corresponding to a given wrench at B.
Given: 1
1
0
B
BF
0
0
0
B
BM
, ,A A A AB B B BP R P R are the same as in the velocity example above.
A BAB WW T W
0.866 0.5 0 0 0 0 1 0.366
0.5 0.866 0 0 0 0 1 1.366
0 0 1 0 0 0 0 0
0 0 1.5 0.866 0.5 0 0 0
0 0 2.6 0.5 0.866 0 0 0
0 3 0 0 0 1 0 3
A
A
A
F
M
Check: A BF F A BM r F
Note: the derivations and examples for both velocity and wrench transformations were for the inverse case, i.e. given the end vector values {B}, calculate the vector values on the same rigid link, but inward toward {A}. We could easily adapt the derivations and transformations to perform the forward calculation, i.e. given the inward vector values {A}, calculate the end vector values on the same rigid link, outward towards {B}.
52
2.9 Acceleration Kinematics The acceleration vector is the first time derivative of the velocity vector (and the second time
derivative of the position vector). It is linear in acceleration terms but non-linear in rate components. Both Cartesian acceleration and velocity are vectors, with translational and rotational parts.
Acceleration Kinematics Analysis is useful for: Resolved Acceleration Control Acceleration of any point on manipulator Moving objects in workspace – smooth trajectory generation Required for Dynamics
Translational Acceleration
k ijA is the translational acceleration of the origin of frame {j} with respect to reference frame
{i}, expressed in the basis (coordinates) of {k}. Transport Theorem (Craig, 2005):
A B A B A A BB B Q B B
dR Q R V R Q
dt
General five-part acceleration equation: Position:
A A A B
B B
A A BB
P P R P
P T P
Velocity:
0
A A A B A BB B B
P
V P R P R P
V V V P
Acceleration:
0
2
2
A A A B A A BB B P B B P
A A A B A A B A A B A A B A A A BB B P B B P B B P B B P B B B P
A A A B A A B A A B A A A BB B P B B P B B P B B B P
P
dA V R V R P
dt
A A R A R V R P R V R P
A A R A R V R P R P
A A A V P P
53
Rotational Acceleration
k ij is the rotational acceleration of frame {j} with respect to reference frame {i}, expressed
in the basis (coordinates) of {k}. No angular orientation exists for which we can take the time derivative to find the angular velocity. Instead we use relative velocity equations: Rotational Velocity: A A A B
C B B CR
Rotational Acceleration:
A A A B A A B
C B B C B B C
A A A B A A BC B B C B B C
d dR R
dt dt
R R
For vectors expressed in the local frame:
A B C B CA A A A B A A A BC B B C C B B C CR R R R
Combined Translational and Rotational Acceleration
aX
where:
0 0
0 0
N
N
a A
both a and are (3x1) vectors
Acceleration Example Planar 2R robot – acceleration of end-effector frame with respect to {0}, also expressed in {0}. 1) Craig (2005) acceleration recursion
1
12 0
0
L
P
2
23 0
0
L
P
2 2
21 2 2
0
0
0 0 1
c s
R s c
32 3R I
00
0
0
0
00
0
0
0
00
0
0
0
a
11
1
0
0
11
1
0
0
11
0
0
0
a
54
22
1 2
0
0
22
1 2
0
0
2 2 1 1 1 1 1 12 1 1 2 1 1 2 1
2 22 2 1 1 1 2 1 1 2 1
2 22 2 2 1 1 1 2 1 1 2 1
0
0
0 0 1 0 0
a R P P a
c s L L c L s
a s c L L s L c
3 23 2
1 2
0
0
3 3 2 2 2 2 2 23 2 2 3 2 2 3 2
2 22 1 2 1 2 1 1 2 1
3 23 2 1 2 1 2 1 1 2 1
0
a R P P a
L L c L s
a L L s L c
0 0 33 3 3
221 1 1 1 1 2 12 1 2 12 1 2
20 23 1 1 1 1 1 2 12 1 2 12 1 2
0
a R a
L s c L s c
a L c s L c s
Rewrite for comparison to results in the next section:
1 1 1 2 12 1 2 11 1 2 12 2 120 13
1 1 2 12 2 12 1 22 1 1 1 2 12 1 2
L c L cL s L s L sa
L c L c L c L s L s
The second term above can be written as:
1 1 1 2 12 1 2 2 12 1 2 1
21 1 1 2 12 1 2 2 12 1 2
L c L c L c
L s L s L s
55
Acceleration Example (cont.) Planar 2R robot – acceleration of the end-effector frame with respect to {0}, also expressed in {0}. Alternative method: 2) Differentiation of rate equation
X J X J J
Do in frame {0} – if in frame {2} or {3}, one must account for r .
1 1 2 12 2 120
1 1 2 12 2 12
1 1
L s L s L s
J L c L c L c
ijdJd JJ
dt dt
1 2
1 2
ij ij ij ij N
N
dJ J J J dd d
dt dt dt dt
1
Nij ij
kk k
dJ J
dt
1 1 2 12 1 2 12 2 2 12 1 2 12 20
1 1 2 12 1 2 12 2 2 12 1 2 12 2
0 0
L c L c L c L c L c
J L s L s L s L s L s
1 1 1 2 12 1 2 2 12 1 2
01 1 1 2 12 1 2 2 12 1 2
0 0
L c L c L c
J L s L s L s
0 0 0X J J
1 1 1 2 12 1 2 2 12 1 21 1 2 12 2 12
0 1 11 1 2 12 2 12 1 1 1 2 12 1 2 2 12 1 2
2 21 1 0 0
L c L c L cL s L s L s
X L c L c L c L s L s L s
This yields the same result as before.
56
Uses for general acceleration equation
1
X J
X J J
J X J
i. Forward Acceleration Kinematics Analysis – X J J
predicts the Cartesian
accelerations X given the joint rates and accelerations.
ii. Resolved Acceleration Control – like resolved rate, but acceleration is commanded instead of
velocity. Solve 1J X J
and double integrate to get the commanded joint
angles. iii. Dynamics equations – is required for the Newton/Euler dynamics recursion in this
429/529 NotesBook Supplement, Chapter 4. If acceleration is calculated via numerical differentiation, numerical instability can result, so the analytical approach
1J X J
is better.
Now, if inverse dynamics control is being used in the resolved-rate control algorithm framework, assume X is constant and so 0X . In this case:
1J J
How can we find the time rate of change of the Jacobian matrix J
? See the previous page for a
specific example.
57
2.10 Robot Workspace The workspace (also called work envelope) of a robot manipulator is defined as the volume in space that a robot’s end-effector can reach. The workspace limits are due to the total reach of the combined robot links (both extended and folded upon each other). The workspace limits are also due to the specific joint limits of each joint. Workspace is defined for both position (translational workspace) and orientation (rotational workspace). The translational workspace is easier to determine and display, and is the focus of this section. The figure below shows the translational workspace of a Cybotech P15 articulated robot, in side (left) and top (right) views. Generally large translational and rotational workspaces are desired for a more capable robot. Generally serial robots have a big advantage over parallel robots in terms of larger workspace (exception: parallel cable-suspended robots).
Cybotech P15 Manipulator Translational Workspace
58
The robot workspace attainable at any orientation is called the reachable workspace. The robot workspace attainable at all orientations is called the dextrous workspace. The dextrous workspace is a subset of the reachable workspace. For parallel manipulators the dextrous workspace is often null; therefore a limited dextrous workspace is generally defined (such as all orientations within 30 rotation about all three axes). The reachable workspace of the Adept 550 SCARA robot is shown below. This top-view planar workspace shape is extended 200 mm in the vertical direction by the prismatic joint limits on d3 (into the paper), to form a volume.
Adept 550 SCARA Robot Reachable Workspace
Adept 550 Table-Top Robot User’s Guide, 1995. The next five images represent the reachable, or translational, workspaces of five common manipulator arrangements (Cartesian, cylindrical, spherical, SCARA, and articulated).
59
Cartesian Manipulator Translational Workspace
Cylindrical Manipulator Translational Workspace
60
Spherical Manipulator Translational Workspace
SCARA Manipulator Translational Workspace
61
Articulated Manipulator Translational Workspace
The previous five images came from:
societyofrobots.com/robot_arm_tutorial.shtml
62
The figure below presents the translational workspace for a parallel robot (the three-axis Orthoglide). Imagine how much greater the translational workspace would be if there were only one supporting serial chain to the end-effector. However, the tradeoff is that the parallel robot achieves much greater accelerations, stiffness, and accuracy, with lower moving mass, than an equivalent serial robot with a larger workspace.
Orthoglide Robot Reachable Workspace
parallemic.org/Reviews/Review011.html
63
Again, parallel robots such as the 3-RRR shown generally have small workspaces.
3-RRR Parallel Robot Sample Limited Workspace4F
3
Optimized 3-RRR Parallel Robot Workspace1
A notable exception to the small workspace
rule of parallel robots is the cable-suspended robots. These devices, such as the NIST RoboCrane (right, developed at NIST by Dr. James Albus), can be designed with arbitrarily-large workspaces. Cable suspended robots with workspaces on the order of several kilometers have been proposed.
3 R.L. Williams II, 1988, Planar Robotic Mechanisms: Analysis and Configuration Comparison, Ph.D. Dissertation, Department of Mechanical Engineering, Virginia Polytechnic Institute and State University, Blacksburg, VA, August.
64
Chapter 3. Kinematically-Redundant Robots 3.2 Inverse Velocity (Resolved-Rate) Solution 3.2.1 Pseudoinverse-based Particular Solution – satisfies primary task (satisfies Cartesian trajectory) Minimum Manipulator Kinetic Energy
W M ; the manipulator inertia tensor
P J X
where: 11 1T TJ M J J M J
This Moore-Penrose pseudoinverse form is subject to singularities. Singular Value Decomposition (SVD) would ameliorate this problem.
3.2.3 Klein and Huang’s Algorithm This methods yields the same results as the pseudoinverse with gradient projection into the null-space, but it is more efficient (less computations). Klein and Huang’s algorithm accomplishes the particular and homogeneous solutions at the same time.
TJ J w X k J H ;
solve {w} using Gaussian elimination, then:
TJ w k H .
So much of the existing kinematically-redundant robot literature is dedicated to more efficient redundancy resolution, but I think with today’s processors, this is no longer a problem.
65
3.2.4 Singular Value Decomposition Singular Value Decomposition (SVD) yields the same results as the pseudoinverse with gradient projection into the null-space, but with singularity robustness. If [J] is less than full rank, the solution cannot track arbitrary Cartesian trajectories, but SVD results are bounded so the motion can drive through singularities, as opposed to yielding infinite joint rates at singularities.
TJ U W V
J m x n U m x n column orthogonal W n x n positive semi-definite diagonal V n x n orthogonal
1 T
j
J V diag Uw
wj are the singular values of [J]. For underconstrained systems of equations, there will always be n – m zero singular values, where n – m is the degree of redundancy. Any additional zero singular
values correspond to degeneracies in J. In the above expression, 1
jw is set to zero for 0jw (ain’t
math fun?!?)
Both U and V are column-orthonormal (ignoring the last n – m columns of U). V is also row-orthonormal, i.e.:
T T
nV V V V I
T
mU U I ; however, T
nU U I (see the SVD example)
Columns of U corresponding to non-zero jw are an orthonormal basis which spans the range of
J. Columns of V corresponding to zero wj are an orthonormal basis for the null-space of J.
66
Singular Value Decomposition (SVD) Example
1.366 0.500 0.500
2.366 1.866 0.866J
TJ U W V
3.467 0 0 0.786 0.514 0.344
0.430 0.903 00 0.420 0 0.548 0.836 0
0.903 0.430 00 0 0 0.288 0.188 0.939
T
J
1 T
j
J V diag Uw
*0.786 0.514 0.344 0.288 0 0 0.430 0.903 1.205 0.323
0.548 0.836 0 0 2.381 0 0.903 0.430 1.732 1.000
0.288 0.188 0.939 0 0 0 0 0 0.441 0.118
J
In the above, the third singular value is zero, so 1
jw is set to zero (n – m singular values will always be
zero). This result agrees with *J from the Moore-Penrose pseudoinverse formula in the 429/529
NotesBook. Note:
2
TU U I 3
1 0 0
0 1 0
0 0 0
TU U I
3
T TV V V V I
67
3.2.5 Generalized Inverses
A generalized inverse of a matrix gives an answer to the linear problem even when a true matrix inverse does not exist (underconstrained, overconstrained, or row-rank-deficient). Mathematically, G is a generalized inverse of matrix A if:
A G A G
The Moore-Penrose pseudoinverse is just one possible generalized inverse of a matrix. It is the one applied most often to redundancy resolution of manipulators. In addition to the above relationship, the following relationships hold for the Moore-Penrose pseudoinverse:
*
*
G A G A
G A G A
A G A G
where * indicates the complex conjugate transpose.
68
Chapter 4. Manipulator Dynamics
Kinematics is the study of motion without regard to forces. Dynamics is the study of motion with regard to forces. It is the study of the relationship between forces/torques and motion. Dynamics is composed of kinematics and kinetics. a) Forward Dynamics (simulation) – given the actuator forces and torques, compute the resulting motion (this requires the solution of highly coupled, nonlinear ODEs): Given , calculate
, , (all are N x 1 vectors). b) Inverse Dynamics (control) – given the desired motion, calculate the actuator forces and torques (this linear algebraic solution is much more straight-forward than Forward Dynamics): Given
, , , calculate (all N x 1 vectors). Both problems require the N dynamic equations of motion, one for each link, which are highly coupled and non-linear. Methods for deriving the dynamic equations of motion:
Newton-Euler recursion (force balance, including inertial forces with D'Alembert's principle). Lagrange-Euler formulation (energy method).
Kinetics: Translational: Newton's Second Law: Inertial force at center of mass
Rotational: Euler's Equation: Inertial moment at center of mass (could be anywhere on body)
The kinematics terms, , ,Ci i ia , must be moving with respect to an inertially-fixed frame.
The frame of expression {k} needn't be an inertially-fixed frame. Assumptions:
serial robot rigid links ignore actuator dynamics no friction no joint or link flexibility
69
4.1 Mass Distribution: Inertia Tensor
Spatial generalization of planar scalar moment of inertia; units: m-d2 (kg-m2). The inertia tensor expressed at a given point in the rigid body, relative to frame {A} is:
Mass moments of inertia
2 2xx V
I y z dv 2 2yy V
I x z dv 2 2zz V
I x y dv
Mass products of inertia
xy VI xy dv xz V
I xz dv yz VI yz dv
Principal moments of inertia A certain orientation of the reference frame {A}, the principal axes, yields zero products of inertia. The invariant eigenvalues of a general AI are the principal moments of inertia, and the eigenvectors are the principal axes. More interesting facts regarding inertia tensors
1) If two axes of the reference frame form a plane of symmetry for the mass distribution, the products of inertia normal to this plane are zero.
2) Moments of inertia must be positive, products of inertia may be either sign. 3) The sum of the three moments of inertia are invariant under rotation transformations. Parallel-axis theorem We can obtain the mass moment of inertia tensor at any point {A} if we know the inertia tensor
at the center of mass {C} (assuming these two frames have the same orientation): T
C C C CP x y z
is the vector giving the location of the center of mass {C} from the origin of {A}. Here are some example inertia tensor components using the parallel axis theorem:
2 2A Czz zz C C
A Cxy xy C C
I I m x y
I I mx y
Here is the entire inertia tensor expressed in vector-matrix form using the parallel axis theorem:
3A C T T
C C C CI I m P P I P P
70
4.2 Newton-Euler Recursive Algorithm This is a recursive approach (Craig, 2005) based on free-body diagrams (FBDs) to determine the dynamics relationships link-by-link. Used numerically it can calculate the inverse dynamics solution efficiently. Free-body diagram of link i: fi: internal force exerted on link i by link i-1. ni: internal moment exerted on link i by link i-1. Inertial loads Newton and Euler translational and rotational dynamics equations: Force balance Moment balance (about CGi) (using D'Alembert’s principle, the inertial force is –ma).
71
Newton-Euler Recursive Algorithm Summary This methods can be used to find the robot dynamics equations of motion. It can also be used to
directly solve the inverse dynamics problem numerically. The summary of equations below, from Craig (2005), assume an all revolute-joint manipulator (prismatic joint dynamics have different equations).
Outward iteration for kinematics: : 0 1i N (without regard for frames of expression, for clarity) Velocities and accelerations (kinematics):
1 1 1
1 1 1 1 1
1 1 1
1 11 1 1 1 1 1 1
ˆ
ˆ ˆi i i i
i i i i i i i
i ii i i i i i i
i iCi i i Ci i i Ci
Z
Z Z
a a P P
a a P P
Inertial loading (kinetics):
1 1 1
1 1 1 1
i i Ci
C Ci i i i
F m a
N I I
Inward iteration for kinetics: : 1i N (without regard for frames of expression, for clarity) Internal forces and moments:
1
1 1 1
i i i
i ii i Ci i i i i
f f F
n n P F P f N
Externally applied joint torques:
i i in Z
Inclusion of gravity forces:
00a g
This is equivalent to a fictitious upward acceleration of 1g of the robot base, which accounts for the downward acceleration due to gravity (i.e. this conveniently includes the weight of all links).
72
4.3 Lagrange-Euler Energy Method
This is an alternative method to find the robot dynamics equations of motion. It requires only translational and rotational link velocities, not accelerations. The Lagrangian is formed from the kinetic energy k and potential energy u of the robot system.
,L k u k u
0 0
1 1
2 2T T Ci
i i Ci Ci i i i
i REF i Ci
k mV V I
u u m g P
1
,N
ii
k k
1
N
ii
u u
Note:
1,
2Tk M ; where M is the manipulator mass matrix.
Dynamic equations of motion These are found for each active joint from the following expression involving the Lagrangian, joint variable, and actuator torque. Perform this equation N times, once for each joint variable, to yield N independent dynamics equations of motion.
d L L
dt
This expression may be rewritten using ,L k u :
d k k u
dt
73
4.4 Simple Dynamics Example Derive the dynamic equation of motion for a planar one-link 1R mechanism by three methods: 1) Sophomore dynamics method – FBD, force and moment dynamics balance. 2) Newton-Euler recursion. 3) Lagrange-Euler formulation.
1) Sophomore methods - FBD, force balance Free-Body Diagram:
Y
LX
g
mg
-FX
-FY
FY
FX
74
1) Sophomore methods – FBD, force balance (cont.)
a)
0 2
0 2
2 2
2
x Cx
x
x
F ma
L LF m s c
mLF s c
b)
0 2
0 2
2 2
2
y Cy
y
y
F ma
L LF mg m c s
LF mg m c s
c) 0
0
0
2
2
z
mLM I gc
mLI gc
where:
20 2
4C C
zz
mLI I md I
75
Simple Dynamics Example (cont.)
2) Newton-Euler recursion Outward iteration: i = 0 (the only iteration)
01
0
0
0
P
11
20
0C
L
P
10
0
0
0 0 1
c s
R s c
11
0 0
0 0
0 0
xxC
yy
zz
I
I I
I
00
0
0
0
00
0
0
0
00
0
0
a g
to account for gravity
1 1 0 0 0 0 0 01 0 0 1 0 0 1 0
11
0 0
0
0 0 1 0 0
a R P P a
c s gs
a s c g gc
2
1 1 1 1 1 1 11 1 1 1 1 1 1
2
20
C C C
Lgs
La P P a gc
2
1 11 1
2
20
C
Lgs
LF m a m gc
1 11 1 1 11 1 1 1 1 1
0
0C C
zz
N I I
I
76
Inward iteration: i = 1 (the only iteration)
22
0
0
0
f
22
0
0
0
n
2
1 1 2 11 2 2 1
2
20
Lgs
Lf R f F m gc
1
1 1 2 1 1 1 1 2 11 2 2 1 2 2 2 1
11
0 0
0 0
2 2
C
zz
n R n P F P R f N
n
ImL Lgc
1 1
1 1
2
ˆ
4 2zz
n Z
mL mLI gc
Check with the sophomore dynamics method above:
2 2
0 0 1 21 1 1
2 2 20
02 2 2
0 0 1 0 0
L L Lgs c s
c sL L L
f R f s c m gc m s c g
77
3) Lagrange-Euler formulation
11
0
0
11
0 0
0 0
0 0
xxC
yy
zz
I
I I
I
Kinetic and potential energy are scalars invariant with frame of expression – write k in {1} and u in {0}.
1
1
0
20
C
Lv
11 11 1 1
0 0
0 0C
zz
I
I
22 21 1
2 4 2 zz
mLk I
20
02 2
0 0
Lc
L Lu m g s mg s
2
21
2 4 2zz
mL LL k u I mg s
d L L
dt
2
4zz
L mLI
2
4zz
d L mLI
dt
2
L Lmg c
2
4 2zz
mL mLI gc
This result agrees with the sophomore and Newton-Euler recursion dynamics methods above to solve
this same problem.
78
4.5 Structure of Manipulator Dynamics Equations State Space Equation
,M V G
M N N mass matrix; symmetric and positive definite
,V 1N vector of Coriolis and centrifugal terms
G 1N vector of gravity terms
Configuration Space Equation
2M B C G
M N N mass matrix; symmetric and positive definite
B 1
2
N NN
Coriolis matrix
C N N centrifugal matrix
G 1N vector of gravity terms
11
2
N N : 1 2 1 3 1
T
N N
2 1N : 2 2 2
1 2
T
N
Cartesian State Space Equation
,x x xF M X V G
xM N N Cartesian mass matrix; symmetric and positive definite
,xV 1N vector of Cartesian Coriolis and centrifugal terms
xG 1N vector of gravity terms in Cartesian space
where: note:
1
1, ,
Tx
Tx
Tx
M J M J
V J V M J J
G J G
X J
X J J
79
4.6 Robot Dynamics Example
Here we summarize the dynamics equations of motion for the two-link planar 2R robot when each link is modeled as a homogeneous rectangular solid of dimensions Li, wi, hi of mass mi.
Newton-Euler recursion with outward kinematics and inertial calculations, followed by inward kinetics balances yields:
1
2 222 1 2 2 2 2 2 2 2 1 2 2 2 2 12
2 2 1 2 2
2 2 221 1 2 2 2 1 2 2 2 2
1 1 2 2 1 2 1 2 2 1 2 2
22 1 2 22 2
2 4 4 2 2
4 4 2 4
2
zz zz
zz zz zz
m L L c m L m L m L L s m L cI I g
m L m L m L L c m LI I m L m L L c I
m L L sm
1 1 1 2 2 121 2 2 1 2 2 1 12 2
m L c m L cL L s m L c g
where 2 2
12i
zzi i i
mI L h .
These dynamics equations of motion are very complicated – imagine how much worse these
equations will be for a spatial 6-axis robot such as the PUMA industrial robot.
X
Y
0
0
L 2
L 1
1
2
80
State Space Representation For the planar 2R robot, the dynamics equations of motion from the previous page are expressed in state-space form below:
,M V G 1
2
1
2
2 2 221 1 2 2 2 1 2 2 2 2
1 2 2 1 2 1 2 2 2
2 22 1 2 2 2 2 2 2
2 2
22 1 2 22 2 1 2 2 1 2
22 1 2 21
1 1 12 1 1
4 4 2 4
2 4 4
2,
2
2
zz zz zz
zz zz
m L m L m L L c m LI I m L m L L c I
Mm L L c m L m L
I I
m L L sm L L s
Vm L L s
m L c mm L c
G
2 2 12
2 2 12
2
2
L c
gm L c
Configuration Space Representation For the planar 2R robot, the dynamics equations of motion from the previous page are expressed in configuration-space form below:
2M B C G
,M G same as above
2 1 2 21 2
2 1 2 22
2 12
2 1 2 2 2
0
02
02
m L L sB
m L L s
Cm L L s
81
Numerical Dynamics Example For the planar 2R robot whose dynamics equations of motion were presented analytically above, here we calculate the required torques (i.e. solve the inverse dynamics problem) to provide the commanded motion at every time step in a resolved-rate control scheme. Identical results are obtained by both analytical equations and numerical Newton/Euler recursion. Given:
L1 = 1.0 m L2 = 0.5 m
Both links are solid steel with mass density = 7806 3kg m and both have width and thickness
dimensions w = t = 5 cm. The revolute joints are assumed to be perfect, connecting the links at their very edges (not physically possible, just a simplified model).
The initial robot configuration is:
1
2
10
90
The constant commanded Cartesian velocity is: 0 0
0 0
0.5
xX
y
(m/s)
The dynamics equations require the relative joint accelerations ; how do we find these? (See
the last page of the Acceleration Kinematics notes in this 429/529 Supplement: 1J X J . In
this example, 0X .)
Simulate motion for 1 sec, with a control time step of 0.01 sec. The plots for various variables of interest (joint angles, joint rates, joint accelerations, joint torques, and Cartesian pose) for this problem are given on the following page.
In the last plot, note that the robot travels 0.5 m in the Y0 direction in 1 sec (which agrees with the constant commanded rate of 0.5 m/s). The robot does not move in X; must move to compensate for the pure Y motion, but we cannot control independently with only two-dof. The first three plots are kinematics terms related to the resolved-rate control scheme; they are inputs to the inverse dynamics problem. The joint torques are calculated by the numerical recursive Newton-Euler inverse dynamics algorithm. These are the joint torques necessary to move this robot’s inertia in the commanded manner. Notice from the joint angles, joint rates, joint accelerations, and joint torques plots that the robot is approaching the 2 = 0 singularity towards the end of this simulated motion.
82
Numerical Dynamics Example: Plots
Dynamics Results Ignoring Gravity Dynamics Results Including Gravity
0 0.2 0.4 0.6 0.8 10.6
0.8
1
1.2
1.4
1.6
1.8
2Cartesian Pose
time (sec)
x (r
), y
(g
),
(b
) (
m,r
ad)
0 0.2 0.4 0.6 0.8 10
10
20
30
40
50
60
70
80
90Joint Angles
time (sec)
1 (
r),
2 (g
) (d
eg)
0 0.2 0.4 0.6 0.8 1-3
-2.5
-2
-1.5
-1
-0.5
0
0.5
1
1.5Joint Rates
time (sec)
join
t 1
(r),
join
t 2
(g)
(rad
/s)
0 0.2 0.4 0.6 0.8 1-20
-15
-10
-5
0
5
10Joint Accelerations
time (sec)
join
t 1
(r),
join
t 2
(g)
(rad
/s2)
0 0.2 0.4 0.6 0.8 10
10
20
30
40
50
60
70
80Joint Torques
time (sec)
(1
-red
; 2-
gree
n) (N
m)
0 0.2 0.4 0.6 0.8 1-50
0
50
100
150
200
250Joint Torques
time (sec)
(1
-red
; 2-
gree
n) (N
m)
83
Chapter 5. Robot Control Architectures With the robotic kinematics and dynamics we have learned and simulated, we can simulate robot control using various popular robot control architectures. 5.1 Inverse Pose Control Architecture 5.2 Inverse Velocity (Resolved-Rate) Control Architecture
84
5.3 Inverse Dynamics Control Architecture
The inverse dynamics method is also called the computed torque control method, and it is also called the feedback linearization control method.
In Chapter 4 we learned that the robot dynamics equations of motion are highly coupled and non-linear. Here they are in matrix/vector form:
, ,M V F G
vector of applied joint torques
M N N mass matrix; symmetric and positive definite
,V 1N vector of Coriolis and centrifugal terms
,F 1N vector of friction torques
G 1N vector of gravity terms
,, joint angles, rates, and accelerations
Computed Torque Control Architecture The computed-torque control method is based on canceling the dynamics effects by using the inverse dynamics method, in order to linearize the robot system for standard controller methods, such as PID control.
85
5.4 NASA Langley Telerobotics Resolved-Rate Control Architecture
86
5.5 Naturally-Transitioning Rate-to-Force Controller Architecture Resolved-rate control automatically (naturally) changes to force control when the robot end-effector enters into contact with the environment. A force/torque (F/T) sensor and force/moment accommodation (FMA) algorithm is required to accomplish this. After contact, the constant velocity command becomes a constant force command. In addition, if there is a human teleoperator with haptic interface, the force/moment that the human applies to the interface becomes proportional to the force/moment that the robot exerts on the environment at contact. This works and feels great in real-world applications, providing telepresence.
This control architecture was implemented by Dr. Bob and teams at NASA Langley Research Center and Wright-Patterson AFB6F
4.
4 R.L. Williams II, J.M. Henry, M.A. Murphy, and D.W. Repperger, 1999, Naturally-Transitioning Rate-to-Force Control in Free and Constrained Motion, ASME Journal of Dynamic Systems, Measurement, and Control, Trans. ASME, 121(3): 425-432.
ResolvedRate
F/TSensorX
K
X
F
F
F
XX M A
M
C C C+
-ManipulatorMaster
TJ
M
(FMA)
(Force Reflection - Optional)
87
5.6 Single Joint Control Every controller architecture we’ve considered requires linearized independent (but
simultaneous) single joint angle control, presented in this section. Manipulator dynamics is extremely complicated considering the number of terms. We can easily
use symbolic MATLAB to crank out the terms but they go on for pages and their structure is difficult to understand. In this case, numerical Newton-Euler recursion a’la Craig (2005, Chapter 6) is useful to get around the need for analytical expressions.
How is manipulator dynamics done in industry for control purposes? Answer: in almost all cases
it is ignored. How is this possible? (Large gear ratios tend to decouple the dynamic coupling in motion of one link on its neighbors – we will see this in modeling soon.) The vast majority of multi-axis industrial robots are controlled via linearized, independent single joint control. So, robot control in industry is generally accomplished by using N independent (but simultaneous) linearized joint controllers, where N is the number of joint space freedoms of the robot.
We now will briefly discuss single joint control. We will focus on a common system, the
armature-controlled DC servomotor driving a single robot joint / gear train / link combination, shown below. This requires dynamics, but it is not coupled nor non-linear.
Open-Loop System Diagram
V(t) armature voltage M(t) generated motor torque L(t) load torque L armature inductance M(t) motor shaft angle L(t) load shaft angle R armature resistance M(t) motor shaft velocity L(t) load shaft velocity iA(t) armature current JM lumped motor inertia JL(t) total load inertia vB(t) back emf voltage CM motor viscous damping CL load viscous damping n gear ratio
88
Closed-Loop Feedback High-level Control Diagram
High-level Computer Control Diagram
Simplifying Assumptions
rigid motor, load shafts n >> 1 (the large gear ratio n reduces speed and increases torque)
Real-world vs. model characteristics
Real World Our Model (simplified) non-linear
linearized
multiple-input, multiple output (MIMO)
single-input, single output (SISO)
coupled
decoupled
time-varying load inertia treat as a disturbance; the large gear ratio diminishes this problem
hysteresis, backlash, stiction, Coulomb friction
ignore
discrete and continuous
continuous for control design
89
Single Joint/Link System Modeling
We must derive all linear ordinary differential equations (ODEs) describing the dynamics of an armature-controlled DC servomotor driving a single robot joint / link. This is an electromechanical system, including a gear train. We must perform modeling, simulation, and control. The system diagram was shown earlier in this section.
Armature Circuit Diagram Electrical Model Kirchoff’s voltage law
(1) Electromechanical coupling
The generated motor torque is proportional to the armature current
(2) The back-emf voltage is proportional to the motor shaft angular velocity
(3)
(KT = KB can be shown)
90
Mechanical Model Euler’s Equation (the rotational form of Newton’s Second Law):
Free-body diagrams Load
(4) Motor
(5) where ( )LR t is the load torque reflected to the motor shaft.
Gear ratio n
(6) Substituting (6) into (5) yields (7):
(7)
91
Reflect load inertia and damping to motor shaft
Substitute into (4):
(8)
Substitute (8) into (7) to eliminate L:
(9)
Combine terms:
(10)
define:
2L
E M
JJ J
n effective inertia, total reflected to motor shaft
2L
E M
CC C
n effective damping, total reflected to motor shaft
92
Final Mechanical Model (ODE):
(11)
This is a second-order ODE in M. Can also be written as first-order ODE in M:
(12)
For common industrial robots, n >> 1 so 2
1
n is small; therefore we can choose a nominal JL and
assume it is constant without much error in control.
For example, the NASA 8-axis ARMII robot downstairs has n = 200 with harmonic gearing. This is why we can ignore the time-varying robot load inertia and design decoupled independent linear joint controllers. The gear-reduced load inertia variation is then treated as a disturbance to the single joint controller.
An alternative is to reflect the motor inertia and damping to the load shaft as shown below, but we will use the first case above.
2 2L M L L M L LJ n J C n C
93
Open-Loop Block Diagram Use Laplace transforms on different ODE pieces of the system model and then connect the
components together in blocks connected by arrows representing variables. A transfer function goes inside each block representing component dynamics. This block diagram and MATLAB/Simulink implementation including a disturbance torque are give below:
94
Closed-Loop Feedback Control Diagram Assuming perfect encoder sensor for angular position feedback, we include block for the single-
joint PID controller. The MATLAB/Simulink implementation is:
95
PID Controller Design
PID Controller characteristics
PID stands for:
Controller transfer function:
Each term does this: Use PID w/ approximate derivate in Simulink – numerical differentiation can lead to errors and numerical instability and thus it is to be avoided if possible. Trial and Error PID Design
Start with the P gain value – start low and increase P to get the desired time nature of response. Then add the D gain value term for stability. Add the I gain value to reduce steady-state error. Always use Simulink to simulate control and dynamics response of the single joint/link system for different PID choices; compare various cases and select a suitable controller in simulation.
A better approach is to perform analytical design for PID controllers using classical control theory, such as in ME 401. Controller Performance Criteria
Stability Rise Time Peak Time Percent Overshoot Settling Time Steady-state error
These performance criteria provide a rational basis for choosing a suitable controller, at least in
theory and simulation. The real world always provides some additional challenges (noise, modeling errors, non-linearities, calibration, backlash, etc.).
96
Include Gravity as a Disturbance Torque
Unmodeled, unexpected disturbances are modeled as disturbances in control systems. It is convenient to do so at the motor torque level in the block diagram: Gravity is known and expected. However, in single joint control we can include the effect of gravity as a disturbance. First let us model the gravity effect for a single joint. Lump all outboard links, joints, and motors as a single rigid body:
test with original PID gains redesign new PID gains with gravity disturbance considered
97
Chapter 6. Parallel Robots 6.1 Introduction
Serial vs. Parallel robot – demonstrate with wooden model kit. Example parallel manipulators: Stewart Platform, Variable Geometry Trusses, NASA Hyper-redundant Serpentine Arm, Backhoe.
Serial Robot: the joints and links extend from the base in a serial fashion to the end-effector. The overall structure is cantilevered which is not good for resisting end-effector loads. All joints must be active.
Parallel Robot: the joints and links are arranged in parallel from the base to the end-effector. There are multiple load paths from the end-effector to the base. There is a combination of active and passive joints.
Comparison of parallel vs. serial robots
Advantages of parallel robots Higher loads Higher accelerations Lower mass Higher stiffness Better accuracy, repeatability Ground-mounted actuators Direct-drive Open structure for cabling
Disadvantages of parallel robots SMALLER WORKSPACE Generally analysis is more difficult
Mobility – the Grubler (planar) and Kutzbach (spatial) mobility equations (see Section 1.3 in this 429/529 Supplement) for calculating overall manipulator dof are very important for parallel manipulators to ensure the correct number and type of active and passive joints are used to obtained the desired dof. There are infinite variations and possible designs for parallel robots.
98
Kinematics problems we will consider for parallel robots forward and inverse pose solutions workspace forward and inverse velocity solutions
Both inverse pose and rate problems have been presented for all possible planar parallel robots with
3 identical 3-dof legs8F
5.
There exists an interesting duality between serial and parallel robots: for parallel robots, the inverse pose and velocity solutions are generally straight-forward and the forward pose and velocity problems are harder. We know the opposite case is true for serial robots in general.
We could also do parallel robot dynamics but this is of less concern for a parallel robot than for a serial robot due to the parallel nature and its inherent advantages. Serial robot kinematics – DH parameters, homogeneous transformation matrices Parallel robot kinematics – Vector loop-closure equations; phasors; Euler’s identity Phasors – a powerful Re-Im manner to represent a vector. Euler’s Identity: Phasors enable compact notation and are very convenient for time derivatives (velocity analysis). Example Vector loop-closure equation for the four-bar mechanism:
5 R.L. Williams II and B.H. Shelley, Inverse Kinematics for Planar Parallel Manipulators, CD Proceedings of the 1997 ASME Design Technical Conferences, 23rd Design Automation Conference, DETC97/DAC-3851, Sacramento, CA, September 14-17, 1997.
99
6.2 Planar 3-RPR Manipulator Pose Kinematics
Planar 3-RPR Manipulator Diagram Inverse Pose Kinematics (for pose control)
Given:
Find:
Vector loop-closure equations The vector loop-closure equations are rewritten below: The inverse pose solution is straight-forward: We can also calculate the intermediate passive joint variables 1 2 3, , , for use in velocity and dynamics
analyses.
100
3-RPR parallel robot Forward Pose Kinematics (for simulation and sensor-based control)
Given: Find:
This is a coupled, nonlinear problem to solve – it is difficult to solve and multiple solutions generally exist, like the Inverse Pose Kinematics problem for serial robots. We use the same vector loop-closure equations: VLCE details expanded for one RPR leg: Considering all three legs (the problem is coupled), these represent six scalar equations in the six unknowns 1 2 3, , , , ,x y .
We will use the Newton-Raphson numerical iteration technique to solve this Forward Pose
Kinematics Problem. We can directly solve the above six equations for the six unknowns. However, we don’t always need the intermediate variables 1 2 3, , (also, we can calculate these angles later, using
Inverse Pose Kinematics, if required for velocity, dynamics, or computer simulation). So, to simplify the Forward Pose Kinematics Problem, square and add each XY equation pair (for all three legs) to eliminate the intermediate variables 1 2 3, , . Then we will have three equations to solve for the three
primary unknowns , ,x y :
Bring 2iL to the other side and solve via the Newton-Raphson iterative numerical method.
101
6.3 Newton-Raphson Method
The Newton-Raphson Method involved numerical iteration to solve coupled sets of n nonlinear equations (algebraic/transcendental – not ODEs) in n unknowns. It requires a good initial guess of the solution to get started and it only yields one of the possible multiple solutions. The Newton-Raphson method is an extension of Newton’s single function/single variable root-finding technique to n functions and n variables. The following is the form of the given functions to solve:
F X 0
where the n functions are: 1 2
T
nF F FF X X X X
and the n variables are: 1 2
T
nx x xX
Perform a Taylor Series Expansion of F about X:
2
1
ni
i i jj j
FF F x O
x
X X X X 1, 2, ,i n
Where iNR NR
j
FJ J
x
X is the Newton-Raphson Jacobian Matrix, a multi-dimensional form of the
derivative and a function of X. If X is small, the higher-order terms 2O X from the expansion are
negligible. For solution, we require: 0iF X X 1, 2, ,i n
Now with 2 0O X we have:
1
0n
ii i j i NR
j j
FF F x F J
x
X X X X X 1, 2, ,i n
So to calculate the required correction factor X at each solution iteration step, we must solve
0NRJ F X X :
1NRJ X F X
Actually, Gaussian elimination on NRJ X F X is preferable numerically, since it is more efficient
and more robust than matrix inversion.
102
Newton-Raphson Method Algorithm Summary
0) Establish the functions and variables to solve for: F X 0
1) Make an initial guess to the solution: 0X
2) Solve NR k k kJ X X F X for kX , where k is the iteration counter
3) Update the current best guess for the solution: 1k k k X X X
4) Iterate until k X , where we use the Euclidean norm and is a small, user-defined
solution tolerance. Also halt the iteration if the number of steps becomes too high (which means the solution is diverging). Generally less than 10 iterations is required for even very tight solution tolerances.
If the initial guess to the solution 0X is sufficiently close to an actual solution, the Newton-
Raphson technique guarantees quadratic convergence. Now, for manipulator forward pose problems, the Newton-Raphson technique requires a good
initial guess to ensure convergence and yields only one of the multiple solutions. However, this does not present any difficulty since the existing known pose configuration makes an excellent initial guess for the next solution step (if the control rate is high, many cycles per second, the robot cannot move too far from this known initial guess). Also, except in the case of singularities where the multiple solution branches converge, the one resulting solution is generally the one you want, closest to the initial guess, most likely the actual configuration of the real robot.
There is a very interesting and beautiful relationship between numerical pose solution and the
velocity problem for parallel robots: the Newton-Raphson Jacobian Matrix is nearly identical to the Inverse Velocity Jacobian Matrix for parallel robots. (In the planar case it is identical, in the spatial it is related very closely.) This reduces computation if you need both forward pose computation and inverse-velocity-based resolved-rate control.
3-RPR manipulator forward pose kinematics solution Use the Newton-Raphson numerical iteration method for solution, with:
Tx y X
The three coupled, nonlinear, transcendental functions Fi are the squared and added equations for
each RPR leg, with 2iL brought to the other side to equate the functions to zero.
103
Derive the required Newton-Raphson Jacobian Matrix:
iNR
j
FJ
x
X
iF
x
1, 2,3i
iF
y
1, 2,3i
iF
1, 2,3i
where: xBi
y
AA
A
and xHi
y
PC
P
The index i was dropped for convenience; use 1, 2,3i in the above definitions in the proper
places in the overall Newton-Raphson Jacobian Matrix. After the forward pose problem is solved at each motion step, we can calculate the intermediate
variables 1 2 3, , as in inverse pose kinematics solution above.
Alternate 3-RPR manipulator forward pose Newton-Raphson solution As we said, we could have solved the original six equations in the six unknowns including the
three intermediate variables 1 2 3, , . The functions are simpler (no squaring and adding) but the size of
the problem is doubled to 1, 2, , 6i . Below is the required Jacobian Matrix for this case, where the odd functions are the X equations and the even functions are the Y equations; also the variable ordering
is 1 2 3
Tx y X .
104
1 1 1 1
1 1 1 1
2 2 2 2
2 2 2 2
3 3 3 3
3 3 3 3
1 0 0 0
0 1 0 0
1 0 0 0
0 1 0 0
1 0 0 0
0 1 0 0
x y
x y
x y
NRx y
x y
x y
P s P c L s
P c P s L c
P s P c L sJ
P c P s L c
P s P c L s
P c P s L c
X
Alternate analytical 3-RPR manipulator forward pose solution Figure: Branch 1 1 2 2AC C A is a 4-bar mechanism with input angle 1 and output angle 2. With given
lengths L1 and L2, this four-bar mechanism has 1-dof, and it can trace out a coupler curve for point C3 in the plane. In general, this coupler curve is a tricircular sextic. The forward pose kinematics solution may be found by intersecting leg 3 3A C (a circle of given radius L3, centered at A3) with the coupler
curve. There are at most six intersections between a circle and tricircular sextic and so there may be up to six real multiple solutions to the 3-RPR parallel robot forward pose kinematics problem. There are always six solutions, but 0, 2, 4, or 6 of these will be real, depending on the commanded configuration and robot geometry.
105
6.4 Parallel Manipulator Workspace Since reduced workspace of parallel robots (when compared to serial robots) is their chief disadvantage, it becomes very important to determine the workspace of parallel robots and maximize it through design. There are two workspaces to consider (the same as for serial robots in Section 2.10 of this 429/529 Supplement):
1) reachable workspace is the volume in 3D space reachable by the end-effector in any orientation
2) dexterous workspace is a subset of the reachable workspace because it is the volume in 3D space reachable by the end-effector in all orientations.
For parallel robots, the dexterous workspace is almost always null since the rotation capability
is never full for all three Euler angles; therefore we usually define a reduced dexterous workspace wherein all Euler angles can reach 30 or some other user-definable range. 3-RPR Example
For planar parallel robots we can generally find the reachable workspace using a geometric method, figuring out what the end-effector can reach guided by each leg on its own, and then intersecting the results: For determination of the dexterous workspace, it is most convenient to numerically or geometrically generate in MATLAB the reachable workspace for different values (end-effector orientation) within the desired limits. Then stack these up and intersect them to find the dexterous workspace, defined for a reduced desired rotational range LIMIT .
106
6.5 Planar 3-RPR Manipulator Velocity Kinematics
First the pose configuration variables must all be known. Then we can define and solve two problems: Forward velocity kinematics (for simulation) Given: Find: Inverse velocity kinematics (for resolved-rate control) Given: Find:
In both cases we will have intermediate passive joint rate unknowns 1 2 3, , involved. Both
velocity kinematics problems use the same rate equations; we will derive these from looking at the three single RPR legs separately (meeting at the end-effector). Figure: Vector loop-closure equation using phasors: XY component equations: Angle equation: The velocity equations for one RPR leg are obtained from the first time derivatives of the XY and angle equations:
107
These equations are written in matrix-vector form to yield the RPR leg Jacobian matrix: Written in compact notation:
T
zX x y is the same for all three legs (the Cartesian end-effector rates). i includes one active
and two passive joint rates for each of the three RPR legs, 1, 2,3i . Let us now find the overall robot Jacobian matrix, using only active rates and ignoring the passive rates. Invert the leg Jacobian matrix: Invert symbolically: Pull out only the active joint row of this result: Assemble all three active joint rows into the overall robot Jacobian matrix: Note: this above equation solves the Inverse Velocity Problem. No inversion is required and so the Inverse Velocity problem is never singular. Here it is expressed in compact notation:
108
Forward Velocity Problem Compact notation (solve numerically from the Inverse Velocity Solution): Ironically, it is the Forward Velocity Problem that is subject to singularities for parallel robots. i for use in velocity equations Figure:
1 1 1
2 2 2
3 3
120
270
The second two relationships assume 1 2 3 30 (i.e. there is an equilateral end-effector triangle).
109
Example Symbolic MATLAB Code Here is the MATLAB code that was used to symbolically invert the RPR leg Jacobian matrix as presented earlier. Symbolic computing has a lot of power in robot kinematics, dynamics, and control. % % Symbolic MATLAB code to invert the RPR leg Jacobian % clear; clc; % Declare symbolic variables L = sym('L'); LH = sym('LH'); t1 = sym('t1'); b1 = sym('b1'); c1 = sym('cos(t1)'); s1 = sym('sin(t1)'); cp = sym('cos(t1+b1)'); sp = sym('sin(t1+b1)'); % Jacobian elements j11 = -L*s1-LH*sp; j21 = L*c1+LH*cp; j13 = -LH*sp; j23 = LH*cp; % Jacobian matrix J = sym([j11 c1 j13;j21 s1 j23;1 0 1]); % Invert Jinv = inv(J); % Simplify Jinvsimp = simple(Jinv); % Check Ident1 = simple(Jinvsimp * J); Ident2 = simple(J * Jinvsimp);
