[数据结构与算法]混合式A星代码解析_4 修正优化算法smoothPath.cpp

5. 优化函数Smoother

5.1 tracePath()

• 功能:此函数的功能就是要找到一条从start节点到goal节点的路径path.
• 参数:此处带入的初始节点指针node是已经找到的goal的指针,i是第回溯到第一个节点了,path是回溯的路径.

本函数采用了递归调用的模式,不断去找node的前任节点node->getPred(),不断更新路径path,知道node为空指针nullptr,意味着已经到了start节点.

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

5.2 smoothPath()

• 功能:把tracePath()回溯到的路径path进行优化
• 参数:

void Smoother::smoothPath(DynamicVoronoi& voronoi) {
  
}

• 步骤:

5.2.1 初始化参数

包括voronoi的尺寸,最大叠代次数和路径长度.

  // load the current voronoi diagram into the smoother
  this->voronoi = voronoi;
  this->width = voronoi.getSizeX();
  this->height = voronoi.getSizeY();
  // current number of iterations of the gradient descent smoother
  int iterations = 0;
  // the maximum iterations for the gd smoother
  int maxIterations = 500;
  // the lenght of the path in number of nodes
  int pathLength = 0;

path的数据结构是vector.而pathLength = path.size(),意味着pathLength的意思就是path中节点的数量.
totalWeight计算公式中的变量都是超参数,是固定值.

• wSmoothness 0.2
• wCurvature 0
• wVoronoi 0
• wObstacle 0.2

 // path objects with all nodes oldPath the original, newPath the resulting smoothed path
  pathLength = path.size();
  std::vector<Node3D> newPath = path;

  // descent along the gradient untill the maximum number of iterations has been reached
  float totalWeight = wSmoothness + wCurvature + wVoronoi + wObstacle;

5.2.2 开始叠代优化

while (iterations < maxIterations) {
	 // choose the first three nodes of the path
	 for (int i = 2; i < pathLength - 2; ++i) {
	  ...}
	 iterations++;
}

(1)从第三个点开始选取,每次选5个

      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;

(2)如果有尖角,则跳过该点

      // the following points shall not be smoothed
      // keep these points fixed if they are a cusp point or adjacent to one
      if (isCusp(newPath, i)) { continue; }

如下图所示,左2就是一个尖角.

(3)分别进行障碍物修正obstacleTerm,平滑度修正smoothnessTerm和曲线修正curvatureTerm
不断累加修正值correction,并判断修正后的节点还是否位于地图以内.

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

      //todo not implemented yet
      // voronoiTerm(); 

      // ensure that it is on the grid
      correction = correction - smoothnessTerm(xim2, xim1, xi, xip1, xip2);
      if (!isOnGrid(xi + correction)) { continue; }

      // ensure that it is on the grid
      correction = correction - curvatureTerm(xim1, xi, xip1);
      if (!isOnGrid(xi + correction)) { continue; }

(4)对xi节点进行修正,并且对xi节点前一个节点的角度进行修正setT

      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()));