Skip to the lesson
FixTheRobot Explore Robotics / Navigation / M5 Trajectories and Path Following / M5-T2

Navigation · Theory · M5

M5-T2 — Smoothing: Shortcuts, Splines and Gradient Methods

A grid path is correct and undrivable. Smoothing makes it drivable without making it unsafe.

  • Lesson 2 of 7
  • Module 5: Trajectories and Path Following
  • Theory
  • About 9 minutes to read

Search on a grid returns a staircase: forty tiny steps and nineteen turns of 90 degrees where one diagonal would do. The route is right and the robot cannot drive it, or can only at a crawl. Smoothing replaces the staircase with something a real robot can follow, and the whole difficulty is doing so without cutting through a wall.

Learning objectives

After this lesson, you should be able to:

  • Explain why grid and graph paths need smoothing and what a smoothed path must still satisfy.
  • Apply greedy shortcutting and say what it cannot do.
  • Write the gradient smoothing update and say what its two terms do.
  • Compute the clearance lost to a rounded corner, and check a smoothed path against a minimum turning radius.

The intuition

Pull a loose string tight between two pegs and it becomes straight. Put a few posts between them and it wraps around the posts. Smoothing a path is the same: pull on it, and let the walls push back. Three families of method differ in how they pull.

Shortcutting replaces stretches of path with straight lines wherever a line is free. Splines fit a smooth curve through or near the waypoints. Gradient smoothing moves every waypoint a little towards the middle of its neighbours, over and over, with a second pull that stops the path from drifting far from where it began.

None of them knows about walls by themselves. A smoother that moves a waypoint can move it into an obstacle, so every method must be followed by, or built around, a collision check.

A staircase path, its shortcut and its smoothed versionA staircase path from lower left to upper right in orange, a straight shortcut in blue that grazes a wall block, and a smooth green curve that stays clear of the block.wall grid path: 19 corners of 90° shortcut: straight, but it touches the wall smoothed: clear of the wall, no sharp corner Each method keeps the start and the goal.
Three versions of one route: the grid path, a shortcut that grazes the wall block, and a smooth curve that stays clear.

The mathematics

Shortcutting

  1. Start at the first waypoint, i = 0.
  2. Find the farthest later waypoint j such that the straight line from i to j is free. Search from the goal backwards: the first free line is the farthest.
  3. Keep waypoint j, set i = j, and repeat until the goal is reached.

The test for a free line samples the segment at least every quarter of a cell, or rasterises it exactly (Bresenham). A path of n waypoints needs O(n²) tests in the worst case. Shortcutting never lengthens the path and keeps every segment free, but it leaves corners wherever the line grazes an obstacle, and it cannot move a waypoint away from a wall.

Splines

A cubic spline joins waypoints with cubic polynomials. For the Catmull-Rom spline the tangent at a waypoint is half the difference of its neighbours, and the curve passes through every waypoint. Halfway between P1 and P2 it passes through

Catmull-Rom midpoint
P(12)=−P0+9⋅P1+9⋅P2−P316P(\tfrac{1}{2})=\frac{-P0+9\cdot P1+9\cdot P2-P3}{16}

This curve is smooth in heading (G1) but its curvature can jump at the waypoints. A cubic B-spline is smoother (curvature continuous) but only approximates the waypoints, which can move it into a wall. A clothoid, whose curvature grows linearly with length, is the classic transition between a straight and a bend.

Gradient smoothing

Minimise a cost with two terms, by moving each waypoint a small step downhill:

Gradient smoothing
cost=wdata⋅Σ ∣pi−oi∣2+wsmooth⋅Σ ∣pi−1−2⋅pi+pi+1∣2\text{cost}=w_{\text{data}}\cdot \Sigma \,\lvert p_{i}-o_{i}\rvert ^{2}+w_{\text{smooth}}\cdot \Sigma \,\lvert p_{i-1}-2\cdot p_{i}+p_{i+1}\rvert ^{2}
pi←pi+wdata⋅(oi−pi)+wsmooth⋅(pi−1+pi+1−2⋅pi)p_{i}\leftarrow p_{i}+w_{\text{data}}\cdot (o_{i}-p_{i})+w_{\text{smooth}}\cdot (p_{i-1}+p_{i+1}-2\cdot p_{i})
oi
the original waypoint i, which the data term pulls back towards.
pi
the current position of waypoint i.
wdata
weight of fidelity to the original path; a larger value keeps the path where it was.
wsmooth
weight of smoothness; keep it below 0.5 for the update to be stable.

The first and last waypoint never move. Every accepted move is tested: the two segments next to the moved waypoint must stay free. Without the data term the path collapses towards a straight line between the end points, and the collision test is the only thing keeping it off the walls.

Is the result drivable?

A car-like robot with wheelbase L and steering limit δmax cannot turn tighter than Rmin = L / tan δmax. A smoothed path whose radius of curvature falls below that is smooth and still undrivable. Replacing a corner with deflection angle θ by an arc of radius r moves the path inside the corner by

Rounding a corner
inward shift=r⋅(1cos⁡(θ2)−1)\text{inward shift}=r\cdot (\frac{1}{\cos (\frac{\theta }{2})}-1)
tangent length=r⋅tan⁡(θ2)\text{tangent length}=r\cdot \tan (\frac{\theta }{2})

A worked example

The staircase. A grid path climbs 5 m east and 5 m north in ten steps of 0.5 m in each direction. It is 20 × 0.5 = 10.0 m long and has 19 corners of 90°, a total turning of 19 × π/2 = 29.8 rad. The straight line is √(5² + 5²) = 7.07 m, 29 % shorter (10 / 7.07 = 1.41), with no turning at all.

