Hybrid-A-Star 满足车辆运动学约束的全局路径规划算法源码（补充）_path planning in unstructured environments : a rea

相关博客：
＜论文阅读＞ Path Planning in Unstructured Environments : A Real-time Hybrid A* Implementation
适于用阿克曼底盘的基于动力学约束的混合A*算法源码

混合A星算法在A星算法的基础上，添加了车辆转向的约束，从$2D(x,y)$点路径规划转变成$3D(x,y,theta)$点的路径规划算法。不过保留了A星经典的启发式思想，不过在混合A星中，添加了转弯、转向等惩罚。

之前在使用该算法时已经简单介绍了一下混合A星的源码，因为有些地方并不好用，所以对混合A星其他细节进行调整。

---

首先阅读node3d.cpp和node2d.cpp两个文件中定义的节点。

1.node3d

普通A星算法，在搜寻的过程中，一般为4邻域、8邻域的搜索方法。但是此方法搜寻得到的路径往往不满足车辆的运动学约束，因此在混合A星中，以满足当前转弯半径的角度前向或后向搜索。

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};
1
2
3

在混合A星算法中，提供了六个方向（允许倒车的情况下）。在搜寻下一节点时，通过定义好的$dx,dy,dz$计算得到新的节点坐标。

Node3D* Node3D::createSuccessor(const int i) {
  float xSucc;
  float ySucc;
  float tSucc;
  // calculate successor p ositions forward
  if (i < 3) {//前向 Successor
    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 {//后向 Successor
    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);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

对于一个3D节点，其代价函数updateG()的惩罚项加入了方向和角度。prim代表了当前节点的朝向，{0,1,2}分别代表左前、前、右前，{3,4,5}分别代表左后、后、右后。

如果当前朝向为前，此时则包括三种情况：
（1） 当前节点与前向节点方位一致，即pred->prim == prim，此时惩罚项则仅仅需要添加两节点的距离。
（2） 前向节点pred->prim > 2，即向后，此时方向和转角都和前向节点不一致，因此需要添加转向penaltyCOD和转角penaltyTurning两个惩罚项。
（3） 前向节点pred->prim < 2，即向前，此时仅仅是转角不一样，但时和前向节点的方向都是向前。
具体代码实现如下，后退情况同理：

  if (prim < 3) {//前进情况
    // penalize turning
    if (pred->prim != prim) {//方向发生改变时
      // penalize change of direction
      if (pred->prim > 2) {
        g += dx[0] * Constants::penaltyTurning * Constants::penaltyCOD;//改变方向的惩罚
      } else {
        g += dx[0] * Constants::penaltyTurning;//没有改变方向
      }
    } else {//方向没有发生变化
      g += dx[0];
    }
  }
1
2
3
4
5
6
7
8
9
10
11
12
13

对于一个3D的节点，判断该节点被访问过，即需要加入到closed中。不同于传统的A星算法，栅格坐标一样则被认为时同一个节点，需要额外判断两个节点的转向theta是否在一个阈值范围内。

2.node2d

node2d也就是传统的A星算法节点，以8邻域为例，以当前节点为中心向外扩张。

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);
}
1
2
3
4
5
6
7
8

---

3. 碰撞检测

在混合A星里，向外扩张的节点首先要满足在地图的阈值内nSucc->isOnGrid(width, height)，还要是未被访问的节点，即在open中，最后该节点不会产生碰撞。前面两个条件较容易判断，这里只看是否会产生碰撞configurationSpace.isTraversable(nSucc)。

在isTraversable函数中，对于2D节点的处理就直接获得当前节点的坐标值，并检查在栅格地图中是否占据，如果占据则存在障碍物。但是对于3D节点就比较复杂，因为其可能跨越了多个栅格。

static const int positionResolution = 10;//每个cell里的离散位置数量的平方根
int iX = (int)((x - (long)x) * Constants::positionResolution);//得出X方向在cell中的偏移量
int iY = (int)((y - (long)y) * Constants::positionResolution);//Y方向在cell中的偏移量
1
2
3

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;
    }//若grid的某个小网格存在值，说明有障碍，则返回false表示不在自由网格
  }
}
return true;//否则所有检测都没有检测到被占用，说明没有障碍，可以通行
1
2
3
4
5
6
7
8
9
10
11

在collisionLookup表中存储了不同朝向和偏移量下的车体姿态所占据的栅格。

---

在constants.h中定义了混合A星的相关参数
重要参数解释：
（1）可视化，该参数会将搜索过程可视化，方便找到问题。不过在该模式中，每次可视化会主动sleep，使速度变慢，可在源码中删去该过程。

static const bool visualization = true && manual;
1

// RViz visualization
if (Constants::visualization) {
  visualization.publishNode3DPoses(*nPred);
  visualization.publishNode3DPose(*nPred);
  d.sleep();
}
1
2
3
4
5
6

（2）确定车辆是否可以倒退，在混合A星的搜索过程中，可以倒退的话会以6个方向向外扩张，否则只以前三个方向。

static const bool reverse = true;
1

（3）地图分辨率大小

static const float cellSize = 1;
1

（4）惩罚项，分别包括转角、倒车、转向，具体在计算G值中介绍。

static const float penaltyTurning = 1.05;
static const float penaltyReversing = 2.0;
static const float penaltyCOD = 2.0;
1
2
3

（5）混合A星搜索时的步长

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};
1
2
3

（6）改变节点相等的阈值，使dubins更容易击中。