Trajectory Planning — Trapezoidal Profiles & Quintic Polynomials

Trajectory Planning — Trapezoidal Profiles & Quintic Polynomials

1. Trapezoidal Velocity Profile (Bang-Coasting-Bang)

Given: start position $q_0$, end position $q_f$, maximum velocity $V$, maximum acceleration $A$. The profile has three phases:

1. Acceleration: $0 \leq t \leq t_1$, acceleration = $+A$, $v(t) = At$, position $q(t) = q_0 + \frac{1}{2}At^2$.
2. Constant velocity: $t_1 \leq t \leq t_2$, $v = V$.
3. Deceleration: $t_2 \leq t \leq T$, acceleration = $-A$.

Blend time: $t_1 = V/A$. If distance $d = |q_f - q_0| < V^2/A$, the profile is triangular (no constant-velocity phase):

$$T_{tri} = 2\sqrt{d/A}, \quad V_{peak} = \sqrt{A \cdot d}$$

Otherwise (trapezoidal):

$$t_1 = V/A, \quad T_{trap} = d/V + V/A$$

2. Quintic Polynomial — Jerk-Limited Trajectory

A 5th-degree polynomial $q(t)$ satisfies six boundary conditions: $q(0)=q_0$, $\dot{q}(0)=v_0$, $\ddot{q}(0)=a_0$, $q(T)=q_f$, $\dot{q}(T)=v_f$, $\ddot{q}(T)=a_f$. Normalising $\tau = t/T$:

$$q(\tau) = q_0 + (q_f-q_0)\left(10\tau^3 - 15\tau^4 + 6\tau^5\right)$$

for the rest-to-rest case ($v_0=v_f=a_0=a_f=0$). Peak acceleration occurs at $\tau = 0.5$:

$$|\ddot{q}|_{max} = \frac{30(q_f-q_0)}{4T^2} \cdot 3 \cdot \frac{1}{4} = \frac{30|\Delta q|}{4T^2} \cdot \frac{3}{4}$$

Wait — more precisely: $\ddot{q}(\tau) = (q_f-q_0)(60\tau - 120\tau^2 + 60\tau^3)/T^2$. Setting $d\ddot{q}/d\tau = 0$ gives $\tau = (1 \pm 1/\sqrt{3})/2$; substituting gives $|\ddot{q}|_{max} = 5.77|\Delta q|/T^2$.

The minimum trajectory time to respect $|\ddot{q}| \leq a_{max}$:

$$T_{min} = \sqrt{\frac{5.77|\Delta q|}{a_{max}}}$$

3. Cartesian vs Joint-Space Planning

Joint-space planning (planning $\theta_i(t)$ for each joint independently) is computationally simple but the Cartesian path is non-linear — the end-effector traces a curved arc, not a straight line. Cartesian planning requires inverting the kinematics at every time step (expensive) but guarantees straight-line motion needed for welding, painting, or gluing. FANUC and ABB robots implement both modes; Cartesian mode selects the IK solution closest to the current configuration at each step.