Survey
* Your assessment is very important for improving the workof artificial intelligence, which forms the content of this project
* Your assessment is very important for improving the workof artificial intelligence, which forms the content of this project
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