Inverse Kinematics

Inverse kinematics is the opposite of forward kinematics, that is given the final position and orientation, the joint angles are returned. This is very important because it can tell if the robot is able to reach a certain point, and in what orientations. Also, Cartesian trajectories created by moving the arm must be converted to joint angles by the inverse kinematic solution before becoming viable data for most control schemes. Finally, since inverse kinematics is nonlinear, many possible solutions exist. When the arm moves, the controlling program can choose which joint angles from this possible set are best given the current position of the arm and any obstacles in the current path.

Calculating the inverse kinematic solution is not easy. Matlab provides a function for this, but it is of limited use since it is very slow and only returns one answer, not a set. This makes it good for testing, but useless for real time application. Therefore a symbolic, closed form solution needs to be found.

There are two basic ways of calculating this closed form solution: algebraically and geometrically [5]. The algebraic method involves equating a generic final pose transform (Equation 3) with the forward kinematic matrix from the ground to the last link (Equation 4). Since that forward kinematic matrix (T₀₅ in this case) is the product of other matrices (T₀₁, T₁₂, T₂₃, T₃₄, T₄₅) those matrices can be moved individually over to the other side of Equation 3 and a system of equations is formed (Eqns 5-8). Please note that only two of these equations are shown since the others are quite large and are not used in this particular inverse kinematic calculation. They are, however, shown in the complete Maple worksheet in Appendix C of the User Instructions.

(Eqn 3)

Individual elements from each side of the equations can be equated and used to solve for the joint angles. This is a tedious process involving many trigonometric substitutions and algebraic manipulations. It is also difficult to figure out which elements to work with since some may be easier to use than others.

The second method for solving the inverse kinematics involves pure geometry. However, calculating the entire solution this way can be very difficult. Therefore, if geometry is to be used ,usually part of the solution is derived algebraically and the remaining joints found geometrically. This is the approach used in finding the inverse kinematic solution for the final arm design.

First, elements (3,4) from Equation 6 are equated forming:

Using trigonometry, ₁ is found to be:

Now that ₁ is known, elements (3,3) can be equated to form:

Using the trigonometric identity sin2+cos2=1, ₄ is found to be:

Since ₁ and ₄ are now known, elements (3,1) are equated giving:

And ₅ can be solved for in the same manner as ₄:

From here ₂ and ₃ are all that remain to be solved for. Originally the algebraic method was used, but it kept giving bad results. One reason might be that in order to solve for ₂ and ₃, ₂₃ must first be solved for using the rotation part of T_g. Since the rotation part deals only with orientation and ₂ and ₃ deal primarily with position instead of orientation, invalid equations resulted. Perhaps there is an error that was not found even after almost a dozen checks, but whatever the reason, an algebraic solution for ₂ and ₃ simply would not work.

Since that method did not work, the geometric approach was employed. Because only ₂ and ₃ need to be solved, we can look at the plane containing the arm (Figure 22) [5].

₃ can be solved for using the law of cosines. First, the law is applied to the triangle ABC, forming:

Then cos(₃ ) is solved for and finally ₃ using the sin2+cos2=1 identity:

₂ is solved for by finding the sum of angles and . Using the definitions of sine and cosine, segments BD and DC are equal to the following:

AD is simply the link length l1 added to segment BD giving:

Now using these lengths and arctangent, angle is equal to:

Angle is simply the arctangent of segments AO and AP:

So finally ₂ is the sum of and :

These five equations for ₁-₅ are the closed form kinematic solution for the final arm design. Note that there are many solutions since each square root can be positive or negative. When calculating a particular solution, it is best to start with ₁ since all the others except ₂ and ₃ are based on its result. Take both answers and use them to calculate four answers for ₄ and so on. This will result in a set of answers, many of which will be redundant. However, the correct angles will be contained in that set. When used for actual robot control, a controlling program will need to find the correct set by comparing the robot's current position to it's last and finding the angles closest to the previous ones.

To ensure validity of this solution, it was tested in two ways. In a Matlab program a routine was written that allowed the user to input joint angles and the forward kinematics were calculated for the arm tip. This resulted in a T matrix which could then be broken down and used in finding the closed form inverse kinematic solution. It was also passed to Matlab's own inverse kinematic function so that result could be compared with the closed form result.

Five tests were made using random joint angles. They included positive, negative, and fractional angles. As expected, the results (Table 9) show that the original angles are contained in the closed form solution. The Matlab solution also matches the original and calculated angles, further validating the solution.

Test 1

Inputs: 12 23 34 45 56 Symbolic Answers: All answers are in degrees. Theta 1: 187.202 12 Theta 2: -26.154 22.919 -202.919 -153.846 Theta 3: 34.1466 145.762 Theta 4: 137.463 -137.463 45 -45 Theta 5: 129.955 50.0449 56 124 -129.955 -50.0449 -56 -124 Matlab Answers: Theta 1-5: 12 23 34 45 56

Test 2

Inputs: 0 180 0 0 45 Symbolic Answers: All answers are in degrees. Theta 1: 0 -172.372 Theta 2: 0.000176447 77.1955 102.804 180 Theta 3: 0.113529 179.795 Theta 4: 0 -0 172.372 -172.372 Theta 5: 0 180 45 135 -0 -180 -45 -135 Matlab Answers: Theta 1-5: -7.42003e-18 180 -4.50903e-15 -3.26417e-12 45

Test 3

Inputs: -13.4 -129.8 179.12 -0.023 99.4 Symbolic Answers: All answers are in degrees. Theta 1: -13.4 -66.6494 Theta 2: -124.944 -48.4375 -131.562 -55.0559 Theta 3: 0.993551 178.915 Theta 4: 0.023 -0.023 53.2644 -53.2644 Theta 5: 80.6 99.4 31.293 148.707 -80.6 -99.4 -31.293 -148.707 Matlab Answers: Theta 1-5: -13.4 -53.1686 0.88 0.0220481 -158.991

Test 4

Inputs: 10 30 -10 0 -90 Symbolic Answers: All answers are in degrees. Theta 1: 183.304 10 Theta 2: -54.8338 29.9611 -209.961 -125.166 Theta 3: -9.88406 189.793 Theta 4: 173.304 -173.304 8.53774e-07 -8.53774e-07 Theta 5: 70 110 90 90 -70 -110 -90 -90 Matlab Answers: Theta 1-5: 10 30 -10 4.30522e-10 -90

Test 5

Inputs: 90 -90 90 -90 90 Symbolic Answers: All answers are in degrees. Theta 1: 90 90 Theta 2: -87.8789 -87.8789 -92.1211 -92.1211 Theta 3: 89.9544 89.9544 Theta 4: 90 -90 90 -90 Theta 5: 90 90 90 90 -90 -90 -90 -90 Matlab didn't finish.

A final test was made to see what would happen if a T matrix was entered that was impossible to reach. The results of this test are shown in Table 9 and indicate the symbolic solution will only return incorrect angels for a bad matrix, but give no indication that those numbers are bad and the desired point is out of reach. A solution would be to write a function that checks the distance of the point against the arm's reach. If it is inside, then calculate the solution. If it is not, return a message saying that that point is unreachable.

While these tests do not cover every possible position, they do indicate that the derived inverse kinematic solution is valid and should work in most conditions.

If something is spelled wrong or broken please tell me!