Optimal Planning for Vehicles with Bounded Curvature: Coordinated Vehicles and Obstacle Avoidance Andy Perrin

Two Problems Optimal Planning for multiple vehicles. Paper by Antonio Bicci and Lucia Pallottino Optimal Planning for obstacle avoidance. My variation (so to speak) on the above paper.

Dubin’s Problem Given an initial position and direction, and a final position and direction, find the shortest path from one to the other, given that the curvature is bounded.

Dubins’ Solution All Dubins’ paths consist of circular arcs and straight lines. Arcs will have the minimum curvature (which is specified in the problem statement) For least time problems, speed = max speed

Multiple Vehicles Consider a Dubins’-type problem with n vehicles, and require that they not collide. If we wish to minimize the overall time, what paths should they take, and how should we schedule them?

Problem Statement

Problem Statement R1 1 d13 3 d12 R3 d23 2 R2

Problem Restatement Can re-write this as an example of Pontryagin’s Minimization Principle. (PMP) Authors do this, find a Hamiltonian, take variations, and find necessary conditions for stationary solutions. Solutions consist of unconstrained arcs, and constrained arcs.

Case of no collisions (ie unconstrained arcs only) Simply calculate the Dubins’ path for each vehicle independently.

Case of some collisions Authors decompose the possible types of collisions into several categories of constrained arcs, and find the solution for each category. The arcs are then stitched together with some matching conditions. Dubins’ paths are extremely fast to calculate!

Advantages + Disadvantages Very Fast Global optimum found Smooth paths Disadvantages: No obvious way to include obstacles # of sub-problems increases combinatorially with # of vehicles Vehicles can’t go backwards (but others have worked on this) Feed-forward

Optimal Planning with Obstacles I concentrated on modeling a single vehicle with arbitrary obstacles. But, my method extends naturally to the n-vehicle case.

Parametize then Optimize! Given a starting and an ending point, I divide the path into n-1 pieces. Velocity is assumed constant on each piece. I add up the total time T, and minimize with fmincon, subject to some constraints…

Problem Statement

An Example without Obstacles

Another one…

How to Represent Obstacles? Obstacles are simply contour surfaces (“blobbies” or “metaballs”), analogous to the sliding surfaces we talked about earlier in the semester. Typically, sums of Gaussian distributions are used, but this is not necessary.

Some Obstacle Pictures

And a Small Problem… Local Minima!

Problems and Future Improvements Number of design variables increases quickly Local minima Speed is slow compared to Bicci paper. Possible Improvements Change from using line segments to cubic splines (Hermite polynomials) Multiple vehicles