MPC based obstacle avoidance with uncertainty in the auto-pilot dynamics and the obstacle

This project investigates autonomous vehicle trajectory planning in dynamic environments using Model Predictive Control (MPC) under motion uncertainty. The work is motivated by the fact that real world obstacle avoidance problems are fundamentally difficult optimal control problems. In theory, the globally optimal solution can be obtained through Dynamic Programming (DP), where the control policy is computed over the entire state space while accounting for future disturbances and constraints. However, once moving obstacles, nonlinear constraints, and uncertainty are introduced, the optimization rapidly becomes computationally intractable for real time applications. To address this challenge, the project reformulates the problem using receding horizon MPC, which computes a locally optimal approximation of the full optimal control solution online at every time step.

The environment consists of an autonomous vehicle navigating a multi lane road populated with multiple moving obstacle vehicles. Unlike simplified deterministic obstacle avoidance methods, the project explicitly models uncertainty in both the obstacle predictions and the ego vehicle dynamics. Sensor noise, imperfect motion prediction, actuator disturbances, and delayed state estimation introduce randomness into the system, making exact future trajectories impossible to know in advance. Instead of assuming precise obstacle locations, each obstacle vehicle is represented probabilistically using a Gaussian position distribution whose covariance evolves over time. This allows the controller to reason not only about where an obstacle is expected to be, but also about the confidence of that prediction.

To safely navigate through uncertainty, the project leverages the Mahalanobis distance as a probabilistic safety metric. Rather than avoiding only the mean obstacle position, the controller avoids an uncertainty ellipse generated from the obstacle covariance distribution. These ellipsoidal safety regions expand naturally as prediction uncertainty increases, creating dynamically changing no go regions inside the optimization problem. This formulation enables the MPC controller to generate trajectories that remain robust even when obstacle motion becomes unpredictable or stochastic.

The optimization problem additionally enforces strict road boundary constraints so that all generated trajectories remain inside the drivable corridor. The vehicle cannot simply leave the roadway to avoid collisions, which makes the feasible space highly constrained and non convex. Lane structure and roadway geometry are embedded directly into the MPC constraints, forcing the optimizer to balance collision avoidance, smooth tracking performance, and control effort simultaneously. Furthermore, overtaking behavior is intentionally constrained so that the autonomous vehicle performs passing maneuvers strictly from the left side, closely mimicking realistic highway driving rules and structured traffic behavior.

The resulting MPC problem is solved repeatedly online over a finite prediction horizon using the current estimated system state. At every iteration, the optimizer predicts future robot motion, predicts future obstacle distributions, evaluates safety constraints, and computes an optimal control sequence consisting of steering and acceleration actions. Only the first control input is applied before the optimization is solved again at the next time step. This receding horizon strategy allows the controller to continuously adapt to evolving obstacle behavior and environmental uncertainty while remaining computationally feasible for real time deployment.

Simulation results demonstrate smooth and collision free overtaking trajectories in the presence of several moving vehicles with stochastic motion. The controller successfully maintains safe probabilistic separation distances, respects road constraints, and performs structured left side overtaking even under uncertainty in both the ego vehicle and surrounding traffic. Overall, the project highlights how Optimal Control Theory, Dynamic Programming approximations, Probabilistic Motion Prediction, and Constrained Nonlinear MPC can be combined to create robust autonomous driving behavior in complex and uncertain traffic environments.