FastPlanner论文解读（一）——前端路径搜索

本系列将更新FastPlanner论文内容对应代码的解读，其github的工程位置如下：

GitCode - 全球开发者的开源社区,开源代码托管平台

一、摘要

        在本文中，提出了一种稳健高效的四旋翼运动规划系统，主要架构如下：

1.kinodynamic 路径搜索方法在离散控制空间中找到一个安全的、kinodynamic 可行且时间最短初始轨迹。

2.我们通过 B 样条优化来改善轨迹的平滑度和间隙，该优化结合了来自欧几里得距离场 （EDF） 的梯度信息和动态约束，有效地利用了 B 样条的凸包特性。

3.将最终轨迹表示为非均匀 B 样条，采用迭代时间调整方法来保证动态可行和非保守轨迹。

前端采用Hybrid A*算法初步搜索出一条路径，进入后端使用软约束优化得到优化好的B样条曲线，再将其表示为非均匀B样条的方式，调整控制点和时间间隔。

二、前端

1.算法架构

        前端的kinodynamic path searching使用hybrid A*算法，在地图中搜索出一条安全的、动力学的可行轨迹，算法流程如下：

2.图搜索算法

        首先我们说明图搜索算法的思路：

图搜索算法本质上是在维护一个容器，里面装着未来可能访问的节点，开始为容器里面节点数量为0，然后放入第一个节点进行循环，在循环中：

1.把容器中最符合条件的节点找到并访问弹出

2.将这个节点对周围的节点进行扩展

3.将扩展的新节点放入容器中

        同时，为了防止节点被反复循环，还需要另外一个容器装载被访问的节点，后面搜索时不能再进行访问。

        在这个架构下我们分析经典的基于搜索的算法：

Dijkstra算法每次弹出容器中距离源节点代价最小的节点（距离源节点的代价我们使用g来表示）。

A*算法容器的代价函数记作f，其由两个部分组成，第一个部分是g，即从源节点到当前节点累计代价，第二个部分是h，h代表启发式Heuristic代价，f=g+h。对于h的衡量有多种选择，如L2距离、曼哈顿距离等。

        在原本的A*算法下，hybrid A*不同于启发式函数和动力学可行kinodynamic。在这里采用离散输入量的办法，输入量为加速度a，则位置p可以表示为：

        在a的范围内进行离散采样，根据初始位置和速度可以算出该采样输入a作用下转移的新状态，由此拓展出新的节点来，并且是动力学可行的。

3.代码架构

对于这里的算法结构

首先说明其中的变量含义：

P为开集，即优先级队列，C为闭集，即已经进行扩展后的节点容器，nc为优先级队列中队首元素，nodes为拓展出的周围节点，primitives是离散输入作用的一段位移，gc为代价g，fc为总的代价f，整个流程为：

这里对应fast planner中的代码为：path_searching/src/kinodynamic_astar.cpp

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)//A*搜索
{
  start_vel_ = start_v;
  start_acc_ = start_a;//设置起始点的速度和加速度
  PathNodePtr cur_node = path_node_pool_[0];//当前节点nc
  cur_node->parent = NULL;//父节点为空
  cur_node->state.head(3) = start_pt;//当前节点的位置
  cur_node->state.tail(3) = start_v;//当前节点的速度
  cur_node->index = posToIndex(start_pt);//将三维位置转化至栅格地图的index
  cur_node->g_score = 0.0;//当前节点的gc，即从起点到当前节点的cost，这里起点为0
  Eigen::VectorXd end_state(6);
  Eigen::Vector3i end_index;
  double time_to_goal;
  end_state.head(3) = end_pt;
  end_state.tail(3) = end_v;
  end_index = posToIndex(end_pt);
  cur_node->f_score = lambda_heu_ * estimateHeuristic(cur_node->state, end_state, time_to_goal);//计算启发式代价，初始点的fc总代价就是启发式代价，因为gc为0
  cur_node->node_state = IN_OPEN_SET;//当前节点的状态，一共两种，OPEN和CLOSED，在CLOSED中的节点是已经访问过的
  open_set_.push(cur_node);//OPEN加入当前节点，即优先级队列
  use_node_num_ += 1;
  if (dynamic)//为节点附加时间维度信息，动态搜索情况
  {
    time_origin_ = time_start;//记录起始时间
    cur_node->time = time_start;
    cur_node->time_idx = timeToIndex(time_start);//转换时间索引
    expanded_nodes_.insert(cur_node->index, cur_node->time_idx, cur_node);//将 (index, time_idx) 插入到 expanded_nodes_ 中，expanded_nodes_存储已访问节点的集合
    // cout = horizon_;//接近边界
    bool near_end = abs(cur_node->index(0) - end_index(0)) index(1) - end_index(1)) index(2) - end_index(2)) state, end_state, time_to_goal);//计算启发式函数，主要得到optimal_time
        computeShotTraj(cur_node->state, end_state, time_to_goal);//根据这个optimal_time来计算能否直接生成一条动力学可行的路直接连接到目标点
        if (init_search)//如果是初始搜索，会报错，在初始搜索阶段，如果能够直接从起点生成一条连接到目标点的轨迹（shot trajectory），通常是不合理的
          ROS_ERROR("Shot in first search loop!");
      }
    }
    if (reach_horizon)//处理各种情况
    {
      if (is_shot_succ_)
      {
        std::cout input = um;
            pro_node->duration = tau;
            pro_node->parent = cur_node;
            pro_node->node_state = IN_OPEN_SET;
            if (dynamic)
            {
              pro_node->time = cur_node->time + tau;
              pro_node->time_idx = timeToIndex(pro_node->time);
            }
            open_set_.push(pro_node);//将该节点加入open_set中
            if (dynamic)//记录一下节点的时间序列键值对
              expanded_nodes_.insert(pro_id, pro_node->time, pro_node);
            else
              expanded_nodes_.insert(pro_id, pro_node);
            tmp_expand_nodes.push_back(pro_node);//放在临时存储的节点
            use_node_num_ += 1;//内存检查
            if (use_node_num_ == allocate_num_)
            {
              cout g_score)//检查fc是否更小，若更优，则更新该节点的信息
            {
              // pro_node->index = pro_id;
              pro_node->state = pro_state;
              pro_node->f_score = tmp_f_score;
              pro_node->g_score = tmp_g_score;
              pro_node->input = um;
              pro_node->duration = tau;
              pro_node->parent = cur_node;
              if (dynamic)
                pro_node->time = cur_node->time + tau;
            }
          }
          else
          {
            cout time + tau;
              pro_node->time_idx = timeToIndex(pro_node->time);
            }
            open_set_.push(pro_node);//将该节点加入open_set中
            if (dynamic)//记录一下节点的时间序列键值对
              expanded_nodes_.insert(pro_id, pro_node->time, pro_node);
            else
              expanded_nodes_.insert(pro_id, pro_node);
            tmp_expand_nodes.push_back(pro_node);//放在临时存储的节点
            use_node_num_ += 1;//内存检查
            if (use_node_num_ == allocate_num_)
            {
              cout g_score)//检查fc是否更小，若更优，则更新该节点的信息
            {
              // pro_node->index = pro_id;
              pro_node->state = pro_state;
              pro_node->f_score = tmp_f_score;
              pro_node->g_score = tmp_g_score;
              pro_node->input = um;
              pro_node->duration = tau;
              pro_node->parent = cur_node;
              if (dynamic)
                pro_node->time = cur_node->time + tau;
            }
          }
          else
          {
            cout