Inverse Kinematics

Inverse kinematics

Inverse kinematics is the function that gives us the joint angles needed to achieve a particular end-effector pose.

This is described:

$q = \kappa ^{-1} (\xi_N)$

To explain how we can do this, we will look at the inverse kinematics of a simple 2-joint robot arm for starters.

Figure 1: 2-link Manipulator with 2D Motion

The objective is to calculate the joint angles $q_1$ and $q_2$ needed to achieve the end-effector pose $\zeta_E = (x, y)$

It should be noted that either $q_1$ or $q_2$ can be defined in terms of the other.

We start by defining r, simply using Pythagoras i.e. $r = \sqrt {x^2 + y^2}$

we then need to use the cosine rule to determine the angle alpha, between the two links.

cosine rule: $a^2=b^2+c^2 -2bc \ \cos\alpha$

Where, b and c are represented by the link lengths $a_1$ and $a_2$ , respectively, and length a is r calculated above. From this we can compute an expression for angle $\alpha$ :

$\alpha= \cos^{-1} \left ( \frac {-x^2 - y^2 + a_1^2 + a_2^2}{2a_1a_2} \right )$

which we can see from Figure 1 is also related to angle $q_2$ by $q_2= \pi - \alpha$ . Therefore, considering that $\cos \alpha= -\cos q_2$ , we can compute the joint angle to be:

$q_2= \cos^{-1} \left ( \frac {x^2 + y^2 - (a_1^2 + a_2^2)}{2a_1a_2} \right )$

Taking a second triangle about the second link and joint angle $q_2$ , we can compute the sides of the triangle in terms of the joint angle $q_2$ and link length $a_2$ :

We can then use these lengths to find the angle $\beta$ with the relationship:

$tan \beta = \frac {a_2 sin q_2}{a_1 + a_2 cos q_2}$

Given the sum of $q_2$ and joint angle $\beta$ relate to the triangle of base x and height y, shown below, we can see that the angle Beta relates to the joint angle $q_1$ by:

$q_1 = \left ( \tan^{-1} \frac {y}{x} \right ) - \beta$

Figure 4: Two-Link arm relationship between End-effector Pose and q1

We can then substitute the expression for $\beta$ to compute the equation of $q_1$ in terms of the link lengths and second joint angle $q_2$ :

$q_1 = \tan^{-1} \left (\frac {y}{x} \right ) - \tan^{-1} \frac {a_2 sin q_2}{a_1 + a_2 cos q_2}$

Looking at our expression for $q_2$ we can see that our inverse kinematics give us two possible solutions for the joint angle, positive or negative.

$\cos q_2= \left ( \frac {x^2 + y^2 - (a_1^2 + a_2^2)}{2a_1a_2} \right )$

Either one will give us the desired end-effector pose, as the expression for joint angle $q_1$ is itself a function of $q_2$ . However, they correspond to different joint angle configurations.

The method covered above is a geometry based approach. However, we can solve the inverse kinematics taking a more analytic approach, using algebra.

$\bf{Analytic \ Approach}$

For the same 2-joint robot arm we discussed in the previous example, shown in Figure 1, we have an end-effector pose that can be described by the forward kinematics expression:

$E= R(q_1) T_x(a_1) R(q_2) T_x(a_2)$

This is a sequence of two translations and two rotations.

Multiplying out and expanding this expression, gives a homogeneous transformation matrix of the form:

$E =\left (\begin{matrix} cos(q_1+q_2) & -sin(q_1+q_2) & a_2 cos(q_1+q_2) + a_1 cosq_1 \\ sin(q_1+q_2) & cos(q_1+q_2) & a_2 sin(q_1+q_2) + a_1 sinq_1 \\ 0&0&1 \end{matrix} \right)$

In this case, what we are most interested in is the position of the end-effector defined by the x and y coordinates, which are described by the 3rd column of the matrix above.

