Trajectory Optimization for Autonomous Navigation in Dynamic Environments

Motion planning for Autonomous Ground Vehicles (AGVs) in dynamic environments is an extensively studied and complex problem. State of the art methods provide approximate solutions that make conservative assumptions to provide safety and feasibility. We aim to outperform current methods by following a trajectory optimization-based approach, providing a Local Model Predictive Contouring Control framework. Our method allows AGVs to execute reactive motion while tracking a locally parametrized reference path, anticipating on the predicted evolution of the environment. Given the static environment configuration in an occupancy grid map and dynamic obstacles represented by ellipses, we formulate explicit collision avoidance constraints. Well-informed planning decisions are made through a cost function with trade-offs between competing performance variables such as tracking accuracy, maintaining the reference velocity, and clearance from obstacles. An efficient implementation of the method is presented that satisfies the real-time constraint of online navigation tasks. Furthermore, we present an implementation of a complete navigation system to emphasize our ability to deal with real sensor data and onboard processing. We show that the general definition of the framework applies to both unicycle and bicycle kinematic models, commonly used to represent mobile robots and autonomous cars, respectively. Simulation results for a car and experimental results with a mobile robot show that our method is a feasible and scalable approach. Proposed improvements of the method include 1) considering obstacle velocities and positioning with respect to the AGV in the penalty term that creates clearance, 2) incorporating prediction uncertainty of obstacles, and 3) improving our method that deals with infeasible solutions of the optimal control problem.