Optimal Path Smoothing while Maintaining a Region of Safe Operation

Multi-vehicle control scenarios often require controlling vehicles to move together in coordinated formations. A virtual structure formation control approach is taken to generate collision-free trajectories. These trajectories are refined within constraints imposed by the structure to create collision-free, executable trajectories. Trajectory refinements are made by taking advantage of the differential flatness of wheeled vehicle trajectories. A decoupled, linear quadratic optimal control formulation is presented with constraints defined to respect the nonlinear dynamics of a wheeled vehicle. An example is provided to understand the computational trade-offs of the various proposed constraints.