social and community planning and reporting guidelines
TRANSCRIPT
A.A. 2007/2008
Roboticafor Computer Engineering students
Marcello RestelliDipartimento di Elettronica e InformazionePolitecnico di Milanoemail: [email protected]: 02-2399-4015
Robot Kinematics
2
Study of Motion
Kinematics studies the relation between the independent variables of the joints and the Cartesian positions reached by the robotDynamics studies the equations that characterize the robot motion (speed and acceleration)Trajectories computation consists of determining a way provide a robot for the sequence of points (or joint variables) to move from one point to another (kinematics) with suitable speeds and accelerations (dynamics)Control aims at performing trajectories that are similar to those computed
3
Kinematics
Two types of kinematics:Forward kinematics (angles to position):
what are you given:the length of each link
the angle of each joint
what you can find:the position of any point (i.e., its (x,y,z) coordinates)
Inverse Kinematics (position to angle):what are you given:
the length of each link
the position of some point on the robot
what you can findthe angles of each joint needed to obtain that position
4
Forward Kinematics: Example with Planar RR
Situationyou have a robotic arm that starts aligned with the x
0-axis
you tell the first link to move by θ1 and the second link to
move by θ2
The quest:what is the position of the end of the robotic arm?
Solution:Geometric approach
easier in simple situationsAlgebraic approach
involves coordinate transformations
5
Forward Kinematics: Example with 3 link arm
Situationyou have a 3 link arm that starts aligned with the x-axisl1,l
2,l
3 are the lengths of the 3 links
The three links are moved respectively by θ1, θ
2, θ
3
The questFind the Homogeneous matrix to get the position of the yellow dot in the X0Y0 frame
Solution H = Rz(θ1
) * Tx1(l1) * Rz(θ2) * Tx2(l2) * Rz(θ3
)
multiplying H by the position vector of the yellow dot in the X3Y3 frame gives its coordinates relative to the X0Y0 frame
X2\
X3Y2
Y3
θ1
θ2
θ3
1
2 3
X1
X0
Y0
6
Forward Kinematics
With more than 3 joints and with kinematic chains that do not lay on the plane the geometric method is too difficultWe need a systematic method:
represent the open kinematic chain with the same formalismfind algebraic solutions
using homogeneous coordinatesbuild references using a quasi-algorithmic procedure
Denavit-Hartenberg Notation
7
Denavit-Hartenberg Notation
Each joint is assigned a coordination frame
Using the Denavit-Hartenberg notation, you need 4 parameters to describe how a frame (i) relates to a previous frame ( i -1 )To align two axes we need 4 parameters: α, a, d, θ
8
The Parameters
1) ai
Technical definition: is the length of the perpendicular between the joint axes. The joint axes is the axes around which revolution takes place which are the Z(i-1) and Zi axes. These two axes can be viewed as lines in space. The common perpendicular is the shortest line between the two axis-lines and is perpendicular to both axis-lines.
9
The Parameters
1) ai
Visual approach: “A way to visualize the link parameter ai is to imagine an expanding cylinder whose axis is the Z(i-1) axis - when the cylinder just touches the joint axis i the radius of the cylinder is equal to ai ”(Manipulator Kinematics)
10
The Parameters
1) ai
It’s Usually on the Diagram Approach: If the diagram already specifies the various coordinate frames, then the common perpendicular is usually the X(i-1) axis. So ai is just the displacement along the X(i-1) to move from the i-1 frame to the i frame.If the link is prismatic, then ai is a variable
11
The Parameters
1) αi
Technical Definition: Amount of rotation around the common perpendicular so that the joint axes are parallel.i.e. How much you have to rotate around the X(i-1) axis so that the Z(i-1) is pointing in the same direction as the Zi axis. Positive rotation follows the right hand rule.
12
The Parameters
1) di
Technical Definition: The displacement along the Zi axis needed to align the a(i-1) common perpendicular to the ai
common perpendicular.In other words, displacement along the Zi to align the X(i-1) and Xi axes.
13
The Parameters
1) θi
Technical Definition: Amount of rotation around the Zi axis needed to align the X(i-1) axis with the Xi axis.
14
The Denavit-Hartenberg Matrix
Just like the Homogeneous Matrix, the Denavit-Hartenberg Matrix is a transformation matrix from one coordinate frame to the next. Using a series of D-H Matrix multiplications and the D-H Parameter table, the final result is a transformation matrix from some frame to your initial frame.
[cosθ i −cosαi sinθi sinα i sinθi ai cosθisinθi cosθi cosαi −sinα i cosθi ai sinθi0 sinα i cosαi di
0 0 0 1]
[cosθ i −sinθi 0 0
sinθi cosθi 0 0
0 0 1 d i
0 0 0 1] [1 0 0 ai
0 cosαi −sinα i 0
0 sinα i cosαi 0
0 0 0 1]
15
Algorithm to Fix the Reference Frames
1) The robot is put in resting position. X0Y
0Z
0 is fixed to the
robot basis2) For each pair i of consecutive joints
1) Fix the axis of joint i+1 on Zi
2) Put the origin of frame i at the intersection of axes Zi e Z
(i-1).
If the axes do not intersect, then put the origin at the intersection between Z
i and the minimum distance segment
between the two axes3) Set the X
i axis on the same direction of the minimum
distance segment, or on the link direction towards the hand4) The Y
i axis is fixed following the right-hand rule
3) Fix the reference frame for the hand
16
Algorithm to Fix the Reference Frames
4) For each pair i of consecutive reference frames compute the four parameters1) Compute d
i: distance between axes X
(i-1) and X
i along axis
Z(i-1)
(variable for a prismatic joint)
2) Compute a(i-1)
: distance between axes Z(i-1)
and Zi along axis
Xi (link length)
3) Compute θi: revolution angle between X
(i-1) and X
i around
axis Z(i-1)
(variable for a revolute joint)
4) Compute α(i-1)
: revolution angle between Z(i-1)
and Zi around
axis Xi (twist angle)
17
Hand and Grasp Reference Frames
For what regards the reference frame of the hand:The origin is placed between the fingersThe Y-axis follows the sliding direction of the fingersThe Z-axis follows the approaching direction, pointing towards the finger opening
For what regards the reference frame of the grasp point:To simplify the grasping operations it may be useful to place the object reference frame on the grasping pointInverse kinematics is important in this context
18
Algorithm for Forward Kinematics
1) Put the manipulator in resting position2) Set the reference frames to joints and links3) Compute Denavit-Hartenberg parameters4) Compute transformation matrix A
i that allows to pass from
the reference frame of the i-th joint to the one of the (i+1)-th joint
5) Multiply matrices Ai to get matrix T that allows to pass from
the reference frame of the base X0Y
0Z
0 to the one of the
hand XnY
nZ
n
6) From the matrix T extract the coordinates of the current position
7) Look at the rotation sub-matrix and extract orientation components
19
Transformation Matrix A
The transformations required to pass from reference frame i-1 to reference frame i w.r.t. joint i-1 are:
Rotate X(i-1)
by θi around Z
(i-1), to align it with X
i: Rot(Z
(i-1),θ
i)
Translate the origin of reference frame i-1 by di along Z
(i-1),
to overlap X(i-1)
and Xi: Transl(0,0,d
i)
Translate the origin of reference frame i-1 by ai along X
i, to
place it at the origin of reference frame i: Transl(ai,0,0)
Rotate Z(i-1)
around Xi by α
i to overlap the two reference
frames i-1 and i: Rot(Xi,α
i)
Ai-1,i
i-1 = Transl(0,0,di)*Rot(Z
(i-1),θ
i)*Transl(a
i,0,0)*Rot(X
i,α
i)
20
Transformation Matrix A
Revolute joint
Prismatic joint
[cosθ i −cosαi sinθi sinα i sinθi ai cosθisinθi cosθi cosαi −sinα i cosθi ai sinθi0 sinα i cosαi di
0 0 0 1]
[cosθ i −cosαi sinθi sinα i sinθi 0
sinθi cosθi cosαi −sinα i cosθi 0
0 sinα i cosαi d i
0 0 0 1]
21
Transformation Matrix T
Aim: given a point in the mobile system, we want its coordinates in the fixed systemT = A
0,60 = A
0,10· A1,2
1· A2,32· A3,4
3· A4,54· A5,6
5
Pi-1 = Ai-1,ii-1· Pi
T=[x i yi zi pi0 0 0 1 ]=[R i pi
0 1 ]
22
RR Manipulator
Workspace: internal area of a sphereInitial part of a spherical or a hinged robot
T=A0,10 ⋅A1,2
1 =[cosθ1 0 sinθ1 0
sinθ1 0 −cosθ1 0
0 1 0 d 10 0 0 1
]⋅[cosθ2 −sinθ2 0 l2⋅cosθ2sinθ2 cosθ2 0 l 2⋅sinθ20 0 1 00 0 0 1
]
Link θ α a d
1 90° 0
2 0° 0
θ1
d1
θ2
l2
23
Planar RR Manipulator
Workspace: circle areaPossible part of a SCARA robot
T=A0,10 ⋅A1,2
1 =[cosθ1 −sinθ1 0 l1⋅cosθ1sinθ1 cosθ1 0 l 1⋅sinθ10 0 1 00 0 0 1
]⋅[cosθ2 −sinθ2 0 l2⋅cosθ2sinθ2 cosθ2 0 l2⋅sinθ20 0 1 00 0 0 1
]
Link θ α a d
1 0° 0
2 0° 0
θ1
l1
θ2
l2
24
RT Manipulator
Workspace: circle areaPossible part of a cylindric robot
T=A0,10 ⋅A1,2
1 =[cosθ1 0 sinθ1 0
sinθ1 0 −cosθ1 0
0 1 0 d 10 0 0 1
]⋅[1 0 0 00 1 0 00 0 1 d 20 0 0 1
]
Link θ α a d
1 90° 0
2 0° 0° 0
θ1
d1
d2
25
TR Manipulator
Workspace: round cornered rectangle
T=A0,10 ⋅A1,2
1 =[1 0 0 00 0 −1 00 1 0 d 10 0 0 1
]⋅[cosθ2 −sinθ2 0 l2⋅cosθ2sinθ2 cosθ2 0 l 2⋅sinθ20 0 1 00 0 0 1
]
Link θ α a d
1 0° 90° 0
2 0° 0
d1
θ2
l2
26
TT Manipulator
Workspace: rectanglePossible part of a Cartesian robot
T=A0,10⋅A1,2
1=[1 0 0 00 0 −1 00 1 0 d 10 0 0 1
]⋅[1 0 0 00 1 0 00 0 1 d 20 0 0 1
]
Link θ α a d
1 0° 90° 0
2 0° 0° 0
d1
d2
27
Hand Orientation
We use the Euler's angles...
... and equal them to the orientation sub-matrix:
R=Rot z ,⋅Rot u ,⋅Rot w ,=
=[coscos−sincos sin −cos sin−sin cos cos sin sin
sincoscoscos sin −sin sincos cos cos −cossin
sin sin sin cos cos ]
T=[x i yi zi pi0 0 0 1 ]=[R i pi
0 1 ]
R i=[nx ox axn y o y a ynz oz a z
]
28
Hand Orientation: A Simple Solution
A simple solution can be obtained by solving the set of 9 equations and 3 unknown quantities
This solution is useless:the accuracy of arccos function depends by the angle since cos(θ) = cos(-θ)
when θ approaches 0° or 180° the last two equations give inaccurate or indefinite solutions
=cos−1[az ]
=cos−1 [ o z
sin ]=cos−1 [−a y
sin ]
29
Hand Orientation: using atan2
A trigonometric function that does not have these problems is the atan2(x,y) function, that computes tan-1(y/x) placed in the right sub-frame
=atan2 y , x={0 °90 ° , forX∧Y
90 °180 ° , for− X∧Y−180 °−90 ° , for−X∧−Y−90 °0° , forX∧−Y
}
30
Hand Orientation: Paul Method
The Paul method computes the hand orientation using the atan2 functionThe method consists of pre-multiplying both ends of equation by the inverse of one of the rotation matrices.
Now Φ is on the left side
R z ,−1
⋅[n x ox axn y o y a ynz o z az
]=Ru ,⋅R w ,
= tan−1 [ ax
−a y ]=atan2 ax ,−a y
= tan−1 [ sincos ]= tan−1 [−ox cos−o y sin
nx cosny sin ]=atan2 −ox cos−o y sin ,nx cosn y sin
= tan−1[ sin cos ]=tan−1 [ax sin−ay cos
az ]=atan2 ax sin−a y cos ,az
31
Hand Orientation: Paul Method
The same method can be applied trough post-multiplying both ends of equation by the inverse of one of the rotation matrices.
We do not know when it it is better to use the pre or the post-method
[nx o x axn y o y a ynz oz a z
]⋅Rw ,−1=R z ,⋅Ru ,
= tan−1 [ nz
oz ]=atan2 nz ,oz
=tan−1 [ sincos ]= tan−1 [ ny coso y sin
nx cosox sin ]=atan2 n ycosoy sin , nx cosox sin
= tan−1[ sin cos ]=tan−1 [nz sinoz cos
az ]=atan2 nz sinoz cos ,az
32
Forward Kinematics: Conclusions
The forward kinematic problem has always one and only one solution, that can be obtain through the computation of transformation matrix TSince T is obtained through the product of 6 matrices, for computational reasons use only two matrices:
one for the 3 DOF of the armone for the 3 DOF of the hand
Nevertheless there are still some real-time problems to compute the Cartesian coordinates
Often joint variables are used
33
Inverse Kinematics: Problem Formulation
Given a position and an orientation in the Cartesian space, inverse kinematics aims at finding a configuration of joints that allows to reach them
Solution existenceSolution uniquenessSolution methodsReal-time solutions
θ
X
Y
S
(x , y)
θ=arctan yx
S= x2 y2
34
Inverse Kinematics: the Solution
Unfortunately, the transformation of position from Cartesian to joint coordinates generally does not have a closed-form solutionThe solution can be obtained by solving the system obtained by setting the T matrix of the hand (that contains the Cartesian coordinates) equal to its symbolic expression (that contains the joint coordinates
12 equations and 6 unknownsonly 3 of the 9 rotation terms are independentso we have 6 non-linear, transcendental equations with 6 unknowns
[xx y x zx p xx y y y z y p xx z y z zz p x0 0 0 1
]=[nx o x a x d xn y o y a y d xn z oz a z d x0 0 0 1
]
35
Inverse Kinematics: Solution Existence
If the point to be reached falls into the workspace and the robot has 6 DOF then a solution existsIf the robot has less than 6 DOF it has to be verified that the point can be reachedIt is hard to determine whether a point falls or not inside the workspaceThings are even harder when considering the dexterous spaceThe solution does not exist when:
the goal point is outside the workspacethe goal point is inside the workspace but there are physical constraintsthe goal point must be reached following a trajectory that requires infinite acceleration
36
Inverse Kinematics: Solution Uniqueness
What makes Inverse Kinematics a hard problem?Redundancy: a uniques solution to this problem does not existThe number of possible solutionsincreases with the number of DOFThe number of solutionsdepends by the number of Denavit-Hartenberg non-null parameters
For a 6R manipulator we may have at most 16 solutions
We are interested in all the possible solutionswe need a criterion to select the best solution:
the “closest” to the current configurationmove outermost links the mostenergy minimizationminimum time
(x , y)l2
l1l2
l1
37
Inverse Kinematics: Solution Methods
Several solution methods have been proposed:Closed form solutions
Geometrical methodsreduce the larger problem to a series of plane geometry problems
Algebraic methodstrigonometric equations
Iterative (numerical) solutions
38
Geometrical Method: An Example
Using the law of cosines:
Using the law of sines:
l1
l2θ2
θ1
α
(x , y)
x 2 y2 =l12l2
2−2 l1 l 2cos −2
2=arccos x2 y 2−l1
2−l2
2
2 l1 l2 : REDUNDANT
sin 1−
l2=sin −2
x2 y2=sin 2
x 2 y2
1=arcsin l2sin 2 x2 y2 atan2 y , x: REDUNDANCY caused by the two values of 2
39
Algebraic Method: The Same Example
We already saw the kinematics equations:
l1
l2θ2
θ1
θ1+θ2(x , y)
x=l1cos 1l2cos12
y=l1 sin1 l2sin 12
x 2 y 2=l12l2
22 l1 l 2cos 12cos 1sin 12sin 1
x 2 y 2=l12l2
22 l1 l 2cos2
2=arccos x2 y 2−l1
2−l2
2
2 l1 l2 : REDUNDANT
x=l1cos 1l2 cos 1cos2 −sin 1sin 2
y=l1 sin1 l2 sin 1cos 2 cos 1 sin2
1=arcsin y l2 cos2l1− x l2 sin 2
l1l 2cos 2 2 l2 sin 2
2
40
Inverse Kinematics: Closed Form Solutions
An arbitrary 6 DOF manipulator cannot be solved using a closed form solutionPieper's method: solution of a 4th order polynomialPaul's method: pre- and post-multipliesOther methods
helicoidal algebradouble matricesquaternionsalgebraic approaches based on substitutions
u = tan(θ/2)cos(θ) = (1-u2)/(1+u2)sin(θ) = 2u/(1+u2)
41
Inverse Kinematics: Pieper's Solution
Suitable for manipulators with 6 DOF in which 3 consecutive axes intersect at a point (including robots with 3 consecutive parallel axes, since they met at a point at infinity)Pieper's method applies to the majority of commercially available industrial robots (Puma 560, SCARA)The Pieper's solution computes separately the first 3 and the last 3 joints:
Locate the intersection of the last 3 joint axesSolve IK for first 3 jointsCompute T
0,3 and determine T
3,6 as T
0.3'T
0.6
Solve IK for last three joints
42
Inverse Kinematics: Paul's Solution
Set the known Cartesian matrix T equal to the manipulator matrix (containing the joint unknowns)Search the second matrix for:
Elements with only one joint unknownPair of elements that lead to an expression with only one unknown once divided by each otherElements that can be simplified
Solve the equations involving the selected elements, thus finding joint unknowns as function of known elements belonging to the T matrixIf no element has been identified at step 2, pre-multiply both terms for the inverse of the transformation matrix of the first link (and iteratively for the following links)
as an alternative it is possible to post-multiply for the inverse of the transformation matrix of the last link
43
Example of Paul's method: RR
Select px e p
y
Pre-multiply by (A0,1
0)-1
T=[cosθ1 cosθ2 −cosθ1 sinθ2 sinθ1 l2 cosθ1 cosθ2sinθ1 cosθ2 −sinθ1 sinθ2 −cosθ1 l2 sinθ1 cosθ2sinθ2 cosθ2 0 d 1l 2 sinθ20 0 0 1
]=[n x ox a x pxn y o y a y p yn z o z a z p z0 0 0 1
]p y
p x
=l2⋅sen θ1 ⋅cos θ2
l2⋅cos θ1⋅cosθ2 ⇒θ1=tan
−1 p y
px
p x⋅cos θ1 p y⋅sin θ1
pz−d 1=
l2⋅cos θ 2
l2⋅sin θ 2 ⇒θ 2=tan
−1 p z−d 1p x⋅cos θ1 p y⋅sin θ1
44
Example of Paul's method: TT
Select py
Select pz
T=[1 0 0 00 0 −1 −d 20 1 0 d 10 0 0 1
]=[n x ox a x pxn y o y a y p ynz o z a z pz0 0 0 1
]d 2=−p y
d 1= pz
45
Iterative Solutions
For robots with coupled geometries the closed form solution may not exist => iterative solutions
m equations with n unknownsit starts with an initial estimationcompute T matrix and error w.r.t. Cartesian valuesupdate the estimate to reduce the errorindefinite execution time to get a definite error or indefinite error to get a definite execution timeiterative methods may fail to find all the solutions to IK
While in the '80s these methods were not feasible, nowadays they represent an alternativeNevertheless, industrial robots are typically built to meet one of the Pieper's sufficient conditions in order to use closed form solutions
46
Redundancies and Degenerations
A manipulator is redundant if it can reach the same position with several different configurationsManipulators with more of 6 DOF are infinitely redundantA point reachable with infinite configurations is called degeneration pointPhysical constraints are imposed to avoid degeneration points
47
Static Precision
Accuracydifference between desired position and actually reached positionless tolerance => higher accuracy
Repeatabilitythe variance of the reached position while repeating the same commandimportant when the robot is programmed on the field
Spatial resolutionthe minimum distance that can be measured or commandedit depends by the resolution of the internal sensors
48
Manipulator Performances
Maximum payloadthe maximum weight that can be carried by a robot at low speed while keeping the accuracythe nominal payload is measured at maximum speed while keeping the accuracythe payload is any weight put on the wrist
Maximum speedthe maximum velocity at which the robot end can be moved while extended
Cycle timeThe cycle time is the time required to execute a standard cycle of pick-and-place of 12 inchesFor industrial robots ~ 1s