Problems with Reverse Motion for Ackermann Steering Vehicles Using Nav2 MPPI Controller

Required Info:

Operating System:
- ubuntu 22.04
ROS2 Version:
- Humble
DDS implementation:
- rmw_cyclonedds_cpp

Issue Description:

I am encountering significant challenges when using the Nav2 MPPI controller for reverse motion with an Ackermann steering vehicle. While the planned trajectories appear correct, the vehicle fails to follow these trajectories accurately when moving in reverse. The primary issue seems to stem from the steering geometry not being adequately handled by the current controller settings.

Steps to reproduce issue

1- Configure an Ackermann steering vehicle with the Nav2 stack and MPPI controller.

2- remove the cost of the PreferForwardCritic (after visualizing the trajectory, I found that this cost makes it very bad in reverse motion).

3- Plan a path that includes reverse segments while using a planning frequency of 0.01 in the behavior tree to see how the vehicle will handle this path with no path updates.

4- Observe the vehicle's behavior while executing the planned path, particularly during the reverse segments.

Here are the parameters for the controller

controller_server:
  ros__parameters:
    use_sim_time: True
    speed_limit_topic: "/speed_limit"
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 5.0
      movement_time_allowance: 10.0
    # Goal checker parameters
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 4.0
      yaw_goal_tolerance: 1.0
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      time_steps: 75
      model_dt: 0.05
      batch_size: 2000
      vx_std: 0.2
      vy_std: 0.2
      wz_std: 0.4
      vx_max: 1.5
      vx_min: -1.5
      vy_max: 0.0
      wz_max: 0.5
      iteration_count: 1
      prune_distance: 7.5
      transform_tolerance: 0.1
      temperature: 0.3
      gamma: 0.015
      motion_model: "Ackermann"
      visualize: true
      enforce_path_inversion: true
      inversion_xy_tolerance: 0.2
      inversion_yaw_tolerance: 0.3
      TrajectoryVisualizer:
        trajectory_step: 100
        time_step: 10
      AckermannConstraints:
        min_turning_r: 7.0
      critics: ["ConstraintCritic", "CostCritic", "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: 3.0
        threshold_to_consider: 0.5
      PreferForwardCritic:
        enabled: true
        cost_power: 1
        cost_weight: 0.0
        threshold_to_consider: 0.5
      CostCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.81
        critical_cost: 300.0
        consider_footprint: true
        collision_cost: 1000000.0
        near_goal_distance: 1.0
      PathAlignCritic:
      PathAlignCritic:
        enabled: 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: 20
        use_path_orientations: false
      PathFollowCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        offset_from_furthest: 5
        threshold_to_consider: 1.4
      PathAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 2.0
        offset_from_furthest: 4
        threshold_to_consider: 0.5
        max_angle_to_furthest: 1.0
        forward_preference: true
      TwirlingCritic:
        enabled: true
        twirling_cost_power: 1
        twirling_cost_weight: 10.0

Expected behavior

The vehicle should accurately follow the planned path in both forward and reverse motions, with steering angles adjusted appropriately for reverse kinematics.

Actual behavior

The vehicle's steering in reverse does not align correctly with the planned path, resulting in significant deviations and failure to follow the intended trajectory.

https://github.com/ros-navigation/navigation2/assets/99565733/50ebeeb1-787d-4a0a-bc9b-5ff58307377f

Additional information

I put the base_link in the middle of the two front wheels and the vehicle aligns correctly with the path in forward motion, but very bad in reverse motion. I trying putting the base_link between the two rear wheels, but that caused the vehicle to not align correctly in both forward and reverse motion, but it's a bit better in the reverse motion than having the base_link between the two front wheels.

I think the problem can be caused by the planner it self as this path is impossible for the vehicle to follow. I am using hybrid A* with the reeds_shepp motion model, and with the right min turning radius.

I not an expert but I think the vehicle geometry ( wheelbase and track width ) should be taken into consideration too while planning the path, not just the min turning radius.

While the planned trajectories appear correct, the vehicle fails to follow these trajectories accurately when moving in reverse

Just to be clear, the MPPI trajectories or the path plans inputted to MPPI? If the MPPI trajectories look good/right, then they probably are!

The primary issue seems to stem from the steering geometry not being adequately handled by the current controller settings.

It is on your controller to handle this feature, not the higher level trajectory planner. The commands are published in Twist format, which are body-fixed frame velocity commands. The angular velocity needs to be translated into the appropriate mechanics for your ackermann vehicle (i.e. turning rates and so forth). It is commonly misinterpreted that these are turning rates, they are not. They are body-fixed frame velocity commands like any vehicle / robot would take and then convert using their kinematics into motor efforts/velocities. If you already know this, then skip this paragraph :laugh:

I put the base_link in the middle of the two front wheels and the vehicle aligns correctly with the path in forward motion, but very bad in reverse motion.

I believe the convention for ackermann is the center of the back axle, but you should check literature about this. I'm assuming your talking a car-like ackermann drive where the front wheels are the ones that are actuated to turn.

I not an expert but I think the vehicle geometry ( wheelbase and track width ) should be taken into consideration too while planning the path, not just the min turning radius.

See aforementioned paragraph on kinematics :-) If you were moving at autonomous driving speeds, you are correct that the abstractions we make for mobility in robotics would probably not apply in a satisfying way and more of the exact nature of your particular vehicle would need to be addressed in high level path and low level trajectory planning. But, this is not an autonomous driving stack and this has been shown to work well on tractors and other large slower moving equipment.

