EGO-Planner文章解读（一）——论文原理和算法实现

        在fastplanner中，ESDF 对于评估梯度大小和方向至关重要。然而轨迹优化过程只覆盖了 ESDF 更新范围的非常有限的子空间，计算全图的ESDF是非常冗余的。除此之外，在建图的时候只能看见障碍物的表面，看不见障碍物的后面/内部，在优化的过程中会使用ESDF去产生一个排斥轨迹远离障碍物，在下图所示的例子中，上面的障碍物将轨迹往下推，下面的障碍物将轨迹往上推，会出现轨迹卡在障碍物中的情况。

        EGO-Planner中将碰撞项从fastplanner中依据ESDF图进行改进。仅当轨迹撞到新的障碍物时，生成的障碍物信息才会被存储，这使得规划器只提取必要的障碍物信息。然后，如果违反了动力学可行性，我们就会延长时间分配，引入各向异性曲线拟合算法，在保持原始形状的同时调整轨迹的高阶导数。

一、不使用ESDF场产生轨迹推力

        在egoplanner这篇论文中，决策变量是控制点Qi。主要思想是，轨迹在优化的过程中，如果轨迹与障碍物发生碰撞，我们就基于这个碰撞对轨迹产生一个反方向的力来把轨迹推出来，远离刚刚碰到的障碍物。

        我们对轨迹段进行检查，找到在障碍物内部的控制点Qi，以及与该控制点左右临近的两个不在障碍物内部的控制点。根据这两个不在障碍物内部的控制点，利用A*生成一条远离障碍物的路径。取该控制点的切向量，以该向量为法向量生成一个平面。该平面与障碍物相交，在之前根据左右两个控制点生成的轨迹一侧与障碍物表面的交点为pij点（i代表控制点的索引，j代表障碍物的索引），从Qi到该p点的方向向量为向量v。这样就组成了一个{p,v}对，一个{p,v}对只属于一个控制点Qi。

        这里定义第i个控制点Qi到第j个障碍物的距离为dij

        为了避免轨迹从当前障碍物中逃脱之前生成重复的 {p， v} 对，只有当Qi对于所有的j，dij>0都满足，我们才认为当前障碍物是新发现的。同时，该方法使得只有对最终轨迹有贡献的障碍物才在优化项中进行优化，因此，计算时间显着减少。

二、前端搜索

Fast Planner的前端使用的是Hybrid A * 算法，而Ego Planner并不需要一个与障碍物不发生碰撞的初值轨迹，所以它的前端不需要Hybrid A * 这种比较复杂的算法，它只需要一条不考虑障碍物，满足给定起点和目标点状态的轨迹即可，然后在后端优化中再把轨迹优化成无碰撞的。

前端的路径搜索没有在论文中说明，我们在代码中去解读，对应代码如下：

