Introduction to Robotics #3: Forward and Inverse Kinematics

By Nikolaus Correll On September 7, 2011

In order to plan a robot’s movements, we have to understand the relationship between the actuators that we can control and the robot’s resulting position in the environment. For static arms, this is rather straightforward: if we know the position/angle of each joint, we can calculate the position of its end-effectors using trigonometry. This process is known as forward kinematics. If we want to calculate the position each joint needs to be at, we need to invert this relationship. This is known as inverse kinematics.

The goals of this lecture are

to introduce the forward kinematics of mobile robots
show how solutions for the inverse kinematics for both static and mobile robots can be derived
provide an intuition on the relationship between inverse kinematics and path-planning

Forward Kinematics of Mobile Robots

Whereas the pose of a robotic manipulator is uniquely defined by its joint angles – which can be made available using encoders in almost real-time – this is not the case for a mobile robot. Here, the encoder values simply refer to wheel orientation and need to be integrated over time, which will be a huge source of uncertainty as we will later see.

What complicates matters is that for non-holonomic systems, it is not sufficient to simply measure the distance that each wheel traveled, but also when each movement was executed. A system is non-holonomic when closed trajectories in its configuration space (reminder: the configuration space of a two-link robotic arm is spanned by the possible values of each angle) may not have it return to its original state. A simple arm is holonomic, as each joint position corresponds to a unique position in space. Going through whatever trajectory that comes back to the starting point in configuration space will put the robot at the exact same position. A train on a track is holonomic: moving its wheels backwards by the same amount they have been moving forward brings the train to the exact same position in space. A car and a Roomba are non-holonomic vehicles: performing a straight line and then a right-turn leads to the same amount of wheel rotation than doing a right turn first and then go in a straight line; getting the robot to its initial position requires not only to rewind both wheels by the same amount, but also getting their relative speeds right. (Make a drawing of this.)

A differential wheel drive with an associated robot and world coordinate frame.

It should be clear by now, that for a mobile robot, not only traveled distance per wheel matters, but also the speed of each wheel as a function of time. Lets introduce the following conventions. We establish a coordinate system $X_R$ on the robot and express its speed $\dot{\xi}$ as a vector $\dot{\xi}=[\dot{x}, \dot{y}, \dot{\theta}]^T$ . Here $\dot{x}$ and $\dot{y}$ correspond to the speed along the x and y directions, whereas $\dot{\theta}$ corresponds to the rotation around the imaginary z-axis, that you can imagine to be sticking out of the ground. We denote speeds with dots over the variable name, as speed is simply the derivative of distance. We will also establish a world coordinate system $X_I$ , which is known as the inertial frame by convention. Think about the robot’s position in $X_R$ real quick. It is always zero, as the coordinate system is fixed on the robot. Therefore, $\dot{x_R}$ is the only interesting quantity here and we need to understand how speeds in $X_R$ map to positions in $X_I$ , which we denote by $\xi_I=[x, y, \theta]^T$ . These coordinate systems are shown in the figure to the right. Notice that the positioning of the coordinate frames and their orientation are arbitrary. Here, we chose to place the coordinate system in the center of the robot’s axle and align $x_R$ with its default driving direction.

In order to calculate the robot’s position in the inertial frame, we need to first find out, how speed in the robot coordinate frame maps to speed in the inertial frame. This can be done again by employing trigonometry. There is only one complication: a movement into the robot’s x-axis might lead to movement along both the x-axis and the y-axis of the world coordinate frame. By looking at the figure above, we can derive the following components to $\dot{x}_I$ . First, $\dot{x}_{I,x}=cos(\theta) \dot{x}_R$ . There is also a component of motion coming from $\dot{y}_R$ . For positive $\theta$ , we can see that the robot actually moves into negative $X_I$ direction. The projection from $\dot{y}_R$ is therefore given by $\dot{x_{I,y}}=-sin(\theta)\dot{y_R}$ . We can now write

$\dot{x_I}=cos(\theta) \dot{x_R} - sin(\theta) \dot{y_R}$

Similar reasoning leads to

$\dot{y_I}=sin(\theta) \dot{x_R} + cos(\theta) \dot{y_R}$ .

and

$\dot{\theta_I}=\dot{\theta_R}$

which is the case because both robot’s and world coordinate system share the same z-axis in this example. We can now conveniently write