I think perhaps your issue is just converting the "robot: I demand of you that you move forward this fast and you rotate your body as such" to "robot: I demand of you to move your wheels with this velocity and rotate your drive mechanism at this turning rate to accomplish this body request" which is what your kinematics provide the translation for. Its not different than Omnidirectional or Differential or Legged or other types of robots. We say "do this with your body" and then there's a controller that translates that into actuator commands to accomplish. Admittedly, Differential drive are really trivial equations, standard Ackermann isn't all that difficult. More complicated Ackermann or actuated-caster Omnidirectional can be a bit more involved, but this is something you need to know about your vehicle as you are "BYO-Vehicle".

Just to be clear, the MPPI trajectories or the path plans inputted to MPPI? If the MPPI trajectories look good/right, then they probably are!

The trajectories are fine, It was bad at first, and the car just kept steering randomly in reverse motion, but the problem was solved when I set the PreferForwardCritic to 0. It's now a planner problem as there is no way for the car to follow this exact path as you can see in the video. If the base_link was between the two rear wheels and the minimum turning radius was larger, the car would follow the path better in reverse motion, but it won't be able to do the path inversion. It would get stuck like this:

car

And if I enforce path inversion in a situation like this one, it will just get of the path and try to steer back again.

It is on your controller to handle this feature, not the higher level trajectory planner.

I have a controller that takes this into consideration and convert the twist message to the appropriate commands. I was wrong to assume it's a controller problem, It's a planner problem.

this is not an autonomous driving stack and this has been shown to work well on tractors and other large slower moving equipment.

I am working on a golf car, and it's working really great with the nav2 stack, but it's only missing the reverse motion. The problem is not just about following the exact path, The main problem is using the reeds shepp motion model. As the planner updates every second, it reverses the directions a lot and I get these small little segments in the path and the car just keeps going back and forth. I thought if I fixed the path, and work on getting the car to folllow this exact path, it would solve the problem, but I now realize that it won't. I tried experimenting with different planner parameters and penalties but I couldn't get it to work. I modified the planner code to use the dubin motion model, and if it can't find a path, use reeds shepp, but I found out that it takes like 9 seconds to reinitialize the A* algorithm. I modified the codes to get around this and switch fast between the two models, but it can't handle switching this fast and it crashes everything after few switches.

How do I get around these problems?

As the planner updates every second, it reverses the directions a lot and I get these small little segments in the path and the car just keeps going back and forth.

Ah, if this is your main problem, then that's easily solvable! Nav2 is built on Behavior trees and it is easy for you to specify your own more optimal navigation logic rather than relying on the default configuration. You can learn more about behavior trees and workflows [1]. Also, many examples [2].

