Ackermanシャーシでの使用に適した動的制約に基づくハイブリッドA*アルゴリズムのソースコード

この記事は、「新人クリエーションセレモニー」イベントに参加し、一緒にゴールドクリエーションの道を歩み始めました。

翻訳プロジェクトのホームページを参照してください：github.com/teddyluo/mo…github.com/karlkurzer/…コアステップ：

hybridAStar（）関数を呼び出してパスを取得します
パスポイントを取得（3Dノード）->元のパス
ボロノイ図に従ってパスをスムーズにする->パスをスムーズにする

	// 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());
复制代码

1.algorithm.cppのコアアルゴリズム

主な機能はhybridAStar、最初に可能な方向の数を確認することです。そのうち3つは前進運転に使用され、他の3つは後進運転に使用され、後進が可能な場合は6つの方向があります。

開始点から目標までのヒューリスティック値を計算しますupdateH
開いているコレクションに開始を追加します

メインループに入り、セット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。デフォルト値が100であることがわかりますdubinsShotDistance。マップ範囲が大きい場合、この値も増やす必要がありますか？

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中，即没有被遍历过。

更新合格点的G值
如果扩展节点不在OPEN LIST中，或者找到了更短G值的路径，或者该节点索引与前一节点在同一网格中（用新点代替旧点）
计算H值
如果下一节点仍在相同的cell, 但是代价值要小，则用当前successor替代前一个节点
将生成的子节点加入到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;
｝
复制代码

以下几点将不被平滑

ポイントが尖点またはポイントに隣接している場合は、固定してください。

if (isCusp(newPath, i)) { continue; }
复制代码

補正方向が現在監視しているグリッド範囲を超えた場合、処理は行われません。

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()));
复制代码

ループが終了したら、ウェイポイントを更新して公開します。

このアルゴリズムでは、パスを計算するための2つの停止条件があります。1つはパスを検索してターゲットポイントに到達すること、もう1つは最大反復回数に到達することです。これにより、パスが煩雑すぎると、ターゲットポイントへの完全なパスを計画できないため、2番目の条件がここで削除されます。

ここに画像の説明を挿入

このようにして、計画されたパスが不完全になる状況はありません。

ここに画像の説明を挿入