Fast Planner规划算法（四）—— Fast Planner程序框架

---

   本系列文章用于学习记录Fast-Planner规划算法的相关内容，主要学习资料是深蓝学院的移动机器人运动规划课程

   五、Fast Planner程序框架

   Fast Planner算法相关的程序存放在fast_planner文件夹内

   fast_planner文件夹下包含八个文件夹，各文件夹的作用如下：

   （1）、bag：脚本文件

   （2）、bspline：非均匀B样条，用于后端时间重分配部分

   （3）、bspline_opt ：均匀B样条，用于后端轨迹优化部分

   （4）、path_searching：包含A*算法、Hybrid A * 算法、基于拓扑的PRM算法，三个规划算法，用于前端的路径规划

   （5）、plan_env：用于地图生成，包括栅格地图生成、ESDF生成等

   （6）、plan_manage：主文件夹，主节点所在文件夹，

   （7）、poly_traj：定义了一个多项式的轨迹类

   （8）、traj_utils：可视化相关内容

   要理解Fast Planner的程序框架，就应该从主节点所在的plan_manage文件夹开始：

   （1）fast_planner_node.cpp

   fast_planner_node.cpp文件是我们通过launch文件启动Fast Planner程序的接口，也是ROS中Fast Planner的主节点，该文件在完成节点初始化后，会根据参数planner的值选择进行kino_replan或者topo_replan的初始化。

   （2）kino_replan_fsm.cpp

   若参数planner的值为1，则执行kino_replan的初始化，kino_replan的初始化的具体程序在kino_replan_fsm.cpp文件中，这个文件也是ROS跟 Fast Planner程序的接口性程序，包括参数加载、回调、订阅等。

   kino_replan_fsm.cpp文件中的execFSMCallback函数是状态基，用于处理在机器人运行过程中遇到各种情况下，是选择急停、还是重新规划、还是继续执行等。

void KinoReplanFSM::execFSMCallback(const ros::TimerEvent& e)

   kino_replan_fsm.cpp文件中的checkCollisionCallback是用于检查碰撞的函数

void KinoReplanFSM::checkCollisionCallback(const ros::TimerEvent& e)

   kino_replan_fsm.cpp文件中的callKinodynamicReplan函数中，会调用kinodynamicReplan函数，本系列前三篇文章介绍的内容全包含在和这个函数中，即局部的包含前端和后端的规划方法。该函数在planner_manager.cpp文件中。

bool KinoReplanFSM::callKinodynamicReplan()

 bool plan_success =
      planner_manager_->kinodynamicReplan(start_pt_, start_vel_, start_acc_, end_pt_, end_vel_);

---

   ☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆

   （3）planner_manager.cpp

   kino_replan_fsm.cpp文件通过调用kinodynamicReplan函数来调用该文件。

bool FastPlannerManager::kinodynamicReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel,
                                           Eigen::Vector3d start_acc, Eigen::Vector3d end_pt,
                                           Eigen::Vector3d end_vel)

   kinodynamicReplan函数中完成一些初始化操作后，首先后使用Hybrid A * 算法进行前端的路径搜索，该部分内容通过调用kinodynamic_astar.cpp中的search函数完成。

 int status = kino_path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel, true);

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)

   接下来会将前端得到的路径转换成B样条曲线的形式，作为后端优化的初值，该部分内容在parameterizeToBspline，这个函数的具体程序在non_uniform_bspline.cpp文件中

NonUniformBspline::parameterizeToBspline(ts, point_set, start_end_derivatives, ctrl_pts);

void NonUniformBspline::parameterizeToBspline(const double& ts, const vector<Eigen::Vector3d>& point_set,
                                              const vector<Eigen::Vector3d>& start_end_derivative,
                                              Eigen::MatrixXd&               ctrl_pts)

   接下来就是后端的使用均匀B样条及优化算法进行轨迹优化了，该部分内容是通过调用BsplineOptimizeTraj函数来完成的，该函数的具体程序在bspline_optimizer.cpp文件中

ctrl_pts = bspline_optimizers_[0]->BsplineOptimizeTraj(ctrl_pts, ts, cost_function, 1, 1);

Eigen::MatrixXd BsplineOptimizer::BsplineOptimizeTraj(const Eigen::MatrixXd& points, const double& ts,
                                                      const int& cost_function, int max_num_id,
                                                      int max_time_id)

   最后，就是时间的重分配了，通过调用非均匀的B样条来进行时间的重分配，该部分内容在类NonUniformBspline中完成，该类的具体程序在non_uniform_bspline.cpp文件中

  NonUniformBspline pos = NonUniformBspline(ctrl_pts, 3, ts);

  double to = pos.getTimeSum();
  pos.setPhysicalLimits(pp_.max_vel_, pp_.max_acc_);
  bool feasible = pos.checkFeasibility(false);

  int iter_num = 0;
  while (!feasible && ros::ok()) {

    feasible = pos.reallocateTime();

    if (++iter_num >= 3) break;
  }

NonUniformBspline::NonUniformBspline(const Eigen::MatrixXd& points, const int& order,
                                     const double& interval) {
  setUniformBspline(points, order, interval);
}

   上面就是 Fast Planner程序的大概流程，跟前三篇文章 介绍的流程是相同的。

   ☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆

---

   我们现在来看一下上面流程中，比较重要的使用均匀B样条曲线及优化算法进行后端优化的位于bspline_optimizer.cpp文件中的BsplineOptimizeTraj函数

Eigen::MatrixXd BsplineOptimizer::BsplineOptimizeTraj(const Eigen::MatrixXd& points, const double& ts,
                                                      const int& cost_function, int max_num_id,
                                                      int max_time_id)

   首先是对优化问题进行一些设置，points是控制点的初值，ts是B样条的时间，setCostFunction(cost_function)是选择在优化过程中加入那些类型的代价，如平滑、无碰撞、距离，等代价函数。

  setControlPoints(points);
  setBsplineInterval(ts);
  setCostFunction(cost_function);
  setTerminateCond(max_num_id, max_time_id);

   然后，执行optimize()函数来进行优化

 optimize();

   在optimize()函数中需要特别注意的是需要最小化的目标函数的设置，即costFunction函数的计算

 opt.set_min_objective(BsplineOptimizer::costFunction, this);

   costFunction函数计算目标函数的具体程序如下：

double BsplineOptimizer::costFunction(const std::vector<double>& x, std::vector<double>& grad,
                                      void* func_data) {
  BsplineOptimizer* opt = reinterpret_cast<BsplineOptimizer*>(func_data);
  double            cost;
  opt->combineCost(x, grad, cost);
  opt->iter_num_++;

  /* save the min cost result */
  if (cost < opt->min_cost_) {
    opt->min_cost_      = cost;
    opt->best_variable_ = x;
  }
  return cost;
}

   具体cost的计算是在其调用的combineCost函数中

void BsplineOptimizer::combineCost(const std::vector<double>& x, std::vector<double>& grad,
                                   double& f_combine) 

   在计算cost时，我们传入的是控制点，得到的是目标函数值及其梯度值

---

---

   参考资料：

   1、深蓝学院-移动机器人运动规划

---