Fast Planner——kinodynamic_astar.cpp代码分析

叉车司机

已于 2024-09-11 19:41:43 修改

阅读量724

点赞数 23

分类专栏：轨迹规划文章标签：算法

于 2024-08-15 13:50:30 首次发布

本文链接：https://blog.csdn.net/m0_61616957/article/details/140266469

版权

轨迹规划专栏收录该内容

3 篇文章 0 订阅

订阅专栏

参考文章：Faster Planner——Kinodynamic Astar详解-CSDN博客

代码链接：GitHub - HKUST-Aerial-Robotics/Fast-Planner: A Robust and Efficient Trajectory Planner for Quadrotors

1. KinodynamicAstar 类

用于实现一个考虑动力学约束的A*搜索算法来规划路径

1.1 KinodynamicAstar::search

int KinodynamicAstar::search(Eigen::Vector3d start_pt, Eigen::Vector3d start_v, Eigen::Vector3d start_a,
                             Eigen::Vector3d end_pt, Eigen::Vector3d end_v, bool init, bool dynamic, double time_start)
{
  // 初始化起点
  start_vel_ = start_v;
  start_acc_ = start_a;

  // 设置起点节点信息
  PathNodePtr cur_node = path_node_pool_[0];
  ...
  cur_node->g_score = 0.0; // 实际代价

  // 初始化终点信息
  Eigen::VectorXd end_state(6);
  ...

  // 设置终点状态和索引
  end_state.head(3) = end_pt;
  ...

  // 计算起点节点的启发函数代价值并加入open列表
  cur_node->f_score = lambda_heu_ * estimateHeuristic(cur_node->state, end_state, time_to_goal);
  cur_node->node_state = IN_OPEN_SET;
  open_set_.push(cur_node);
  use_node_num_ += 1;
  
  // 动态环境初始化
  if (dynamic)
  {
    ...
  }
  ...
  // 主搜索循环
  while (!open_set_.empty())
  {
    cur_node = open_set_.top();
    ...
   // 检查是否达到终点或达到搜索范围的边界
   bool reach_horizon = (cur_node->state.head(3) - start_pt).norm() >= horizon_;
    ...
    // 如果达到终点或搜索范围边界，则终止搜索
    if (reach_horizon || near_end)
    {
      ...
    }
    ...

    // 检查是否存在直接连接的路径
    if (near_end)
    ...
    // 从open列表移除当前节点，并标记
    open_set_.pop();
    ...
    // 生成可能的输入加速度向量和时间组合，拓展当前节点
    for (int i = 0; i < inputs.size(); ++i)
      for (int j = 0; j < durations.size(); ++j)
      {    
        ...
        // 检查扩展节点的有效性()，并更新代价和启发函数
        ...
      }
     ...
    }
    ...    
}

主要步骤：

1.初始化起点节点：设置起点位置、速度、加速度等信息，并计算启发函数值将其加入open_set；

2.主搜索循环 while (!open_set_.empty()) ：

a.对每个open_set 中的节点，检查其是否到达预设范围（horizon）或接近终点；

b.如果到达终点或预设范围，检索路径并返回成功;

c.否则，生成可能的输入（加速度）和持续时间，拓展当前节点；

d.对于每个可能的输入和持续时间，计算下一个状态并检查其有效性（速度限制、障碍物碰撞），并更新代价和启发函数；

e.将有效的拓展节点添加到open_set中。

3.路径找到后的处理：如果找到路径或满足终止条件，调用retrievePath函数从终点回溯到起点，构建完整路径。

辅助函数：

stateTransit：根据输入加速度和时间，将当前状态转换到下一状态。
estimateHeuristic：估计启发式成本。
computeShotTraj：计算从当前节点到终点的直接路径是否可行。

1.2 KinodynamicAstar::setParam

void KinodynamicAstar::setParam(ros::NodeHandle& nh)
{
  nh.param("search/max_tau", max_tau_, -1.0);
  ...
}

设置参数 ：从ROS节点句柄 (nh) 中获取参数并设置类成员变量。

1.3 KinodynamicAstar::retrievePath

void KinodynamicAstar::retrievePath(PathNodePtr end_node)
{
  ...
}

路径回溯检索：

从终点节点开始，沿着父节点链回溯，直到起点节点。
将路径节点存储在 path_nodes_ 中，并将其顺序反转，以得到从起点到终点的路径

