Here is the README file for the code:

---

Autonomous Vehicle Control System with MPC and PID

Overview

This repository contains a control system for an autonomous vehicle, which uses a combination of PID control and Model Predictive Control (MPC) to control the vehicle's steering and throttle. The system is designed for real-time control, and its main goal is to track a desired path while adjusting the vehicle's velocity and yaw to meet specified waypoints.

Theory

The control system consists of multiple components working together to achieve a smooth and accurate path-following behavior:

1. PID Control:

   • Used for controlling the throttle (velocity). A cascaded PID controller is employed to decouple the position and velocity control, allowing for more precise control.
   • The PID controller adjusts the throttle output based on the error between the current vehicle's position and velocity, and the desired position and velocity.

2. Model Predictive Control (MPC):

   • Used for steering control. The MPC optimizes the steering angle over a finite prediction horizon to minimize the error in yaw (orientation) and avoid large steering commands.
   • The system considers the dynamics of the vehicle, including its current state (position, velocity, yaw), and applies steering inputs that result in smoother trajectories.

3. Kinematic Bicycle Model:

   • A simplified model of a vehicle's motion that accounts for the vehicle’s position, velocity, and yaw angle. This model is used to predict the next state of the vehicle and update its trajectory.

Key Concepts

• PID (Proportional-Integral-Derivative) Control:

  • The PID controller helps correct errors in both position and velocity. The proportional term adjusts the output based on the immediate error, the integral term compensates for cumulative errors over time, and the derivative term accounts for the rate of change of the error.

• Model Predictive Control (MPC):

  • MPC is a method of control that solves an optimization problem at each time step. It predicts the future behavior of the system based on the current state, and controls the vehicle by optimizing the trajectory over a finite horizon.

• Kinematic Bicycle Model:

  • This model describes the vehicle's motion based on the steering angle and throttle. It is commonly used in vehicle dynamics for path-following tasks.

ROS Communication

The system is built to work with ROS (Robot Operating System), where it communicates through topics to control the vehicle. The main ROS topics are:

• Odometry Subscriber:

  • /carla/ego_vehicle/odometry: Receives the current vehicle pose (x, y, velocity, yaw) from the odometry sensors.
  • Callback Function: odom_callback

• Path Subscriber:

  • /path: Receives the desired path information (x, y, velocity, yaw) to be followed.
  • Callback Function: path_callback

• Throttle Publisher:

  • /throttle: Publishes the throttle command to the vehicle model.
  • Message Type: Float64

• Steering Publisher:

  • /steering: Publishes the steering command to the vehicle model.
  • Message Type: Float64

System Components

1. ControlSystem Class:

   • Manages the control loop by receiving the desired path, computing throttle and steering commands, and updating the vehicle's state using the bicycle model.
   • Contains the path-following logic using PID and MPC.

2. PID Class:

   • Implements the PID controller for throttle control. Adjusts the throttle based on the desired position and velocity.

3. CascadedPID Class:

   • A cascaded version of the PID controller that independently handles position and velocity control to enhance the system’s performance.

4. MPC Class:

   • Implements Model Predictive Control for steering. Optimizes steering inputs over a prediction horizon to minimize yaw errors and prevent oversteering.

5. KinematicBicycleModel Class:

   • Models the vehicle's motion using kinematic equations. Computes the new state (position, velocity, yaw) based on the throttle and steering inputs.

Hyperparameters

• PID Tuning:

  • Kp_pos, Ki_pos, Kd_pos: Position control parameters for the PID.
  • Kp_vel, Ki_vel, Kd_vel: Velocity control parameters for the PID.

• MPC Parameters:

  • Horizon: The prediction horizon used in MPC.
  • dt: The time step for each update.
  • Wheelbase: The distance between the front and rear wheels of the vehicle.

• Vehicle Constants:

  • Wheelbase: 1 meter.
  • Max acceleration and deceleration: [-3:3] m/s² (for backward motion, stopping).
  • Max velocity: Typically 10 m/s.
  • Max steering angle: [-π/4, π/4].

Control Process

The control loop follows these steps:

1. Path Reception: The control system receives the desired state (x, y, velocity, yaw) via the /path topic.
2. Throttle Control: The PID controller computes the throttle output to ensure the vehicle reaches the desired velocity.
3. Steering Control: The MPC computes the optimal steering angle to follow the desired path while maintaining an appropriate yaw angle.
4. State Update: The vehicle’s state is updated using the Kinematic Bicycle Model, which accounts for the effects of throttle and steering.

Vehicle Model Update

The vehicle state is updated by the following equations:

• Position (x, y) is updated based on the current velocity and yaw angle.
• Yaw (θ) is updated based on the steering input and vehicle dynamics.
• Velocity (v) is updated based on throttle input and the forces acting on the vehicle, including friction and air drag.

Installation

Dependencies

• ROS (Robot Operating System)
• NumPy
• CasADi
• Matplotlib

ROS Setup

• Ensure that the appropriate topics (/carla/ego_vehicle/odometry, /path, /throttle, /steering) are available.
• Modify the topic names in the code if necessary to match your setup.

Running the Code

1. Launch your ROS environment.
2. Execute the script to start the control system and begin the path-following task:

   python control_system.py

3. The system will compute and display the vehicle's trajectory.

Future Improvements

1. Hyperparameter Tuning: Fine-tune PID and MPC parameters for better performance in different environments.
2. Trajectory Testing: Test the system on different trajectories under various conditions (e.g., varying path curvature, velocity, etc.).
3. C++ Conversion: Convert the Python code to C++ for better performance in real-time applications.
4. Acceleration Gradient Consideration: Account for acceleration jerk to improve throttle control.
5. Coupled Control: Experiment with giving higher priority to steering over throttle to improve performance in curves.

Conclusion

This system provides a foundational framework for autonomous vehicle control, combining PID for velocity and MPC for steering. It is a powerful approach to path-following tasks that can be further enhanced by incorporating more advanced techniques such as coupled control and higher-order dynamics models.