Related blogs:
Path planning in unstructured environments: a real time hybrid a * implementation
Source code of hybrid A * algorithm based on dynamic constraints suitable for Ackerman chassis

Hybrid A-star algorithm adds vehicle steering constraints on the basis of A-star algorithm 2 D ( x , y ) 2D(x,y) 2D(x,y) point path planning is transformed into 3 D ( x , y , t h e t a ) 3D(x,y,theta) Path planning algorithm for 3D(x,y,theta) points. However, the classic heuristic idea of star A is retained, but in the hybrid star A, penalties such as turning and turning are added.

Before using this algorithm, we have briefly introduced the source code of hybrid A star. Because some places are not easy to use, we have adjusted other details of hybrid A star.

First read the nodes defined in node3d.cpp and node2d.cpp files.

1.node3d

Ordinary A-star algorithm, in the search process, is generally a 4-neighborhood and 8-neighborhood search method. However, the path searched by this method often does not meet the kinematic constraints of the vehicle, so in the hybrid a star, it is searched forward or backward to meet the angle of the current turning radius.

const float Node3D::dy[] = { 0,        -0.0415893,  0.0415893};
const float Node3D::dx[] = { 0.7068582,   0.705224,   0.705224};
const float Node3D::dt[] = { 0,         0.1178097,   -0.1178097};

In the hybrid A-star algorithm, six directions are provided (when reversing is allowed). When searching for the next node, through the defined d x , d y , d z dx,dy,dz dx,dy,dz calculate the new node coordinates.

Node3D* Node3D::createSuccessor(const int i) {
  float xSucc;
  float ySucc;
  float tSucc;
  // calculate successor p ositions forward
  if (i < 3) {//Forward winner
    xSucc = x + dx[i] * cos(t) - dy[i] * sin(t);
    ySucc = y + dx[i] * sin(t) + dy[i] * cos(t);
    tSucc = Helper::normalizeHeadingRad(t + dt[i]);
  }
  // backwards
  else {//Backward winner
    xSucc = x - dx[i - 3] * cos(t) - dy[i - 3] * sin(t);
    ySucc = y - dx[i - 3] * sin(t) + dy[i - 3] * cos(t);
    tSucc = Helper::normalizeHeadingRad(t - dt[i - 3]);
  }
  return new Node3D(xSucc, ySucc, tSucc, g, 0, this, i);
}

For a 3D node, the penalty term of the cost function updateG() adds direction and angle. prim represents the orientation of the current node, {0,1,2} represents front left, front right, and {3,4,5} represents rear left, rear right.

If the current orientation is forward, there are three situations:
(1) The orientation of the current node is consistent with that of the forward node, that is, PRED - > prim = = prim. At this time, the penalty item only needs to add the distance between the two nodes.
(2) The forward node PRED - > prim > 2, that is, backward. At this time, the direction and corner are inconsistent with the forward node. Therefore, two penalty items of steering penaltyCOD and corner penaltyTurning need to be added.
(3) The forward node PRED - > prim < 2, that is, forward. At this time, the rotation angle is only different, but the direction of the forward node and the forward node is forward.
The specific code implementation is as follows. The same is true for backward:

  if (prim < 3) {//Progress
    // penalize turning
    if (pred->prim != prim) {//When the direction changes
      // penalize change of direction
      if (pred->prim > 2) {
        g += dx[0] * Constants::penaltyTurning * Constants::penaltyCOD;//Punishment for changing direction
      } else {
        g += dx[0] * Constants::penaltyTurning;//No change of direction
      }
    } else {//The direction has not changed
      g += dx[0];
    }
  }

For a 3D node, judge that the node has been accessed, that is, it needs to be added to closed. Different from the traditional A-star algorithm, the same grid coordinates are considered to be the same node, so it is necessary to judge whether the steering theta of the two nodes is within a threshold range.

2.node2d

node2d, that is, the traditional A-star algorithm node, takes 8 neighborhoods as an example and expands outward with the current node as the center.

const int Node2D::dx[] = { -1, -1, 0, 1, 1, 1, 0, -1 };
const int Node2D::dy[] = { 0, 1, 1, 1, 0, -1, -1, -1 };

Node2D* Node2D::createSuccessor(const int i) {
  int xSucc = x + Node2D::dx[i];
  int ySucc = y + Node2D::dy[i];
  return new Node2D(xSucc, ySucc, g, 0, this);
}

3. Collision detection

In hybrid A star, the outward expanding node must first meet the requirements of nsucc - > isongrid (width, height) within the threshold of the map, and also the node that is not accessed, that is, in open, and finally the node will not collide. The first two conditions are easy to judge. Here we only look at whether there will be A collision configurationSpace.isTraversable(nSucc).

In the isTraversable function, the processing of 2D nodes directly obtains the coordinate value of the current node and checks whether it occupies in the grid map. If it occupies, there are obstacles. However, 3D nodes are more complex because they may span multiple grids.

static const int positionResolution = 10;//The square root of the number of discrete locations in each cell
int iX = (int)((x - (long)x) * Constants::positionResolution);//Get the offset of X direction in the cell
int iY = (int)((y - (long)y) * Constants::positionResolution);//Offset of Y direction in cell

for (int i = 0; i < collisionLookup[idx].length; ++i) {
  cX = (X + collisionLookup[idx].pos[i].x);
  cY = (Y + collisionLookup[idx].pos[i].y);
  // make sure the configuration coordinates are actually on the grid
  if (cX >= 0 && (unsigned int)cX < grid->info.width && cY >= 0 && (unsigned int)cY < grid->info.height) {
    if (grid->data[cY * grid->info.width + cX]) {
      return false;
    }//If there is a value in a small grid of the grid, indicating that there is an obstacle, false is returned, indicating that it is not in the free grid
  }
}
return true;//Otherwise, no occupancy is detected in all tests, indicating that there are no obstacles and can pass

The collisionLookup table stores the grid occupied by the vehicle body attitude under different orientations and offsets.

The parameters of hybrid A star are defined in constants.h
Interpretation of important parameters:
(1) Visualization, this parameter will visualize the search process and facilitate problem finding. However, in this mode, each visualization will take the initiative to sleep to slow down the speed. This process can be deleted from the source code.

static const bool visualization = true && manual;

// RViz visualization
if (Constants::visualization) {
  visualization.publishNode3DPoses(*nPred);
  visualization.publishNode3DPose(*nPred);
  d.sleep();
}

(2) Determine whether the vehicle can reverse. In the search process of hybrid A star, if it can reverse, it will expand outward in 6 directions, otherwise it will only expand in the first three directions.

static const bool reverse = true;

(3) Map resolution size

static const float cellSize = 1;

(4) Penalty items, including turning angle, reversing and steering, are introduced in the calculation of G value.

static const float penaltyTurning = 1.05;
static const float penaltyReversing = 2.0;
static const float penaltyCOD = 2.0;

(5) Step size for hybrid A-star search

const float Node3D::dy[] = { 0,        -0.0415893,  0.0415893};
const float Node3D::dx[] = { 0.7068582,   0.705224,   0.705224};
const float Node3D::dt[] = { 0,         0.1178097,   -0.1178097};

(6) Change the threshold of node equality to make dubins easier to hit.