1.4 KinodynamicAstar::estimateHeuristic

double KinodynamicAstar::estimateHeuristic(Eigen::VectorXd x1, Eigen::VectorXd x2, double& optimal_time)
{
  // x1 、x2 起始和目标状态，包括位置和速度
  const Vector3d dp = x2.head(3) - x1.head(3); // x1和x2间位置差异
  const Vector3d v0 = x1.segment(3, 3); // 起始位置速度
  const Vector3d v1 = x2.segment(3, 3); // 目标位置速度
  
  // 多项式系数
  double c1 = -36 * dp.dot(dp);
  double c2 = 24 * (v0 + v1).dot(dp);
  double c3 = -4 * (v0.dot(v0) + v0.dot(v1) + v1.dot(v1));
  double c4 = 0;
  double c5 = w_time_;
  
  // 求解五次多项式，得到Th
  std::vector<double> ts = quartic(c5, c4, c3, c2, c1);
  
  // 求最短估计时间
  double v_max = max_vel_ * 0.5;
  double t_bar = (x1.head(3) - x2.head(3)).lpNorm<Infinity>() / v_max;
  ts.push_back(t_bar);

  double cost = 100000000;
  double t_d = t_bar;
  
  // 循环计算多项式的解 计算对应代价，找到最小代价和时间
  for (auto t : ts)
  {
    ...
  }

  optimal_time = t_d;

  return 1.0 * (1 + tie_breaker_) * cost;
}

启发函数：计算当前状态到目标状态的启发函数值。

根据论文中求得的最优启发代价函数 $eq?J%5E%7B*%7D%28T%29%3D%5Csum_%7B%5Cmu%20%5Cin%20%28x%2Cy%2Cz%29%7D%5E%7B%7D%5Cleft%20%28%20%5Cfrac%7B1%7D%7B3%7D%5Calpha%20_%7B%5Cmu%20%7D%5E%7B2%7DT%5E%7B3%7D+%5Calpha%20_%7B%5Cmu%20%7D%5Cbeta%20_%7B%5Cmu%20%7DT%5E%7B2%7D+%5Cbeta%20_%7B%5Cmu%20%7D%5E%7B2%7DT%5E%7B2%7D%20%5Cright%20%29$ ，将系数项 $eq?%5Calpha%20_%7B%5Cmu%20%7D$ 、 $eq?%5Cbeta%20_%7B%5Cmu%20%7D$ 带入整理得：

$eq?J%5E%7B*%7D%28T%29%3D%5Csum_%7B%5Cmu%20%5Cin%20%28x%2Cy%2Cz%29%7D%5E%7B%7D%5Cleft%20%28%20%5Cfrac%7B12%7D%7BT%5E%7B3%7D%7D%5Cleft%20%28%20p_%7B%5Cmu%20c%7D-p_%7B%5Cmu%20g%7D%20%5Cright%20%29%5E%7B2%7D-%5Cfrac%7B12%7D%7BT%5E%7B2%7D%7D%5Cleft%20%28%20v_%7B%5Cmu%20c%7D+v_%7B%5Cmu%20g%7D%20%5Cright%20%29%5Cleft%20%28%20p_%7B%5Cmu%20g%7D-p_%7B%5Cmu%20c%7D%20%5Cright%20%29+%5Cfrac%7B4%7D%7BT%7D%5Cleft%20%28%20v_%7B%5Cmu%20c%7D%5E%7B2%7D%20+v_%7B%5Cmu%20c%7D*v_%7B%5Cmu%20g%7D+v_%7B%5Cmu%20g%7D%5E%7B2%7D%5Cright%20%29%5Cright%20%29$

令其对 $eq?T$ 求偏导：

$eq?%5Cfrac%7B%5Cpartial%20J%5E%7B*%7D%28T%29%7D%7B%5Cpartial%20T%7D%3D%5Csum_%7B%5Cmu%20%5Cin%20%28x%2Cy%2Cz%29%7D%5E%7B%7D%5Cleft%20%28%20-%5Cfrac%7B36%7D%7BT%5E%7B4%7D%7D%5Cleft%20%28%20p_%7B%5Cmu%20c%7D-p_%7B%5Cmu%20g%7D%20%5Cright%20%29%5E%7B2%7D+%5Cfrac%7B24%7D%7BT%5E%7B3%7D%7D%5Cleft%20%28%20v_%7B%5Cmu%20c%7D+v_%7B%5Cmu%20g%7D%20%5Cright%20%29%5Cleft%20%28%20p_%7B%5Cmu%20g%7D-p_%7B%5Cmu%20c%7D%20%5Cright%20%29-%5Cfrac%7B4%7D%7BT%5E%7B2%7D%7D%5Cleft%20%28%20v_%7B%5Cmu%20c%7D%5E%7B2%7D+v_%7B%5Cmu%20c%7Dv_%7B%5Cmu%20g%7D+v_%7B%5Cmu%20g%7D%5E%7B2%7D%20%5Cright%20%29%20%5Cright%20%29$

