Improving Freespace Planning (Parking)

[mkquda]

Background

The current implementation of freespace planner / parking is limited in what it can do, and recently some issues were reported when trying to utilize the parking functionality in field tests.

The aim here is to share experiences using free space planner, clarify the existing issues and limitations, and discuss ideas to improve quality and performance of free space planner.

Problem Statement

The problem can be split into two parts:

• Improving the path planning algorithm
• Improving the overall design of the parking feature

Path Planning Algorithm
Currently two freespace planning algorithms are available (astar and rrtstar), astar is primarly being used while rrtstar is not as mature and rarely used. The path planning algorithm should be able to:

• Generate a feasible path satisfying the vehicle constraints
• Generate a smooth path so that the controller can track the path easily
• Generate a path that does not unnecessarily go back and forth repeatedly
• Generate a path within a reasonable period of time
• Generate a path with minimal deviation from desired goal position and orientation

Parking Feature Design
Currently the design specifications / requirements are not clearly stated, so it is difficult to define the situation which the system can / cannot handle. It is important to discuss and define the requirements and limitation, as well as share encountered issues to help improve the feature.

Current Work

Currently we (TIER IV) are working on some improvements to the path planning algorithm to address the points listed above. The improvements include:

Refactoring & Improving Hybrid A-star implementation:

• Refactor code for better readability and maintainability
• Use actual vehicle info (max_steering + base_length) to compute node transitions
• Fix node exploration logic to update opened states if explored again with lower cost
• Add cost for turning and cost for change in steering angle
• Add minimum distance threshold between direction changes to avoid frequent switching

Implement Obstable EDT Map

• Implement edt map computation to store distance to nearest obstacle for each grid cell
• Use obstacle edt to limit collision check necessity
• Use obstacle edt to apply cost on proximity to obstacles

Implement Backward Search Option

• Modify algorithm to support backward search from goal to start
• Make it configurable
• Add option to constrain the direction of the path near goal for better arrival

Considerations

Due to limited resources, the current work being done by Tier IV is limited to improving the planning algorithm.
Making design / requirements modifications to the overall parking feature is out of the scope of the current work.

[mkquda]

Corresponding PR's:

• refactor(freespace_planning_algorithm): refactor and improve astar search autoware.universe#8068

• feat(freespace_planning_algorithms): use distance to nearest obstacle to improve path planning autoware.universe#8089

• feat(freespace_planning_algorithms): implement option for backward search from goal to start autoware.universe#8091

• feat(freespace_planning_algorithms): implement support for multiple goal candidates in A star planner autoware.universe#8092

• feat(scenario_selector, freespace_planner): improve freespace planner edge case behavior autoware.universe#8348

• feat(freespace_planning_algorithms): add new parameters for astar planning algorithm autoware_launch#1120

• perf(freespace_planning_algorithms): tune freespace planner parameters autoware_launch#1097

• feat(freespace_planning_algorithm): update freespace planner params autoware_launch#1080