/*** STEP 1: INIT ***/
    double ts = (start_pt - local_target_pt).norm() > 0.1 ? pp_.ctrl_pt_dist / pp_.max_vel_ * 1.2 : pp_.ctrl_pt_dist / pp_.max_vel_ * 5; // pp_.ctrl_pt_dist / pp_.max_vel_ is too tense, and will surely exceed the acc/vel limits
    //ts是时间间隔，这里是判断起点距终点是否接近，距离目标点较远时，初始轨迹的时间间隔稍长；距离目标点较近时，时间间隔较短，生成更紧密的控制点
    vector<Eigen::Vector3d> point_set, start_end_derivatives;
    static bool flag_first_call = true, flag_force_polynomial = false;
    bool flag_regenerate = false;
    do
    {
      point_set.clear();
      start_end_derivatives.clear();
      flag_regenerate = false;

      if (flag_first_call || flag_polyInit || flag_force_polynomial /*|| ( start_pt - local_target_pt ).norm() < 1.0*/) // Initial path generated from a min-snap traj by order.
      //三个标志位，首次使用，强制使用多项式，
      {
        flag_first_call = false;
        flag_force_polynomial = false;

        PolynomialTraj gl_traj;

        double dist = (start_pt - local_target_pt).norm();//轨迹距离
        double time = pow(pp_.max_vel_, 2) / pp_.max_acc_ > dist ? sqrt(dist / pp_.max_acc_) : (dist - pow(pp_.max_vel_, 2) / pp_.max_acc_) / pp_.max_vel_ + 2 * pp_.max_vel_ / pp_.max_acc_;
        //轨迹时间，如果起点和目标点距离小于速度-加速度约束的临界值，用加速度时间，如果距离较远，采用加速-匀速-减速时间，time为总时间
        if (!flag_randomPolyTraj)//不需要随机中间点
        {
          gl_traj = PolynomialTraj::one_segment_traj_gen(start_pt, start_vel, start_acc, local_target_pt, local_target_vel, Eigen::Vector3d::Zero(), time);
        //直接使用起点和终点的mininum snap轨迹
        }
        else//需要随机中间点
        {
          Eigen::Vector3d horizen_dir = ((start_pt - local_target_pt).cross(Eigen::Vector3d(0, 0, 1))).normalized();
          Eigen::Vector3d vertical_dir = ((start_pt - local_target_pt).cross(horizen_dir)).normalized();
          Eigen::Vector3d random_inserted_pt = (start_pt + local_target_pt) / 2 +//随机生成中间点
                                               (((double)rand()) / RAND_MAX - 0.5) * (start_pt - local_target_pt).norm() * horizen_dir * 0.8 * (-0.978 / (continous_failures_count_ + 0.989) + 0.989) +
                                               (((double)rand()) / RAND_MAX - 0.5) * (start_pt - local_target_pt).norm() * vertical_dir * 0.4 * (-0.978 / (continous_failures_count_ + 0.989) + 0.989);
          Eigen::MatrixXd pos(3, 3);//轨迹点
          pos.col(0) = start_pt;
          pos.col(1) = random_inserted_pt;
          pos.col(2) = local_target_pt;
          Eigen::VectorXd t(2);
          t(0) = t(1) = time / 2;
          gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, local_target_vel, start_acc, Eigen::Vector3d::Zero(), t);//使用mininum snap生成轨迹
        }

        double t;
        bool flag_too_far;
        ts *= 1.5; // ts will be divided by 1.5 in the next//将时间变为之前的1.5倍，放宽一点，
        do
        {
          ts /= 1.5;//在每次循环中缩小时间间隔
          point_set.clear();
          flag_too_far = false;
          Eigen::Vector3d last_pt = gl_traj.evaluate(0);
          for (t = 0; t < time; t += ts)
          {
            Eigen::Vector3d pt = gl_traj.evaluate(t);//每隔ts采样一个点pt
            if ((last_pt - pt).norm() > pp_.ctrl_pt_dist * 1.5)//计算pt与上一个点的距离，如果超过了pp_.ctrl_pt_dist * 1.5，说明轨迹不够平滑，终止循环
            {
              flag_too_far = true;
              break;
            }
            last_pt = pt;
            point_set.push_back(pt);//将pt加入点集中
          }
        } while (flag_too_far || point_set.size() < 7); // To make sure the initial path has enough points.//采样多项式轨迹点，直到满足控制点密度约束
        //存在两个轨迹点的间距超过阈值，点的数量太少，会重新进行采样
        t -= ts;//t是采样终止的时间点，将时间t修正到轨迹的实际终点
        start_end_derivatives.push_back(gl_traj.evaluateVel(0));//起点速度
        start_end_derivatives.push_back(local_target_vel);//终点速度
        start_end_derivatives.push_back(gl_traj.evaluateAcc(0));//起点加速度
        start_end_derivatives.push_back(gl_traj.evaluateAcc(t));//终点加速度
      }
      else // Initial path generated from previous trajectory.//不是第一次生成轨迹，从上一次生成的轨迹提取点集，用作当前规划的初始路径
      {

        double t;
        double t_cur = (ros::Time::now() - local_data_.start_time_).toSec();//计算起始时间，当前时间相对于轨迹开始时间移动了多少秒，此时开始为起点时间

        vector<double> pseudo_arc_length;//记录轨迹点之间的累积弧长，用于插值采样
        vector<Eigen::Vector3d> segment_point;//提取的轨迹点集合
        pseudo_arc_length.push_back(0.0);//起始点弧长为0
        for (t = t_cur; t < local_data_.duration_ + 1e-3; t += ts)//以固定步长ts来遍历时间
        {
          segment_point.push_back(local_data_.position_traj_.evaluateDeBoorT(t));//调用 evaluateDeBoorT(t) 函数，从B样条轨迹中计算时间 t 对应的轨迹点位置
          if (t > t_cur)//跳过起点，只有当当前时间大于起点时间时才计算弧长
          {
            pseudo_arc_length.push_back((segment_point.back() - segment_point[segment_point.size() - 2]).norm() + pseudo_arc_length.back());//(segment_point.back() - segment_point[segment_point.size() - 2]).norm()当前位置减去前一个点的位置的弧长，加入到pseudo_arc_length累积弧长中
          }
        }
        t -= ts;//由于 t 在最后一次循环中被多加了一个时间步长 ts，需要减去 ts，回退到最后一个有效采样时间点

        double poly_time = (local_data_.position_traj_.evaluateDeBoorT(t) - local_target_pt).norm() / pp_.max_vel_ * 2;//估计从当前轨迹末点到目标点的时间
        if (poly_time > ts)//时间大于时间间隔ts，当前点到目标点的轨迹较长，且需要根据该时间来生成新的轨迹，否则就不需要生成新的轨迹
        {
          PolynomialTraj gl_traj = PolynomialTraj::one_segment_traj_gen(local_data_.position_traj_.evaluateDeBoorT(t),
                                                                        local_data_.velocity_traj_.evaluateDeBoorT(t),
                                                                        local_data_.acceleration_traj_.evaluateDeBoorT(t),
                                                                        local_target_pt, local_target_vel, Eigen::Vector3d::Zero(), poly_time);//根据当前轨迹的末端状态（位置、速度、加速度）和目标点的状态（位置、速度）生成一个单段的多项式轨迹

          for (t = ts; t < poly_time; t += ts)//使用时间步长 ts 对生成的多项式轨迹 gl_traj 进行采样
          {
            if (!pseudo_arc_length.empty())