$\dot{\xi_I}=T(\theta)\dot{\xi_R}$

with

$T(\theta)=\left(\begin{array}{ccc} cos(\theta) & -sin(\theta) & 0 \\ sin(\theta) & cos(\theta) & 0 \\ 0 & 0 & 1\end{array}\right)$

We are now left with the problem of how to calculate the speed $\dot{\xi_R}$ in robot coordinates. For this, we make use of the kinematic constraints of the robotic wheels. For a standard wheel, the kinematic constraints are that every rotation of the wheel leads to strictly forward or backward motion and does not allow side-way motion or sliding. We can therefore calculate the forward speed of a wheel $\dot{x}$ using its rotational speed $\dot{\phi}$ (assuming the encoder value/angle is expressed as $\phi$ ) and radius $r$ by $\dot{x}=\dot{\phi}r$ . This becomes apparent when considering that the circumference of a wheel with radius $r$ is $2\pi r$ . The distance a wheel rolls when turned by the angle $\phi$ (in radians) is therefore $x=\phi r$ . Taking the derivative of this expression on both sides leads to the above expression.

How each of the two wheels in our example contributes to the speed of the robot’s center – where its coordinate system is anchored – requires the following trick: we calculate the contribution of each individual wheel while assuming all other wheels remaining un-actuated. In this example, the distance traveled by the center point is exactly half of that traveled by each individual wheel, assuming the non-actuated wheel rotating around its ground contact point. (Make a drawing of this!) We can therefore write

$\dot{x_R}=\frac{r\dot{\phi_l}}{2}+\frac{r\dot{\phi_r}}{2}$

given the speeds $\dot{\phi_l}$ and $\dot{\phi_r}$ of the left and the right wheel, respectively.

Exercise: Think about how the robot’s speed along its y-axis is affected by the wheel-speed given the coordinate system in the drawing above. Think about the kinematic constraints that the standard wheels impose.

Hard to believe at first, but the speed of the robot along its y-axis is always zero. This is because the constraints of the standard wheel tell us that the robot can never slide. We are now left with calculating the rotation of the robot around its z-axis. That there is such a thing can be immediately seen when imaging the robot’s wheels spinning in opposite directions. We will again consider each wheel independently. Assuming the left wheel to be non-actuated, spinning the right wheel forwards will lead to counter-clockwise rotation. Given an axle diameter (distance between the robot’s wheels) $d$ , we can now write

$\omega_l d = \phi_r r$

with $\omega_l$ the angle of rotation around the left wheel. Taking the derivative on both sides yields speeds and we can write

$\dot{\omega} = \frac{\dot{\phi_r} r}{d}$

Adding the rotation speeds up (with the one around the right wheel being negative based on the right-hand grip rule), leads to

$\dot{\theta}=\frac{\dot{\phi_r} r}{d}-\frac{\dot{\phi_l} r}{d}$

Putting it all together, we can write

$\left(\begin{array}{c} \dot{x_I}\\\dot{y_I}\\\dot{\theta}\end{array}\right)=\left(\begin{array}{ccc} cos(\theta) & -sin(\theta) & 0 \\ sin(\theta) & cos(\theta) & 0 \\ 0 & 0 & 1\end{array}\right)\left(\begin{array}{c}\frac{r\dot{\phi_l}}{2}+\frac{r\dot{\phi_r}}{2}\\0\\\frac{\dot{\phi_r} r}{d}-\frac{\dot{\phi_l} r}{d}\end{array}\right)$

Inverse Kinematics

The main problem for the engineer is now to find out how to chose the control parameters to reach a desired position. This problem is known as inverse kinematics. Solving the forward kinematics in closed form is not always possible, however. It can be done for the differential wheel platform we studied above. Lets first establish how to calculate the necessary speed of the robot’s center given a desired speed $\dot{\xi_I}$ in world coordinates. We can transform the expression $\dot{\xi_I}=T(\theta)\dot{\xi_R}$ by multiplying both sides with the inverse of $T(\theta)$ :

$T^{-1}(\theta)\dot{\xi_I}=T^{-1}(\theta)T(\theta)\dot{\xi_R}$

which leads to $\dot{\xi_R}=T^{-1}(\theta)\dot{\xi_I}$ . Here

