explainer
Path smoothing: shorten a route with checked shortcuts
Shorten a robot path by removing unnecessary waypoints while checking every replacement segment for collision. Compare length and clearance, trace accepted and rejected shortcuts, and separate a simpler path from smooth robot motion.
What you will learn
- Compare a polyline section's length with a proposed straight shortcut.
- Check the whole shortcut against inflated obstacles and room walls.
- Trace a deterministic sequence of accepted and rejected replacements.
- Explain why shorter paths can have less obstacle clearance.
- Distinguish waypoint removal from continuous tangent direction and executable timing.
Before you start
A planner can find a valid route that takes unnecessary bends. Path shortcutting removes some of those bends by testing direct connections between waypoints.
The key rule is simple: each replacement must remain collision-free and reduce the chosen cost. This lesson uses geometric length and a disk robot, so you can calculate both checks by hand.
Start with a valid path
Use a 0.2 m radius disk inside a 6 m square. A circular obstacle sits at (3, 3) m with radius 1 m. The robot starts at (0.6, 3) m and must finish at (5.4, 3) m.
The robot center must avoid the obstacle inflated to radius 1.2 m. It must also stay more than 0.2 m from every wall. Contact counts as collision, following the configuration-space model.
Start with this explicit path. Each neighboring pair joins with a straight segment:
| Waypoint | Center (x, y), in meters |
|---|---|
| 0 | (0.6, 3.0) |
| 1 | (0.6, 4.6) |
| 2 | (1.8, 4.8) |
| 3 | (3.0, 4.6) |
| 4 | (4.2, 4.8) |
| 5 | (5.4, 4.6) |
| 6 | (5.4, 3.0) |
Every segment clears the obstacle and walls. The path has seven waypoints, length 8.066210 m, and minimum robot-surface clearance 0.400000 m. These hand-chosen points make the arithmetic repeatable.
A planner such as RRT can supply an initial route in the same format. Shortcutting starts after that route passes its validity checks.
Propose a shorter connection
Choose two nonadjacent retained waypoints, qᵢ and qⱼ. The current path travels through the waypoints between them. A candidate shortcut connects qᵢ directly to qⱼ.
Compare the lengths:
L_old = Σ ‖qₖ₊₁ − qₖ‖₂, for k = i, …, j − 1
L_new = ‖qⱼ − qᵢ‖₂
saving = L_old − L_new
In this formula, i and j index the current ordered list. The diagram keeps the original waypoint labels after removals.
The triangle inequality makes the straight connection no longer than the original section. It can have equal length, as when all points lie in order on one straight segment. This experiment accepts only a strict reduction, so it leaves that equal-length case unchanged.
On acceptance, remove the intermediate waypoints. Keep both shortcut endpoints and every waypoint outside the replaced section. The overall start and goal therefore remain fixed.
Check the full replacement motion
A shorter segment can cross an obstacle. Use the closest-point collision test to check its entire length, including the space between its endpoints.
For segment a to b and obstacle center c, set d = b − a. For d ≠ 0:
τ = clamp(((c − a) · d)/(d · d), 0, 1)
clearance_obstacle = ‖a + τd − c‖₂ − 1.2 m
For identical endpoints, check that single center. For each wall, the nearest segment point occurs at an endpoint because the coordinate changes linearly. The smallest of all obstacle and wall clearances gives the candidate's minimum clearance.
The implementation requires clearance greater than 10⁻¹⁰ m and length saving greater than 10⁻¹⁰ m. These separate thresholds handle floating-point comparisons. A physical clearance margin would require an additional design choice.
Reject the tempting straight route
The first candidate connects waypoint 0 directly to waypoint 6. Its length is 4.8 m, which could save 3.266210 m.
That segment runs along y = 3 through the obstacle center. Its closest fraction is τ = 0.5, giving center distance 0 and clearance −1.2 m. Reject it and retain the complete original path.
A negative clearance reports overlap. The start and goal themselves are clear, so checking only those endpoints would accept the wrong motion. The candidate's possible saving becomes an actual saving only after its collision check passes.
Calculate an accepted shortcut
The fourth candidate connects waypoint 0 to waypoint 3. Its old section passes through waypoints 1 and 2:
L_old = 1.6 + 2√1.48 ≈ 4.033105 m
L_new = √(2.4² + 1.6²) ≈ 2.884441 m
saving ≈ 1.148664 m
Here a = (0.6, 3), b = (3, 4.6), and d = (2.4, 1.6) m. The closest fraction is τ = 5.76/8.32 = 9/13. The closest center is approximately (2.261538, 4.107692) m.
Its distance from the obstacle center is 1.331280 m, giving clearance 0.131280 m. The wall clearance is at least 0.4 m, so the obstacle sets the minimum. The shortcut passes both checks and removes waypoints 1 and 2.
The new whole-path length is 6.917546 m. Later, the matching shortcut from waypoint 3 to waypoint 6 removes waypoints 4 and 5. The final retained path is 0 → 3 → 6, with length 5.768882 m.
Choose candidates in a repeatable order
The experiment uses a fixed scan:
- Start at the earliest retained waypoint that has a nonadjacent successor.
- Try later waypoints from the farthest path index to the nearest nonadjacent index.
- Keep scanning after a rejection.
- After an acceptance, restart the scan on the shorter waypoint list.
- Stop after one full scan accepts nothing.
Numbers always refer to the original waypoints, so removing a point never renames another. The room's first four candidates are 0 → 6, 0 → 5, 0 → 4, and 0 → 3. The first three collide; the fourth succeeds.
Each acceptance removes at least one waypoint, so this finite process must stop. Its stopping rule means none of the remaining vertex pairs yields an accepted shortcut under these checks. It does not search every possible curve or move a retained waypoint to a new position.
OMPL's PathSimplifier includes vertex reduction and methods that also sample points along segments. This lesson implements the simpler vertex-only rule with an explicit scan order.
Step through the decisions
The initial view shows the first rejected shortcut. Step forward to attempt 4 to see the first accepted replacement. Finish the scan to inspect all nine attempts, including the final rejected connection through the obstacle.
Choose Open zigzag to remove the obstacle while retaining the same initial points. One checked start-to-goal shortcut leaves two waypoints and length 4.8 m. Choose Already straight to see a collision-free candidate fail the strict-improvement test.
The gray dashed line preserves the original route. The blue line shows the retained route after the selected decision. Use the slider to revisit an earlier attempt and compare its proposed saving with the actual change in path length.
Compare distance with clearance
The room path saves 2.297328 m, but its minimum clearance falls from 0.4 m to 0.131280 m. It remains valid under the stated model. Maximizing clearance would be a different objective from minimizing length.
The final three-waypoint route still has room for improvement. Move its middle point from (3, 4.6) to (3, 4.5) m. The two new segments remain clear and shorten the total length to 5.660389 m.
The current algorithm cannot make that change because it only removes waypoints. A completed scan therefore gives no global shortest-path guarantee. The route also depends on the starting polyline and candidate order.
RRT* revises connections while planning. Shortcutting revises an already available path. Either method needs a cost model that matches the intended result; a shorter geometric path alone says nothing about energy use or travel time.
Separate fewer waypoints from smooth motion
The returned path is piecewise linear. Its straight segments still meet at corners. Removing waypoints does not guarantee C¹ continuity, meaning a continuous first derivative, or continuous tangent direction.
At a remaining corner, a robot moving at nonzero speed would need an instant change in velocity direction. Finite acceleration limits rule that out. A motion system can stop at the corner or replace it with a curve, then check the new geometry and timing.
Moving or averaging waypoints changes the route. Every new connecting segment or curve needs its own validity check. OMPL treats B-spline smoothing as a separate operation and checks adjacent motions before accepting a moved state in its PathSimplifier implementation.
A forward-only vehicle with a minimum turning radius needs connections that respect both endpoint headings. Dubins paths construct those connections from circular arcs and straight segments in an obstacle-free plane. A route through obstacles still needs collision checks.
LaValle's planning decomposition separates geometric path construction, motion constraints, timing, and tracking. Continue with trajectory time scaling to see how a clock sets velocity and acceleration along an appropriate path.
Reproduce the shortcuts in Python
This Python 3 example uses only the standard library. It performs the same vertex scan and continuous disk checks. The output reports the initial and final paths for all three presets.
from math import hypot
RADIUS, SIZE, EPS = 0.2, 6.0, 1e-10
DETOUR = [(0.6, 3), (0.6, 4.6), (1.8, 4.8), (3, 4.6),
(4.2, 4.8), (5.4, 4.6), (5.4, 3)]
def distance(a, b):
return hypot(b[0] - a[0], b[1] - a[1])
def length(points):
return sum(distance(a, b) for a, b in zip(points, points[1:]))
def clearance(a, b, obstacles):
result = min(min(v - RADIUS, SIZE - RADIUS - v)
for point in (a, b) for v in point)
dx, dy = b[0] - a[0], b[1] - a[1]
norm2 = dx * dx + dy * dy
for center, radius in obstacles:
t = 0 if norm2 == 0 else max(0, min(1,
((center[0] - a[0]) * dx + (center[1] - a[1]) * dy) / norm2))
closest = (a[0] + t * dx, a[1] + t * dy)
result = min(result, distance(closest, center) - radius - RADIUS)
return result
def simplify(points, obstacles):
assert all(clearance(a, b, obstacles) > EPS
for a, b in zip(points, points[1:]))
path = list(range(len(points)))
attempts = accepted = 0
while True:
changed = False
for i in range(len(path) - 2):
for j in range(len(path) - 1, i + 1, -1):
attempts += 1
a, b = points[path[i]], points[path[j]]
old = length([points[k] for k in path[i:j + 1]])
if clearance(a, b, obstacles) > EPS and old - distance(a, b) > EPS:
path = path[:i + 1] + path[j:]
accepted += 1
changed = True
break
if changed:
break
if not changed:
return path, attempts, accepted
for name, points, obstacles in [
("room", DETOUR, [((3, 3), 1)]),
("open", DETOUR, []),
("straight", [(0.6, 3), (3, 3), (5.4, 3)], []),
]:
path, attempts, accepted = simplify(points, obstacles)
final = [points[index] for index in path]
gap = min(clearance(a, b, obstacles) for a, b in zip(final, final[1:]))
print(f"{name}: {length(points):.6f} -> {length(final):.6f} m; "
f"path={path}; attempts={attempts}; accepted={accepted}; clearance={gap:.6f} m")
Expected output:
room: 8.066210 -> 5.768882 m; path=[0, 3, 6]; attempts=9; accepted=2; clearance=0.131280 m
open: 8.066210 -> 4.800000 m; path=[0, 6]; attempts=1; accepted=1; clearance=0.400000 m
straight: 4.800000 -> 4.800000 m; path=[0, 1, 2]; attempts=1; accepted=0; clearance=0.400000 m
Try it yourself
1. Test a better middle point. Use the room's start and goal, but replace the final middle waypoint with (3, 4.5) m. Calculate the whole-path length and minimum obstacle clearance. Does the result establish a globally shortest path?
Show the changed-waypoint calculation
Each segment has length √(2.4² + 1.5²) = √8.01 ≈ 2.830194 m. The total is 5.660389 m, approximately 0.108493 m shorter than the widget's final route.
The closest point falls inside each segment. Its distance from the obstacle center is 2.4 × 1.5 / √8.01 ≈ 1.271997 m, giving clearance 0.071997 m after subtracting 1.2 m. Wall clearance remains at least 0.4 m, so both segments pass the motion check.
This example proves that the widget's completed result can improve by moving a waypoint. It does not prove that the newly chosen point or this two-segment path is globally optimal.
2. Explain a harmless rejection. In Already straight, connect waypoint 0 directly to waypoint 2. Why does the algorithm keep the middle waypoint despite positive clearance? What would changing the acceptance rule need to account for?
Show the equal-length decision
The original two segments and the candidate both have length 4.8 m. The saving is zero, so the strict-improvement rule rejects the candidate. Its minimum clearance is 0.4 m; collision does not cause this rejection.
A separate cleanup rule could remove a redundant collinear waypoint while preserving the same geometry. It would need to preserve any waypoint meaning, such as a required stop or task event, beyond the geometry represented here.
Sources and further study
- OMPL: PathSimplifier class reference. Vertex reduction, shortcuts along segments, and a separate B-spline smoothing operation.
- OMPL: PathSimplifier implementation. Motion checks before accepting path changes.
- Steven LaValle, Planning Algorithms: decoupling motion planning. The roles of geometric paths, motion constraints, timing, and tracking.
Next, choose timing for the route and check what its remaining corners require from the robot.