MPPI Local Planner

Overview

Navigation2 Controller plugin. Currently tested on ros2 foxy.

This is a controller (local trajectory planner) that implements model predictive path integral control to track a path with collision avoidance.

The main idea of the algorithm is to sample batch of consecutive controls (linear and angular velocities) by given time window and dt, use model to predict real velocities (linear and angular) for each time_step by given controls and initial velocities (current Twist), this can be explained as follows V( t+1 ) = F( X(t) ), where

• V(t+1) - velocities (linear and angular) of all batches at time step t + 1, shape [batch size x 2]
• X(T) - batches of time step t, consisting current velocities (linear, angular), controls (linear, angular) and dt, shape [batch size x 5]

then integrate these velocities to get trajectories, evalueate cost for each trajectory, and take relative scale of original controls (mean of batches controls with overweight for those controls which propagated trajectories has lesser cost) using softmax function calculated with costs.

This implementation has reworked cost function, which for now has 4 components which are related to obstacle avoidance, goal following, reference alignment, goal angle considerring near the goal. May be i'll "pluginize" this in future like in dwb There is no GPU acceleration / parallelisation, but you can implement generic "model" function which uses neural network (which predict linear and angular speed of the robot by given control and current state (curent linear and angular velocities)) which can opperate on whole batch in parrallel. (i'll probably implement this in near future)

This plugin implements the nav2_core::Controller interface allowing it to be used across the navigation stack as a local trajectory planner in the controller server's action server (controller_server).

Dependencies

MPPIc package requires a modern C++ compiler supporting C++17, and Conan C++ package manager:

pip install conan

Configuration

Example fully-described XML with default parameter values:

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 10.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 1.0
    min_theta_velocity_threshold: 0.001
    progress_checker_plugin: "progress_checker"
    goal_checker_plugin: "goal_checker"
    controller_plugins: ["FollowPath"]

    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0

    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
      stateful: True

    FollowPath:
      plugin: "mppi::Controller<float>"
      time_steps: 20
      model_dt: 0.1
      batch_size: 300
      v_std: 0.1
      w_std: 0.3
      v_limit: 0.5
      w_limit: 1.3
      iteration_count: 2
      temperature: 0.25
      reference_cost_power: 1
      reference_cost_weight: 5
      goal_cost_power: 1.0
      goal_cost_weight: 20.0
      goal_angle_cost_power: 1.0
      goal_angle_cost_weight: 5.0
      obstacle_cost_power: 2
      obstacle_cost_weight: 10
      inflation_cost_scaling_factor: 3
      inflation_radius: 0.75
      threshold_to_consider_goal_angle: 0.3
      approx_reference_cost: false

Topics

References

AutoRally