[Robotics] Trajectory Generation
The previous posts in this series developed the geometric and differential machinery of manipulators—spatial transforms, forward and inverse kinematics, the Jacobian, and the dynamic equations of motion. With these tools in hand we can ask, given a current configuration and a desired goal configuration, what the manipulator joints should do over time in order to arrive at the goal in a manner that is smooth, predictable, and physically realizable. This is the problem of trajectory generation.
The distinction between a path and a trajectory is fundamental and worth stating at the outset. A path is a purely geometric object: a curve in either joint space or task space, parameterized by some scalar variable $s \in [0,1]$, with no notion of time attached. A trajectory is the same curve endowed with a time scaling $s = s(t)$, so that one can speak unambiguously of position, velocity, and acceleration at every instant $t \in [0, T]$. The trajectory generator is the module that converts a geometric path—possibly together with a handful of via points and timing constraints—into a continuous, twice-differentiable function $\mathbf{q}(t)$ (or $\mathbf{x}(t)$) that is handed off to the low-level controller at the path-update rate.
A *path* is a geometric mapping $\boldsymbol{\theta}: [0,1] \to \Theta$ from a scalar parameter $s$ into the configuration space $\Theta$. A *time scaling* is a monotonically increasing, twice-differentiable function $s: [0,T] \to [0,1]$ with $s(0) = 0$ and $s(T) = 1$. A *trajectory* is the composition $\boldsymbol{\theta}(s(t))$, i.e. a path together with a time scaling. By the chain rule, velocity and acceleration along the trajectory read $$ \dot{\boldsymbol{\theta}} \;=\; \frac{d\boldsymbol{\theta}}{ds}\,\dot{s}, \qquad \ddot{\boldsymbol{\theta}} \;=\; \frac{d\boldsymbol{\theta}}{ds}\,\ddot{s} \,+\, \frac{d^2\boldsymbol{\theta}}{ds^2}\,\dot{s}^2. $$
In what follows we restrict attention to single-joint planning, since the joint-space schemes treat each of the $n$ joints independently using identical mathematics; the resulting per-joint timing laws are then bundled to produce $\mathbf{q}(t) \in \mathbb{R}^n$. Operational-space schemes are then discussed as a natural extension where the same one-dimensional interpolators are applied componentwise to a Cartesian (and orientation) representation of the end-effector frame.
Joint-Space vs. Task-Space Trajectories
A trajectory may be planned either in joint space or in task space (operational / Cartesian space), and the choice has practical consequences.
In a joint-space scheme, each via point—originally specified by the user as a Cartesian frame $\{T_i\}$ relative to the station $\{S\}$—is first mapped through inverse kinematics to a vector of joint angles $\mathbf{q}_i \in \mathbb{R}^n$. A smooth function of time is then constructed independently for each joint, with the requirement that every joint reach its via value at the same instant so that the end-effector arrives at the desired Cartesian via point simultaneously. Between via points, the geometric shape of the path in joint space is simple (a polynomial, say), but its image under the forward kinematics in Cartesian space is generally a complicated curve.
In a task-space scheme (also called Cartesian-space or operational-space planning), the interpolation is performed directly on a representation of the end-effector pose—a position vector plus some parameterization of orientation. The shape of the path in Cartesian space (a straight line, a circular arc, etc.) is therefore explicitly under the user’s control, but the price is that inverse kinematics must be solved at the path-update rate to recover the joint command actually issued to the actuators.
The trade-offs are summarized below.
*Joint-space planning.* - Computationally cheap: inverse kinematics is solved once per via point, off-line. - No problems with kinematic singularities along the path, since the path is generated in joint coordinates that already correspond to feasible configurations. - The Cartesian shape of the end-effector path is unpredictable and depends on the manipulator's kinematics. *Task-space planning.* - The Cartesian shape of the path is explicit (e.g. a straight line for welding, or a circular arc). - Inverse kinematics must run at the control rate, increasing the computational load. - Suffers from three geometric pathologies (Craig): (i) *intermediate points unreachable* (the straight-line path passes through holes in the workspace), (ii) *high joint rates near a singularity* (Jacobian inversion blows up), and (iii) *start and goal reachable only in different IK branches* (no single continuous joint trajectory traces the path).
As a rule of thumb, joint-space schemes are the default for free-space point-to-point motion (pick-and-place); task-space schemes are required whenever the spatial path matters, as in arc welding, deburring, or following the seam of a workpiece.
Point-to-Point Polynomial Trajectories
We turn to the prototypical sub-problem of joint-space planning: move a single joint from an initial angle $q_0$ at time $t = 0$ to a final angle $q_f$ at time $t = t_f$, smoothly and at rest at both ends.
Cubic Polynomial
Four boundary conditions are immediately evident:
\[q(0) = q_0, \qquad q(t_f) = q_f, \qquad \dot{q}(0) = 0, \qquad \dot{q}(t_f) = 0.\]The first pair fixes the endpoints; the second pair enforces continuity with rest, so that the joint does not start or end with an abrupt velocity step. Four conditions require four free parameters, so the lowest-order polynomial that can interpolate the boundary conditions is the cubic
\[q(t) \;=\; a_0 + a_1 t + a_2 t^2 + a_3 t^3.\]Its time derivatives are
\[\dot{q}(t) = a_1 + 2 a_2 t + 3 a_3 t^2, \qquad \ddot{q}(t) = 2 a_2 + 6 a_3 t.\]Substituting the boundary conditions yields a $4 \times 4$ linear system that decouples in pairs. From $q(0) = q_0$ and $\dot{q}(0) = 0$ we read off $a_0 = q_0$ and $a_1 = 0$. The remaining two conditions
\[q_0 + a_2 t_f^2 + a_3 t_f^3 = q_f, \qquad 2 a_2 t_f + 3 a_3 t_f^2 = 0\]solve to
\[a_2 \;=\; \frac{3}{t_f^2}\,(q_f - q_0), \qquad a_3 \;=\; -\frac{2}{t_f^3}\,(q_f - q_0).\]For a cubic timing law that starts and ends at rest, the position is a smooth S-shaped curve, the *velocity profile is a parabola* attaining its maximum at the midpoint $t = t_f / 2$, and the *acceleration profile is linear* in time, decreasing from $a_{\max} = 6(q_f - q_0)/t_f^2$ at $t = 0$ to $a_{\min} = -6(q_f - q_0)/t_f^2$ at $t = t_f$. Acceleration is therefore *discontinuous* at the boundaries, which implies infinite jerk and may excite mechanical resonances.
Cubic With Nonzero Boundary Velocities (Via Points)
When the joint is to pass through a via point without stopping, the boundary velocities are no longer zero but some prescribed values $\dot{q}_0$ and $\dot{q}_f$. The form of the cubic is unchanged; only the linear system that determines its coefficients is generalized. The four boundary conditions
\[q(0) = q_0, \qquad q(t_f) = q_f, \qquad \dot{q}(0) = \dot{q}_0, \qquad \dot{q}(t_f) = \dot{q}_f\]give
\[\begin{aligned} a_0 &= q_0, \\ a_1 &= \dot{q}_0, \\ a_2 &= \frac{3}{t_f^2}\,(q_f - q_0) \;-\; \frac{2}{t_f}\,\dot{q}_0 \;-\; \frac{1}{t_f}\,\dot{q}_f, \\ a_3 &= -\frac{2}{t_f^3}\,(q_f - q_0) \;+\; \frac{1}{t_f^2}\,(\dot{q}_f + \dot{q}_0). \end{aligned}\]This solution specializes to the rest-to-rest case when $\dot{q}_0 = \dot{q}_f = 0$. The boundary velocities at internal via points may be supplied by the user, computed automatically by a heuristic (e.g. average of the slopes of the secant lines joining adjacent via points, or zero if the slopes change sign), or chosen so as to make the acceleration continuous at the via point—the last option leads to a cubic spline and is treated below.
Quintic Polynomial
A cubic does not allow the user to constrain the boundary accelerations. If a path segment must blend smoothly with another segment in such a way that not just velocity but also acceleration is continuous, six boundary conditions are needed and the lowest-order polynomial that can satisfy them is the quintic:
\[q(t) \;=\; a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5.\]Imposing $q(0), \dot{q}(0), \ddot{q}(0), q(t_f), \dot{q}(t_f), \ddot{q}(t_f)$ gives a $6 \times 6$ linear system whose solution is
\[\begin{aligned} a_0 &= q_0, \\ a_1 &= \dot{q}_0, \\ a_2 &= \tfrac{1}{2}\ddot{q}_0, \\ a_3 &= \frac{20(q_f - q_0) - (8 \dot{q}_f + 12 \dot{q}_0) t_f - (3 \ddot{q}_0 - \ddot{q}_f) t_f^2}{2 t_f^3}, \\ a_4 &= \frac{30(q_0 - q_f) + (14 \dot{q}_f + 16 \dot{q}_0) t_f + (3 \ddot{q}_0 - 2 \ddot{q}_f) t_f^2}{2 t_f^4}, \\ a_5 &= \frac{12(q_f - q_0) - 6(\dot{q}_f + \dot{q}_0) t_f - (\ddot{q}_0 - \ddot{q}_f) t_f^2}{2 t_f^5}. \end{aligned}\]Compared with the cubic, the quintic gives smoother motion at the price of a higher maximum velocity for the same boundary positions and total time.
Trapezoidal Velocity Profile (LSPB)
Polynomial timing laws have one practical drawback: their velocity profile is quadratic (or higher), and so reaches its peak only instantaneously at the midpoint of the motion. If the joint actuator’s velocity capability is the limiting resource, it is more efficient to coast at the maximum allowable velocity for as long as possible, accelerating and decelerating only at the very beginning and end. The resulting trapezoidal velocity profile is the workhorse of industrial motor control.
The construction is also called LSPB, for Linear Segments with Parabolic Blends: a constant-velocity linear segment in position is sandwiched between two parabolic segments of constant acceleration. The position profile $q(t)$ therefore looks like a smooth ramp; its derivative $\dot{q}(t)$ is the eponymous trapezoid; the acceleration $\ddot{q}(t)$ is a piecewise-constant square wave (zero in the middle, $+\ddot{q}_c$ at the start, $-\ddot{q}_c$ at the end).
By symmetry we may insist that both parabolic blends have the same duration $t_b$ (also called $t_c$ in Siciliano) and the same acceleration magnitude $\ddot{q}_c$. Let $q_h$ and $t_h = t_f / 2$ denote the position and time at the midpoint of the motion. Two requirements must hold:
- Velocity continuity at the end of the first blend. At time $t = t_b$ the velocity reached by the parabolic blend, $\ddot{q}_c \, t_b$, must equal the constant velocity of the linear segment $(q_h - q_b)/(t_h - t_b)$, where $q_b = q_0 + \tfrac{1}{2} \ddot{q}_c t_b^2$ is the position at the end of the blend:
- Position match at the midpoint. By the symmetry $q_h = (q_0 + q_f)/2$.
Combining the two with $t_f = 2 t_h$ yields the master equation
\[\ddot{q}_c \, t_b^2 \;-\; \ddot{q}_c \, t_f \, t_b \;+\; (q_f - q_0) \;=\; 0,\]a quadratic in $t_b$ for prescribed $\ddot{q}_c$ and $t_f$. Solving,
\[t_b \;=\; \frac{t_f}{2} \;-\; \frac{\sqrt{\ddot{q}_c^{\,2} \, t_f^2 \;-\; 4 \, \ddot{q}_c\, (q_f - q_0)}}{2 \, \ddot{q}_c}.\]A real solution exists if and only if the radicand is nonnegative, which is the constraint
\[|\ddot{q}_c| \;\geq\; \frac{4 \, |q_f - q_0|}{t_f^2}.\]In other words, the acceleration must be large enough to complete the move—intuitively, even with the longest possible blends ($t_b = t_f / 2$, no coasting), the joint must still be able to travel the required distance in the allotted time.
The peak velocity attained during the constant segment is
\[\dot{q}_{\max} \;=\; \ddot{q}_c \, t_b.\]When equality holds in the acceleration constraint, $|\ddot{q}_c| = 4 |q_f - q_0| / t_f^2$, the two blends abut at the midpoint and the constant-velocity segment shrinks to zero length: $t_b = t_f / 2$. The motion is then *bang-bang* in acceleration—maximum positive acceleration for the first half, maximum negative for the second half—and the velocity profile degenerates from a trapezoid to a triangle. The peak velocity in this limit satisfies $\dot{q}_{\max} \cdot t_f = 2 \, (q_f - q_0)$, which is to say the area under the triangular velocity curve equals the required displacement.
An alternative way to specify the trapezoidal profile is to fix the cruise velocity $\dot{q}_c$ and total time $t_f$ rather than the acceleration; the constraint then becomes
\[\frac{|q_f - q_0|}{t_f} \;<\; |\dot{q}_c| \;\leq\; \frac{2 \, |q_f - q_0|}{t_f},\]and the blend time and acceleration are recovered as
\[t_b \;=\; \frac{q_0 - q_f + \dot{q}_c\, t_f}{\dot{q}_c}, \qquad \ddot{q}_c \;=\; \frac{\dot{q}_c^{\,2}}{q_0 - q_f + \dot{q}_c \, t_f}.\]A useful sanity check: switching to a trapezoidal profile from the rest-to-rest cubic, with the same boundary conditions, increases the integral $\int_0^{t_f} \tau^2(t) \, dt$ (a common proxy for energy dissipated in the motor) by only about $12.5\%$, while giving the user direct control over the velocity and acceleration limits.
Multi-Segment Trajectories Through Via Points
Real manipulator tasks rarely consist of a single point-to-point motion. A pick-and-place cycle, for instance, typically needs an approach point, a grasp point, a lift point, a transit waypoint, an approach above the placement, and a release. Such a sequence of via points $q_1, q_2, \dots, q_N$ at scheduled times $t_1 < t_2 < \dots < t_N$ defines a multi-segment trajectory.
Multi-Segment LSPB With Parabolic Blends
Following Craig’s formulation, one strings together a sequence of linear segments connecting the via points, and adds a parabolic blend region around each via point. Let $\dot{q}{jk}$ denote the constant velocity of the linear segment from point $j$ to point $k$, let $\ddot{q}_k$ denote the (constant) acceleration during the blend at point $k$, and let $t_k$ be the duration of the blend at point $k$. For an *interior* point $k$ adjacent to points $j$ (preceding) and $l$ (following), with prescribed segment durations $t{djk}$ and $t_{dkl}$,
\[\dot{q}_{jk} \;=\; \frac{q_k - q_j}{t_{djk}}, \qquad \ddot{q}_k \;=\; \mathrm{sgn}(\dot{q}_{kl} - \dot{q}_{jk}) \, |\ddot{q}_k|, \qquad t_k \;=\; \frac{\dot{q}_{kl} - \dot{q}_{jk}}{\ddot{q}_k}, \qquad t_{jk} \;=\; t_{djk} - \tfrac{1}{2} t_j - \tfrac{1}{2} t_k.\]The first and last segments need a slightly different treatment because an entire blend region (rather than half) lies inside the segment’s nominal duration. Solving the equality of two expressions for the velocity of the linear portion of the first segment gives, for the initial blend,
\[\ddot{q}_1 \;=\; \mathrm{sgn}(q_2 - q_1) \, |\ddot{q}_1|, \qquad t_1 \;=\; t_{d12} - \sqrt{t_{d12}^{\,2} - \frac{2(q_2 - q_1)}{\ddot{q}_1}}.\]A symmetric formula handles the final blend at $q_N$.
In the multi-segment LSPB construction, the trajectory does *not* pass exactly through any interior via point. Because the blend smoothly merges the incoming and outgoing linear segments, the actual position at the via point is slightly displaced toward the chord. The displacement shrinks as the blend acceleration is increased (shorter blend), and vanishes only in the bang-bang limit. If exact passage through an interior point is required, that point is replaced by *two pseudo via points*, one on either side, with the original point lying on the (now exactly straight) linear segment between them.
Cubic-Spline Trajectories Through Via Points
A more elegant alternative, which does pass through every via point and which provides continuous acceleration at the knots, is to fit a separate cubic polynomial $\Pi_k(t)$ to each segment $[t_k, t_{k+1}]$ and to enforce the appropriate matching conditions at the interior knots. For each interior point $t_k$ with $k = 2, \dots, N - 1$ four constraints must hold:
\[\begin{aligned} \Pi_{k-1}(t_k) &= q_k, \\ \Pi_k(t_k) &= q_k, \\ \dot{\Pi}_{k-1}(t_k) &= \dot{\Pi}_k(t_k), \\ \ddot{\Pi}_{k-1}(t_k) &= \ddot{\Pi}_k(t_k). \end{aligned}\]The first two enforce position interpolation (the same value $q_k$ from both sides), the third enforces continuity of velocity, and the fourth—the key additional requirement—enforces continuity of acceleration. Counting equations, with $N$ via points one has $N - 1$ cubic segments, each with $4$ coefficients, for a total of $4(N - 1)$ unknowns. The interior matching conditions provide $4(N - 2)$ equations, and the two endpoint conditions (initial and final position) add $2$ more. That leaves $4(N-1) - 4(N-2) - 2 = 2$ degrees of freedom, which are pinned down by specifying the initial and final velocities (typically zero for rest-to-rest sequences).
The resulting system can be reorganized into a single tridiagonal linear system in the unknown via-point accelerations $\{\ddot{\Pi}(t_k)\}$, which is solved efficiently in $O(N)$ time. Once the second derivatives at the knots are known, each cubic segment is fully determined by the explicit formula
\[\Pi_k(t) \;=\; \frac{\ddot{\Pi}(t_k)}{6 \Delta t_k}(t_{k+1} - t)^3 \;+\; \frac{\ddot{\Pi}(t_{k+1})}{6 \Delta t_k}(t - t_k)^3 \;+\; \Big(\frac{\Pi(t_{k+1})}{\Delta t_k} - \frac{\Delta t_k \ddot{\Pi}(t_{k+1})}{6}\Big)(t - t_k) \;+\; \Big(\frac{\Pi(t_k)}{\Delta t_k} - \frac{\Delta t_k \ddot{\Pi}(t_k)}{6}\Big)(t_{k+1} - t),\]where $\Delta t_k = t_{k+1} - t_k$. The function $q(t)$ produced this way is termed a cubic spline and is the smoothest interpolant in a precise variational sense (it minimizes $\int \ddot{q}^2 \, dt$ among all $C^2$ interpolants).
A subtle point raised by Siciliano: if one wishes to specify both the initial and final velocities and the initial and final accelerations, the system is over-constrained for purely cubic polynomials. The standard workaround is to introduce two virtual via points $t_2$ and $t_{N+1}$ whose positions are not specified—only continuity is enforced at them—raising the count to $N + 1$ cubics with the additional acceleration boundary conditions absorbed into the extra freedom.
Time Scaling
We have so far written all our timing laws directly as functions of time $t$. A useful conceptual refinement, emphasized by Lynch and Park, is to factor the trajectory $\boldsymbol{\theta}(t)$ into a path $\boldsymbol{\theta}(s)$ and a time scaling $s(t)$ with $s \in [0,1]$.
The path captures the shape of the motion in configuration (or task) space; the time scaling determines how fast the manipulator traverses that shape. Concretely, for a straight-line path in joint space,
\[\boldsymbol{\theta}(s) \;=\; \boldsymbol{\theta}_{\text{start}} \;+\; s \, (\boldsymbol{\theta}_{\text{end}} - \boldsymbol{\theta}_{\text{start}}), \qquad s \in [0, 1],\]and any of the timing laws of the previous sections may be applied to $s(t)$ rather than to $\boldsymbol{\theta}(t)$. For instance, the cubic rest-to-rest time scaling
\[s(t) \;=\; \frac{3 t^2}{T^2} \;-\; \frac{2 t^3}{T^3}\]gives $s(0) = 0, s(T) = 1, \dot{s}(0) = \dot{s}(T) = 0$; substituting into the straight line yields the familiar cubic interpolant. The fifth-order time scaling adds $\ddot{s}(0) = \ddot{s}(T) = 0$. The trapezoidal time scaling parameterizes the LSPB construction analogously.
The motivation for the separation is twofold.
-
Replanning the speed without recomputing the path. Once a geometric path has been validated (clear of obstacles, free of singularities, within joint limits), the user may need to slow the motion down (e.g. to avoid spilling a glass of water the robot is carrying) or speed it up. With the path-and-scaling separation, this requires only a new $s(t)$—a one-dimensional function—rather than a re-interpolation of the entire $n$-dimensional joint trajectory.
-
Uniform treatment of joint-space and task-space. The same timing laws apply verbatim to a path in $\mathbb{R}^n$ (joint space) or in $SE(3)$ (task space) once the path is parameterized by $s$.
A *time scaling* of a path $\boldsymbol{\theta}: [0,1] \to \Theta$ is a monotonically nondecreasing, twice-differentiable function $s: [0, T] \to [0, 1]$ with $s(0) = 0$ and $s(T) = 1$. Under the time scaling, the trajectory velocity and acceleration along the path are $$ \dot{\boldsymbol{\theta}}(t) \;=\; \boldsymbol{\theta}'(s) \, \dot{s}(t), \qquad \ddot{\boldsymbol{\theta}}(t) \;=\; \boldsymbol{\theta}'(s) \, \ddot{s}(t) \;+\; \boldsymbol{\theta}''(s) \, \dot{s}(t)^2, $$ where primes denote differentiation with respect to $s$.
Time-Optimal Trajectories Under Torque Limits
Trapezoidal time scalings yield time-optimal motion only under the simplifying assumption that the joint velocity and acceleration limits are constant along the path. For a real manipulator, both bounds are state-dependent: they depend on the configuration $\boldsymbol{\theta}$ and joint rate $\dot{\boldsymbol{\theta}}$ through the dynamic equations of motion
\[M(\boldsymbol{\theta}) \ddot{\boldsymbol{\theta}} \;+\; c(\boldsymbol{\theta}, \dot{\boldsymbol{\theta}}) \;+\; g(\boldsymbol{\theta}) \;=\; \boldsymbol{\tau},\]subject to actuator bounds $\tau_i^{\min}(\boldsymbol{\theta}, \dot{\boldsymbol{\theta}}) \leq \tau_i \leq \tau_i^{\max}(\boldsymbol{\theta}, \dot{\boldsymbol{\theta}})$.
The classical Bobrow / Shin-McKay approach reformulates the problem in the $(s, \dot{s})$ phase plane: with the path fixed as $\boldsymbol{\theta}(s)$, the dynamics can be rewritten as a scalar second-order equation
\[m(s) \, \ddot{s} \;+\; c(s) \, \dot{s}^2 \;+\; g(s) \;=\; \boldsymbol{\tau},\]where $m(s)$, $c(s)$, $g(s) \in \mathbb{R}^n$ are derived from $M$, the Christoffel symbols, and the gravity vector evaluated along the path. The torque limits translate into per-joint inequalities of the form
\[L_i(s, \dot{s}) \;\leq\; \ddot{s} \;\leq\; U_i(s, \dot{s}),\]and taking the maximum lower bound and minimum upper bound over all joints yields a state-dependent acceleration cone $L(s, \dot{s}) \leq \ddot{s} \leq U(s, \dot{s})$ in the phase plane. Above a certain velocity limit curve $\dot{s}_{\lim}(s)$, $L > U$ and the state is inadmissible.
A standard argument shows that the time-optimal time scaling, when it exists, is of bang-bang type with respect to acceleration: at almost every $s$ it operates either at the maximum acceleration $U(s, \dot{s})$ or at the minimum deceleration $L(s, \dot{s})$, switching at isolated points. The simplest case is a single switch: integrate $\ddot{s} = U(s, \dot{s})$ forward from $(0, 0)$ and $\ddot{s} = L(s, \dot{s})$ backward from $(1, 0)$; the switching point is the intersection of the two curves. In general the velocity limit curve may force multiple switches, and a more elaborate algorithm is needed to identify them. The construction is beyond the scope of this introduction, but the framework should be kept in mind whenever the maximum manipulator throughput is at stake.
Operational-Space Trajectories
The same one-dimensional interpolators—cubic, quintic, LSPB, splines—may be applied componentwise to a vector representation of the end-effector pose in order to plan in task space. For the position component this is immediate: simply interpolate the three Cartesian coordinates with, say, an LSPB profile. For the orientation component matters are more delicate, since a linear interpolation of the entries of a rotation matrix between two valid rotations $R_1, R_2 \in SO(3)$ does not generally produce an orthonormal matrix.
| A common workaround, following Craig, is to use the angle-axis representation: at each via point the orientation is encoded by a single rotation $R(\hat{K}, \theta)$, packed into a $3$-vector ${}^S K = \theta \hat{K}$, and the same LSPB scheme is applied to this $3$-vector componentwise. Together with the position vector ${}^S P$ this gives a $6$-vector $\boldsymbol{\chi} = ({}^S P, {}^S K)$ that fully parameterizes the end-effector pose. One subtlety is that the angle-axis representation is not unique ($(\hat{K}, \theta) \sim (\hat{K}, \theta + 2 \pi n)$); when interpolating across multiple via points one must, at each step, choose the representative ${}^S K_B$ that minimizes $ | {}^S K_B - {}^S K_A | $ to guarantee a path of minimum rotation. |
A more modern alternative, due to Lynch and Park, is to perform a screw motion along the fixed screw axis $S = \log(X_{\text{start}}^{-1} X_{\text{end}})$ from $X_{\text{start}}$ to $X_{\text{end}}$:
\[X(s) \;=\; X_{\text{start}} \, \exp\!\big(\log(X_{\text{start}}^{-1} X_{\text{end}}) \, s\big), \qquad s \in [0, 1].\]This is a true “straight line” on $SE(3)$ in the sense that the screw axis is constant. Alternatively, the rotational and translational parts may be decoupled: $p(s) = p_{\text{start}} + s (p_{\text{end}} - p_{\text{start}})$ for position and $R(s) = R_{\text{start}} \exp(\log(R_{\text{start}}^T R_{\text{end}}) s)$ for orientation.
Whichever representation is chosen, the trajectory generator is responsible for ensuring that the blend times used for the position and orientation components agree, so that the multi-axis motion remains synchronized.
Worked Example: Cubic Trajectory $q_0 = 0 \to q_f = \pi/2$ in $t_f = 2 \text{ s}$
To anchor the formulas in a concrete computation, consider a single rotary joint that is to move from $q_0 = 0$ rad to $q_f = \pi/2$ rad in $t_f = 2$ s, starting and ending at rest. We want the coefficients of the cubic timing law and the resulting peak velocity and acceleration.
From the rest-to-rest formulas of the cubic section,
\[\begin{aligned} a_0 &= q_0 = 0, \\ a_1 &= 0, \\ a_2 &= \frac{3}{t_f^2}\,(q_f - q_0) = \frac{3}{4} \cdot \frac{\pi}{2} = \frac{3 \pi}{8} \approx 1.1781, \\ a_3 &= -\frac{2}{t_f^3}\,(q_f - q_0) = -\frac{2}{8} \cdot \frac{\pi}{2} = -\frac{\pi}{8} \approx -0.3927. \end{aligned}\]The position, velocity, and acceleration histories are therefore
\[\begin{aligned} q(t) &= \tfrac{3\pi}{8}\, t^2 \;-\; \tfrac{\pi}{8}\, t^3, \\ \dot{q}(t) &= \tfrac{3\pi}{4}\, t \;-\; \tfrac{3\pi}{8}\, t^2, \\ \ddot{q}(t) &= \tfrac{3\pi}{4} \;-\; \tfrac{3\pi}{4}\, t. \end{aligned}\]Setting $\ddot{q}(t) = 0$ identifies the peak-velocity instant as $t^\star = 1$ s, the midpoint of the motion, where the velocity attains
\[\dot{q}_{\max} \;=\; \dot{q}(1) \;=\; \frac{3 \pi}{4} - \frac{3\pi}{8} \;=\; \frac{3 \pi}{8} \;\approx\; 1.1781 \text{ rad/s}.\]The peak acceleration occurs at the boundaries:
\[\ddot{q}(0) \;=\; +\frac{3\pi}{4} \;\approx\; +2.3562 \text{ rad/s}^2, \qquad \ddot{q}(t_f) \;=\; -\frac{3\pi}{4} \;\approx\; -2.3562 \text{ rad/s}^2.\]As a consistency check, the average velocity over the interval is $(q_f - q_0)/t_f = \pi/4 \approx 0.7854$ rad/s, exactly $2/3$ of the peak velocity $3\pi/8$, which is the well-known ratio between the average and the peak of a parabolic velocity curve.
| If the same motion had been planned with a trapezoidal profile constrained by an acceleration $ | \ddot{q}_c | = 3$ rad/s$^2$ (slightly above the minimum $4 | q_f - q_0 | / t_f^2 = \pi \approx 3.14$… actually this is just below the minimum, so the move is infeasible at this acceleration). Choosing instead $ | \ddot{q}_c | = 4$ rad/s$^2$, the blend time is |
so the joint accelerates for about $0.23$ s, coasts at the cruise velocity $\dot{q}_c = \ddot{q}_c t_b \approx 4 \cdot 0.2276 = 0.91$ rad/s for about $1.54$ s, then decelerates symmetrically. The peak velocity of the trapezoidal scheme ($0.91$ rad/s) is lower than that of the cubic ($1.18$ rad/s) because the trapezoidal profile spends most of the time at the cruise velocity rather than reaching the peak only instantaneously.
Summary
We have developed the standard one-dimensional toolkit used to convert geometric path descriptions into time-parameterized trajectories suitable for robot control:
- A path is a geometric curve $\boldsymbol{\theta}(s)$; a trajectory is a path together with a time scaling $s(t)$. The trajectory generator produces a $C^2$ function $\mathbf{q}(t)$ from a sparse user specification (start, goal, possibly via points, total time, acceleration bounds).
- Trajectories may be planned in joint space (cheap, robust against singularities, but with an unpredictable Cartesian shape) or in task space (geometrically explicit but susceptible to reachability and singularity problems).
- The cubic polynomial is the lowest-order rest-to-rest interpolant; with nonzero boundary velocities it serves as the building block of via-point trajectories. The quintic polynomial additionally accommodates boundary accelerations.
-
The trapezoidal / LSPB profile accelerates at constant rate, coasts at maximum velocity, and decelerates at constant rate; it is time-optimal under constant velocity and acceleration bounds and is the standard in industrial motion control. It degenerates to a bang-bang triangular profile when $ \ddot{q}_c \, t_f^2 = 4 (q_f - q_0)$. - Multi-segment trajectories are constructed either by stringing LSPB segments with parabolic blends at via points (interior via points are not hit exactly; pseudo via points repair this if needed) or by cubic splines that enforce position, velocity, and acceleration continuity at each knot through a tridiagonal linear system.
- The time-scaling abstraction $\boldsymbol{\theta}(s(t))$ decouples geometric path planning from temporal planning and is convenient for replanning speed without altering shape.
- Time-optimal trajectories along a fixed path under torque limits reduce to a phase-plane integration in $(s, \dot{s})$ with state-dependent acceleration cones derived from the manipulator dynamics; the optimal scaling is generically bang-bang with switches between maximum acceleration and maximum deceleration.
These ingredients are the bridge between the kinematic and dynamic layers of the previous posts and the control layer to come: the trajectory generator outputs the reference $\mathbf{q}^d(t), \dot{\mathbf{q}}^d(t), \ddot{\mathbf{q}}^d(t)$ that the feedback controller is tasked with tracking.
Reference
[1] John J. Craig, Introduction to Robotics: Mechanics and Control, 3rd Edition, Pearson Prentice Hall, 2005. Chapter 7: “Trajectory Generation.”
[2] Bruno Siciliano, Lorenzo Sciavicco, Luigi Villani, and Giuseppe Oriolo, Robotics: Modelling, Planning and Control, Springer, 2010. Chapter 4: “Trajectory Planning.”
[3] Kevin M. Lynch and Frank C. Park, Modern Robotics: Mechanics, Planning, and Control, Cambridge University Press, 2017. Chapter 9: “Trajectory Generation.”
Leave a comment