It might be good for instance to just modify the first behavior tree to reduce the planning rate to every 10 seconds or something just to try it out, but then if that's what you want, you can decide how you want to move forward there (replan at a reduced rate, plan only once at start, plan once at start and replan only when invalid, etc), we have examples of a few of those + the documentation walks you through some somewhat more complicated behavior trees to help you build up the skills. The ability to customize the robot behavior is a key benefit for Nav2 users and it is expected for professional applications that you create a custom BT that is most appropriate to your application needs :-)

[1] https://docs.nav2.org/behavior_trees/index.html [2] https://github.com/ros-navigation/navigation2/tree/main/nav2_bt_navigator/behavior_trees

I can't speak to why your custom modification crash, so I can't comment on that. I'm not entirely sure what the problem in general that you're having is and if it really is a planning problem at all with reversing. If you robot stops and can move its wheels in place before starting motion, that should get you over that rotational hurdle to start from a left turn forward to a right turn reverse. That just has to be baked into your kinematic model or I suppose controller for processing these commands that this is the right move before attempting forward/reverse driving to follow the path to the accuracy that you would like. So is it just path tracking issues - that wouldn't really be a planner problem but a controller tuning problem. The Reeds-Shepp model assumes that you can go from a right to a left turn on directional shifts via turning the wheels while stopped.

Using the navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml behavior tree is really helpful. Thank you!

Now, my only problem is with the path inversion point, where the car gets stuck and can't complete the path inversion as shown here: car

The car can't rotate in place and, in this position, there is actually nothing it can do. If it tries to steer toward the forward path, it will go way off course. The correct approach here is to keep going backward until it can face the forward path and then move forward. A workaround is to increase the minimum turning radius to create a smoother path that the car can follow, but this significantly limits the car's movement.

The path is for the frame that is specified - you might want to visualize with debug_visualizations parameter's planned_footprints topic for the footprints that the plan is trying creating. If the front of the vehicle is what's on the frame at the inversion point / planning w.r.t., it should be able to rotate the wheels from the left to the right turn and start the reversing.

Setting the debug_visualizations parameter to true doesn't work and there is no topic named planned_footprints . I can't find this parameter in any of the codes related to the nav2_smac_planner.

If the front of the vehicle is what's on the frame at the inversion point / planning w.r.t., it should be able to rotate the wheels from the left to the right turn and start the reversing.

Yes, that's exactly what I need. However, as I mentioned, if I place the base_link between the front wheels, the vehicle cannot move in reverse, as you saw in the video. On the other hand, if I place the base_link between the rear wheels, the vehicle can move in reverse but gets stuck at the inversion point. I need a solution where the reverse path extends at the inversion point, allowing the front of the vehicle to align with the forward path, enabling it to continue moving forward.

I understand this is frustrating, but without more information there's not much I can say on this right now that I can think of. I'm doing a mental model of what I think should be happening and it seems like everything is doing the right job to me. The robot is able to follow the path that you point out, but it follows it w.r.t. the back frame (... or whatever frame is currently at the path inversion, I assume that's the back?). There could be something wrong here (?) but I'd need a description of what is incorrect to work back from.

I think what you might be thinking is that it should plan from the back, but then the front axle is the frame that the path should align to - but its not, its the back which you set as the fixed frame for the vehicle. So in that path inversion point, you can turn the wheels and then that back axes can follow the path and the front will swing around. This is why we set the rear as the fixed frame in ackermann vehicles.

I was wanting you to look at the visualizations of the planned footprints yourself so you could make sense of that visually, however debug_visualizations is only in Jazzy/Rolling. You could clone main and make a couple of small rclcpp API updatse to get main running locally to see it as well, but that's about 20 minutes of compiling (while you're at lunch!).

I suppose if you're using the front axes still as your fixed frame, then that breaks down -- but also the Ackermann models all assume the center of the rear axle for a car-like ackermann drive, so that's what you should be using here too. I think your choice to use the front frame is incorrect and possibly as a result of a misunderstanding from the description above (unless you can detail what's wrong requiring you to use the front exclusively?) and could resolve your issue

ros-navigation / navigation2