# Kinematics Equations for Control of Robotic Systems

## Controlling the motion of a robotic system often requires knowledge of the relationships between the end effector orientation (often referred to as tool tip or commanded reference frame or commanded coordinate system) and the orientation of the physically controlled actuators or motors that ultimately manipulate the end effector (also commonly referred to as the controlled reference frame or co...

A three degree-of-freedom (DOF) crane, like the one shown here, lets the user control crane length, rotation angle, and tilt angle, but does not allow the working end of the crane to travel in straight line. Source: www.rki-us.com |

Controlling the motion of a robotic system often requires knowledge of the relationships between the end effector orientation (often referred to as tool tip or commanded reference frame or commanded coordinate system) and the orientation of the physically controlled actuators or motors that ultimately manipulate the end effector (also commonly referred to as the controlled reference frame or controlled coordinate system). This knowledge can be generally referred to as the kinematic structure of the robotic system, and is represented by kinematic equations. Some high performance motion controllers have the capability to utilize such equations to generate coordinated motion trajectories, allowing real time position control of a robotic system. In this article, kinematic equations will be developed for two specific robotic systems.

## Why use kinematics? 3 DOF crane

A relatively simple robotic system is a three degree-of-freedom (DOF) crane, in which the user can control crane length, crane rotation angle, and crane tilt angle (see crane image from www.rki-us.com ). Often a crane has a fourth DOF, suspension length of the payload, though here it will be ignored, without loss of generality.

Typically a crane will swing its arm around to create a path that looks like an arc from above. In addition, if the tilt angle of the crane arm is increased, the end effector (payload on the end of the crane) will follow an arc path upwards. These naturally curved paths may be acceptable for certain situations, but what if the user wants the payload to follow a path that is comprised of straight lines, or even arbitrary shapes in three dimensions? This could be the case for example, if the payload is some sort of measurement device or even an image acquisition system. A simple example would be to move the payload in a path described by a square, which could be useful for certain systems.

The controlled coordinate system of the crane should actually be familiar for those who have studied coordinate geometry - it is commonly referred to as the Spherical coordinate system. mathworld.wolfram.com |

The controlled coordinate system of the crane should actually be familiar for those who have studied coordinate geometry — it is commonly referred to as the *Spherical coordinate system* . A point in space can be described by 3 parameters in spherical coordinates: distance from origin r, azimuthal angle in the x-y plane from the x-axisθ (with range 0 – 2π), and polar angle from the z axis θ (with range 0 – π). A diagram of this coordinate system is shown in SphericalCoordinates.gif ( mathworld.wolfram.com ).

To command moves that follow line segments, we would like to work in the standard *3D Cartesian coordinate system* . A point in Cartesian coordinates is simply described by (x, y, z), and it is intuitive that this system allows for line segments to be commanded easily. For example, a path to make a square would simply be made up of four line segments, the most basic moves in Cartesian coordinates. The question then becomes: how do we implement this transformation of coordinate systems. The answer? Kinematic equations. Kinematic equations relate the Cartesian coordinate system (x, y, z) to the crane (Spherical) coordinate system (r,θ, Φ).

Before going further, let’s quickly justify why these equations are necessary. If the user wants to command a path in Cartesian coordinates he/she will specify a path to follow by giving a series of (x, y, z) positions. When using a motion controller, explicitly specifying this path usually isn’t necessary for many types of moves. The motion controller will generate the motion profile (series of (x, y, z) positions) for a commanded move, such as a point-to-point move, which generates a line in Cartesian space. If we know the desired (x, y, z) position of the end effector, then the inverse kinematics are needed so the motion controller can calculate how to position the actual crane (what should the arm length, tilt angle, and swing angle be– (r, θ, Φ)?).

The forward kinematic equations will be used more for initialization and verification purposes. They can be used to take the measured feedback positions in terms of (r,θ, Φ) and transform them into what the user cares about, which is (x, y, z). This process is also useful for determining startup position, and initializing the relative (x, y, z) positions for an arbitrary crane position.

Now the necessity of the kinematic equations has been discussed, and it is time to actually solve the equations. Starting with the inverse kinematics for the crane, we want to solve the following for (r,θ, Φ):

This equation is actually solvable upon inspection of the spherical/Cartesian coordinate set, where some trigonometric formulas are employed. The following equations can be readily verified:

Note that in the third equation above,Φ is in terms of r, however r is solved in terms of (x, y, z) in the first equation.

The forward kinematic equations will take a similar form:

These equations are also solvable via inspection and easily verified:

## A more involved example - 6 DOF Stewart Platform hexapod

The Stewart Platform hexapod is used in many applications including automotive testing, robotic surgery, satellite and telescope positioning, machine simulators, and many more. The hexapod consists of 6 individually controlled actuators (lengths) joined on one end to a fixed base, and on the other end a planar platform, allowing six DOF (α (roll), ß (pitch), γ (yaw), x, y, z). A picture of the geometry is shown in 'Hexapod Geometry.jpg’.

The Stewart Platform hexapod is used in many applications including automotive testing, robotic surgery, satellite and telescope positioning, machine simulators, and many more. |

For this system, the inverse kinematic equations tell us: what should the lengths of the actuators (l _{1} , l _{2} , l _{3} , l _{4} , l _{5} , l _{6} ) be for a given (α, ß, γ, x, y, z), also known as pose (P). The forward equations calculate the pose, P, in terms of the actuator leg lengths, l. The forward equations are not solvable in a computationally efficient closed-form set of equations. Instead, the forward kinematics can be solved using an iterative approach involving a generalized Newton-Raphson method, which will be described later.

To solve the inverse kinematic equations of this system, one must determine what are the platform and base actuator joint locations, since a leg length is simply the distance between these points. The platform actuator joint position in terms of the base joint coordinate system is described by:

The subscripts in the equation above denote the reference frame of the vector. Here, the points are actually homogeneous coordinates, describing position in terms of (x, y, z, and w) or (x/w, y/w, z/w), where we can think of w as 1 for simplicity. R is a transformation matrix that converts the platform joint (Pp _{i} ), which is in platform coordinates, into the value (Bp _{i} ), which is in base coordinates. R is a 3 by 4 matrix that consists of a 3 by 3 rotational matrix term and a three by one translational matrix term.

In the equation for R, 's’ is the sine function; 'c’ is the cosine function. The rotation matrix term in the above equation is the standard matrix for rotation of roll, pitch, and yaw. The translation term is a simple vector addition. Once the values for the (Bp _{i} )’s are known, one can now find the lengths of the legs by computing the distance between two joints, since the (Bb _{i} )’s are already known:

The above equation is actually fairly simple, but has many terms due to the matrix algebra involved. Below is the final inverse kinematics equation (for leg 'i’):

The forward kinematic equations for this system are relatively complex, and are not easy to solve in real time because of processing requirements. A better method than direct solution here is to use an iterative solution in which an initial guess is evaluated, updated, and repeated until the error in the guess converges below some threshold. The computations will not be examined in detail here; rather the approach will be introduced as a general procedure to follow. This approach is a generalization of the Newton-Raphson method, as mentioned earlier. There is plenty of information on this in the literature, but the application to forward kinematics will be discussed here. The first step is to guess the initial pose, k, or in other words, guess what the values are of (α , ß, γ, x, y, z). For a motion controller, this initial guess is often the commanded position of (α, ß, γ, x, y, z). From the guess, the inverse kinematic equations can be used to calculate actuator lengths based on the guess, let’s call them (g _{1} , g _{2} , g _{3} , g _{4} , g _{5} , g _{6} ) or in vector form, g. Mathematically this looks like *g = I(k)* .

Next, these lengths based on the guess can be compared to the actual lengths, l, that are known from a feedback device to form a 'guess error’, e. That looks like *e = g– l.*

If the guess error is below some threshold, then the process is done! If the guess error is not below the threshold, then a new, better guess is needed. This process is repeated until the guess is good enough (then the current guess is used as the solution!) To understand how one can mathematically determine a 'better guess’, first consider the following simple exercise in calculus. Assume we have a general function y = f(x), where f is generally non-linear. If we wanted to calculate the change in y, resulting from some change in x, the following equations hold true:

Now, if we look over a small range of f(x), we can approximate linearity to reach the following:

The above equation is more commonly recognized as a localized approximation of slope from basic calculus, which is rewritten below:

If the previous formula is rewritten one more time, we can achieve an equation for the change in x:

Now let’s use this same idea and apply it to our situation. Recall we have a non-linear function g=I(k), to determine the estimated lengths of the legs (g) based on the assumed pose (k) . We also know the actual lengths of the legs (l), from the feedback devices. The error in our guess is evaluated as e=g-l, and the desire is to get a new guess for the pose (k*) if the amount of error is unacceptable. We’ll use the same approach as before:

The equation above should look familiar; it is essentially what we showed in the simple calculus example previously. We can simply rewrite the equation one more time to get a formula for our new guess of the pose (k*).

The matrix dI(k)/dk is what we call the Jacobian matrix. This matrix must be inverted in order to be used in the above equation, which in the case of the hexapod requires 6 by 6 matrix inversion, a computationally intense process. Once the new guess of the pose is calculated, then the process can be started over again, and repeated until the error (e = g– l) is below some acceptable level.

The following is a recap of the steps to continuously calculate general forward kinematics, if the inverse kinematic equation is known:

Guess a starting pose for the platform, (k). This is usually the initial commanded pose in the motion controller.

Calculate the leg lengths based on the guess, using the inverse kinematic equation (g=I(k)).

Calculate the error of the guess by comparing to the current actual leg lengths read from the encoders (e = g– l). If the error magnitude is below some threshold, the algorithm has converged to the current value of k, so skip to step 6. If the error magnitude is above the threshold, continue to step 4.

Use the inverse Jacobian to generate a new guess (k*=k-inv(dI(k)/dk)*e).

Update the guess with the result from step 4.

Start another iteration from step 2.

This algorithm is not specific to the Hexapod system; rather it can be used to find forward kinematics for any system (with a converging solution) with known inverse kinematic equations.

## Conclusions

The key to controlling robotic systems in a robust manner is kinematic equations. These equations not only describe the geometry of the system; they also allow modern motion controllers with adequate processing power and speed to perform the necessary calculations that provide smooth motion control of the system. Kinematic equations can often be solved directly in real time, however in complex systems where this isn’t possible, there are algorithms that can still provide the required solutions. High performance motion controllers that provide a structure for implementing these equations into a control system provide many industries the opportunity to explore robotic system applications.

Author Information |

Jason Goerges is an application engineer at ACS Motion Control, Since 1985 ACS Motion Control has developed and manufactured advanced multi-axis motion controllers and integrated control modules. With North American headquarters in Eden Prairie, MN and offices in Israel, Europe and Asia. |

- Events & Awards
- eGuides
- Magazine Archives
- Digital Reports
- Global SI Database
- Oil & Gas Engineering
- Survey Prize Winners
- CFE Edu

- Events & Awards
- eGuides
- Magazine Archives
- Digital Reports
- Global SI Database
- Oil & Gas Engineering
- Survey Prize Winners
- CFE Edu

- Events & Awards
- eGuides
- Magazine Archives
- Digital Reports
- Global SI Database
- Oil & Gas Engineering
- Survey Prize Winners
- CFE Edu