式中各项分别对应多项式系数 c1-c5，c5 为时间权重。代码中设 $eq?t%3D%5Cfrac%7B1%7D%7BT%7D$ ，求解该四次多项式的根。求解四次多项式的根时，调用 quartic() 函数，该函数使用费拉里方法来求解四次多项式的根。在求解三次多项式时，代码使用两种方法，当判别式大于0及等于0的情况利用求根公式，判别式小于0的情况使用三角函数解法。

1.5 KinodynamicAstar::computeShotTraj

bool KinodynamicAstar::computeShotTraj(Eigen::VectorXd state1, Eigen::VectorXd state2, double time_to_goal)
{
  /* ---------- get coefficient ---------- */
  const Vector3d p0 = state1.head(3);
  const Vector3d dp = state2.head(3) - p0;
  const Vector3d v0 = state1.segment(3, 3);
  const Vector3d v1 = state2.segment(3, 3);
  const Vector3d dv = v1 - v0;
  double t_d = time_to_goal;
  MatrixXd coef(3, 4);
  end_vel_ = v1;

  Vector3d a = 1.0 / 6.0 * (-12.0 / (t_d * t_d * t_d) * (dp - v0 * t_d) + 6 / (t_d * t_d) * dv);
  Vector3d b = 0.5 * (6.0 / (t_d * t_d) * (dp - v0 * t_d) - 2 / t_d * dv);
  Vector3d c = v0;
  Vector3d d = p0;

  // 1/6 * alpha * t^3 + 1/2 * beta * t^2 + v0
  // a*t^3 + b*t^2 + v0*t + p0
  coef.col(3) = a, coef.col(2) = b, coef.col(1) = c, coef.col(0) = d;

  Vector3d coord, vel, acc;
  VectorXd poly1d, t, polyv, polya;
  Vector3i index;

  Eigen::MatrixXd Tm(4, 4);
  Tm << 0, 1, 0, 0, 0, 0, 2, 0, 0, 0, 0, 3, 0, 0, 0, 0;

  /* ---------- forward checking of trajectory ---------- */
  double t_delta = t_d / 10;
  for (double time = t_delta; time <= t_d; time += t_delta)
  {
    t = VectorXd::Zero(4);
    for (int j = 0; j < 4; j++)
      t(j) = pow(time, j);

    for (int dim = 0; dim < 3; dim++)
    {
      poly1d = coef.row(dim);
      coord(dim) = poly1d.dot(t);
      vel(dim) = (Tm * poly1d).dot(t);
      acc(dim) = (Tm * Tm * poly1d).dot(t);

      if (fabs(vel(dim)) > max_vel_ || fabs(acc(dim)) > max_acc_)
      {
        // cout << "vel:" << vel(dim) << ", acc:" << acc(dim) << endl;
        // return false;
      }
    }

    if (coord(0) < origin_(0) || coord(0) >= map_size_3d_(0) || coord(1) < origin_(1) || coord(1) >= map_size_3d_(1) ||
        coord(2) < origin_(2) || coord(2) >= map_size_3d_(2))
    {
      return false;
    }

    // if (edt_environment_->evaluateCoarseEDT(coord, -1.0) <= margin_) {
    //   return false;
    // }
    if (edt_environment_->sdf_map_->getInflateOccupancy(coord) == 1)
    {
      return false;
    }
  }
  coef_shot_ = coef;
  t_shot_ = t_d;
  is_shot_succ_ = true;
  return true;
}

计算从当前节点到终点的直接路径是否可行

1.6 KinodynamicAstar::cubic

vector<double> KinodynamicAstar::cubic(double a, double b, double c, double d)
{
  ...
}

求解三次多项式

1.7 KinodynamicAstar::quartic

