Skip to content
Tutorials Center
My learning

Topics

Lesson 24 of 38 ยท Path Planning

Trajectory Generation and Path Smoothing

Turn a jagged path into a smooth, timed trajectory with shortcutting, cubic and quintic polynomials and trapezoidal velocity profiles, in runnable Python.

Planners return paths: ordered waypoints with sharp corners and no notion of time. A real motor cannot turn a corner instantly or jump from rest to full speed. If you feed it a jagged path with an abrupt velocity change, you get vibration, wheel slip, jerky arms, and worn gearboxes.

Trajectory generation fixes this. It produces a function q(t) that gives the position at every moment, with limits on velocity, acceleration, and ideally jerk. In this lesson you will smooth a path, then build the time law three ways: a cubic polynomial, a quintic polynomial, and a trapezoidal velocity profile.

Step 1: shorten the path

Planners such as A* and RRT can return staircases. Shortcutting removes unnecessary corners: for a waypoint i, look ahead for the farthest waypoint j that you can reach in a straight collision-free line, then skip everything between.

Python Line-of-sight shortcutting

Press Run (or Ctrl + Enter) to see the result here.

Program input ยท what you'd type while the program runs

Type the answers before you press Run, one per line. Each time the program reads a line (with input()), it gets the next line from this box.

Shortcutting never makes the path longer and keeps it collision free, because each skipped segment was checked.

Step 2: polynomial time laws

Between two waypoints we want a smooth function. A polynomial in time is the easiest building block: you choose the boundary conditions (position, velocity, acceleration at start and end) and solve for the coefficients.

Cubic (4 coefficients) fixes position and velocity at both ends:

q(t) = a0 + a1 t + a2 t^2 + a3 t^3

With start q0, end q1, zero start and end velocity, and duration T: a0 = q0, a1 = 0, a2 = 3 (q1 - q0) / T^2, a3 = -2 (q1 - q0) / T^3. Velocity is continuous, but acceleration jumps at the ends.

Quintic (6 coefficients) also fixes acceleration at both ends, so acceleration is continuous and the motion is gentler on the hardware. You solve a 6 by 6 linear system, which numpy does in one call.

Python Cubic and quintic point-to-point motion

Press Run (or Ctrl + Enter) to see the result here.

Program input ยท what you'd type while the program runs

Type the answers before you press Run, one per line. Each time the program reads a line (with input()), it gets the next line from this box.

Compare the acceleration columns. The cubic starts and ends with a nonzero acceleration (an instant torque jump), while the quintic eases to zero. For a path through many waypoints, cubic splines or B-splines join the segments so position and velocity stay continuous across every waypoint.

Step 3: trapezoidal velocity profile

Motors have a maximum speed v_max and acceleration a_max. The classic trapezoidal profile respects both: accelerate at a_max until v_max, cruise, then decelerate. The velocity-versus-time plot is a trapezoid.

flowchart LR
  A["Rest"] --> B["Accelerate at a_max"]
  B --> C["Cruise at v_max"]
  C --> D["Decelerate at a_max"]
  D --> E["Rest at the goal"]

If the distance is too short to reach v_max, the profile becomes a triangle: accelerate, then immediately decelerate.

Python Trapezoidal velocity profile

Press Run (or Ctrl + Enter) to see the result here.

Program input ยท what you'd type while the program runs

Type the answers before you press Run, one per line. Each time the program reads a line (with input()), it gets the next line from this box.

For the long move the speed rises, holds at 2.0, and falls; for the short one it never reaches 2.0. The same logic, one axis at a time, drives CNC machines and many robot joints. Acceleration still jumps at the corners of the trapezoid, so for smoother motion an S-curve profile limits jerk (the derivative of acceleration) too.

Putting it together

A common pipeline for a mobile or arm robot:

  1. Plan a collision-free path (A*, RRT).
  2. Shortcut it and optionally fit a spline through the remaining waypoints.
  3. Time-parameterize: choose speeds so the limits v_max, a_max (and for arms the torque limits) are respected. For paths, time-optimal path parameterization (TOPP) finds the fastest feasible timing.
  4. Sample the trajectory at the controller rate (for example 100 to 1000 Hz) and hand each setpoint to the motor controller.

Smoothing can push the path into an obstacle. After fitting a spline or rounding corners, always check the new path for collisions again, and keep a safety margin when planning.

Check your understanding

Quick check ยท 4 questions

of 4 answered You scored / 4 ยท perfect!

  1. 1. What is the difference between a path and a trajectory?

    A path is geometry only. A trajectory assigns a time to every point.
  2. 2. Which boundary condition does a quintic polynomial add compared with a cubic?

    A quintic has six coefficients so it can fix position, velocity and acceleration at both ends.
  3. 3. In a trapezoidal velocity profile, when does the profile become a triangle?

    If accelerating to v_max and braking would need more distance than the move, the robot peaks lower and brakes at once.
  4. 4. Why recheck collisions after smoothing a path?

    Rounded or fitted curves leave the original polyline, so they must be validated against obstacles again.

Summary

  • Paths need shortcutting, smoothing, and timing before a motor can follow them.
  • Line-of-sight shortcutting removes unnecessary waypoints without causing collisions.
  • Cubic polynomials fix position and velocity; quintics also fix acceleration; splines join many segments.
  • Trapezoidal (and S-curve) profiles respect speed, acceleration and jerk limits.
  • Always revalidate the smoothed trajectory and sample it at the controller's rate.

Next: Probability and the Bayes Filter