适于用阿克曼底盘的基于动力学约束的混合A*算法源码

参考翻译项目主页：
https://github.com/teddyluo/motion-planning-chs
https://github.com/karlkurzer/path_planner
核心步骤：

1. 调用hybridAStar()函数获取一条路径
2. 获取路径点(3D Node) -> 原始路径
3. 对路径依据Voronoi图进行平滑->平滑路径

	// FIND THE PATH
	Node3D* nSolution = Algorithm::hybridAStar(nStart, nGoal, nodes3D, nodes2D, width, height, 
    configurationSpace, dubinsLookup, visualization);
    // TRACE THE PATH
    smoother.tracePath(nSolution);
    // CREATE THE UPDATED PATH
    path.updatePath(smoother.getPath());
    // SMOOTH THE PATH
    smoother.smoothPath(voronoiDiagram);
    // CREATE THE UPDATED PATH
    smoothedPath.updatePath(smoother.getPath());

一丶algorithm.cpp 核心算法

主要函数为hybridAStar，首先确认可能的方向数量，其中3个用于正向行驶，另外3个用于反向行驶，如果可以后退则存在6种方向。

1. 计算起始点到目标的启发式值updateH
2. 将start加入open 集合

进入主循环， while (!O.empty())，从OPEN集合中取出一个最低代价的点，如果为closed，说明该点已经处理过，忽略(将它从open set中移除)，否则即为正在扩展的点。

将该点标记为closed，防止重复处理，然后将该点从open set中移除。

if (nodes3D[iPred].isClosed()) {
   O.pop();
   continue;
 }
else if (nodes3D[iPred].isOpen()) {
  nodes3D[iPred].close();
  O.pop();

如果当前点即为目标点，或者超出求解的迭代次数，则直接返回，表示搜索结束。

if (*nPred == goal || iterations > Constants::iterations) {
   return nPred;

否则继续寻找目标点，如果车子是在前进，优先考虑用Dubins去命中目标点。如果Dubins方法能直接命中，即不需要进入Hybrid A*搜索了，直接返回结果。

if (Constants::dubinsShot && nPred->isInRange(goal) && nPred->getPrim() < 3) {
    nSucc = dubinsShot(*nPred, goal, configurationSpace);
    if (nSucc != nullptr && *nSucc == goal) {
      return nSucc;
    }
  }

在上面的if条件中，有一个较为重要的参数调试isInRange，下面可以看到这里dubinsShotDistance默认为100，在地图范围较大的时候，该值是不是也需要调大？

bool Node3D::isInRange(const Node3D& goal) const {
  int random = rand() % 10 + 1;//产生位于[1, 10]的随机数
  float dx = std::abs(x - goal.x) / random;
  float dy = std::abs(y - goal.y) / random;
  return (dx * dx) + (dy * dy) < Constants::dubinsShotDistance;//距离的平方和在100以内则认为可达
}

这里补充一下Dubins方法，Dubins路径算法的原理是基于：在运动方向已知和转向半径最小的情况下，从初始向量到终止向量的最短的路径是由直线和最小半径转向圆弧组成的。

如果Dubins方法未能命中，则需要对每个方向去搜索，前向三种，后向三种，通过createSuccessor产生新的点。当前源码的dx，dy， dt为人为指定的值，可以根据实际需要进行修改。

  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]);
  }
  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]);
  }

产生新的点后，首先判断产生的节点是否在范围内，其次判断产生的节点会不会产生碰撞，只有同时满足在可视范围内且不产生碰撞的节点才是合格的节点。

 if (nSucc->isOnGrid(width, height) && configurationSpace.isTraversable(nSucc))

如果确定为合格的节点后，则确定新扩展的节点不在close list中，即没有被遍历过。

1. 更新合格点的G值
2. 如果扩展节点不在OPEN LIST中，或者找到了更短G值的路径，或者该节点索引与前一节点在同一网格中（用新点代替旧点）
3. 计算H值
4. 如果下一节点仍在相同的cell, 但是代价值要小，则用当前successor替代前一个节点
5. 将生成的子节点加入到open list中

二丶smoother.cpp 路径平滑

将得到的路径从后向前添加到path容器中。

void Smoother::tracePath(const Node3D* node, int i, std::vector<Node3D> path) {
  if (node == nullptr) {
    this->path = path;
    return;
  }
  i++;
  path.push_back(*node);
  tracePath(node->getPred(), i, path);
}

/// get a pointer to the predecessor
  Node2D* getPred() const { return pred; }

---

将tracePath跟踪得到的路径更新到path中。

  /// returns the path of the smoother object：返回平滑后的路径
  std::vector<Node3D> getPath() {return path;}

// 对每个3D节点信息，分别进行：增加线性addSegment、节点addNode、和车辆标记符addVehicle
void Path::updatePath(std::vector<Node3D> nodePath) {
  path.header.stamp = ros::Time::now();
  int k = 0;
  for (size_t i = 0; i < nodePath.size(); ++i) {
    addSegment(nodePath[i]);
    addNode(nodePath[i], k);
    k++;
    addVehicle(nodePath[i], k);
    k++;
  }
  return;
}

---

最终开始平滑路径

smoother.smoothPath(voronoiDiagram);

沿梯度下降，直到达到最大迭代次数为止。主要优化的四项权重分别为：平滑项wSmoothness ，坡度wCurvature ，voronoi的权重wVoronoi(等于0未用到)，障碍物权重wObstacle（0.2）。

 // descent along the gradient untill the maximum number of iterations has been reached
  float totalWeight = wSmoothness + wCurvature + wVoronoi + wObstacle;//四项的权重数

进入主要的循环过程，while (iterations < maxIterations)，在迭代次数限制之内不停的优化。

每次选择五个点，即当前点，和前面两个点，以及后面两个点。

for (int i = 2; i < pathLength - 2; ++i) {
      Vector2D xim2(newPath[i - 2].getX(), newPath[i - 2].getY());
      Vector2D xim1(newPath[i - 1].getX(), newPath[i - 1].getY());
      Vector2D xi(newPath[i].getX(), newPath[i].getY());
      Vector2D xip1(newPath[i + 1].getX(), newPath[i + 1].getY());
      Vector2D xip2(newPath[i + 2].getX(), newPath[i + 2].getY());
      Vector2D correction;
｝

以下几点将不被平滑

1. 如果这些点是尖点或与某个点相邻，则将其保持固定。

if (isCusp(newPath, i)) { continue; }

2. 假如校正方向超出当前监视的网格范围，不做处理

correction = correction - obstacleTerm(xi);
      if (!isOnGrid(xi + correction)) { continue; }

在确保平滑点在网格内的情况下，计算校正方向。

correction = correction - obstacleTerm(xi); //返回离最近障碍的梯度方向
correction = correction - smoothnessTerm(xim2, xim1, xi, xip1, xip2);
correction = correction - curvatureTerm(xim1, xi, xip1);//返回梯度方向

得到新的路径点，赋值并计算方向角。

 xi = xi + alpha * correction/totalWeight;
 newPath[i].setX(xi.getX());
 newPath[i].setY(xi.getY());
 Vector2D Dxi = xi - xim1;
 newPath[i - 1].setT(std::atan2(Dxi.getY(), Dxi.getX()));

循环结束后，更新路径点并发布。

该算法中，计算路径的停止条件有两个，一个是搜索到达目标点的路径，二是达到最大迭代次数。这就导致，当路径过于繁琐的时候，无法规划出一条完整的到达目标点的路径，因此这里将其第二个条件去掉：

这样将不会出现规划路径不完整的情况。