0
$\begingroup$

I would like to use NavThroughPoses BT to navigate my robot with the MPPI controller. My problem is that the controller doesn't follow the path correctly. When the robot comes close to an intermediate point, which is on a bend, it doesn't go to the point, but cuts off the path.

enter image description here

I tried to tune the MPPI parameters, mostly the PathFollowCritic values, but the result was no better. The only parameter that made the robot follow the path better was to reduce the value of prune_distance, but I don't think this is a good solution. My question is, which parameters should be set correctly to achieve the desired result?

My parameter settings are:

controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 16.0
min_x_velocity_threshold: 0.001
min_theta_velocity_threshold: 0.001
failure_tolerance: 0.3
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
progress_checker:
  plugin: "nav2_controller::SimpleProgressChecker"
  required_movement_radius: 0.25
  movement_time_allowance: 10.0
general_goal_checker:
  stateful: True
  plugin: "nav2_controller::SimpleGoalChecker"
  xy_goal_tolerance: 0.05
  yaw_goal_tolerance: 0.05
FollowPath:
  plugin: "nav2_mppi_controller::MPPIController"
  time_steps: 90
  model_dt: 0.0625
  batch_size: 2000
  vx_std: 0.1
  vy_std: 0.0
  wz_std: 0.1
  vx_max: 0.5 
  vx_min: -0.5 
  vy_max: 0.0
  wz_max: 0.4 
  iteration_count: 1
  prune_distance: 3.0 
  transform_tolerance: 0.1
  temperature: 0.3
  gamma: 0.015
  motion_model: "DiffDrive" 
  visualize: false
  reset_period: 1.0 
  TrajectoryVisualizer:
    trajectory_step: 5
    time_step: 3
  max_robot_pose_search_dist: 5.0 
  enforce_path_inversion: True 
  inversion_xy_tolerance: 0.2 
  inversion_yaw_tolerance: 0.4 
  AckermannConstrains:
    min_turning_r: 0.5
  critics:
    [
      "ConstraintCritic",
      "ObstaclesCritic",
      "GoalCritic",
      "GoalAngleCritic",
      "PathAlignCritic",
      "PathFollowCritic",
      "PathAngleCritic",
      "PreferForwardCritic",
    ]
  ConstraintCritic:
    enabled: true
    cost_power: 1
    cost_weight: 4.0
  GoalCritic:
    enabled: true
    cost_power: 1
    cost_weight: 5.0
    threshold_to_consider: 1.4 
  GoalAngleCritic:
    enabled: true
    cost_power: 1
    cost_weight: 5.0 
    threshold_to_consider: 0.5 
  PreferForwardCritic:
    enabled: false
    cost_power: 1
    cost_weight: 5.0 
    threshold_to_consider: 1.4 
  ObstaclesCritic:
    enabled: true
    cost_power: 1
    repulsion_weight: 1.5 
    critical_weight: 20.0 
    consider_footprint: True 
    collision_cost: 10000.0
    collision_margin_distance: 0.1 
    near_goal_distance: 0.5 
    inflation_radius: 3.9 
    cost_scaling_factor: 1.5 
  PathAlignCritic:
    enabled: true
    use_path_orientations: True 
    cost_power: 1
    cost_weight: 14.0 
    max_path_occupancy_ratio: 0.05
    trajectory_point_step: 3 
    threshold_to_consider: 0.5 
    offset_from_furthest: 13 
  PathFollowCritic:
    enabled: true
    cost_power: 1
    cost_weight: 17.0 
    offset_from_furthest: 5 
    threshold_to_consider: 0.5 
  PathAngleCritic:
    enabled: true
    cost_power: 1
    mode: 2 
    cost_weight: 4.0 
    offset_from_furthest: 20 
    threshold_to_consider: 1.4 
    max_angle_to_furthest: 0.25 
$\endgroup$

1 Answer 1

0
$\begingroup$

You can tune the controller to more closely follow the original path using the PathAlignCritic critic. The PathFollowCritic is the critic that drives the robot forward towards the path, but not specifically aligning to it more than general following. The Alignment critic controls the weight on alignment.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.