Trajectory Optimization

Instead of searching a graph, trajectory optimization starts with any path, even one that cuts straight through obstacles, and improves it step by step. Each waypoint is nudged to make the path smoother and to push it away from obstacles, until the cost stops dropping.

Smoothness weight Obstacle weight Safety margin Waypoints

The cost

For waypoints $\mathbf{x}_1, \dots, \mathbf{x}_n$ with fixed ends, the cost adds a smoothness term, the squared second differences, and an obstacle term that grows as each waypoint's clearance $d$ falls below a margin $\epsilon$:

$$J = w_s \sum_i \lVert \mathbf{x}_{i-1} - 2\mathbf{x}_i + \mathbf{x}_{i+1}\rVert^2 + w_o \sum_i \max(0,\ \epsilon - d(\mathbf{x}_i))^2$$

Gradient descent moves every waypoint downhill on $J$ at once. CHOMP uses a smarter version of this step, and TrajOpt solves it as a sequence of convex problems.

What to notice

The straight start passes right through obstacles, yet after a few hundred steps the path has bent around them. Which side it goes around depends on where it started: optimization finds a nearby good path, not the best one overall, so it can get stuck in a narrow gap or on the wrong side. That is why it is often seeded with a path from a sampling planner. With too little obstacle weight, smoothness wins and the path cuts corners.