vector<double> KinodynamicAstar::quartic(double a, double b, double c, double d, double e)
{
 ...
}

quartic 函数用来求解五次多项式的根，得到时间的候选值：

1.8 KinodynamicAstar::init()

void KinodynamicAstar::init()
{
  /* ---------- map params ---------- */
  this->inv_resolution_ = 1.0 / resolution_;
  inv_time_resolution_ = 1.0 / time_resolution_;
  edt_environment_->sdf_map_->getRegion(origin_, map_size_3d_);

  cout << "origin_: " << origin_.transpose() << endl;
  cout << "map size: " << map_size_3d_.transpose() << endl;

  /* ---------- pre-allocated node ---------- */
  path_node_pool_.resize(allocate_num_);
  for (int i = 0; i < allocate_num_; i++)
  {
    path_node_pool_[i] = new PathNode;
  }

  phi_ = Eigen::MatrixXd::Identity(6, 6);
  use_node_num_ = 0;
  iter_num_ = 0;
}

功能: 初始化Astar对象，设置地图参数并预分配节点池。
实现: 获取地图的区域信息，初始化节点池。

1.9 KinodynamicAstar::setEnvironment

void KinodynamicAstar::setEnvironment(const EDTEnvironment::Ptr& env)
{
  this->edt_environment_ = env;
}

功能: 设置环境评估对象。
实现: 将传入的环境评估对象赋值给成员变量edt_environment_。

1.10 KinodynamicAstar::reset()

void KinodynamicAstar::reset()
{
  expanded_nodes_.clear();
  path_nodes_.clear();

  std::priority_queue<PathNodePtr, std::vector<PathNodePtr>, NodeComparator> empty_queue;
  open_set_.swap(empty_queue);

  for (int i = 0; i < use_node_num_; i++)
  {
    PathNodePtr node = path_node_pool_[i];
    node->parent = NULL;
    node->node_state = NOT_EXPAND;
  }

  use_node_num_ = 0;
  iter_num_ = 0;
  is_shot_succ_ = false;
  has_path_ = false;
}

功能: 重置Astar对象的状态，以便进行新的搜索。
实现: 清空已扩展节点集合、路径节点集合，重置优先级队列和节点池的状态。

1.11 KinodynamicAstar::getKinoTraj

std::vector<Eigen::Vector3d> KinodynamicAstar::getKinoTraj(double delta_t)
{
  vector<Vector3d> state_list;

  /* ---------- get traj of searching ---------- */
  PathNodePtr node = path_nodes_.back();
  Matrix<double, 6, 1> x0, xt;

  while (node->parent != NULL)
  {
    Vector3d ut = node->input;
    double duration = node->duration;
    x0 = node->parent->state;

    for (double t = duration; t >= -1e-5; t -= delta_t)
    {
      stateTransit(x0, xt, ut, t);
      state_list.push_back(xt.head(3));
    }
    node = node->parent;
  }
  reverse(state_list.begin(), state_list.end());
  /* ---------- get traj of one shot ---------- */
  if (is_shot_succ_)
  {
    Vector3d coord;
    VectorXd poly1d, time(4);

    for (double t = delta_t; t <= t_shot_; t += delta_t)
    {
      for (int j = 0; j < 4; j++)
        time(j) = pow(t, j);

      for (int dim = 0; dim < 3; dim++)
      {
        poly1d = coef_shot_.row(dim);
        coord(dim) = poly1d.dot(time);
      }
      state_list.push_back(coord);
    }
  }

  return state_list;
}

1.12 KinodynamicAstar::getSamples

