Skip to content

Latest commit

 

History

History
227 lines (167 loc) · 9.1 KB

README_En.md

File metadata and controls

227 lines (167 loc) · 9.1 KB
Raw

Autonomous Driving Simulation

This project is a simulation of an autonomous driving project based on planning and vehicle dynamics.

Implementation Approach

Planning

The planning part uses the Dijkstra algorithm for global path planning and the DWA algorithm for local path planning.

  • Implemented based on move_base

  • Global path planning: Dijkstra algorithm

    The basic idea of this algorithm is to start from the initial point and gradually expand to other nodes until the target node is found or all nodes are traversed. By maintaining a distance table that records the shortest distance from the starting point to each node and a predecessor table that records the previous node in the shortest path to each node, the algorithm continuously updates the values of these two tables until the target node is found or expansion is not possible.

  • Local path planning: DWA algorithm

    The Dynamic Window Approach (DWA) algorithm is a method for local path planning in robot navigation. It is a dynamic window-based approach to search for the best velocity command for the robot's current state to avoid obstacles and reach the target position.

    The core idea of the DWA algorithm is to search a dynamic window in the velocity-turning space based on the robot's dynamics model and environmental information. This window includes some possible ranges of changes in the robot's current velocity. Then, by evaluating the safety and proximity to the target for each velocity command, the algorithm selects the best velocity command as the robot's next move.

    The implementation steps of the algorithm are as follows:

    1. Get the current state of the autonomous vehicle, including position, velocity, and orientation.
    2. Define a dynamic window in the velocity-turning space, determined by the maximum velocity and maximum turning velocity limits of the autonomous vehicle.
    3. For each velocity command in the dynamic window, predict the trajectory of the autonomous vehicle within a given time.
    4. For each trajectory, evaluate its safety distance from obstacles and its distance from the target position.
    5. Based on the evaluation results, calculate the score for each velocity command and choose the velocity command with the highest score as the next move for the autonomous vehicle.
    6. Send the selected velocity command to the autonomous vehicle for execution.

    The advantage of this algorithm is that it does not require smoothing the path generated by global planning, as the local paths generated by DWA conform to the vehicle's kinematics. Additionally, due to its real-time planning nature, it enables real-time obstacle avoidance.

    ==Refer to the .yaml files under /src/simulation_launch/param for the parameter settings of the path planning algorithm.==

Control

The control part uses dual PID controllers. The advantages of using PID controllers include ease of implementation and better handling of nonlinear and time-varying systems, improving the stability and performance of the control system.

One PID controller's output is the throttle size, and the other PID controller's output is the steering angle.

//—————————————————Dual PID Controller————————————————————//
// Define PID controller parameters
// Throttle
double throttle_Kp = 1.0;
double throttle_Ki = 0.0;
double throttle_Kd = 0.0;
// Steering
double steering_Kp = 0.8;
double steering_Ki = 8.0;
double steering_Kd = 0.0;

// Outputs of the dual PID controller
double throttle_delta[2] = {0.}; // Throttle change 2D, {Throttle, Steering};

// Define dual PID controller
class PIDController {
private:
  double error; // Error
  double integral; // Integral
  double derivative; // Derivative
  double previous_error; // Previous error

public:
  double Kp; // Proportional coefficient
  double Ki; // Integral coefficient
  double Kd; // Derivative coefficient

  // Constructor
  PIDController(double kp, double ki, double kd) {
    Kp = kp; 
    Ki = ki;
    Kd = kd;
    error = 0.0;
    integral = 0.0;
    derivative = 0.0;
    previous_error = 0.0;
  }
  
  // Calculate output (Throttle, Steering)
  double calculate(double expectation, double measured, double dt) {
    error = expectation - measured;
    double output;

    integral += error ;
    derivative = (error - previous_error);
    output = Kp * error + Ki * integral + Kd * derivative;
    previous_error = error;
    return output;
  }
};

// Create PID controller objects
PIDController throttle_controller(throttle_Kp, throttle_Ki, throttle_Kd);
PIDController steering_controller(steering_Kp, steering_Ki, steering_Kd);

For the throttle_controller, the input is the forward velocity, and the output is the throttle size.

For the steering_controller, the input is the angular velocity, and the output is the steering angle.

Dynamic Vehicle Dynamics Model

Update the state based on the given vehicle dynamics model.

