Computational receding horizon approach to safe trajectory tracking

This paper presents a receding horizon methodology for trajectory tracking with safe collision conflict resolution for multiple autonomous vehicles. The proposed decentralized scheme is formulated in discrete-time domain where each vehicle's objective function represents deviations from its desired trajectory. The safety constraints penalizing if two or more vehicles get closer than a prescribed safety distance are incorporated using avoidance functions. These avoidance functions are added to the objective functions to be minimized by each vehicle. They also represent the coupling elements in the decentralized scheme allowing for implicit coordination among the vehicles in the case of a possible collision. Vehicles are modeled by the unicycle model subject to bounds on both velocity and angular velocity. The optimization scheme performed by each vehicle uses a sequential quadratic programming method which is well suited for minimization of a scalar nonlinear function subject to nonlinear equality constraints (dynamic model) and multiple inequality constraints (velocity constraints and safety conditions). Outputs of the optimization process are kinematic control inputs for each vehicle. For symmetric cases where the gradient based methods are known to perform poorly (such as singular cases) a limit cycle method is implemented to modify segments of the desired trajectories leading to feasible solutions in terms of the optimization process performed by each vehicle.