Robotics Kinematics and Dynamics/Serial Manipulator Position Kinematics

Forward Position Kinematics

The forward position kinematics problem can be stated as follows: given the different joint angles, what is the position of the end-effector? With the previous sections in mind, the answer is rather simple: construct the different transformation matrices and combine them in the right way, the result being $\underset{ee}{\overset{}{bs}}T$, where $\{bs\}$ is the base frame of the robot manipulator.

Solution

Suppose the mutual orientation matrices between adjacent links are known. (As the fixed parameters of each link are known, and the joint angles are a given to the problem, these can be calculated. One possible way to do this would be to make use of the Denavit-Hartenberg convention.) The transformation that relates the last and first frames in a serial manipulator arm, and thus, the solution to the forward kinematics problem, is then represented by the compound homogeneous transformation matrix. The axes are moving, thus, the compound homogeneous transformation matrix is found by premultiplying the individual transformation matrices:

$\underset{ee}{\overset{}{bs}}T={}_0^{N}T={}_0^{1}T{}_1^{2}T\dots {}_{N-2}^{N-1}T{}_{N-1}^{N}T$

Examples

The Planar Three-Link Manipulator

A planar three link manipulator. Each $X_i$-axis lies along the $i$th link. Each $Y_i$-axis lies perpendicular to the corresponding $X_i$-axis in such a way that a positive ${\theta }_i$ corresponds with a rotation from $X_i$ to $Y_i$.

The equations below use 3 × 3 pose matrices, as this is just a 2-dimensional case (cf. the figure on the right).

The pose of the first link, relative to the reference frame, is given by (recall the elementary rotation about the z-axis from the previous section):

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

The pose of the second link, relative to the first link, is given by:

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

This corresponds to a rotation by an angle ${\theta }_2$ and a translation by a distance $l_1$, where $l_1$ is the length of the first link.

The pose of the third link, relative to the second link, is given by:

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

The pose of the end effector, relative to the third link, is given by:

$T_4=\left(\begin{array}{ccc}1& 0& l_3\\ 0& 1& 0\\ 0& 0& 1\end{array}\right)$

The solution to the forward kinematics problem is then:

$\underset{ee}{\overset{}{bs}}T={}_0^{4}T({\theta }_1,{\theta }_2,{\theta }_3)=\left(\begin{array}{ccc}{c}_{123}& -s_{123}& l_1{c}_1+l_2{c}_{12}+l_3{c}_{123}\\ s_{123}& c_{123}& l_1{s}_1+l_2{s}_{12}+l_3{s}_{123}\\ 0& 0& 1\end{array}\right)$

Hence:

$\left(\begin{array}{c}x\\ y\\ 1\end{array}\right)=T\left(\begin{array}{c}0\\ 0\\ 1\end{array}\right)$

The resulting kinematic equations are:

$\begin{array}{c}x=l_1\cos {\theta }_1+l_2\cos ({\theta }_1+{\theta }_2)+l_3\cos ({\theta }_1+{\theta }_2+{\theta }_3)\\ y=l_1\sin {\theta }_1+l_2\sin ({\theta }_1+{\theta }_2)+l_3\sin ({\theta }_1+{\theta }_2+{\theta }_3)\end{array}$

Inverse Position Kinematics

The inverse kinematics problem is the opposite of the forward kinematics problem and can be summarized as follows: given the desired position of the end effector, what combinations of the joint angles can be used to achieve this position?

An example of two different solutions for the inverse kinematics problem leading to the same end-effector position and orientation.

Two types of solutions can be considered: a closed-form solution and a numerical solution. Closed-form or analytical solutions are sets of equations that fully describe the connection between the end-effector position and the joint angles. Numerical solutions are found through the use of numerical algorithms, and can exist even when no closed-form solution is available. There may also be multiple solutions, or no solution at all.

Example: Planar Three-Link Manipulator

The inverse kinematics problem for this 2D manipulator can quite easily be solved algebraically.

From the earlier results (for simplicity, the displacement over the distance $l_3$ shall be omitted here):

$\underset{3}{\overset{}{0}}T=\left(\begin{array}{cccc}{c}_{123}& -s_{123}& 0& l_1{c}_1+l_2{c}_{12}\\ s_{123}& c_{123}& 0& l_1{s}_1+l_2{s}_{12}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right)$

Now assume a given end-effector orientation in the following form:

$\underset{ee}{\overset{}{bs}}T=\left(\begin{array}{cccc}{c}_{\varphi }& -s_{\varphi }& 0& x\\ s_{\varphi }& c_{\varphi }& 0& y\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right)$

Equating the two previous expressions results in:

$\begin{array}{ccc}{c}_{\varphi }& =& c_{123}\\ s_{\varphi }& =& s_{123}\\ x& =& l_1{c}_1+l_2{c}_{12}\\ y& =& l_1{s}_1+l_2{s}_{12}\end{array}$

As:

$\begin{array}{ccc}{c}_{12}& =& c_1{c}_2-s_1{s}_2\\ s_{12}& =& c_1{s}_2+s_1{c}_2\end{array}$,

squaring both the expressions for $x$ and $y$ and adding them, leads to:

$x^2+y^2=l_1^2+l_2^2+2l_1{l}_2{c}_2$

Solving for $c_2$ leads to:

$c_2=\frac{x^2+y^2-l_1^2-l_2^2}{2l_1{l}_2}$,

while $s_2$ equals:

$s_2=\pm \sqrt{1-c_2^2}$,

and, finally, ${\theta }_2$:

${\theta }_2=\text{Atan2}(s_2,c_2)$

Note: The choice of the sign for $s_2$ corresponds with one of the two solutions in the figure above.

The expressions for $x$ and $y$ may now be solved for ${\theta }_1$. In order to do so, write them like this:

$\begin{array}{ccc}x& =& k_1{c}_1-k_2{s}_1\\ y& =& k_1{s}_1+k_2{c}_1\end{array}$

where $k_1=l_1+l_2{c}_2$, and $k_2=l_2{s}_2$.

Let:

$\begin{array}{ccc}r& =& \sqrt{k_1^2+k_2^2}\\ \gamma & =& \text{Atan2}(k2,k1)\end{array}$

Then:

$\begin{array}{ccc}{k}_1& =& r\cos \gamma \\ k_2& =& r\sin \gamma \end{array}$

Applying these to the above equations for $x$ and $y$:

$\begin{array}{ccc}x/r& =& \cos \gamma c_1+\sin \gamma s_1\\ y/r& =& \cos \gamma s_1+\sin \gamma c_1\end{array}$,

or:

$\begin{array}{ccc}\cos (\gamma +{\theta }_1)& =& \frac{x}{r}\\ \sin (\gamma +{\theta }_1)& =& \frac{y}{r}\end{array}$

Thus:

$\gamma +{\theta }_1=\text{Atan2}(y,x)$

Hence:

${\theta }_1=\text{Atan2}(y,x)-\text{Atan2}(k_2,k_1)$

Note: If $x=y=0$, ${\theta }_1$ actually becomes arbitrary.

${\theta }_3$ may now be solved from the first two equations for $s_{\varphi }$ and $c_{\varphi }$:

${\theta }_3=\varphi -{\theta }_1-{\theta }_2=\text{Atan2}(s_{\varphi },c_{\varphi })-{\theta }_1-{\theta }_2$