$T^{-1}=\left(\begin{array}{ccc}cos \theta & sin \theta & 0 \\ -sin \theta & cos \theta & 0 \\ 0 & 0 & 1\end{array}\right)$

which can be determined by actually performing the matrix inversion or by deriving the trigonometric relationships from the drawing. Similarly, we can now solve

$\left(\begin{array}{c} \dot{x_R}\\\dot{y_R}\\\dot{\theta}\end{array}\right)=\left(\begin{array}{c}\frac{r\dot{\phi_l}}{2}+\frac{r\dot{\phi_r}}{2}\\0\\\frac{\dot{\phi_r} r}{d}-\frac{\dot{\phi_l} r}{d}\end{array}\right)$

for $\phi_l$ , $\phi_r$ . (do this!) You will now see that your kinematic constraints actually render some desired velocities, namely those that would lead to non-negative $\dot{y_R}$ unfeasible.

Inverse Kinematics of a Manipulator Arm

We will now look at the kinematics of a 2-link arm that was introduced in last week’s lecture. Similar to a the process of calculating the required wheel-speed for achieving a desired speed of the local coordinate system, we need to solve the equations determining the robot’s forward kinematics by solving for $\alpha$ and $\beta$ . This is tricky, however, as we have to deal with complicated trigonometric expressions.

You can give it a shot using Mathematica using the code below. For simplicity, $l_1$ and $l_2$ are assumed to be 1.

sol = Solve[Sin[α + β] + Sin[α] == x

              && Cos[α + β] + Cos[α] == y,
             {α, β}];

min = sol /. {x -> 1, y -> 1}

This will solve for $\alpha$ and $\beta$ for x=1, y=1. The solutions for this case are obviously $\left(0,\frac{\pi}{2}\right)$ and $\left(\frac{\pi}{2},\frac{-\pi}{2}\right)$ . (Think about this real quick.) The solutions to this problem are not nice, however, with 8 complicated expressions, 6 of which yielding complex solutions,such as

$\alpha \rightarrow -ArcCos(\frac{x^2 y + y^3 - \sqrt(4 x^4 - x^6 + 4 x^2 y^2 - 2 x^4 y^2 - x^2 y^4)}{2 (x^2 + y^2)}$

and

$\beta \rightarrow -ArcCos(1/2(-2+x^2+y^2))$

As such solutions quickly become unhandy with more dimensions, you can calculate a numerical solution using an approach that we will see is very similar to path planning in mobile robotics. For this, you need to plot the distance of the end-effector from the desired solution in configuration space. To plot this, you need to solve the forward kinematics for every point in configuration space and use the Euclidian distance to the desired target as height. This is shown below and can be accomplished using the commands

x = 1;
y = 1;

 Show[Plot3D[
   Sqrt[(Sin[α + β] + Sin[α] - x)^2 + (Cos[α + β] + Cos[α] - y)^2], {α, -π/2,
    π/2}, {β, -π, π}], ListPointPlot3D[{α, β, 0.1} /. {min}]]

: Minima in the configuration space for reaching the point (1/1) using the mechanism shown above.

The key point here is that the inverse kinematics problem is equivalent to a path-planning problem in the configuration space. How to find shortest paths in space, that is finding the shortest route for a robot to get from A to B will be a major part of this class.

Take-home lessons

For calculating the forward kinematics of a robot, it is easiest to establish a local coordinate frame on the robot and determine the transformation into the world coordinate first.
Forward and Inverse Kinematics of a mobile robot are performed with respect to the speed of the robot and not its position.
For calculating the effect of each wheel on the speed of the robot, you need to consider the contribution of each wheel independently.
Calculating the inverse kinematics analytically becomes quickly infeasible. You can then plan in configuration space of the robot using path-planning techniques.

Todd Bernhard

Great lecture! You really made it easy to comprehend. Just wanted to let you know I think there’s a typo in the Forward Kinematics section, just before you first introduce the T(theta) matrix. In the transform equation ( XIr = T(theta)*XIi ), the inertial speed vector and robot speed vector are switched, just the subscripts.
- http://correll.cs.colorado.edu/?page_id=19 Nikolaus Correll
  
  Thanks for catching this!
Pingback: Advanced Robotics #3: Forward kinematics of manipulator arms | Correll Lab

Introduction to Robotics #3: Forward and Inverse Kinematics

Share this: