Trajectory Planning

From path to trajectory

A path is purely geometric — the ordered set of configurations the robot passes through, with no clock attached. A trajectory is that path plus a time law q(t), and from the time law everything else follows by differentiation: velocity q̇(t), acceleration q̈(t), and jerk q⃛(t). The planner's job is to choose q(t) so that the motion is fast, smooth, and inside the machine's physical envelope.

Given:  start q0, goal q1, distance D = q1 − q0
Limits: v_max (speed), a_max (accel), j_max (jerk)

Find a time law q(t), t ∈ [0, T], with:
   q(0) = q0,  q(T) = q1
   |q̇(t)|  ≤ v_max     (motor / encoder limit)
   |q̈(t)|  ≤ a_max     (torque limit:  T = J·q̈)
   |q⃛(t)| ≤ j_max     (vibration / shock limit)
   minimize T  (or track a fixed sample clock)

Why these three limits? Speed is capped by the motor's rated rpm and the encoder's countable rate. Acceleration is capped by torque, because for a rotary inertia J the torque is T = J·q̈ — demanding more acceleration than the drive can deliver simply saturates the amplifier. Jerk is capped because torque proportional to acceleration means a step in acceleration is a step in torque, which the current loop cannot produce instantly and which rings every lightly-damped resonance in the belts, couplings and structure.

The trapezoidal velocity profile

The simplest practical profile accelerates at a constant a_max, cruises at v_max, then decelerates at −a_max. Plotted against time, velocity traces a trapezoid; position is the area under it. The move has two regimes:

Distance to reach v_max from rest:  d_ramp = v_max² / (2·a_max)

If  2·d_ramp ≤ D   →   trapezoidal (a cruise phase exists)
    t_a = v_max / a_max                       (accel time)
    d_cruise = D − v_max² / a_max
    t_c = d_cruise / v_max                     (cruise time)
    T   = 2·t_a + t_c

If  2·d_ramp > D    →   triangular (never reaches v_max)
    v_peak = √(a_max · D)
    T      = 2 · √(D / a_max)

Worked example. A SCARA joint must rotate D = 1.5 rad with v_max = 4 rad/s and a_max = 30 rad/s². The ramp distance is 4²/(2·30) = 0.267 rad, so 2·d_ramp = 0.533 rad < 1.5 rad — the move is trapezoidal. Accel time t_a = 4/30 = 0.133 s, cruise distance = 1.5 − 16/30 = 0.967 rad, cruise time = 0.967/4 = 0.242 s, and total T = 2(0.133) + 0.242 = 0.508 s. The same joint asked to move only 0.3 rad would never reach 4 rad/s: it is triangular, peaks at √(30·0.3) = 3.0 rad/s, and finishes in 2·√(0.3/30) = 0.20 s.

S-curve: bounding jerk

The trapezoidal profile's flaw is at the corners. Acceleration jumps from 0 to +a_max instantly, then from +a_max to −a_max at the top of the trapezoid — infinite jerk at each kink. The motor torque mirrors that step exactly, and the mechanism answers with an audible knock and residual ringing that the servo must wait out before the next move.

The S-curve (also called seven-segment, or double-S) fixes this by limiting jerk to a finite j_max. Acceleration now ramps linearly up and down, so the velocity curve gains rounded shoulders shaped like an "S". A full S-curve has seven phases: jerk up, constant accel, jerk down, cruise, jerk down (to decelerate), constant decel, jerk up (to zero). When the move is short, phases drop out — the cruise vanishes first, then the constant-accel segments, leaving a pure jerk-limited "mini-S".

Seven-segment S-curve phases (each bounded by a_max, j_max):
  1. q⃛ = +j_max   accel ramps  0 → a_max
  2. q⃛ = 0        accel held at a_max
  3. q⃛ = −j_max   accel ramps  a_max → 0   (top speed reached)
  4. q⃛ = 0        cruise at v_max
  5. q⃛ = −j_max   accel ramps  0 → −a_max
  6. q⃛ = 0        decel held at −a_max
  7. q⃛ = +j_max   accel ramps  −a_max → 0  (stop)

Extra time vs. trapezoid ≈ a_max / j_max  (per ramp pair)

The price is roughly a_max/j_max of extra duration per acceleration ramp. With a_max = 30 rad/s² and a generous j_max = 600 rad/s³, that is 0.05 s per ramp — about 0.1 s added to the 0.5 s move above. In return, settling time after the move frequently drops by more than that, so the net cycle time often improves while the machine runs quieter and the gears live longer.

Profile comparison

	Trapezoidal	S-curve (7-seg)	Cubic polynomial	Quintic polynomial	Minimum-time (bang-bang)
