Robotics, Vision and Control - 3

3 Time and Motion

3.1 Trajectories

A path is a spatial construct – a locus in space that leads from an initial pose to a final pose. A trajectory is a path with specified timing.
An important characteristic of a trajectory is that is smooth – position and orientation vary smoothly with time.

3.1.1 Smooth One-Dimensional Trajectories

Smoothness in this context means that its first few temporal derivatives are continuous. Typically velocity and acceleration are required to be continuous and sometimes also the derivative of acceleration or jerk.

An obvious candidate for such a function is a polynomial function of time. Polynomials are simple to compute and can easily provide the required smoothness and boundary conditions. A quintic (fifth-order) polynomial is often used
$S(t) = At^5 + Bt^4 + Ct^3 + Dt^2 + Et + F$
The trajectory has defined boundary conditions for position, velocity and accelerationand commonly the velocity and acceleration boundary conditions are all zero.
在这里插入图片描述

For a quintic polynomial acceleration will be a smooth cubic polynomial, and jerk will be a parabola.

A real robot joint has a well defined maximum velocity and for minimum-time motion we want to be operating at that maximum for as much of the time as possible. We would like the velocity curve to be flatter on top.
在这里插入图片描述
A well known alternative is a hybrid trajectory which has a constant velocity segment with polynomial segments for acceleration and deceleration. The trajectory comprises a linear segment (constant velocity) with parabolic blends, hence the name lspb. The term blend is commonly used to refer to a trajectory segment that smoothly joins linear segments.

The trajectories for these different cases are overlaid in Fig. 3.2b. We see that as the velocity of the linear segment increases its duration decreases and ultimately its duration would be zero. In fact the velocity cannot be chosen arbitrarily, too high or too low a value for the maximum velocity will result in an infeasible trajectory and the function returns an error.

3.1.2 Multi-Dimensional Case

Most useful robots have more than one axis of motion or degree of freedom. We represent this in vector form as $x ∈ \mathbb R^M$ where M is the number of degrees of freedom. A wheeled mobile robot is characterised by its position $(x, y)$ or pose $(x, y, θ)$ . The tool of an arm robot has position $(x, y, z)$ , orientation $(θ_r, θ_p, θ_y)$ or pose $(x, y, z, θ_r, θ_p, θ_y)$ . We therefore require smooth multi-dimensional motion from an initial vector to a final vector.

3.1.3 Multi-Segment Trajectories

In robotics applications there is often a need to move smoothly along a path through one or more intermediate or via points without stopping. This might be to avoid obstacles in the workplace, or to perform a task that involves following a piecewise continuous trajectory such as applying a bead of sealant in a manufacturing application.

To formalize the problem consider that the trajectory is defined by N points $x_k, k ∈ [1, N]$ and there are $N − 1$ motion segments. As in the previous section $x_k ∈ \mathbb R^M$ is a vector representation of pose.
在这里插入图片描述
The robot starts from $x_1$ at rest and finishes at $x_N$ at rest, but moves through (or close to) the intermediate points without stopping. The problem is over constrained and in order to attain continuous velocity we surrender the ability to reach each intermediate point. This is easiest to understand for the one dimensional case shown in Fig. 3.4. The motion comprises linear motion segments with polynomial blends, like lspb, but here we choose quintic polynomials because they are able to match boundary conditions on position, velocity and acceleration at their start and end points.

The first segment of the trajectory accelerates from the initial pose $x_1$ and zero velocity, and joins the line heading toward the second pose $x_2$ . The blend time is set to be a constant $t_{acc}$ and $t_{acc}/ 2$ before reaching $x_2$ the trajectory executes a polynomial blend, of duration $t_{acc}$ , onto the line from $x_2$ to $x_3$ , and the process repeats. The constant velocity ·k can be specified for each segment. The average acceleration during the blend is
在这里插入图片描述
If the maximum acceleration capability of the axis is known then the minimum blend time can be computed.

For the multi-axis case it is likely that some axes will need to move further than others on a particular motion segment and this becomes complex if the joints have different velocity limits. The first step is to determine which axis will be the slowest to complete the motion segment, based on the distance that each axis needs to travel for the segment and its maximum achievable velocity. From this the duration of the segment can be computed and then the required velocity of each axis. This ensures that all axes reach the next target $x_k$ at the same time.

3.1.4 Interpolation of Orientation in 3D