$\left (\begin{matrix} x \\ y \end{matrix} \right) =\left (\begin{matrix} a_2 cos(q_1+q_2) + a_1 cosq_1 \\ a_2 sin(q_1+q_2) + a_1 sinq_1 \end{matrix} \right)$

We can add the square of the expressions defining the x and y coordinate of the robot end-effector, which resolves to:

$x^2 + y^2 = a_2^2 + a_1^2 + 2a_1a_2 \cos q_2$

we can rearrange the equation to isolate the $q_2$ term and give us an expression for the required joint angle in terms of the end-effector coordinates:

$\cos q_2 = \frac {x^2 + y^2 - a_2^2 - a_1^2}{2a_1a_2}$

This is the same solution computed using the geometry based approach.

Now to find the solution to $q_1$ . We can take simplify the x and y coordinate expressions substituting short-hand notations:

$\cos q_2 \rightarrow C_2$ $\sin q_2 \rightarrow S_2$

This gives us:

$x = (a_1 + a_2C_2) \cos q_1 - a_2S_2 \sin q_1$

$y = (a_1 + a_2C_2) \sin q_1 - a_2 S_2 \cos q_1$

These expressions are of the form:

$a \cos q + b \cos q = c$

which has the solution:

$q= tan^{-1} \frac {c}{\pm \sqrt{a^2 + b^2 - c^2}} - tan^{-1} \frac {a}{b}$

Therefore, by substituting the variables of the expression y into the identity above such that $a_2 S_2$ , $b=a_1 + _2 C_2$ and $c = y$ we can solve for $q_1$ :

$q_1 = \tan^{-1} \left (\frac {y}{x} \right ) - \tan^{-1} \frac {a_2 sin q_2}{a_1 + a_2 cos q_2}$

We can see this mirrors the solution obtained from the geometry based approach.

$\bf { Inverse \ Tangent \ Function}$

The inverse tan function: $tan^{-1} \left ( \frac {y}{x} \right ) = \theta$

sometimes also denoted atan or arctan suffers from a limitation.

The standard inverse tan function is incapable of recognizing an angle passing into the (LHS) or imaginary plane. this is because the standard tan function only has range $-\pi /2 \rightarrow \pi /2$ and therefore cannot account for a rotation in the LHS or a negative sign. The inverse tan function atan is used to get around this issue, which has a range $-\pi \rightarrow \pi$ and accounts for the sign of the coordinates input.

The function atan2 is found in many mathematical libraries, including MATLAB, where it considers the sign of both coordinates to evaluate the angle:

$atan2(a, b) \in [-\pi , \pi]$

