Hybrid A* search

This post introduces the implementation of Hybrid A* search on a grid, inspired by the paper “Practical Search Techniques in Path Planning for Autonomous Driving.”

Difficulty of the common A*

Fundamental problem A* is discrete whereas real world is continuous. Therefore, it is hard to drive with common A* in autonomous driving world.

Is there version of A* that can deal with the continuous nature?

Hybrid A* handle this problem with a state transition function!

Above image from this paper

Comparison between A* and hybrid A*

with Hybrid A*, completeness and optimality are lost. However, it can be drivable which makes the planning algorithm more practical.

Implementation Hybrid A*

Let’s find the optimal path from start to goal position with this grid map. ‘1’ is the wall so, the path can’t be generated

vector<vector<int>> grid = {
  {0,0,1,0,0,0,0,0,0,0,1,0,0,0,0,0,},
  {0,0,1,0,0,0,0,0,0,1,0,0,0,0,0,0,},
  {0,0,1,0,0,0,0,0,1,0,0,0,0,0,0,0,},
  {0,0,1,0,0,0,0,1,0,0,0,0,1,1,1,0,},
  {0,0,1,0,0,0,1,0,0,0,0,1,1,1,0,0,},
  {0,0,1,0,0,1,0,0,0,0,1,1,1,0,0,0,},
  {0,0,1,0,1,0,0,0,0,0,1,1,0,0,0,0,},
  {0,0,0,1,0,0,0,0,0,0,1,0,0,0,0,0,},
  {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,},
  {0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,},
  {0,0,0,0,0,1,0,0,0,0,0,1,1,1,1,1,},
  {0,0,0,0,1,0,1,0,0,0,1,1,1,1,1,1,},
  {0,0,0,1,0,0,0,0,0,0,1,1,1,1,1,1,},
  {0,0,1,0,0,0,0,0,0,0,0,1,1,1,1,1,},
  {0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,},
  {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,},};

vector<double> start_pos = {0.5f, 0.5f, 0.0f}; // x , y, theta
vector<double> goal_pos = {15.5, 15.5, M_PI_2}; // x , y, theta

State transition equations

Let’s use bicycle model for the states:

\(\begin{align} x_{t+1} &= x_t + v_t \cdot \cos(\psi_t) \cdot \Delta t \\ y_{t+1} &= y_t + v_t \cdot \sin(\psi_t) \cdot \Delta t \\ \psi_{t+1} &= \psi_t + \frac{v_t}{L_f} \cdot \delta_t \cdot \Delta t \end{align}\)

    double omega = SPEED / VEHICLE_LEGTH * (delta) * DT;
    double next_theta = theta + omega;
    if(next_theta < 0) {
      next_theta += 2*M_PI;
    }
    double next_x = x + SPEED * cos(theta)* DT;
    double next_y = y + SPEED * sin(theta)* DT;

Search steering angle space

The $\delta_t$ is steering angle. The hybrid a* will generate candidate of the steering angle with every 5 degree between -90 degree and 90 degree

Heuristic function

A* selects the path that minimizes

where n is the next node on the path, g(n) is the cost of the path from the start node to n, and h(n) is a heuristic function that estimates the cost of the cheapest path from n to the goal - wiki

In this post, I used heuristic function of euclidean distance and the angle error

  double euclidian_distance = sqrt(pow((x1-x2),2)+pow((y1-y2),2));
  double angle_err = abs(theta1 - theta2);

Simulation result

OpenCV was used for visualization.

First, It shows the open lists. We can recognize the steering angle candidates

Second, the picture shows the optimal path of the hybrid a* which is the green line.

If you would like to see the full code, please visit this repo