In robotics we often need to interpolate orientation, for example, we require the endeffector of a robot to smoothly change from orientation $ξ_0$ to $ξ_1$ . Using the notation from Chap. 2 we require some function $ξ(s) = σ(ξ_0, ξ_1, s)$ where $s ∈ [0, 1]$ which has the boundary conditions $σ(ξ_0, ξ_1, 0) = ξ_0$ and $σ(ξ_0, ξ_1, 1) = ξ_1$ and where $σ(ξ_0, ξ_1, s)$ varies smoothly for intermediate values of s. How we implement this depends very much on our concrete representation of $ξ$ .

If pose is represented by an orthonormal rotation matrix, $ξ ∼ R ∈ SO(3)$ , we might consider a simple linear interpolation $σ(R_0, R_1, s) = (1 − s)R_0+ sR_1$ but this would not, in general, be a valid orthonormal matrix which has strict column norm and intercolumn orthogonality constraints.

A workable and commonly used alternative is to consider a 3-angle representation such as Euler or roll-pitch-yaw angles, $ξ ∼ Γ ∈ \mathbb S^3$ and use linear interpolation
在这里插入图片描述
For large orientation changes we see that the axis around which the coordinate frame rotates changes along the trajectory. The motion, while smooth, sometimes looks uncoordinated. There will also be problems if either $ξ_0$ or $ξ_1$ is close to a singularity in the particular 3-angle system being used.

Interpolation of unit-quaternions is only a little more complex than for 3-angle vectors and produces a change in orientation that is a rotation around a fixed axis in space.

Quaternion interpolation is achieved using spherical linear interpolation (slerp) in which the unit quaternions follow a great circle path on a 4-dimensional hypersphere. The result in 3-dimensions is rotation about a fixed axis in space.

3.1.5 Cartesian Motion

Another common requirement is a smooth path between two poses in $SE(3)$ which involves change in position as well as in orientation. In robotics this is often referred to as Cartesian motion.

However the translational motion has a velocity and acceleration discontinuity at the first and last points. The problem is that while the trajectory is smooth in space the distance s along the trajectory is not smooth in time. Speed along the path jumps from zero to some finite value and then drops to zero at the end – there is no initial acceleration or final deceleration.

3.2 Time Varying Coordinate Frames

The translational velocity is the rate of change of the position of the origin of the coordinate frame. Rotational velocity is a little more complex.

3.2.1 Rotating Coordinate Frame

A body rotating in 3-dimensional space has an angular velocity which is a vector quantity $ω = (ω_x, ω_y, ω_z)$ . The direction of this vector defines the instantaneous axis of rotation, that is, the axis about which the coordinate frame is rotating at a particular instant of time. In general this axis changes with time. The magnitude of the vector is the rate of rotation about the axis – in this respect it is similar to the angle-axis representation for rotation introduced in Sect. 2.2.1.5. From mechanics there is a well known expression for the derivative of a time-varying rotation matrix
在这里插入图片描述
where $R(t) ∈ SO(2)$ or $SO(3)$ and $S(·)$ is a skew-symmetric matrix that, for the 3-dimensional case, has the form

We might ask what does ½ mean? Consider the approximation to the derivative

which we rearrange as

在这里插入图片描述
and substituting Eq. 3.4 we obtain

which describes how the orthnormal rotation matrix changes as a function of angular velocity.

3.2.2 Incremental Motion

Consider a coordinate frame that undergoes a small rotation from $R_0$ to $R_1$ . We can write Eq. 3.7 as
在这里插入图片描述
and rearrange it as

and then apply the vex operator, the inverse of $S(·)$ , to both sides

where $δ_Θ= δ_tω$ is a 3-vector with units of angle that represents an infinitesimal rotation about the world x-, y- and z-axes. (It is in the world coordinate frame because the term ( $δ_tS (ω) + I_{3×3}$ ) premultiplies $R_0$ )

The function vex performs the inverse function of converting a skew-symmetric matrix to a vector.

We have strongly, and properly, cautioned about the non-commutivity of rotations but for infinitesimal angular changes multiplication is commutative.

Given two poses $ξ_0$ and $ξ_1$ that differ infinitesimally we can represent the difference between them as a 6-vector
在这里插入图片描述
comprising the incremental displacement and the incremental rotation. The quantity $δ ∈ \mathbb R^6$ is effectively the spatial velocity which we discuss further in Chap. 8 multiplied by $δ_t$ . If the poses are represented in homogeneous transformation form then the difference is
在这里插入图片描述
where $T_0 = (R_0, t_0)$ and $T_1 = (R_1, t_1)$ .
The inverse operation is

