Download Inverse Kinematics

Inverse Kinematics
February 4, 2016
Once we have a mathematical model of where the robot’s hand is given the position of the
motors (via the angles of the joints) we can begin to ask the real question of what are the joint
angles, and thus the motor positons.
1. We can think of the problem as, given a position, how do we get here, if we’re constrained to
getting there by following along the links of the robot.
2. We’ve already seen one application of this, when we computed the yaw, pitch, and roll of the
hand.
3. This has the nice property of telling us what the yaw pitch and roll are. That’s nice because
it turns the ugly upper lefthand 3 × 3 matrix into something we can understand.
4. It has the unfortunate property of potential innaccuracies:
(a) arcsin is innaccurate when β is π/2, ... etc. Small variations in the input result is large
changes in the angle.
(b) Meanwhile, when β is near π/2, 3π/2, ... etc, the cosine gets close to 0, making those
fractions problematic.
(c) And finally, you can’t tell which quadrant the answer is supposed to be in (i.e. arcsin
etc all have two correct answers.
5. In general, we like to use arctangent when we can calculate both the sine and cosine parts:
(a) We calculate arctan with two arguments, the sin part and the cos part. These correspond
to y and x on the unit circle, right?
(b) We assume the range for arctan is −π to π
(c) We use the following cases:
(a) x = 0, y = 1. Return π/2
(b) x = 0, y = −1. Return −π/2
(c) y = 0, x = 1. Return 0
(d) y = 0, x = −1. Return π
(e) x > 0, y arctan y |y|
x
1
y
(f) x < 0, |y|
(π − arctan xy )
We will call this arctan2 or atan2, though if you see an example of arctan that takes 2
arguments, you know it is this function
6. Now let’s apply it to figuring out the joint angles in two dimensions.
Figure 1: Planar, 2-link arm
Let’s consider this problem by looking at an example. Consider our 2D arm of Figure 1, which
we will now interpret as a 3D arm, consisting of these two elbow joints. Hopefully, it is clear
that this consists of a rotation around the Z axis by the red angle φ1 , a translation along the
X axis of l1 , a rotation around the Z axis by the blue angle φ2 , and a translation along the Y
axis of l2 . Our transformation matrices look as follows (we use c1 = cos(φ1 )):

c1
 s1

 0
0
−s1
c1
0
0
 
0 l1 c1
c2
 s2
0 l1 s1 
·
1
0   0
0
1
0
−s2
c2
0
0
 
0 l2 c2
c1 c2 − s1 s2
 s1 c2 + c1 s2
0 l2 s2 
=
1
0  
0
0
1
0
−c1 s2 − s1 c2
−s1 s2 + c1 c2
0
0
0
0
1
0

l2 c1 c2 − l2 s1 s2 + l1 c1
l2 s1 c2 + l2 c1 s2 + l1 s1 


0
1
Now, remember, this is an instance of our general transformation matrix T . If we make our
desired end of arm location (px , py , pz ), and our roll, pitch, and yaw φ, ψ, and θ, respectively,
then

c1 c2 − s1 s2
 s1 c2 + c1 s2


0
0
−c1 s2 − s1 c2
−s1 s2 + c1 c2
0
0
 
0 l2 c1 c2 − l2 s1 s2 + l1 c1
cθ cφ
 sθ cφ
0 l2 s1 c2 + l2 c1 s2 + l1 s1 
=
  −sφ
1
0
0
1
0
|
−sθ cψ + cθ sφ sψ sθ sψ + cθ sφ cψ
cθ cψ + sθ sφ sψ −cθ sψ + sθ sφ cψ
cφ sψ
cφ cψ
0
0
{z
T

px
py 

pz 
1
}
Given our desired position and roll/pitch/yaw, we can write T (the right side of this equality)
exactly, as numbers, giving us a system of equations to solve. Unfortunately, these are nonlinear, transcendental equations, sometimes making them difficult to solve. But not impossible!
Let’s look back at our example, and see if we can solve for s1 , c1 , s2 , and c2 (because we still
prefer using arctan2 to arcsin or arccos).
If you look carefully, you can see that T (1, 4) = l2 T (1, 1) + l1 c1 . Given this, we can easily
solve for c1 . Similarly, T (2, 4) = l2 T (2, 1) + l1 s1 , allowing us to solve for s1 .
2
Another way to see this is by using trig identities. For example, sin(α) cos(β)+cos(α) sin(β) =
sin(α + β), and cos(α) cos(β) − sin(α) sin(β) = cos(α + β). If we write cos(θ1 + θ2 ) as c12 (and
similarly with sin), we can write our above matrix as:


c12 −s12 0 l2 c12 + l1 c1
 s12 c12 0 l2 s12 + l1 s1 


 0

0
1
0
0
0
0
1
Using the entry in the upper right, which must equal T (1, 4), and using the fact that T (1, 1) =
c12 , we can now see that c1 = T (1,4)−ll12 T (1,1) , giving us the same answer as above. Oftentimes,
trig identities can make the patterns for a particular arm clearer.
Great! Whichever way you prefer, we are now able to calculate φ1 . One down, one to go.
Fortunately, there are now a number of ways to set up a system of equations to solve for s2
and c2 , to allow us to do θ2 = arctan2(s2 , c2 ).
7. OK, so how do we solve the inverse kinematics problem (i.e. find the θs) for a real 3D arm?
The same technique as above, but the second matrix has many θs as unknowns. Thus it is
really ugly.
(a) Let’s pretend we have a robot with just the first 2 links of your arms. In that case, we
have:

 

cos(Θ1 ) − sin(Θ1 ) 0 l1 cos(Θ1 )
cos(Θ2 ) 0 sin(Θ2 ) l2 cos(Θ2 )
 sin(Θ1 ) cos(Θ1 ) 0 l1 sin(Θ1 )   sin(Θ2 ) 0 − cos(Θ2 ) l2 sin(Θ2 ) 

·
=

 

0
0
1
0
0
1
0
0
0
0
0
1
0
0
0
1




(the 2nd matrix looks odd because it is the application of both
and the rotation around x present in the second link)
 
C1 C2 − S1 S2 0 S2 C1 + C2 S1 l1 C1 + l2 C1 C2 − l2 S1 S2
xx
 xy
S1 C2 + C1 S2 0 S1 S2 − C1 C2 l1 S1 + l2 S1 C2 + l2 C1 S2 
=
  xz
0
1
0
0
0
0
0
1
0
the rotation around z

yx zx px
yy zy py 

yz zz pz 
0 0 1
(b) Maybe not immediately illucidating, but again, maybe some trig identities will help us:
sin α cos β + cos α sin β = sin(α + β)
cos α cos β − sin α sin β = cos(α + β)
We’ll represent cos(Θ1 + Θ2 ) as C12 , yielding:

 
C12 0 S12 l1 C1 + l2 C12
 S12 0 −C12 l1 S1 + l2 S12  

=
 0 1
 
0
0
0 0
0
1
3

xx yx zx px
xy yy zy py 

xz yz zz pz 
0 0 0 1
(c) How does this help? Well, lets start picking out individual equations
C12 = xx
l1 C1 + l2 C12 = px
l1 C1 + l2 xx = px
px − l2 xx
C1 =
l1
px − l2 xx
Θ1 = arccos
l1
(d) But recall that we want to use arctan 2 as a function of both cosine and sine...
S12 = xy
l1 S1 + l2 S12 = py
l1 S1 + l2 xy = py
py − l2 xy
S1 =
l1
px − l2 xx py − l2 xy
Θ1 = arctan 2
,
l1
l1
Θ1 + Θ2 = arctan 2 (C12 , S12 )
= arctan 2 (xy , xx )
Θ2 = arctan 2 (xy , xx ) − Θ1
8. Is that all there is? Yes, and no. That is the basic technique, but there are lots of tricks
that you can use from your tool kit on solving equations with trigonometric functions. For
instance:
2
2
(a) What if we can find the cosine, but not the sine? We
p know that cos θ + sin θ = 1, so
if we know C3 , we can calculate θ3 = arctan 2(C3 , ± 1 − C32 ). Note the plus or minus,
this situation often comes up when there are multiple solutions. The danger with this
is that for very small values of the cosine, the square might drip below the range of the
floating point representation.
(b) What if we have no single function in a cell? That is, what if nothing like C5 happens
alone, but only things like C5 S34 ? We can divide one equation by another:
−S34 S5 = yz
C5 S34 = xz
−S34 S5
yz
=
C5 S34
xz
yz
−T5 =
xz
yz
θ5 = arctan −
xz
4
See that in this case we don’t get to use arctan2, so there are two potential solutions
here too, but only one is valid.
(c) In general, any technique for solving systems of equations, especially trigonometric, that
you can bring to bear can help you solve these. It is an art, not a formulaic process I
can describe to you. Fortunately, you just have to do this once for a robot.
9. Now what about all these multiple solutions? What does that mean? In many cases it
means exactly that, there are multiple positions of the robot that can result in the same hand
position. Do we have to find them all or can we just pick one?
(a) Why not try to find them all? Because if there are two answers for θt and two answers
for θu , and θu is calculated using θt , there are four solutions for θu . With n joints and
m solutions at each joint, we could end up with mn solutions. yuck. But wait! There’s
more! Since we not only solve for individual angles, but often first need to solve for angle
pairs (or triples! quadruples! etc.), in worst case, we might need to solve for very many
subsets of all angle sums. This means there are O(2n ) variables, and therefor the number
n
of solutions is O(m(2 ) ). This grows pretty quickly. If m = 2, and there are 5 joints, we
could conceivably see over 4 billion solutions. Maybe finding one is good enough...
(b) But, in fact, many of those solution might not be valid after all. That can be because the
motor doesn’t turn that far, the environment blocks it, or even some other link blocks
it. For example, on your robot, motor 2 cannot turn all the way to π. And if we used
regular arctan, instead of arctan2, one of the potential solution is actually wrong. Thus,
we might have to check them all, all 4 billion of them.
(c) The good news is that we never really see worst case. The angle sums come from joints
turning in the same plane. We’re practically speaking not going to see all the joints in the
same plane, or if we do, there will be a small number of them, like 2. Our final matrices
are also not in practice going to contain all subsets of angle sums, so the calculated worst
case just doesn’t happen. In the real world, we’ll end up with 8 or 16 possible solutions
to check. That’s not so bad, so we usually just check them all.
5