void KinodynamicAstar::getSamples(double& ts, vector<Eigen::Vector3d>& point_set,
                                  vector<Eigen::Vector3d>& start_end_derivatives)
{
  /* ---------- path duration ---------- */
  double T_sum = 0.0;
  if (is_shot_succ_)
    T_sum += t_shot_;
  PathNodePtr node = path_nodes_.back();
  while (node->parent != NULL)
  {
    T_sum += node->duration;
    node = node->parent;
  }
  // cout << "duration:" << T_sum << endl;

  // Calculate boundary vel and acc
  Eigen::Vector3d end_vel, end_acc;
  double t;
  if (is_shot_succ_)
  {
    t = t_shot_;
    end_vel = end_vel_;
    for (int dim = 0; dim < 3; ++dim)
    {
      Vector4d coe = coef_shot_.row(dim);
      end_acc(dim) = 2 * coe(2) + 6 * coe(3) * t_shot_;
    }
  }
  else
  {
    t = path_nodes_.back()->duration;
    end_vel = node->state.tail(3);
    end_acc = path_nodes_.back()->input;
  }

  // Get point samples
  int seg_num = floor(T_sum / ts);
  seg_num = max(8, seg_num);
  ts = T_sum / double(seg_num);
  bool sample_shot_traj = is_shot_succ_;
  node = path_nodes_.back();

  for (double ti = T_sum; ti > -1e-5; ti -= ts)
  {
    if (sample_shot_traj)
    {
      // samples on shot traj
      Vector3d coord;
      Vector4d poly1d, time;

      for (int j = 0; j < 4; j++)
        time(j) = pow(t, j);

      for (int dim = 0; dim < 3; dim++)
      {
        poly1d = coef_shot_.row(dim);
        coord(dim) = poly1d.dot(time);
      }

      point_set.push_back(coord);
      t -= ts;

      /* end of segment */
      if (t < -1e-5)
      {
        sample_shot_traj = false;
        if (node->parent != NULL)
          t += node->duration;
      }
    }
    else
    {
      // samples on searched traj
      Eigen::Matrix<double, 6, 1> x0 = node->parent->state;
      Eigen::Matrix<double, 6, 1> xt;
      Vector3d ut = node->input;

      stateTransit(x0, xt, ut, t);

      point_set.push_back(xt.head(3));
      t -= ts;

      // cout << "t: " << t << ", t acc: " << T_accumulate << endl;
      if (t < -1e-5 && node->parent->parent != NULL)
      {
        node = node->parent;
        t += node->duration;
      }
    }
  }
  reverse(point_set.begin(), point_set.end());

  // calculate start acc
  Eigen::Vector3d start_acc;
  if (path_nodes_.back()->parent == NULL)
  {
    // no searched traj, calculate by shot traj
    start_acc = 2 * coef_shot_.col(2);
  }
  else
  {
    // input of searched traj
    start_acc = node->input;
  }

  start_end_derivatives.push_back(start_vel_);
  start_end_derivatives.push_back(end_vel);
  start_end_derivatives.push_back(start_acc);
  start_end_derivatives.push_back(end_acc);
}

功能：从路径中生成样本点。
步骤：
- 遍历路径中的每一个节点对，计算每一段路径的中间点。
- 将每段路径上的点加入point_set中，将起点和终点的速度信息加入start_end_derivatives中
意义：从路径中生成一系列样本点。
作用：
- 对路径中的每一段进行离散化，计算并存储路径上的点。
- 将路径的起点和终点的速度信息存储到start_end_derivatives中。

1.13 KinodynamicAstar::getVisitedNodes()

std::vector<PathNodePtr> KinodynamicAstar::getVisitedNodes()
{
  vector<PathNodePtr> visited;
  visited.assign(path_node_pool_.begin(), path_node_pool_.begin() + use_node_num_ - 1);
  return visited;
}

1.14 其他辅助函数

// 将位置转换为索引。
Eigen::Vector3i KinodynamicAstar::posToIndex(Eigen::Vector3d pt)
{
  Vector3i idx = ((pt - origin_) * inv_resolution_).array().floor().cast<int>();

  // idx << floor((pt(0) - origin_(0)) * inv_resolution_), floor((pt(1) -
  // origin_(1)) * inv_resolution_),
  //     floor((pt(2) - origin_(2)) * inv_resolution_);

  return idx;
}

// 将时间转换为索引
int KinodynamicAstar::timeToIndex(double time)
{
    int idx = floor((time - time_origin_) * inv_time_resolution_);
    return idx;
}

//意义：根据当前状态、输入加速度和持续时间计算下一个状态。
//作用：通过动力学方程更新状态信息，用于生成下一个节点的状态。

void KinodynamicAstar::stateTransit(Eigen::Matrix<double, 6, 1>& state0, Eigen::Matrix<double, 6, 1>& state1,
                                    Eigen::Vector3d um, double tau)
{
  for (int i = 0; i < 3; ++i)
    phi_(i, i + 3) = tau;

  Eigen::Matrix<double, 6, 1> integral;
  integral.head(3) = 0.5 * pow(tau, 2) * um;
  integral.tail(3) = tau * um;

  state1 = phi_ * state0 + integral;
}

}  // namespace fast_planner