and for homogenous transformation representation is

the function $∆(·)$ is computed by the Toolbox function tr2delta

3.2.3 Inertial Navigation Systems

An inertial navigation system is a “black box” that estimates its velocity, orientation and position with respect to the inertial reference frame (the universe). Importantly it has no external inputs such as radio signals from satellites and this makes it well suited to applications such as submarine, spacecraft and missile guidance. An inertial navigation system works by measuring accelerations and angular velocities and integrating them over time.

Early inertial navigation systems, such as shown in Fig. 2.13, used mechanical gimbals to keep the accelerometers at a constant attitude with respect to the stars using a gyro-stabilized platform. The acceleration measured on this platform was integrated to obtain the velocity of the platform, and integrated again to obtain its position. In order to achieve accurate position estimates over periods of hours or days the gimbals
and gyroscopes had to be of extremely high quality so that the stable platform did not drift, and the acceleration sensors needed to be extremely accurate.

In a modern strap down inertial measurement system the acceleration and angular velocity sensors are rigidly attached to the vehicle. The three orthogonally mounted gyroscopes measure the components of the angular velocity $ω$ and use Eq. 3.7 to continuously update the estimated orientation ${}^0R_B$ of the vehicle’s body-fixed frame {B} with respect to the stars {0}.

A discrete-time version of Eq. 3.7 such a
在这里插入图片描述
is used to numerically integrate changes in pose in order to estimate the orientation of the vehicle. The measured acceleration ${}^Ba$ of the vehicle’s body frame is rotated into the inertial frame

and can then be integrated twice to update the estimate of the vehicle’s position in the inertial frame. Practical systems work at high sample rate, typically hundreds of Hertz, and would employ higher-order numerical integration techniques rather than the simple rectangular integration of Eq. 3.13.

In Eq. 3.13 we added the matrix $δ_tS(ω)R(t)$ to an orthonormal rotation matrix and this is not quite proper – the result will not be an orthonormal matrix. However if the added term is small (Which is why inertial navigation systems operate at a high sample rate and $δ_t$ is small) the result will be close to orthonormal and we can straighten it up. This process is called normalization and enforces the constraints on the elements of an orthonormal matrix. It involves the following steps where $c_i$ is the $i^{th}$ column of $R$ . We first assume that column 3 is correct
在这里插入图片描述
then the first column is made orthogonal to the last two

However the last two columns may not have been orthogonal so

Finally the columns are normalized to unit magnitude

Alternatively we could use unit-quaternions and things, as is generally the case, are a little simpler. The derivative of a quaternion, the quaternion equivalent of Eq. 3.4 is defined as
在这里插入图片描述
Integration of quaternion rates is achieved by

As with Eq. 3.7 the addition is not quite proper and the result will no longer be a unit quaternion. Normalization is achieved by ensuring that the quaternion norm is unity, a straightforward division of all elements by the quaternion norm
在这里插入图片描述
Quaternions are more commonly used in the rotation update equations for strapdown inertial navigation systems than orthonormal rotation matrices. The results will of course be the same but the computational cost for the quaternion version is significantly less.

3.3 Wrapping Up

In this chapter we have considered pose that varies as a function of time from two perspectives. The first perspective was to create a sequence of poses, a trajectory, that a robot can follow. An important characteristic of a trajectory is that it is smooth – the position and orientation varies smoothly with time. We start by discussing how to generate smooth trajectories in one dimension and then extended that to the multi-dimensional case and then to piecewise-linear trajectories that visit a number of intermediate points.

The second perspective was to examine the temporal derivative of an orthonormal rotation matrix and how that relates to concepts from mechanics such as velocity and angular velocity. This allows us to solve the inverse problem, given measurements from sensors we are able to update the estimate of pose for a moving object – the principle underlying inertial navigation. We introduced the infinitesimal motion δ which is related to spatial velocity and which we will encounter again in Chap. 8.

Interpolation of rotation and integration of angular velocity was treated using both orthonormal rotation matrices and quaternions. The results are equivalent but the quaternion formulation is more elegant and computationally more efficient.

Reference

Robotics, Vision and Control: Fundamental Algorithms In MATLAB, Corke, Peter