$\bf {Inverse \ Kinematics \ In \ MATLAB$

In MATLAB, for a modular robot R, we can use the command ikine in the Robotics Toolbox to compute the joint angles required to achieve the specified end-effector pose described by a transform matrix denoted T, as follows:

$>>R.ikine(T)$

The R is an arbitrary notation I have assigned for a serial link robot, but this needs to be defined in the MATLAB library. To simply test the command for the time being, you could use the mdl_hyper3d( $'Number of Joints'$ ) or mdl_puma560 which are already have defined libraries in the Robotics Toolbox. In which case the for the hyper3d, the inverse kinematics can be computed using the command:

$>>h3d.ikine(T);$

Where there are more than one set of joint configurations which achieve the desired end-effector pose, as shown above with the positive or negative values of $q_2$ , the ikine command allows us to specify the desired configuration: ‘r’ for right and ‘l’ for left.

$\bf {Numerical \ Solution}$

We have covered the analytical method of computing the joint angles using inverse kinematics. However, it can be more convenient and practical to use what is called the Numerical method, especially when the robot has a large number of joints, i.e. above 6.

The advantage of using the analytical approach is that it explicitly shows the multiple configurations that the robot arm can be in for a particular end-effector pose. When working with a robot with a small number of joints, the analytical approach can be the best option as it provides a compact solution which is faster to execute than the numerical alternative.

However, it may be more mathematically difficult or even impossible to find the inverse kinematics of a more complex robot. In this case the Numerical method should be used.

We have seen that we can always find the forward kinematics of the robot. To achieve a desired pose we need only adjust the joint angles so as to move our end-effector from the starting pose to the final pose we want. This can be presented as an optimization problem defined:

$q* = arg \ min_q|\kappa(q) \ominus \xi |$

Using MATLAB, we use the ikine command mentioned above. This is fairly simple, and requires us to only specify the final end-effector pose and the initial joint angles, as shown by the example below for a puma560 robot:

$>>p560.ikine(T, qz);$

One shortcoming of the Numerical method, is that it can be difficult to obtain all the possible joint angle configurations which can achieve a particular end-effector pose, as MATLAB will only return the one possible configuration based on the initial joint angles specified.

In addition, if the initial joint coordinates are too far from the joint coordinates needed to achieve the desired pose, than the solution may not converge.

Note: When trying to compute the inverse kinematics of a redundant robot we need to use the numerical solution. But there will be a near infinite number of joint configurations that result in the same end-effector pose.

$\bf Singularity$

Singularity is an problem in robot control similar to Gimbal Lock, which rises when two or more joint axis are aligned.

The alignment of the two joints results in a loss of a degree of freedom as the motion of either joint simply creates the same motion in the end-effector. It, therefore, becomes impossible for the robot end-effector to achieve certain orientations while these joints are aligned, that were otherwise possible when the joints where not aligned.

Due to this singularity, the robot will be unable to adopt certain orientations that were otherwise possible when the joints where not aligned. We will see this problem pop up in several robotics problems.

$\bf {Interpolated \ Motion}$

Moving a robot from one pose to another, we need to determine the joint angles at each pose, by carrying out the inverse kinematics of the poses.

To move smoothly between the two poses, or set of joint angles, we need to interpolate the vector quantities overtime.

We can simulate the motion between two end-effector poses, within the robot workspace, by creating a trajectory between the set of joint angles, and specifying the number of time steps in which to perform the motion. We can use the command jtraj:

$>>Tr=jtraj(q0, q1, 50);$

In this example I have asked MATLAB to perform the notion over 50 time steps, between a set of joint angles q0 and q1, which will return a matrix of 50 rows and 6 columns (for 6 joint robot in 3D such as the puma560), where each row corresponds to a time step and the columns correspond to a joint angle.

We can then visualize this using the plot command with the robot we are considering i.e. for a pum560:

$>>p560.plot(Tr);$

We can then use the qplot(Tr) command to plot a figure of the change in joint angles over time. This should show that the Cartesian position of the end-effector moves smoothly as a function of time, as we would want.

Figure 5: Change of Joint Coordinates in Time for a change in End-Effector Pose

However, if we plot the poll-pitch-yaw angles against the time steps. We can see from this that not all the angles will remain at zero constant. This is because we have only constrained the orientation at the beginning and end of the trajectory, at poses A and B.

This indicates that the end-effector did not follow the desired straight path and that its orientation has deviated along the trajectory.

We can use Cartesian Interpolation to resolve this.

As opposed to interpolating the joint angles, in cartesian interpolation we will directly interpolate the Cartesian pose and the perform the inverse kinematics to find the joint angles at every individual time step between the two specified poses.

$\xi _s = f(\xi _A, \xi _B, s), 0 \leq s \leq 1$

where s varies smoothly from 0 to 1.

So in Matlab we will use the command ctraj for the cartesian trajectory from an initial and final pose and pass in the number of time steps, e.g.:

$>>Tc=ctraj(Ta, Tb, 50):$

This will give a series of 50 poses, each of which are 4×4 matrices, one matrix for every time step.

We can the pass the cartesian trajectory defined (Tc) into the inverse kinematics of the robot (puma560):

$>>qc=p560.ikine6s(Tc)$

This will return a 50 sets of joint angles, one set for every time step.

Inverse Kinematics

Inverse kinematics

Published by Majed Yassin

Leave a comment Cancel reply

Inverse kinematics

Share this:

Related

Published by Majed Yassin

Leave a comment Cancel reply