Ferrier_kinematics4

advertisement
Inverse Kinematics
Professor Nicola Ferrier
ME 2246, 265-8793
ferrier@engr.wisc.edu
Forward Kinematics
• position equation:
End-effector w.r.t. {e}
• Where the transformation for each link
is built from our DH parameters:
Forward Kinematics
• Forward Kinematic Transformation:
• Has an rotation and translation:
Roll,pitch,yaw
Angle-axis
Euler Y-Z-Y
Forward Kinematics
• Pick one rotation description:
• Decompose R, i.e. solve:
Forward Kinematics
• Pick one rotation description:
Find terms that you can
easily solve equations
Decomposition of Rotation Matrices
• Roll, Pitch, and Yaw
• The inverse solution for  in (-/2,/2):
Decomposition of Rotation Matrices
• Roll, Pitch, and Yaw
• The inverse solution for  in (/2,3/2):
Decomposition of Rotation Matrices
• Euler Angles
• Inverse solution for r13 0 and r23 0
(singularity when sin=0):
Decomposition of Rotation Matrices
• OR
Decomposition of Rotation Matrices
• Angle Axis
• Inverse solution (singularity at =0,,...
and there are two solutions for § ):
Forward Kinematics
• Pick one rotation description:
• Decompose T :
Cartesian Space & Joint Space
Forward
kinematics
Joint Space
(q1,q2,…,qN)
Cartesian
Space
(x,y,z,,,)
Inverse
kinematics
(arm solution)
Inverse Relationship?
• Find joint positions for a given endeffector pose
• Also find joint positions, speeds and
accelerations for a sequence of poses
Inverse Relationship?
• Analytic Inverse
– Robot specific
– Multiple solutions
– Once found, computationally simple
• Geometric
– Robot specific
– Insight?
• Iterative Computation (ME739)
– Computationally expensive
– General solution
A Heuristic for Inverse Kinematics
• The solution to the inverse kinematic
equations (called the arm solution) can
often be found using a heuristic
approach.
– Does not guarantee a solution
– Solution may not be unique
– Some solutions may be redundant
IK Heuristic Algorithm
1. Perform forward kinematics to find the
general transformation matrix
2. Equate the transformation matrix to
the manipulator transformation matrix.
a) For a particular solution the manipulator
transformation matrix contains “numbers”
b) For a general solution the manipulator
transformation matrix contains “variables”
IK Heuristic Algorithm
3. Look at both matrices for:
a) Elements which contain only one joint variable
b) Pairs of elements which will produce an expression
in only one joint variable when divided (look for
divisions that result in the atan2 function)
c) Elements, or combinations of elements, that can be
simplified using trigonometric identities
4. Having selected an element, equate it to the
corresponding element in the other matrix to
produce an equation. Solve this equation to
find a description of one joint variable in
terms of the elements of the manipulator
transformation matrix.
IK Heuristic Algorithm
5. Repeat step 4 until all the elements
identified in step 3 have been used.
6. If any of these solutions suffer from
inaccuracies, undefined results, or
redundant results, set them aside and
look for better solutions.
IK Heuristic Algorithm
7. If there are more joint angles to be
found, pre-multiply both sides of the
matrix equation by the inverse of the
A1 matrix to produce a new set of
equivalent matrix elements.
IK Heuristic Algorithm
8. Repeat steps 3 to 7 until either solutions to
all the joint variables have been found, or
you have run out of A matrices to premultiply.
9. If a suitable solution cannot be found for a
joint variable, choose one of those discarded
in step 6, taking note of regions where
problems may occur.
10. If a solution cannot be found for a joint
variable in terms of the elements of the
manipulator transform, it may be that the
manipulator cannot achieve the specified
position and orientation: the position is
outside the manipulator’s workspace.
IK Heuristic Algorithm
• Note: theoretical solutions may not be
physically attainable because of the
mechanical limits on the range of joint
variables.
DH Example: “academic manipulator”
Z0
1
x1
Z2
L1
Z1
L2
x2
1
x0
2
Link
1
2
Var
1
2

d

a
1
2
L1
0
90o
0
L2
0
Steps 1 and 2
• The general transformation matrix
• Set equal to:
Steps 3 and 4
• Look for elements and select terms
More steps….
• Pre-multiply for a new equation:
DH Example: two link planar arm
DH table for two link arm
L2
L1
x0
Z0 1
Link
1
2
Z1
Var
1
2
x1
2
x2
Z2

d

a
1
2
0
0
0
0
L1
L2
Forward Kinematics: planar 2-link manipulator
Forward Kinematics: planar 2-link manipulator
Heuristic solution?
Can’t select an
“obvious” set of
equations…
Trignometric Solutions
• Sometimes simple (but usually not)!
Trig Solutions
• For first angle – revisit equations
Trig Solutions
• A trig hint:
• Let
• Then
• So
Solution to 2 link planar arm
• Arm solution is
– where
Multiple Solutions?!
Geometric Solutions
• Intuition, insight….?
Geometric Solutions
• Intuition, insight….?
Geometric Solutions
• Intuition, insight….?
Get same solution
as trig method
Solution to 2 link planar arm
• Arm solution is
– where
Here we
know the
sign by
observation
Download