Local Path Planning via MPC for Safe Navigation in Unknown Multi-Vehicle Environments

Abstract

This thesis presents the development of safe and versatile local path planning algorithms through Model Predictive Control (MPC) in the absence of a global path planner for applications such as search and rescue, mobile robotics and autonomous vehicle racing. Rather than formulating a local planner that tracks and controls towards a known global path, the foundational novel method presented in this thesis relies solely on continually updating an optimal local reference trajectory and solving a non-convex optimization MPC problem for control commands to track this path. Successive tracking lines are generated through a quadratic optimization to maximize the distance to nearby obstacles (detected through LiDAR) while having heading directions aimed towards large open spaces. Quadratic costs are derived to follow these lines closely, subject to nonholonomic system constraints as described by the kinematic bicycle model. Via a Sequential Least Squares Quadratic Programming (SLSQP) online solver utilizing analytical gradients, feasible and locally optimal solutions are reliably found in real-time operating circumstances. This approach is extended to incorporate dynamic obstacles into vehicle avoidance. Vehicle detection exploits a custom-trained Convolutional Neural Network (CNN) using the You Only Look Once (YOLO) architecture. Red, Green, Blue (RGB) detections are projected into depth space, and an Extended Kalman Filter (EKF) obtains predicted vehicle paths. These tracked vehicle paths are then added to the path planning algorithm to handle adversarial vehicle avoidance in multi-vehicle scenarios. An alternative formulation introduces a fourth-order Bezier curve model in place of the successive tracking lines, which combines the generation of an optimal path and actuation to the path into one non-convex optimization. Constraints on vehicle dynamics are incorporated directly into the construction of the curve, and potential fields from nearby obstacles ensure a safe path is maintained. Computation complexity is reduced, and smooth paths are reliably found in real-time. The aforementioned local planners are incorporated in a leader-follower hierarchy, which balances the need for pursuit tracking with the safety of the generated path as before. Arbitrary following configurations are enabled through the pursuit formulation, and adaptive pursuit vs. safety weightings are dictated by continuously updating obstacle proximity. This framework sets the basis for modular and extendable flexible formation fleets in unknown areas using only local path planning. Results are exhibited in both a simulation environment and on a 1/10th scale autonomous vehicle. Real-time navigation is achievable, and trajectories are shown to be safer and achieve superior performance compared to existing local planners when the assumption of a known global planned path is removed. The algorithms presented in this thesis are compared, and their ability to obtain their desired objectives in varying environments is shown.