[Feature Request] Improve Hybrid A* with Reeds-Shepp Paths and Reversing Support

[ShisatoYano]

Description
Currently, the existing sample code of Hybrid A* Path Planning doesn't support reversing movement.
To make it more practical and professional -- similar to those used in autonomous valet parking -- we need to upgrade the heuristic and the motion model.

I would like to propose an enhancement to the Hybrid A* algorithm by integrating the Reeds-Shepp curves.

Tasks

• Integration of Reeds-Shepp Paths: Use Reeds-Shepp curves as the analytic expansion (shot-to-goal) to improve search efficiency and path quality.
• Support for Reversing: Update the motion model to allow the vehicle to move to backward.
• Direction Change Penalty: Implement a cost function that penalizes switching between forward and backward to ensure smooth paths.
• Heuristic Update: Improve the heuristic considering the non-holonomic constraints.

Reference Code
Current version of Hybrid A* Global Path Planning

[shreyshet]

I can work on it, since I made the PR for hybrid A star.

[ShisatoYano]

Thank you for your suggestion! I’m looking forward to reviewing your PR!!

[shreyshet]

Hi @ShisatoYano ,
I wanted to give an update since it's been almost 10 days. I have been able to update the hybrid start with motion primitives (reverse) and get a final shot with Reed's Shepp curve.
In the simulation below, car starts at (x=0, y=0, yaw = 0 deg) and goal is at (x =50, y=-10, yaw=180 deg).
Here is the result.

I have used the Pure Pursuit controller, which is currently implemented, and added a wrapper to track the reverse path. Hence, you see that in simulation, the look-ahead (green) suddenly jumps, and the tracking near the cusp is not crisp.
Is there a good controller to track a path with rear wheel steer?

[ShisatoYano]

@shreyshet Thank you for your update!!
Currently, my project doesn’t have any controller with rear wheel steering. So you can create a new PR for your update firstly. After the PR was merged, you can start implementing a new better controller for Hybrid A*.

[ShisatoYano]

Additionally, in that scenario, Hybrid A* Planner don’t need to plan the path with backward moving. When you create the gif of simulation in the PR, please modify the scenario which Hybrid A* should plan a path which forward and backward are combined.

[shreyshet]

Okay I will work on a reverse controller first. Thanks!

I am a little confused about the second comment, "in that scenario, Hybrid A* Planner don’t need to plan the path with backward moving". If you are referring to the gif that you sent earlier, I do feel like the reverse would bring a shorter path than the non reverse case as now we are also commanding the desired yaw angle unlike other planners.

[ShisatoYano]

Sorry for making you confused. I would like to clarify my point to align our goals for this issue:

1. Natural Driving Behavior: Even if a backward path is mathematically shorter, it is unnatural for a car to drive long distances in reverse when the front path is clear. A human driver would almost always prefer moving forward.

2. Showcasing Hybrid A*: To demonstrate the true value of Hybrid A*, we should use a simulation scenario where backward maneuvers are essential (e.g., parallel parking, a three-point turn in a narrow corridor, or a dead-end).

My Suggestion:
Could you update the simulation scenario to a more "constrained" environment?
We want to show a result where:

1. The vehicle stays in Forward mode as much as possible in open areas.

2. The Reverse maneuver only kicks in when it's the only way (or the only reasonable way) to reach the goal.

This will make the PR much more convincing and showcase the power of your implementation!

[shreyshet]

I understand now. I think you are right.
Reversing in open areas should be unnecessary.
I will work up a scenario. But I believe that such a path will require a confined space, and even if the planner finds a good solution, the controller (either forward or backward) has to track the path with great accuracy.
I will experiment and keep you posted. Thanks for the guidance!

[ShisatoYano]

Exactly. High-precision path tracking is the next challenge, and that's why we are also working on integrating advanced controllers.

I'm really looking forward to seeing your new scenario and how the planner handles the constraints. Take your time, and let me know if you need any feedback!

[shreyshet]

Hi @ShisatoYano,

I’m following up on our previous discussion with two updates:

1. Reverse Tracking Controller: I have developed a reverse tracking controller based on the Rear Wheel Feedback (RWF) method. You can see the tracking performance in the visualization below:

2. Parallel Parking Scenario: I am currently testing this in a parallel parking scenario. While the Hybrid A* planner is currently tracked using Pure Pursuit, I am moving toward integrating my new reverse controller for better accuracy. I am investigating a segment-based switched controller to address cusps. I'll keep you posted on the results.

Feel free to ask questions. Thank you for your patience on this issue.

[ShisatoYano]

@shreyshet Thank you for updating!! Testing your new controller in the parallel parking scenario looks so good to me. I’m looking forward to seeing your next update!!