Robotics Kinematics and Dynamics/Serial Manipulator Position Kinematics

From Wikibooks, open books for an open world
Jump to: navigation, search

Forward Position Kinematics[edit]

The forward position kinematics problem can be stated as follows: given the different joint angles, what is the 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 ^{ee}_{bs}T \,, where \{bs\} is the base frame of the robot manipulator.

Solution[edit]

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:

^{ee}_{bs}T = \, ^{N}_{0}T = \, ^{1}_{0}T \, ^{2}_{1}T \ldots \, ^{N-1}_{N-2}T \,^{N}_{N-1}T

Examples[edit]

The Planar Three-Link Manipulator[edit]

A planar three link manipulator. Each X_i-axis lies along the ith 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) =
\begin{pmatrix}
\cos(\theta_1) & -\sin(\theta_1) & 0 \\
\sin(\theta_1) & \cos(\theta_1) & 0 \\
0 & 0 & 1
\end{pmatrix}

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


T_2(\theta_2) =
\begin{pmatrix}
\cos(\theta_2) & -\sin(\theta_2) & l_1 \\
\sin(\theta_2) & \cos(\theta_2) & 0 \\
0 & 0 & 1
\end{pmatrix}

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) =
\begin{pmatrix}
\cos(\theta_3) & -\sin(\theta_3) & l_2 \\
\sin(\theta_3) & \cos(\theta_3) & 0 \\
0 & 0 & 1
\end{pmatrix}

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


T_4 =
\begin{pmatrix}
1 & 0 & l_3 \\
0 & 1 & 0 \\
0 & 0 & 1
\end{pmatrix}

The solution to the forward kinematics problem is then:

^{ee}_{bs}T = \, ^4_0T(\theta_1, \theta_2, \theta_3) =
\begin{pmatrix}
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{pmatrix}

Hence:


\begin{pmatrix}
x \\
y \\
1
\end{pmatrix} = T
\begin{pmatrix}
0 \\
0 \\
1
\end{pmatrix}

The resulting kinematic equations are:


\begin{array}{l}
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[edit]

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[edit]

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):


^3_0 T = \begin{pmatrix}
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{pmatrix}

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


^{ee}_{bs} T = \begin{pmatrix}
c_{\phi} & -s_{\phi} & 0 & x \\
s_{\phi} & c_{\phi} & 0 & y \\
0 & 0 & 1 & 0\\
0 & 0 & 0 & 1 \\
\end{pmatrix}

Equating the two previous expressions results in:


\begin{array}{lll}
c_{\phi} & = & c_{123} \\
s_{\phi} & = & 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}{lll}
c_{12} & = & c_1c_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 + 2 l_1 l_2 c_2

Solving for c_2 leads to:


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

while s_2 equals:


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

and, finally, \theta_2:


\theta_2 = \mbox{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}{lll}
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}{lll}
r & = & \sqrt{k_1^2 + k_2^2} \\
\gamma & = & \mbox{Atan2}(k2,k1) \\
\end{array}

Then:


\begin{array}{lll}
k_1 & = & r \cos \gamma \\
k_2 & = & r \sin \gamma \\
\end{array}

Applying these to the above equations for x and y:


\begin{array}{lll}
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}{lll}
\cos (\gamma + \theta_1) & = & \frac{x}{r} \\
\sin (\gamma + \theta_1) & = & \frac{y}{r} \\
\end{array}

Thus:


\gamma + \theta_1 = \mbox{Atan2}(y,x)

Hence:


\theta_1 = \mbox{Atan2}(y,x) - \mbox{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_{\phi} and c_{\phi}:


\theta_3 = \phi - \theta_1 - \theta_2 = \mbox{Atan2}(s_{\phi},c_{\phi}) - \theta_1 - \theta_2