Robotic Mechanics and Modeling/Inverse Kinematics

Inverse Position Kinematics (Copied and edited from Link)

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 joint angles (or prismatic displacements) 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.

Kinematic Equations for a Planar Three-Link Manipulator

The inverse kinematics problem for this 2D manipulator can be solved algebraically. The solution to the forward kinematics problem is:

$\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]$

To find the kinematic equations, we can use the following relationship:

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

Thus, 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 Kinematic Equations for a Planar Two-Link Manipulator

For simplicity in looking at the inverse problem of the three-link manipulator now modeled as a two-link manipulator, the displacement over the distance $l_3$ is set to zero. An appropriate 4x4 homogeneous transform (in 3D) including the orientation of the third body with $l_3=0$ in the above figure is the following:

$\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$

Inverse Kinematics Related to Jacobian Matrix

The inverse velocity problem seeks the joint rates that provide a specified end-effector twist. This is solved by inverting the Jacobian matrix. It can happen that the robot is in a configuration where the Jacobian does not have an inverse. These are termed singular configurations of the robot.

Inverse Velocity Kinematics

This problem can easily be solved by inverting the Jacobian...

Singularities

If the Jacobian $J$ is invertible, inverting it can be used to easily calculate the joint velocities if the (Cartesian) end-effector velocity is given. Locations (combinations of ${\theta }_i$ where the Jacobian is not invertible are called singularities. Setting the determinant of $J$ equal to zero and solving for $\theta$ allows for finding these singularities. These positions correspond to the loss of a degree of freedom.

In vector calculus, the Jacobian matrix (Template:IPAc-en, Template:IPAc-en) of a vector-valued function in several variables is the matrix of all its first-order partial derivatives. When this matrix is square, that is, when the function takes the same number of variables as input as the number of vector components of its output, its determinant is referred to as the Jacobian determinant. Both the matrix and (if applicable) the determinant are often referred to simply as the Jacobian in literature.

Suppose Template:Math is a function such that each of its first-order partial derivatives exist on Template:Math. This function takes a point Template:Math as input and produces the vector Template:Math as output. Then the Jacobian matrix of Template:Math is defined to be an Template:Math matrix, denoted by Template:Math, whose Template:Mathth entry is ${𝐉}_{ij}=\frac{\partial f_i}{\partial x_j}$, or explicitly

$𝐉=\left[\begin{array}{ccc}\frac{\partial 𝐟}{\partial x_1}& \cdots & \frac{\partial 𝐟}{\partial x_n}\end{array}\right]=\left[\begin{array}{ccc}\frac{\partial f_1}{\partial x_1}& \cdots & \frac{\partial f_1}{\partial x_n}\\ ⋮& \ddots & ⋮\\ \frac{\partial f_m}{\partial x_1}& \cdots & \frac{\partial f_m}{\partial x_n}\end{array}\right].$

This matrix, whose entries are functions of Template:Math, is also denoted variously by Template:Math, Template:Math, and Template:Sfrac. (However, some literature defines the Jacobian as the transpose of the matrix given above.)

The Jacobian matrix represents the differential of Template:Math at every point where Template:Math is differentiable. In detail, if Template:Math is a displacement vector represented by a column matrix, the matrix product Template:Math is another displacement vector, that is the best approximation of the change of Template:Math in a neighborhood of Template:Math, if Template:Math is differentiable at Template:Math.Template:Efn This means that the function that maps Template:Math to Template:Math is the best linear approximation of Template:Math for points close to Template:Math. This linear function is known as the derivative or the differential of Template:Math at Template:Math.

When Template:Math = Template:Math, the Jacobian matrix is square, so its determinant is a well-defined function of Template:Math, known as the Jacobian determinant of Template:Math. It carries important information about the local behavior of Template:Math. In particular, the function Template:Math has locally in the neighborhood of a point Template:Math an inverse function that is differentiable if and only if the Jacobian determinant is nonzero at Template:Math (see Jacobian conjecture). The Jacobian determinant also appears when changing the variables in multiple integrals (see substitution rule for multiple variables).

When Template:Math = 1, that is when Template:Math is a scalar-valued function, the Jacobian matrix reduces to a row vector. This row vector of all first-order partial derivatives of Template:Math is the transpose of the gradient of Template:Math, i.e. ${𝐉}_f=(\mathrm{\nabla }f{)}^{⊺}$. Here we are adopting the convention that the gradient vector $\mathrm{\nabla }f$ is a column vector. Specialising further, when Template:Math = Template:Math = 1, that is when Template:Math is a scalar-valued function of a single variable, the Jacobian matrix has a single entry. This entry is the derivative of the function Template:Math.

These concepts are named after the mathematician Carl Gustav Jacob Jacobi (1804–1851).

Inverse

According to the inverse function theorem, the matrix inverse of the Jacobian matrix of an invertible function is the Jacobian matrix of the inverse function. That is, if the Jacobian of the function Template:Math is continuous and nonsingular at the point Template:Math in Template:Math, then Template:Math is invertible when restricted to some neighborhood of Template:Math and

${𝐉}_{{𝐟}^{-1}}\circ 𝐟={{𝐉}_{𝐟}}^{-1}.$

Conversely, if the Jacobian determinant is not zero at a point, then the function is locally invertible near this point, that is, there is a neighbourhood of this point in which the function is invertible.

The (unproved) Jacobian conjecture is related to global invertibility in the case of a polynomial function, that is a function defined by n polynomials in n variables. It asserts that, if the Jacobian determinant is a non-zero constant (or, equivalently, that it does not have any complex zero), then the function is invertible and its inverse is a polynomial function.