Gentle introduction to Hybrid A star
Hybrid A* is considered one of the most popular path planning algorithms for car-like robots. It was developed by Dmitri Dolgov, who currently serves as the co-chief executive officer of Waymo, Google’s self-driving car project.
Academically, Hybrid A* has garnered more than 1000 citations, after demonstrating its exceptional performance in the DARPA challenge>). Even as of 2023, this method remains one of the frequently used frontend algorithms for car motion planning (e.g., papers of Bai Li orZhang).
When I initially read this paper, it was not that easy to follow, as I had to expand from grid domains. This article was written to help you understand the principles and properties.
By the way, code implementations can be found from various platforms such as
- KTH thesis (ROS1) (I personally dug into this version).
- nav2 (ROS2)
- Unity
- Python
- MATLAB
First of all, Djkstra and A*
In order to understand Hybrid, you first recap the search algorithms Djikstra algorithm and A* algorithm.
1. Djikstra
The correct way to understand Djikstra algorithm is to see it as “propagating” computation to find the shortest path tree. The purpose of the algorithm is not to find the shortest path to a single goal! It finds it for all of the possible nodes.
In the above animation, the gray circle notes that we computed the shortest path from the start(blue dot) to the cell (a.k.a., node). The reason why it looks like a path finding is that, the implementation terminated the propagation when it hits the goal.
Thus, the search does nothing to do with a single goal.
2. A*
What most robot researchers do for all days is to find a route to a “single” goal (although the goal can be changed). To this end, the robot researchers tried to modify the Djikstra algorithm to make it propagate toward a single goal.
To guide the search, we propagate toward the node that is “expected” to be the best direction based on two terms:
- Cost-so-far (already determined): Has this cell accumulated a lower cost until it reached this point from the start?
- Cost-to-goal (we do not know exactly): How optimistic is the estimation of this cell’s goodness towards reaching the goal?
The first term is often denoted as g(x) and the latter as h(x). We call the latter as a heuristic. We search the next x_next in a greedy manner: the lowest g(x_{next}) + h(x_{next}) inside the search queue.
Admissibility of a heuristic function
Since we perform a guided search relying on h(x), this function must be thoughtfully designed. For you to find the global optimal, an essential condition for the heuristic is that: it should output the expected cost to goal as optimistically as possible. In the case of using A* in a grid, this function is typically chosen as the Euclidean distance from x to the goal.
The reason for this choice is to ensure that we do not miss the opportunity to discover the best possible solution, that is, the global optimum. If this function does not promise this condition, there is no guarantee that A is (notation for global optimum).
3. Reflecting kinematics: Hybrid A*
A* algorithm works well in a grid domain with many applications including computer games. But this guided search on a grid has a limitation when we try to apply for non holonomic systems such as a car. Although the output of A* can be globally optimal in the grid, a resultant path can be inefficient when it was actually tracked by the dynamics.
In the above example, even though the blue straight line is the global optimal to reach x_{g}, the path cannot be tracked exactly due to the initial heading of the car. That is, the output of A* might not be useful. The original paper tried to tackle this: How can we consider the kinematics of the car while guiding the path search like A*?
Hybrid A* explained
From this section, we start to discuss hybrid A*. I will base my explanation on the original paper by Dimitri.
1. Objective
The conventional A* tries to find the shortest path from a position x_0 in R² to goal position x_f in R² (or course we can use in three-dimension. But for simplicity, we confine to 2-dimension). But this time, hybrid A* will effort to find a sequence of safe poses from s_0 = (x_0, \theta_0) in SE(2) to s_0 = (x_f, \theta_f) in SE(2), minimizing the travel length. Thus, our domain for searched elements is SE(2).
Kinematic models
In the paper, the model is assumed to be car-like moving as below:
In the paper, the expansion to the next step n+1 follows one of the three: two maximum curvatures (left / right) and a straight line. Thus, the shortest movement between any two states s_a and s_b can be analytically computed with Dubins path.
This model can be extended to have reverse motions, giving us six possible expansions. In this case, any two states can be connected with the shortest path following Reeds Shepp. Of course, you can extend the expansion model to suit your scenario such as this paper.
2. Expansion (opening)
When a state x_n \in R² is propagated in A grid field, we opened the neighboring cells, which is equal to moving along the grid. Also, the states correspond to the center of each cell. These become different in hybrid A.
Opening cells
In hybrid A*, a propagation of state s_n = (x_n, \theta_n) opens new cells by simulating the kinematics as the above figure shows. Thus, the new state s{n+1} = (x_{n+1}, \theta_{n+1}) can hardly arrive the center of the cells.
(I tried to show this with the white circles).
Cost-so-far g(s)
Computing the costs of each state s_{n+1} is relatively similar. We determine the cost by considering the length of the arc between s_n and s_{n+1}. The difference lies in our ability to penalize less desirable movements.
For instance, in the above picture, we can impose a higher penalty on turning movements (s_{n+1}²and s_{n+1}³) compared to the straight movement (s_{n+1}¹). Furthermore, when we utilize reverse movements for expansion, the reverse movement will also incur a higher penalty.
Pruning
At this point, you will be confused: what if more than two states share the same cell with the same angular bin? Here pruning comes in.
For performance reasons, we remove the states if another state in the same discretization (same 2d cell and angle bin) has attained a less cost-so-far. In the above picture, assuming a turning movement will be penalized more, the red circle will be pruned as it went through two turning movements. Of course, this pruning will not take place if the angles are treated differently by angle discretization.
3. Heuristic h(s)
From the above section, I explained how to expand and open new cells from a state s_{n}. As in the original A*, we should choose the best state s_n to propagate at the current iteration, based on the heuristic plus cost-so-far.
Nonholonomic heuristic
As we discussed, a heuristic should be best optimistic to be admissible. The concept of heuristic is similar to grid A*: the expected shortest path to goal. But here of course, the goal is in SE(2) not a positional domain.
As I told here, arriving a pose from another pose with the kinematic can be achieved by Dubins path or Reeds Shepp path. Can we use this then? Although it is admissible, we can more accurately estimate the remaining distance by considering obstacles. Take a look at the wonderful picture by Karl.
In the of the picture, we can overestimate the future cost. This can waste our propagation in the wrong direction (in this case, toward the deep in the wall..). We can help this by computing the shortest path in the positional domain as (d) shows.
Holonomic heuristic considering obstacles
How can we know the shortest path in a grid manner then? The original authors do not talk about details except a kind of dynamic programming. I found this part diverged depending on the implementation.
For example, Smac planner of nav2 used dynamic programming similar to dijkstra. But it propagates the shortest distance starting from the goal not from the start. See more details here. Karl’s implementation runs A star until the goal position. In either way, we need to operate on 2D grid when it comes to obstacle heuristic, and some down-sampling resolution seems unavoidable to reduce computation.
Heuristic of hybrid A*
We take the maximum of the two heuristics as our final heuristic h(s). We choose the best state s_{next} with a minimum value of g(s) + h(s) for the next expansion. Choosing the best is automatically handled by popping priority queue If you are not familiar with this data structure, I highly recommend studying it. This is a must-know concept to implement your A* algorithms.
4. Analytical expansion (a.k.a., shot to goal)
This process is a crucial and distinctive aspect that significantly impacts the final result.
Best node can achieve the most optimistic path? That is the solution!
In our heuristic-guided approach, we employ a greedy search method, prioritizing the best candidate in the queue. Actually, we achieve an optimal solution if we can connect this best candidate directly to the goal without any collisions. The best candidate exhibits the most optimistic behavior, making it an ideal choice.
For instance, in the A* search algorithm, if we can connect the last popped node to the goal using a non-colliding straight line, we have found the solution. Unfortunately, due to the possibility of collisions, we avoid attempting this during the search process.
Try the most optimistic shot!
In Hybrid A*, we take the opportunity to explore this direct shot-to-goal approach when it seems promising. During the search, we attempt to connect the best candidate pose to the goal pose using a Dubins path (or Reeds-Shepp path, considering reverse motions).
If this shot succeeds without any collisions, the search is terminated, as we have found an optimal solution. This technique is referred to as “analytic expansion” in the original paper. The success of these shots significantly contributes to the real-time efficiency of the Hybrid A* search. In practical scenarios with obstacle-free regions, the hybrid search often completes in just a few iterations. Essentially, this becomes equivalent to solving a Dubins path from the starting position directly to the goal pose.
It works in sparse regions, but not in dense spots.
However, in obstacle-dense regions, many shots may fail. In such cases, the search relies on obstacle heuristics to guide the exploration. When free space is encountered, the search can swiftly terminate by utilizing successful shots.
In the figure provided, the search process suddenly concludes while expanding the tree, and this is attributed to the successful shot connecting the best candidate to the goal pose without any collisions.
5. Analysis
Optimality for your real robot
Hybrid A*: A globally optimal pathfinding algorithm that has gained popularity, but it does have limitations preventing it from being truly globally optimal for the following reasons:
Control discretization
Our car’s control mechanism is not based on three (or six) steering modes like in the Dubins path. Unlike the assumptions made in Dubins paths, our robot does not follow specific pre-defined rotational arcs. Instead, it has the flexibility to change its rotational arc continuously. This means that the inputs for our car’s movements are considered discrete rather than strictly adhering to the Dubins path assumptions.
Node Pruning
In Hybrid A*, nodes in certain cells can be pruned based solely on their “cost-so-far,” as indicated in this link. However, the nodes that are pruned during this process might have potentially led to a better trajectory. This form of myopic pruning, based solely on the cost-so-far, may lead to suboptimal results. To overcome this limitation, it would be beneficial to consider heuristic information during the pruning process, which could help improve the overall optimality of the generated paths.
In conclusion, while Hybrid A* is a powerful path-finding algorithm, its true global optimality is hindered by the discreteness of our car’s control inputs and the myopic pruning of nodes based solely on their cost-so-far. Addressing these limitations through more advanced control mechanisms and heuristic-informed pruning strategies could lead to further improvements in the algorithm’s optimality.
Performance
It is perfectly okay when applying this algorithm to real-time applications, showing order of msecs computing time in reasonable complex environments. For more information, recommend reading this result in nav2 implementation.
