Trajectory Generation and Tracking

A setpoint is a static target: "go to 90°." The controller's job is to get there, and it doesn't matter much how — the feedback loop will handle the rest.

A trajectory is a time-parameterized path: $r(t)$ specifies the desired position and velocity at every instant. The controller must track a moving reference, not just settle to a fixed one.

Why does this matter? Consider a pick-and-place arm. If you just command the final position, the arm might lunge at full speed, slam into the hard stop, and shatter the gripper. A trajectory generator plans the motion: accelerate smoothly, reach peak velocity, decelerate to rest exactly at the target. The physical result is the same final position — but the path there respects velocity, acceleration, and jerk limits. For any robot that moves fast or carries fragile payloads, trajectory generation is not optional.

The simplest smooth trajectory is the trapezoidal velocity profile: accelerate at constant acceleration $a_{max}$ to maximum speed $v_{max}$, cruise, then decelerate symmetrically.

The three phases:

where $d$ is the total displacement. For short moves where $d < v_{max}^2/a_{max}$, the profile becomes triangular — no cruise phase — with peak velocity $\sqrt{a_{max}\,d}$.

The trapezoidal profile gives bounded acceleration and a piecewise-constant jerk signal. It is the default in most industrial robot controllers and stepper motor drivers.

Trapezoidal profiles have an acceleration discontinuity at phase boundaries — the jerk (derivative of acceleration) is infinite at those corners. For precision applications — surgical robots, laser cutting, semiconductor wafer handling — this causes vibration.

Cubic polynomial $q(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3$ uses four boundary conditions: $q(0)$, $q(T)$, $\dot{q}(0)$, $\dot{q}(T)$. This guarantees continuous position and velocity — but acceleration is piecewise linear (infinite jerk at endpoints if $\dot{q}(0)=0$).

Quintic polynomial $q(t) = a_0 + \cdots + a_5 t^5$ adds $\ddot{q}(0)$ and $\ddot{q}(T)$ as boundary conditions (six total). Now position, velocity, AND acceleration are continuous — finite jerk everywhere.

$\begin{bmatrix}a_0 \\ \vdots \\ a_5\end{bmatrix} = \text{Vandermonde}^{-1} \begin{bmatrix}q_0 \\ q_f \\ \dot{q}_0 \\ \dot{q}_f \\ \ddot{q}_0 \\ \ddot{q}_f\end{bmatrix}$

For most robot arms, quintic splines between via-points give smooth, jerk-limited motion with manageable computational cost.

Jerk is $\dddot{q}$, the rate of change of acceleration. High jerk is the enemy of robot performance for three reasons:

1. Mechanical resonance: Sudden accelerations excite structural vibrations. A robot arm with jerk-limited profiles rings far less at end-effector, enabling tighter positional tolerances.
2. Actuator wear: Torque spikes (proportional to jerk × inertia) stress gearboxes and motor windings. Jerk limiting extends hardware life significantly.
3. Payload stability: A robot carrying a liquid container or fragile part will spill or break under high jerk even if peak forces look acceptable.

The standard industrial measure is jerk limit in units of (m/s³) or (°/s³). High-end robot controllers (Fanuc, KUKA) expose this as a user parameter. Setting it too low slows cycle time; too high causes vibration. The sweet spot is usually 3–10× the peak acceleration value in the same units.

Pure feedback tracks a moving reference but always lags behind — the controller can only respond after the error appears. For a fast trajectory, by the time the feedback corrects one error, the reference has moved and created a new one. The result is systematic tracking error proportional to trajectory speed.

The solution is feedforward + feedback:

$u(t) = \underbrace{K_p e(t) + K_i\!\int\! e\,dt + K_d\dot{e}}_{\text{feedback}} + \underbrace{K_{ff}\,\dot{r}(t)}_{\text{velocity feedforward}} + \underbrace{\hat{G}(q)}_{\text{gravity feedforward}}$

The feedforward term anticipates what the plant needs before the error builds up. The feedback term corrects whatever the feedforward missed. Together they give:

• Feedforward: handles the predictable dynamics, eliminates systematic lag.
• Feedback: handles disturbances, model errors, and any residual offset.

With good feedforward, the required feedback gains drop by 3-10×, improving noise rejection and stability margins.

Here is a complete trapezoidal profile generator. It handles the triangular case automatically:

import numpy as np

def trapezoidal_profile(q0, qf, v_max, a_max, dt=0.001):
    """Returns arrays of (time, position, velocity, acceleration)."""
    d = abs(qf - q0)
    sign = np.sign(qf - q0)

    t_a = v_max / a_max                  # acceleration time
    if 0.5 * a_max * t_a**2 * 2 > d:    # triangular profile
        t_a = np.sqrt(d / a_max)
        v_peak = a_max * t_a
        t_c = 0.0
    else:
        v_peak = v_max
        t_c = (d - a_max * t_a**2) / v_max

    t_total = 2 * t_a + t_c
    t = np.arange(0, t_total + dt, dt)
    q = np.zeros_like(t)
    v = np.zeros_like(t)
    acc = np.zeros_like(t)

    for i, ti in enumerate(t):
        if ti <= t_a:                       # acceleration phase
            acc[i] = sign * a_max
            v[i]   = sign * a_max * ti
            q[i]   = q0 + sign * 0.5 * a_max * ti**2
        elif ti <= t_a + t_c:              # cruise phase
            acc[i] = 0.0
            v[i]   = sign * v_peak
            q[i]   = q0 + sign * (0.5*a_max*t_a**2 + v_peak*(ti-t_a))
        else:                               # deceleration phase
            td = ti - t_a - t_c
            acc[i] = -sign * a_max
            v[i]   = sign * (v_peak - a_max * td)
            q[i]   = qf - sign * 0.5 * a_max * (t_total - ti)**2
    return t, q, v, acc

t, q, v, acc = trapezoidal_profile(0, 90, v_max=60, a_max=120)
print(f"Motion time: {t[-1]:.3f} s,  peak vel: {max(v):.1f} deg/s")

The returned q array is your reference trajectory; v feeds directly into the velocity feedforward term.

Tracking error is $e_{track}(t) = r(t) - y(t)$ during motion — distinct from steady-state error at rest. For a second-order closed-loop system tracking a ramp input at velocity $v$, the tracking error is approximately:

$e_{track} \approx \frac{v}{K_v}$

where $K_v$ is the velocity error constant of the loop. Higher loop gain → smaller $K_v$-based lag.

Sources of tracking error and how to fix them:

Professional motion controllers report RMS tracking error in real time — 0.01° is achievable on a direct-drive joint; 0.1–0.5° is typical with a gearbox.

For a mobile robot following a curved path, the problem extends to 2D. A common architecture is Pure Pursuit: the robot aims at a look-ahead point on the path at distance $L_d$ ahead of its current position. The steering angle command is:

$\delta = \arctan\left(\frac{2\,l\,\sin\alpha}{L_d}\right)$

where $l$ is the wheelbase and $\alpha$ is the angle to the look-ahead point. Larger $L_d$ → smoother tracking but larger cut-corners at high curvature. Smaller $L_d$ → tighter tracking but oscillation.

For a robot arm following a Cartesian path, the controller operates in task space: the desired end-effector position and orientation are the references, inverse kinematics converts them to joint targets, and per-joint PID (or state-space) controllers execute the motion.

In both cases — mobile robot or arm — the architecture is the same: trajectory generator provides $r(t)$, feedforward anticipates the motion, feedback corrects the error, and the system tracks within specification.