//—————————————————State Update Function————————————————————//
// State update, input parameters are the current state xC, throttle change throttle_delta, and time interval dt
void update_xC(double* xC, double* throttle_delta, double dt) {
    
    // Define vehicle parameters
    double Cm1 = 0.287;  // Front wheel slip torque coefficient
    double Cm2 = 0.0545;  // Front wheel slip torque coefficient
    double Cr0 = 0.0218;  // Tire rolling resistance coefficient
    double Cr2 = 0.00035;  // Tire rolling resistance coefficient
    double B_r = 3.3852;  // Rear wheel lateral force coefficient
    double C_r = 1.2691;  // Rear wheel lateral force coefficient
    double D_r = 0.1737;  // Rear wheel longitudinal force coefficient
    double B_f = 2.579;  // Front wheel lateral force coefficient
    double C_f = 1.2;  // Front wheel lateral force coefficient
    double D_f = 0.192;  // Front wheel longitudinal force coefficient

    // Define vehicle mass and inertia
    double m = 0.041;  // Vehicle mass
    double Iz = 27.8e-6;  // Vehicle inertia
    double l_f = 0.029;  // Distance from front axle to center of mass
    double l_r = 0.033;  // Distance from rear axle to center of mass
    double g = 9.8;  // Gravity acceleration

    // Calculate front and rear axle loads
    double Nf = m * g * l_r / (l_f + l_r);
    double Nr = m * g * l_f / (l_f + l_r);


    // Get the current state
    double phi = xC[2];  // Current yaw angle of the vehicle
    double v_x = xC[3];  // Current longitudinal velocity of the vehicle
    double v_y = xC[4];  // Current lateral velocity of the vehicle
    double omega = xC

[5];  // Current angular velocity of the vehicle
    double D = throttle_delta[0];  // Throttle change
    double delta = throttle_delta[1];  // Steering wheel angle

    // Calculate front and rear wheel slip angles
    double alpha_f = std::atan2((l_f * omega + v_y), std::abs(v_x)) - delta;
    double alpha_r = std::atan2((v_y - l_r * omega), std::abs(v_x));

    // Calculate front and rear wheel lateral and longitudinal forces
    double F_fy = D_f * std::sin(C_f * std::atan(-B_f * alpha_f));
    double F_fx = -Cr0 * Nf - Cr2 * v_x * v_x;
    double F_ry = D_r * std::sin(C_r * std::atan(-B_r * alpha_r));
    double F_rx = (Cm1 * D - Cm2 * D * v_x - Cr0 * Nr - Cr2 * v_x * v_x);

    // Calculate dx
    double dx[6] = {0.0};
    dx[0] = v_x * std::cos(phi) - v_y * std::sin(phi);
    dx[1] = v_y * std::cos(phi) + v_x * std::sin(phi); 
    dx[2] = omega;

    dx[3] = 1 / m * (F_rx + F_fx * std::cos(delta) - F_fy * std::sin(delta) + m * v_y * omega);
    dx[4] = 1 / m * (F_ry + F_fx * std::sin(delta) + F_fy * std::cos(delta) - m * v_x * omega); 
    dx[5] = 1 / Iz * (F_fx * std::sin(delta) * l_f + F_fy * l_f * std::cos(delta) - F_ry * l_r);

    // Update xC
    for (int i = 0; i < 6; i++) {xC[i] = xC[i] + dx[i]*dt ;}
    
    // Set initial state constraints to position and velocity to 0
    if(throttle_delta[0] == 0.0 && throttle_delta[1] == 0.0){xC[0] = 0.0;xC[3] = 0.0;}

    // Limit x-axis velocity
    if (xC[3] < min_vx && xC[3]!=0) {xC[3] = min_vx;}
    if (xC[3] > max_vx) {xC[3] = max_vx;}
    
    // Limit y-axis velocity
    xC[4] = 0.0;
    
    // Limit angular velocity
    if (xC[5] > max_vphi) {xC[5] = max_vphi;}
    if (xC[5] < min_vphi) {xC[5] = min_vphi;}
}

Project Description

The controllers are integrated into the main function vehicle_fake_node and are not implemented as separate nodes.

Requirements:

  • Ubuntu 20.04.2
  • ROS1 Noetic
  • rviz
  • map_server
  • move_base
  • amcl

Running the project:

  1. Create a workspace and open a terminal

    mkdir -p ~/catkin_ws

  2. Copy the src folder into this directory.

  3. Compile the packages

    catkin_make

  4. Set the environment variables

    source ./devel/setup.bash

  5. Run the launch file to start the nodes

    roslaunch simulation_launch move_base_simulation.launch

After that, in the opened rviz visualization interface, use 2D Pose Estimate and 2D Nav Goal to select the start and end points for simulation.

image-20230716095238548