Bounds velocity	Yes (flat top)	Yes (flat top)	Peak only	Peak only	Yes
Bounds acceleration	Yes	Yes	Peak only	Peak only	Yes (saturated)
Bounds jerk	No (∞ at kinks)	Yes	No (∞ at ends)	Yes (smooth)	No (∞)
Continuity	C¹ (vel)	C² (accel)	C¹	C² and beyond	C⁰
Move time	Near minimum	Slightly longer	Longer	Longest	Theoretical min
Residual vibration	Noticeable	Low	Moderate	Very low	Severe
Typical use	CNC rapid, basic PLC moves	Pick-and-place, elevators, packaging	Smooth point-to-point	Vibration-sensitive optics, wafer	Textbook only; rarely real

Polynomial trajectories

Instead of stitching constant-jerk segments, you can fit a single polynomial that meets the boundary conditions. A cubic q(t) = a₀ + a₁t + a₂t² + a₃t³ has four coefficients — exactly enough to match start/end position and start/end velocity (typically zero at both ends). Its acceleration is linear and finite, but it has a velocity step problem only at the endpoints if you chain moves.

A quintic q(t) of degree five carries six coefficients, enough to also pin start and end acceleration to zero — giving continuous acceleration (C²) and therefore finite jerk throughout. Quintics are the go-to for vibration-critical motion such as optical scanners and lithography stages, where the cost of a slightly longer move is trivial next to the cost of a smeared exposure.

Quintic with rest-to-rest boundary conditions (D over time T):
  q(t) = q0 + D·[ 10(t/T)³ − 15(t/T)⁴ + 6(t/T)⁵ ]
  q̇(t)  = (D/T)·[ 30τ² − 60τ³ + 30τ⁴ ]        peaks at  1.875·D/T
  q̈(t)  = (D/T²)·[ 60τ − 180τ² + 120τ³ ]       peaks at  5.77·D/T²
  where τ = t/T,  and q̇(0)=q̇(T)=q̈(0)=q̈(T)=0

Note the peak-velocity factor of 1.875: a quintic rest-to-rest move reaches a peak speed almost twice the average speed D/T. Size T so that 1.875·D/T stays under v_max, and 5.77·D/T² stays under a_max, or the drive will saturate mid-move.

Multi-segment and blended trajectories

• Via points. Real tasks pass through several intermediate poses. Stopping at each one (a sequence of rest-to-rest moves) is simple but slow.
• Blending. Instead of stopping, the planner rounds the corner between segments with a parabolic or jerk-limited blend, trading a small path deviation (the "corner radius" or CNC look-ahead tolerance) for continuous motion.
• Linear-segment-with-parabolic-blends (LSPB). The classic industrial scheme: straight constant-velocity legs joined by parabolic blends — effectively a multi-point trapezoidal profile.
• Spline interpolation. Cubic or B-splines through the via points give C² paths; the time law is then layered on top by reparameterizing arc length.
• Look-ahead. CNC and printer firmware buffer dozens of upcoming segments so they can pre-plan deceleration into sharp corners and keep feedrate high on gentle ones.

Time scaling: the universal fix

Suppose a planned trajectory turns out to violate a limit — perhaps an inverse-kinematics solution demands a wrist velocity above its rating. Time scaling replaces the time variable t with s·t for a single scalar s > 1, stretching the whole motion uniformly. Position is unchanged at every point along the path, but velocity scales by 1/s, acceleration by 1/s², and jerk by 1/s³. Pick the smallest s that brings every quantity under its limit and you have the fastest feasible version of the exact same path — no replanning of geometry required. This is why path planning and trajectory planning are kept separate: the geometry can be solved once, then time-scaled freely to fit whatever hardware runs it.

Failure modes and trade-offs

• Velocity/acceleration saturation. Commanding more than the drive can deliver makes actual motion lag the command; following error grows until the controller trips a position-error fault or overshoots the goal. Size the profile to the limits up front.
• Residual vibration from infinite jerk. Trapezoidal corners excite structural resonances; the arm keeps ringing after it "arrives", forcing a settling delay before the next operation. Bound jerk with an S-curve or quintic.
• Sampling artifacts. A continuous profile must be discretized at the controller rate (often 1 kHz). Too coarse a sample injects high-frequency content of its own; round-off in the accumulator can leave a sub-count position error at the goal.
• Over-smoothing. Crank jerk limits too low and cycle time balloons — a packaging line that should run 60 cycles/min drops to 40 for no measurable vibration benefit. Tune j_max to the actual resonance, not to zero.
• Corner deviation in blends. Blending for speed pulls the path inside sharp corners; on a CNC mill that is a dimensional error, so the tolerance band must be set against the part's spec.
• Inertia variation. A robot's effective joint inertia changes with configuration and payload, so a profile tuned for one pose may saturate torque in another. Worst-case sizing or online adaptation is needed for arms that carry heavy, variable loads.