Velocity Kinematics

$\bf {Velocity \ Planar \ Robot \ Arms \ in \ 2D }$

The velocity kinematics express the relationship between the velocity of the individual joints of the robot arm and the velocity of the end-effector in cartesian space.

Figure 1: Angular velocities of two link arm

The translational components of the end-effector pose, i.e. x and y component in 2D, for a 2 joint planar robot is expressed in previous posts in the form:

${\left( \begin{matrix} x\\y \end {matrix} \right ) = {\left( \begin{matrix} a_2 \cos(q_1+q_2) + a_1 \cos q_1 \\ a_2 \sin(q_1+q_2) + a_1 \sin q_1 \end {matrix} \right )$

We can then simply differentiate these terms to fin the velocity components of the end_effector in x and y giving:

$\frac {\delta x}{\delta t}= \dot x = -a_2 (\dot q_1 + \dot q_2) \sin (q_1 + q_2) - a_1 \dot q_1 \sin q_1$

$\frac {\delta y}{\delta t}= \dot y = a_2 (\dot q_1 + \dot q_2) \cos (q_1 + q_2) + a_1 \dot q_1 \cos q_1$

Rearranging the expressions of $\dot x$ and $\dot y$ above in terms of the joint angle velocities $\dot q_1$ and $\dot q_2$ , the expressions can be written in matrix as follows:

$\left( \begin{matrix} \dot x\\ \dot y \end {matrix} \right ) = \left ( \begin{matrix} (-a_2 \sin (q_1 + q_2) - a_1 \sin q_1) \dot q_1 - a_2 \sin (q_1 + q_2) \dot q_2 \\ a_1 \cos q_1 + a_2 \cos (q_1 + q_2) \dot q_1 + a_2 \cos (q_1 + q_2) \dot q_2 \end {matrix} \right )$

From this we can derive the relationship between the x and y components of the end-effector’s velocity an the velocity of the joint angles in the form of a Jacobian Matrix.

$\nu = J(q) \dot q$

$\left ( \begin{matrix} \dot x\\ \dot y \end {matrix} \right ) = \left ( \begin{matrix} -a_2 \sin (q_1 + q_2) - a_1 \sin q_1 && - a_2 \sin (q_1 + q_2) \\ a_1 \cos q_1 + a_2 \cos (q_1 + q_2) && + a_2 \cos (q_1 + q_2) \end {matrix} \right ) \left ( \begin{matrix} \dot q_1 \\ \dot q_2 \end{matrix} \right )$

where $$\nu$$ is the end-effector velocity and the Jacobian is a function of the joint angles of the robot arm. This succinct form also applies to other robot arms with more joints and even in 3D space and can therefore be used to represent the relationship between the joint angles and end-effector velocity.

The Jacobian is the matrix equivalent of the derivative of a function f(x), where y = f(x). It’s the derivative of the function, who’s argument and result are a vector and the who’s elements are the partial derivative of each of the results with respect to the inputs to the function.

$\bf {Inverting \ the \ Jacobian \ Matrix}$

Based on the definition of of the end-effector velocity above, we can obtain an equation for the joint velocities required to achieve a set end-effector velocity.

$\dot q = J (q)^{-1} \nu$

In the 2D 2-link manipulator case the Jacobian J(q) was a 2×2 matrix, as there were only two joints to deal with.

$J^{-1}(q) = det(J(q)) J(q)^T$

$J^{-1}(q) = \frac {1}{a_1 a_2 \sin q_2} \left ( \begin{matrix} a_2 \cos (q_1 + q_2) && a_2 \sin (q_1 + q_2) \\ -a_1 \cos q_1 - a_2 \cos (q_1+q_2) && -a_1 \sin q_1 - a_2 \sin (q_1+q_2) \end{matrix} \right)$

However, there is one issue which may arise when trying to compute the velocity of the joint angles using the the inverse Jacobian, when the inverse Jacobian becomes essentially infinite due to a singularity in the system. This arises when joints align, such as $q_2$ in the above case, whereby the to joints essentially become one and the robot loses a degree of freedom. This is known as a singularity pose.

Mathematically, this is a result of the determinant being zero or close to zero, at a singularity pose.

On MATLAB we can check for a singularity in the inverse of the Jacobian by looking at the condition number :

>> cond(J(q));

$\bf {Velocity \ Ellipse}$

Consider the set of all joint velocities with a unit norm.

Consider the end-effector velocity as a function of this set, such that the set of joint angles have a magnitude of 1.

Based on this we surmise that the set of velocities is a vector x vector product $\dot q ^T \dot q = 1$

This constraint is carried forward into the equation for calculating the required joint angle velocities such that:

$(J^{-1}(q) \nu )^T \times J^{-1}(q) \nu = 1 <=> \nu ^t (J(q)J^T(q))^{-1} \nu = 1$

This equation resembles an ellipse in the velocity space.

Figure 3: Ellipse representing the component of end-effector velocities for a set of joint angles

The ellipse, shown in the Figure above, is a function of the angle configuration, where the shape of the ellipse will vary depending on how the joint angles re configured.

The Ellipse demonstrates that, as per the position of the end-effector, the configuration will alter the manipulator’s ability to move it’s end-effector in a given direction and the speed at which it moves in said direction.

$\bf {Resolved \ Rate \ Motor \ Control}$

If we take a practical scenario where we intend to control a robot arm to move the end-effector along a straight line in cartesian space, we need to specify the cartesian velocity that the robot should follow and compute the required velocity of the individual joint angles.

However, a practical obstacle to overcome is the continuous change in joint angle configuration that results from this motion. The Jacobian which defines the relationship between the cartesian velocity of the end-effector and velocity of the joint angles is a function of the joint angles. Therefore, as the joints move the joint angles and therefore the Jacobian will no longer be correct.

Therefore, we need to periodically update the Jacobian by discretely updating joint angles and computing the Jacobian matrix at every time step, such that for an initial joint velocity:

$\dot q = J(q_k)^{-1} \nu$

we can compute a new set of joint velocities $\dot q = J(q_k)^{-1} \nu$ after every time step $\Delta _t$ , where the new joint configuration is defined:

$q_{k-1} = q_k + \Delta _t \dot q$

This process is repeated until the end-effector reaches its destination.

Velocity Kinematics in 3D

We use velocity kinematics to compute the the rate of change of the end-effector pose.

To describe the velocity kinematics o the end-effector we can use a numerical approximation akin to that of a finite element difference.

we can use this approach to compute the rate of change of the end-effector cartesian coordinates x, y and z as well as the rate of change of it’s orientation.

Over small changes, the approximation of a derivative can also be expressed
by taking a finite difference:

$\frac {df(x)}{dx} \approx \frac {f(x+\delta x) - f(x)}{\delta x}$

In the case of a robot manipulator we can apply this to the forward kinematics.

$\frac {d \kappa (q)}{dq} \approx \frac {\kappa (q+\delta q) - \kappa (q)}{\delta q}$

Where $\delta q_j$ must be a very small change in the joint angle.

We can apply this to the end-effector pose to compute the approxiamte partial derivative which describes the rate of change of the pose (derivative) in terms of the initial and final end-effector pose $T(q) - T(q')$ :

$\frac {\delta T}{\delta q_1} \approx \frac {T(q) - T(q')}{\delta q_1}$

The transformed pose can be written as a matrix, in terms of the rotational and translational components.

$\frac {\delta T}{\delta q_1} \approx \left ( \begin{matrix} \frac {\delta R}{\delta q_1} && \frac {\delta t}{\delta q_1} \\ 0, 0, 0 && 0 \end{matrix} \right )$

The derivative of the translational component of the transformation can then be used to compute the velocity of the end-effector in cartesian space, when multiplied by the change in the joint angle over time $d \tau$ :

$\frac {\delta t}{\delta q_1} \frac {\delta q}{\delta \tau} = \frac {\delta t}{\delta \tau} = \left ( \begin{matrix} \dot x \\ \dot y \\ \dot z \end{matrix} \right )$

The velocity of the end-effector is defined:

$\left ( \begin{matrix} \dot x \\ \dot y \\ \dot z \end{matrix} \right ) = v \dot q_1$

For the Rotational component of the Matrix:

$\frac {\delta R}{\delta q_1} \frac {\delta q_1}{\delta \tau} = \frac {\delta R}{\delta \tau}$

Where $\dot R = S(\omega ) R$

S is the skew symmetric matrix and function of the angular velocity denoted $\omega$

$\underline{Skew \ Symmetric \ Matrix}$

The skew symmetric or anti-symmetric matrix has the following properties:

The Transpose is equal to the negative of the matrix: $S^T = -S$
Is always singular, i.e. the determinant is equal 0;
Any matrix is equal to the sum of a symmetric matrix and skew symmetric matrix;

In 3D space for a position vector of cartesian coordinates x, y and z. The skew symmetric matrix takes the form:

$S(\nu ) = \left ( \begin{matrix} 0&&-z&&y \\ z&&0&&-x \\ -y&&x&&0 \end{matrix} \right )$

We can the vector cross products of a matrix vector product a x b = S(a)b

The derivative of the Rotational component of the Transformation matrix over time is defined as

$\frac {\delta R}{\delta q_1} \times \frac {\delta q_1}{\delta \tau} = \dot R = S(\omega ) R$

where the $\dot q_1 = \frac {dq_1}{d \tau}$ , the above equation can be rearranged with the skew symmetric matrix as the subject:

$S(\omega ) = \frac {\delta R}{\delta q_1} R^T \dot q_1$

where the Skew symmetric matrix takes the form:

$S(\omega ) = \left ( \begin{matrix} 0&&-\omega _{z}&&\omega _y \\ \omega _z&&0&&-\omega _{x} \\ -\omega _{y}&&\omega _x&&0 \end{matrix} \right )$

From this we can show that for joint 1, the component of the angular velocity of the end-effector due to the rate of change of the first joint angle of the robot arm can be defined:

$\left ( \begin{matrix} \omega _{x} \\ \omega _{y} \\ \omega _{z} \end{matrix} \right ) = \left ( \begin{matrix} a \\ b \\ c \end{matrix} \right ) \dot q_1$

where a, b and c are arbitrary elements of the vector.

We can then describe the component of the transnational and angular velocity with respect to the rate of change of joint 1, as a column vector of six elements:

$\left ( \begin{matrix} \nu _ x \\ \nu _y \\ \nu _z \\ \omega _{x} \\ \omega _{y} \\ \omega _{z} \end{matrix} \right ) = \left ( \begin{matrix} a \\ b \\ c \\ d \\ e \\ f \end{matrix} \right ) \dot q_1$

For a 6 joint robot such as the puma 560, we would then have a column for each of the joints meaning that the vector defining the relationship between the joint velocities and end-effector velocities would be a 6×6 matrix. As with the 2D case, this matrix which maps the joint velocities to the end-effector velocity is the Jacobian, which is a function of the joint angle configuration.

$\left ( \begin{matrix} \nu _ x \\ \nu _y \\ \nu _z \\ \omega _{x} \\ \omega _{y} \\ \omega _{z} \end{matrix} \right ) = \left ( \begin{matrix} n_{11}&..&..&n_{61} \\ : & . & & :\\ :& & . & : \\ n_{16} & .. & .. & n_{66} \end{matrix} \right ) \dot q$

In MATLAB, using the Robotics Toolbox, we can compute the Jacobian for a vector of joint angles q simply using the method “jacobn”, e.g. p560.jacobn(q).

Provided it is not singular, we can invert the Jacobian to give us the expression for the joints velocities needed to achieve a specified spatial velocity at the end-effector.

Velocity Kinematics

Velocity Kinematics

Velocity Kinematics in 3D

Published by Majed Yassin

Leave a comment Cancel reply

Velocity Kinematics

Velocity Kinematics in 3D

Share this:

Related

Published by Majed Yassin

Leave a comment Cancel reply