One smoothing sweep. Take five waypoints (0,0), (1,0), (1,1), (2,1), (2,2): three 90° corners, 270° of turning in 4.0 m of path. Use wdata = 0.1, wsmooth = 0.3 and update the interior points in order, using already updated neighbours:

One sweep
p1=(1, 0): x=1+0.1⋅0+0.3⋅(0+1−2)=0.70p1=(1,\,0):\,x=1+0.1\cdot 0+0.3\cdot (0+1-2)=0.70
y=0+0+0.3⋅(0+1−0)=0.30y=0+0+0.3\cdot (0+1-0)=0.30
p2=(1, 1): x=1+0+0.3⋅(0.70+2−2)=1.21p2=(1,\,1):\,x=1+0+0.3\cdot (0.70+2-2)=1.21
y=1+0.3⋅(0.30+1−2)=0.79y=1+0.3\cdot (0.30+1-2)=0.79
p3=(2, 1): x=2+0+0.3⋅(1.21+2−4)=1.763p3=(2,\,1):\,x=2+0+0.3\cdot (1.21+2-4)=1.763
y=1+0.3⋅(0.79+2−2)=1.237y=1+0.3\cdot (0.79+2-2)=1.237

After one sweep the path is 2.98 m long with 59° of turning, from 270°. Repeating the sweep tens of times gives the smooth curve of lab M5-L1, stage 3, where the total turning falls to a tenth.

The cost of rounding. A corridor turns 90° and the path goes through the corner point. Replace the corner by an arc of radius 1.5 m: the tangent length is 1.5 × tan 45° = 1.5 m, and the path moves inside the corner by 1.5 × (1/cos 45° − 1) = 0.62 m. If the corridor is 2 m wide and the path was in the middle, 0.62 m of the 1.0 m clearance to the inner wall is gone before the robot's own width is counted. For a tug with wheelbase 0.8 m and a 35° steering limit Rmin = 0.8 / tan 35° = 1.14 m, so a 1.5 m arc is drivable and a 0.8 m arc is not.

Engineering implications and limitations

Smooth, then check, then time

The order in a real stack is: plan, smooth, collision-check the smoothed path against the costmap, then give it a speed profile (lessons M5-T1 and M5-T3). Checking only the unsmoothed path is the commonest source of a robot clipping a corner it was told was free.

Check against the inflated map

The robot is a point on the inflated costmap (lesson M2-T2). The collision test must use that map, not the raw one, and the line test must not skip cells.

Smoothing changes length and time

A shorter path is usually faster, but a path rounded to a larger radius can be longer and slower at the same curvature limit. Judge smoothing by the time of the profiled trajectory, not by the path length alone.

Where it is used

Many planners smooth internally. Grid planners return paths that follow cell edges and often need a smoothing step (Nav2 provides a smoother server for this); lattice and hybrid planners return paths that are already drivable (lesson M4-T6). If your controller is cutting corners or wobbling, plot the path first: a jagged input is a planner problem (lesson M5-T7).

Common misconceptions

“Smoothing cannot break a collision-free path.”

It can. Every move of a waypoint can push a segment into an obstacle, which is why each move is tested.

“A smooth path is a drivable path.”

Only if its curvature stays within what the robot can do. A smooth path with a 0.5 m radius is undrivable for a 1.14 m minimum radius.

“Shortcutting finds the shortest path.”

It finds a shorter one among those that use the original waypoints. The shortest path may need a waypoint that is not on the grid path.

“More smoothing iterations are always better.”

Without a data term, enough iterations pull the path off the corridor and onto the walls.

Engineering takeaways

  1. Grid paths are staircases; smoothing makes them drivable, and must keep them collision-free and near the original.
  2. Shortcutting jumps to the farthest waypoint with a free line; it needs a line test that cannot skip cells.
  3. Gradient smoothing pulls each waypoint to its neighbours’ midpoint (wsmooth) and back to where it started (wdata), with the end points fixed.
  4. Rounding a corner of deflection θ with radius r moves the path inside by r·(1/cos(θ/2) − 1).
  5. Check the smoothed path against the map and against the robot’s minimum turning radius, then time it.

References and further reading

Shortcutting and smoothing in motion planning, and the optimisation-based smoothing used on the Stanley and Junior vehicles.

  • R. Geraerts and M. H. Overmars, “Creating High-Quality Paths for Motion Planning”, The International Journal of Robotics Research, 26(8), 2007.
  • D. Dolgov, S. Thrun, M. Montemerlo and J. Diebel, “Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments”, The International Journal of Robotics Research, 29(5), 2010.
  • S. Quinlan and O. Khatib, “Elastic Bands: Connecting Path Planning and Control”, IEEE International Conference on Robotics and Automation, 1993.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

Question 1

A grid path goes 3 m east and 3 m north in steps of 0.5 m. How long is it, and how much shorter is the straight line?

Explanation

Twelve steps of 0.5 m: 6.0 m. The straight line is √18 = 4.24 m, 1.76 m (29 %) shorter.

Question 2

In the gradient update, what does setting wdata = 0 do after many iterations, and why does the collision test still matter?

Explanation

Nothing holds the waypoints near the original path, so they drift towards a straight line between the fixed ends. The collision test is then the only thing stopping them from sliding into obstacles.

Question 3

A 90° corner is replaced by an arc of radius 1.0 m. How far inside the corner does the path move?

Explanation

1.0 × (1/cos 45° − 1) = 1.0 × 0.414 = 0.41 m.

What’s next

Next: how fast to drive along the smoothed path.

Practice this lesson

Smooth a Jagged Path

The grid path is full of tiny turns that make the cart rattle: smooth it without hitting a wall.

Next lesson

M5-T3 — Curvature, Velocity and Acceleration Limits

Build a speed profile that respects curvature, acceleration and braking limits.

Nothing below is required to finish this lesson.