FastPlanner论文解读（三）——后端时间优化及代码流程

一、后端时间优化

 尽管我们在路径搜索和优化中限制了 kinodynamic 的可行性，但有时我们会得到不可行的轨迹。基本原因是梯度信息往往会拉长整体轨迹，同时将其推离障碍物。因此，四旋翼飞行器必须更激进地飞行，才能在相同的时间内行驶更长的距离，如果原始运动已经接近物理极限，这不可避免地会导致过度激进的运动。

        Fast Planner中是通过非均匀B样条来进行时间重分配的，与均匀 B 样条的唯一区别是它的每个结跨度 ∆tm = tm+1 − tm 都独立于其他结点区间。只改变时间分配，不改变控制点的位置，生成轨迹的几何情况是不会发生变化的。

        非均匀B样条的一阶导数和二阶导数如下：

我们需要调整时间之间的间隔

速度调整：

速度超限的控制点可以写作

其中，最大的一个超出速度限制的分量记为：，速度的最大限制为，该速度控制点的超限主要受时间区间ti+pb+1-ti+1大小的影响，因此我们要延长该时间区间，即：

，其中，

该速度控制点变为：

对加速度控制点做同样的调整，

这里的

这样通过调整时间区间就使得轨迹满足动力学可行性，同时没有调整控制点的位置保证了轨迹没有发生改变。

对应的算法流程为：

首先找出不满足动力学可行性的控制点集合V和A，分别对其做调整，这里在选择调整的参数时，有两个预设的参数αv和αa，因为结跨度 ∆tm 会影响几个控制点，反之亦然，所以用两个常数 αv 和 αa 略大于 1（第 5、9 行）来界定 μ' v 和 μ' a 可以防止任何时间跨度过度延长。

对应的实现函数为reallocateTime

bool NonUniformBspline::reallocateTime(bool show)
  {
    // SETY << "[Bspline]: total points size: " << control_points_.rows() << endl;
    // cout << "origin knots:\n" << u_.transpose() << endl;
    bool fea = true;

    Eigen::MatrixXd P = control_points_;
    int dimension = control_points_.cols();

    double max_vel, max_acc;

    /* check vel feasibility and insert points */
    for (int i = 0; i < P.rows() - 1; ++i)
    {
      Eigen::VectorXd vel = p_ * (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1));

      if (fabs(vel(0)) > limit_vel_ + 1e-4 || fabs(vel(1)) > limit_vel_ + 1e-4 ||
          fabs(vel(2)) > limit_vel_ + 1e-4)
      {

        fea = false;
        if (show)
          cout << "[Realloc]: Infeasible vel " << i << " :" << vel.transpose() << endl;

        max_vel = -1.0;
        for (int j = 0; j < dimension; ++j)
        {
          max_vel = max(max_vel, fabs(vel(j)));
        }

        double ratio = max_vel / limit_vel_ + 1e-4;
        if (ratio > limit_ratio_)//初始设置了一个调整因子为av
          ratio = limit_ratio_;

        double time_ori = u_(i + p_ + 1) - u_(i + 1);//原本的时间段
        double time_new = ratio * time_ori;//新的时间段变
        double delta_t = time_new - time_ori;//新的时间间隔
        double t_inc = delta_t / double(p_);//将时间均匀分配到p阶样条的每段时间内的增量
        for (int j = i + 2; j <= i + p_ + 1; ++j)//从i+1到i+pb+1的时间段按照比例调整，在i+pb+1之后相同比例调整，调整的点为i+2到i+pb+1
        {
          u_(j) += double(j - i - 1) * t_inc;//这几个点每个增加j-i-1*tinc的值
          if (j <= 5 && j >= 1)
          {
            // cout << "vel j: " << j << endl;
          }
        }

        for (int j = i + p_ + 2; j < u_.rows(); ++j)//这之后的点增加delta_t的时间，相同的时间增加量
        {
          u_(j) += delta_t;
        }
      }
    }

    /* acc feasibility */
    for (int i = 0; i < P.rows() - 2; ++i)
    {

      Eigen::VectorXd acc = p_ * (p_ - 1) *
                            ((P.row(i + 2) - P.row(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) -
                             (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1))) /
                            (u_(i + p_ + 1) - u_(i + 2));

      if (fabs(acc(0)) > limit_acc_ + 1e-4 || fabs(acc(1)) > limit_acc_ + 1e-4 ||
          fabs(acc(2)) > limit_acc_ + 1e-4)
      {

        fea = false;
        if (show)
          cout << "[Realloc]: Infeasible acc " << i << " :" << acc.transpose() << endl;

        max_acc = -1.0;
        for (int j = 0; j < dimension; ++j)
        {
          max_acc = max(max_acc, fabs(acc(j)));
        }

        double ratio = sqrt(max_acc / limit_acc_) + 1e-4;
        if (ratio > limit_ratio_)//初始有一个调整因子为aa
          ratio = limit_ratio_;
        // cout << "ratio: " << ratio << endl;

        double time_ori = u_(i + p_ + 1) - u_(i + 2);//原本的时间
        double time_new = ratio * time_ori;//新的时间
        double delta_t = time_new - time_ori;//新的时间间隔
        double t_inc = delta_t / double(p_ - 1);

        if (i == 1 || i == 2)
        {
          // cout << "acc i: " << i << endl;
          for (int j = 2; j <= 5; ++j)
          {
            u_(j) += double(j - 1) * t_inc;
          }

          for (int j = 6; j < u_.rows(); ++j)
          {
            u_(j) += 4.0 * t_inc;
          }
        }
        else
        {

          for (int j = i + 3; j <= i + p_ + 1; ++j)
          {
            u_(j) += double(j - i - 2) * t_inc;
            if (j <= 5 && j >= 1)
            {
              // cout << "acc j: " << j << endl;
            }
          }

          for (int j = i + p_ + 2; j < u_.rows(); ++j)
          {
            u_(j) += delta_t;
          }
        }
      }
    }

    return fea;//返回是否调整成功，这里是不断循环调整，直到所有的控制点都满足动力学可行性
  }

首先是参数的定义

fea：标记轨迹是否满足约束，如果调整成功，最终返回 true。
P：存储当前 B-Spline 的控制点。
dimension：轨迹的维度（通常为 3D）。
max_vel & max_acc：记录轨迹的最大速度 & 加速度

然后检查速度控制点，

for (int i = 0; i < P.rows() - 1; ++i)
{
  Eigen::VectorXd vel = p_ * (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1));

  if (fabs(vel(0)) > limit_vel_ + 1e-4 || fabs(vel(1)) > limit_vel_ + 1e-4 ||
      fabs(vel(2)) > limit_vel_ + 1e-4)
  {
    fea = false;  // 轨迹超出速度约束，标记为不可行
    max_vel = -1.0;
    for (int j = 0; j < dimension; ++j)
    {
      max_vel = max(max_vel, fabs(vel(j)));
    }

    double ratio = max_vel / limit_vel_ + 1e-4;  // 计算速度放缩比例
    if (ratio > limit_ratio_) ratio = limit_ratio_;  // 限制最大调整比例

    double time_ori = u_(i + p_ + 1) - u_(i + 1);  // 原始时间
    double time_new = ratio * time_ori;  // 计算新的时间
    double delta_t = time_new - time_ori;  // 计算时间增量
    double t_inc = delta_t / double(p_);  // 均匀分配到 B-Spline 段

    for (int j = i + 2; j <= i + p_ + 1; ++j)  // 调整 u_ 向量中的时间
    {
      u_(j) += double(j - i - 1) * t_inc;
    }

    for (int j = i + p_ + 2; j < u_.rows(); ++j)  // 之后的时间点等比例调整
    {
      u_(j) += delta_t;
    }
  }
}

首先利用公式

计算出速度控制点，当该点三个维度中任意一维超限时，标记标志位fea位false，记录下三个维度中最大的超限速度在max_vel中，然后计算调整因子是选择αv还是uv。

计算总总共需要增加的时间长度为delta_t，并将delta_t平均分配到这一段跨度的pb个时间区间中，每一段增加的时间位t_inc。从ti+2开始到ti+pb+1，逐渐根据t_inc将节点向后移动，而ti+pb+1之后的节点，每个都向后移动总的时间增加长度delta_t。

同样的操作流程，通过对加速度控制点的检查，调整时间向量节点。

这里最终返回fea，即我们一直进行时间的调整，直到最终满足了动力学约束，fea为true。在代码中我们设定的循环调整的次数为3。

二、两种重规划的策略

1.当四旋翼飞行器在未知环境中飞行时，由于感应范围有限，它不得不频繁地重新规划其轨迹。为了提高效率，我们采用了后退视界规划方案，其中轨迹仅在已知空间内生成。一旦运动基元在此范围之外结束，路径搜索就会终止，然后是优化和时间调整。在未知空间中进行规划通常是无用的，因此可以节省这样的花费。

2.重新规划触发机制：重新规划在两种情况下触发。

首先，如果当前轨迹与新出现的障碍物相撞，则会触发它，从而确保在检测到任何碰撞时立即提供新的安全轨迹。

其次，以固定的时间间隔调用 planner。它使用最新的环境信息定期更新轨迹

这里重规划对应了两个定时器

/* callback */
    exec_timer_ = nh.createTimer(ros::Duration(0.01), &KinoReplanFSM::execFSMCallback, this);          // 创建一个周期为 0.01 秒的定时器，用于定期调用 execFSMCallback 函数，执行状态机的任务
    safety_timer_ = nh.createTimer(ros::Duration(0.05), &KinoReplanFSM::checkCollisionCallback, this); // 创建一个周期为 0.05 秒的定时器，用于定期调用 checkCollisionCallback 函数，检查碰撞

第一个定时器是每0.01s检查当前状态，执行当前状态

第二个定时器是每0.05s检查当前是否发生碰撞，否则trigger进行重规划。

三、代码流程

现在我们来分析整个代码的流程：

1.fast_planner_node节点

首先是主文件plan_manage/fast_planner_node.cpp

int main(int argc, char** argv) {
  ros::init(argc, argv, "fast_planner_node");//创建节点fast_planner_node
  ros::NodeHandle nh("~");//创建一个 ROS 私有节点句柄，用于访问参数服务器中的私有参数。

  int planner;//参数planner
  nh.param("planner_node/planner", planner, -1);//从参数服务器中获取planner参数，默认值为-1

  TopoReplanFSM topo_replan;
  KinoReplanFSM kino_replan;

  if (planner == 1) {//不同参数选择不同初始化
    kino_replan.init(nh);
  } else if (planner == 2) {
    topo_replan.init(nh);
  }

  ros::Duration(1.0).sleep();//休眠 1 秒，确保 ROS 环境和节点初始化完成
  ros::spin();//启动 ROS 循环，节点进入监听和处理回调的状态

  return 0;
}

这里创建了轨迹规划的节点fast_planner_node，进入kin_replan进行初始化。

这里在plan_manage/kin_replan_fsm.cpp文件中，初始化时，主要创建了定时器、订阅者和发布者

/* callback */
    exec_timer_ = nh.createTimer(ros::Duration(0.01), &KinoReplanFSM::execFSMCallback, this);          // 创建一个周期为 0.01 秒的定时器，用于定期调用 execFSMCallback 函数，执行状态机的任务
    safety_timer_ = nh.createTimer(ros::Duration(0.05), &KinoReplanFSM::checkCollisionCallback, this); // 创建一个周期为 0.05 秒的定时器，用于定期调用 checkCollisionCallback 函数，检查碰撞

    waypoint_sub_ =
        nh.subscribe("/waypoint_generator/waypoints", 1, &KinoReplanFSM::waypointCallback, this); // 订阅 /waypoint_generator/waypoints 主题，接收路径点信息
    odom_sub_ = nh.subscribe("/odom_world", 1, &KinoReplanFSM::odometryCallback, this);           // 订阅 /odom_world 主题，接收里程计信息

    replan_pub_ = nh.advertise<std_msgs::Empty>("/planning/replan", 10);        // 发布规划重新启动
    new_pub_ = nh.advertise<std_msgs::Empty>("/planning/new", 10);              // 发布新规划
    bspline_pub_ = nh.advertise<plan_manage::Bspline>("/planning/bspline", 10); // 发布 B 样条轨迹

主要的定时器为exec_timer定时器，这里每0.01s进入回调函数，这个回调函数是状态的检查，是整个规划过程的全流程，通过不同状态执行不同的操作。

void KinoReplanFSM::execFSMCallback(const ros::TimerEvent &e)
  {
    static int fsm_num = 0;
    fsm_num++;
    if (fsm_num == 100)
    {
      printFSMExecState();
      if (!have_odom_)
        cout << "no odom." << endl;
      if (!trigger_)
        cout << "wait for goal." << endl;
      fsm_num = 0;
    } // 记数100次，即1秒它会打印当前状态信息、是否获取到里程计数据以及是否在等待目标

    switch (exec_state_)
    { // 根据不同的状态执行不同的操作
    case INIT:
    { // 初始状态
      if (!have_odom_)
      {
        return;
      }
      if (!trigger_)
      {
        return;
      }
      changeFSMExecState(WAIT_TARGET, "FSM"); // 等到have_odom_和trigger_都准备好后转移到WAIT_TARGET状态
      break;
    }

    case WAIT_TARGET:
    {
      if (!have_target_)
        return;
      else
      {
        changeFSMExecState(GEN_NEW_TRAJ, "FSM"); // 等到have_target_准备好后，即有目标后，进入GEN_NEW_TRAJ状态
      }
      break;
    }

    case GEN_NEW_TRAJ:
    {
      start_pt_ = odom_pos_;  // 获取里程计数据，得到起点位置
      start_vel_ = odom_vel_; // 得到起点的速度
      start_acc_.setZero();   // 起点加速度为0

      Eigen::Vector3d rot_x = odom_orient_.toRotationMatrix().block(0, 0, 3, 1); // 获得起点的姿态信息
      start_yaw_(0) = atan2(rot_x(1), rot_x(0));
      start_yaw_(1) = start_yaw_(2) = 0.0;

      bool success = callKinodynamicReplan();//调用callKinodynamicReplan() 函数进行动力学重新规划
      if (success)
      {
        changeFSMExecState(EXEC_TRAJ, "FSM");//如果规划成功，进入EXEC_TARJ执行轨迹的状态
      }
      else
      {
        // have_target_ = false;
        // changeFSMExecState(WAIT_TARGET, "FSM");
        changeFSMExecState(GEN_NEW_TRAJ, "FSM");//否则保持GEN—NEW-TRAJ的状态
      }
      break;
    }

    case EXEC_TRAJ:
    {
      /* determine if need to replan */
      LocalTrajData *info = &planner_manager_->local_data_;
      ros::Time time_now = ros::Time::now();
      double t_cur = (time_now - info->start_time_).toSec();
      t_cur = min(info->duration_, t_cur);//计算执行的时间

      Eigen::Vector3d pos = info->position_traj_.evaluateDeBoorT(t_cur);//根据执行时间计算出轨迹的位置

      /* && (end_pt_ - pos).norm() < 0.5 */
      if (t_cur > info->duration_ - 1e-2)//当前时间大于轨迹总时长
      {
        have_target_ = false;
        changeFSMExecState(WAIT_TARGET, "FSM");//表示轨迹已经完成，重新等待目标，进入WAIT-TARGET状态
        return;
      }
      else if ((end_pt_ - pos).norm() < no_replan_thresh_)//当前位置接近目标点且不需要重新规划
      {
        // cout << "near end" << endl;
        return;//直接返回
      }
      else if ((info->start_pos_ - pos).norm() < replan_thresh_)//如果当前位置接近轨迹的起始点，没有达到重新规划的阈值
      {
        // cout << "near start" << endl;
        return;
      }
      else//如果当前位置接近轨迹的起始点，表示轨迹可能不符合要求，需要重新规划，则转到 REPLAN_TRAJ（重新规划轨迹）状态
      {
        changeFSMExecState(REPLAN_TRAJ, "FSM");
      }
      break;
    }

    case REPLAN_TRAJ:
    {
      LocalTrajData *info = &planner_manager_->local_data_;
      ros::Time time_now = ros::Time::now();
      double t_cur = (time_now - info->start_time_).toSec();

      start_pt_ = info->position_traj_.evaluateDeBoorT(t_cur);
      start_vel_ = info->velocity_traj_.evaluateDeBoorT(t_cur);
      start_acc_ = info->acceleration_traj_.evaluateDeBoorT(t_cur);

      start_yaw_(0) = info->yaw_traj_.evaluateDeBoorT(t_cur)[0];
      start_yaw_(1) = info->yawdot_traj_.evaluateDeBoorT(t_cur)[0];
      start_yaw_(2) = info->yawdotdot_traj_.evaluateDeBoorT(t_cur)[0];//获取当前轨迹的数据（位置、速度、加速度、偏航角等）

      std_msgs::Empty replan_msg;
      replan_pub_.publish(replan_msg);//发布重新规划的信号

      bool success = callKinodynamicReplan();
      if (success)
      {
        changeFSMExecState(EXEC_TRAJ, "FSM");//如果重新规划成功，转到 EXEC_TRAJ（执行轨迹）
      }
      else
      {
        changeFSMExecState(GEN_NEW_TRAJ, "FSM");//如果失败，则返回 GEN_NEW_TRAJ（生成新轨迹）状态，尝试重新规划
      }
      break;
    }
    }
  }

这里包括的状态为INIT初始状态，WAIT_TARGET等待目标，GEN_NEW_TRAJ生成轨迹、EXEC_TRAJ执行轨迹，REPLAN_TRAJ重新规划。

核心轨迹规划的函数为callKinodynamicReplan()，在该函数中，调用kinodynamicReplan函数，传入起始点位置速度加速度和终点位置和速度进行规划。

kinodynamicReplan函数在plan_manage/planner_manager.cpp文件中，整个规划前端后端的流程都在这个函数中

bool FastPlannerManager::kinodynamicReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel,
                                             Eigen::Vector3d start_acc, Eigen::Vector3d end_pt,
                                             Eigen::Vector3d end_vel)
  { // 输入起始点目标点的位置速度和加速度

    std::cout << "[kino replan]: -----------------------" << std::endl;
    cout << "start: " << start_pt.transpose() << ", " << start_vel.transpose() << ", "
         << start_acc.transpose() << "\ngoal:" << end_pt.transpose() << ", " << end_vel.transpose()
         << endl; // 打印起点和目标点的状态，便于调试和确认输入

    if ((start_pt - end_pt).norm() < 0.2)
    {
      cout << "Close goal" << endl;
      return false; // 如果起点与目标点距离小于 0.2 米，认为无需规划直接退出
    }

    ros::Time t1, t2;

    local_data_.start_time_ = ros::Time::now();
    double t_search = 0.0, t_opt = 0.0, t_adjust = 0.0;

    Eigen::Vector3d init_pos = start_pt;
    Eigen::Vector3d init_vel = start_vel;
    Eigen::Vector3d init_acc = start_acc; // 起始点位置、速度和加速度

    // kinodynamic path searching

    t1 = ros::Time::now();

    kino_path_finder_->reset(); // kino_path_finder路径搜索器

    int status = kino_path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel, true); // 调用path_searching/kinodynamic_astar.cpp文件中的search函数进行Hbrid A*算法搜索
    // 获得了路径搜索的状态
    if (status == KinodynamicAstar::NO_PATH)
    {
      cout << "[kino replan]: kinodynamic search fail!" << endl;

      // retry searching with discontinuous initial state
      kino_path_finder_->reset();
      status = kino_path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel, false); // 尝试放宽初始状态（例如不严格约束起点速度和加速度）再次搜索

      if (status == KinodynamicAstar::NO_PATH)
      { // 如果还失败
        cout << "[kino replan]: Can't find path." << endl;
        return false; // 返回false没有找到路径
      }
      else
      {
        cout << "[kino replan]: retry search success." << endl;
      }
    }
    else
    {
      cout << "[kino replan]: kinodynamic search success." << endl; // 否则成功搜索到路径
    }

    plan_data_.kino_path_ = kino_path_finder_->getKinoTraj(0.01); // delta_t为0.01来搜索更细的轨迹点，将搜索得到的轨迹存储到 plan_data_.kino_path_，采样时间间隔为 0.01

    t_search = (ros::Time::now() - t1).toSec();

    // parameterize the path to bspline

    double ts = pp_.ctrl_pt_dist / pp_.max_vel_; // ctrl_pt_dist控制点之间的期望距离，max_vel_最大速度，这里定义采样间隔为ts
    vector<Eigen::Vector3d> point_set, start_end_derivatives;
    kino_path_finder_->getSamples(ts, point_set, start_end_derivatives); // 采样得到中间轨迹点和起始和目标点的速度和加速度

    Eigen::MatrixXd ctrl_pts;                                                                 // 存放B样条的控制点
    NonUniformBspline::parameterizeToBspline(ts, point_set, start_end_derivatives, ctrl_pts); // 将前端得到的路径转换成B样条曲线的形式，调用bspline/non_uniform_bsplinecpp中的parameterizeToBspline函数
    // 获得了控制点ctrl_pts
    NonUniformBspline init(ctrl_pts, 3, ts);

    // bspline trajectory optimization

    t1 = ros::Time::now();

    int cost_function = BsplineOptimizer::NORMAL_PHASE;//生成cost_function，NORMAL_PHASE包含代价函数，即平滑项、碰撞项和动力学可行项
    if (status != KinodynamicAstar::REACH_END)
    {
      cost_function |= BsplineOptimizer::ENDPOINT;
    }

    ctrl_pts = bspline_optimizers_[0]->BsplineOptimizeTraj(ctrl_pts, ts, cost_function, 1, 1); // 后端的使用均匀B样条及优化算法进行轨迹优化

    t_opt = (ros::Time::now() - t1).toSec();

    // iterative time adjustment

    t1 = ros::Time::now();
    NonUniformBspline pos = NonUniformBspline(ctrl_pts, 3, ts); // 使用优化后的控制点生成非均匀 B 样条轨迹对象 pos

    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())//如果轨迹不可行，调用 reallocateTime 动态调整时间分配，最多迭代 3 次
    {

      feasible = pos.reallocateTime();

      if (++iter_num >= 3)
        break;
    }
    //
    // pos.checkFeasibility(true);
    // cout << "[Main]: iter num: " << iter_num << endl;

    double tn = pos.getTimeSum();

    cout << "[kino replan]: Reallocate ratio: " << tn / to << endl;
    if (tn / to > 3.0)
      ROS_ERROR("reallocate error.");

    t_adjust = (ros::Time::now() - t1).toSec();

    // save planned results

    local_data_.position_traj_ = pos;//保存最终轨迹到 local_data_.position_traj_

    double t_total = t_search + t_opt + t_adjust;
    cout << "[kino replan]: time: " << t_total << ", search: " << t_search << ", optimize: " << t_opt
         << ", adjust time:" << t_adjust << endl;

    pp_.time_search_ = t_search;//输出搜索、优化和时间调整的耗时信息
    pp_.time_optimize_ = t_opt;
    pp_.time_adjust_ = t_adjust;

    updateTrajInfo();

    return true;
  }

整体的流程为：

1. 输入检查与初始化
输入：起点/终点的位置、速度、加速度。

检查距离：若起点与终点距离小于 0.2m，直接返回 false（无需规划）。

记录初始时间：用于后续统计各阶段耗时。

2. 运动动力学路径搜索（Kinodynamic Path Searching）
重置路径搜索器：调用 kino_path_finder_->reset()。

首次搜索：使用 Hybrid A* 算法（kino_path_finder_->search）进行路径搜索，考虑动力学约束。

状态判断：

若成功（REACH_END），获取轨迹 plan_data_.kino_path_（时间间隔 0.01s 采样）。

若失败（NO_PATH），放宽初始状态约束（如速度和加速度不连续）进行重试。

重试仍失败则返回 false。

3. 路径参数化为 B 样条（B-spline Parameterization）
计算时间间隔：ts = 控制点距离 / 最大速度。

采样路径点：通过 kino_path_finder_->getSamples 获取中间点及起点/终点的速度、加速度。

生成 B 样条控制点：调用 NonUniformBspline::parameterizeToBspline，将路径转换为均匀 B 样条。

4. B 样条轨迹优化（B-spline Optimization）
设置代价函数：

默认包含平滑性、碰撞避障、动力学约束（NORMAL_PHASE）。

若未到达终点（REACH_END），添加终点约束（ENDPOINT）。

优化控制点：调用 BsplineOptimizeTraj，优化后生成新的控制点。

5. 时间调整与可行性检查（Time Reallocation）
生成非均匀 B 样条：基于优化后的控制点创建轨迹对象 pos。

设置物理约束：最大速度、加速度限制。

迭代检查可行性：

若轨迹不可行（速度/加速度超限），调用 pos.reallocateTime 调整时间分配。

最多迭代 3 次，若仍不可行则终止。

6. 结果保存与输出
保存轨迹：最终可行轨迹存入 local_data_.position_traj_。

统计耗时：记录搜索、优化、时间调整的耗时。

更新轨迹信息：调用 updateTrajInfo 完成数据更新。

之后我们再回到callKinodynamicReplan()函数，

bool KinoReplanFSM::callKinodynamicReplan()
  {
    bool plan_success =
        planner_manager_->kinodynamicReplan(start_pt_, start_vel_, start_acc_, end_pt_, end_vel_);//使用 planner_manager_ 的 kinodynamicReplan 方法进行重新规划

    if (plan_success)//规划成功
    {

      planner_manager_->planYaw(start_yaw_);//调用 planner_manager_ 的 planYaw 方法进行航向规划

      auto info = &planner_manager_->local_data_;//获取轨迹数据

      /* publish traj */
      plan_manage::Bspline bspline;//创建消息类型
      bspline.order = 3;
      bspline.start_time = info->start_time_;
      bspline.traj_id = info->traj_id_;

      Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint();

      for (int i = 0; i < pos_pts.rows(); ++i)
      {
        geometry_msgs::Point pt;
        pt.x = pos_pts(i, 0);
        pt.y = pos_pts(i, 1);
        pt.z = pos_pts(i, 2);
        bspline.pos_pts.push_back(pt);
      }

      Eigen::VectorXd knots = info->position_traj_.getKnot();
      for (int i = 0; i < knots.rows(); ++i)
      {
        bspline.knots.push_back(knots(i));
      }

      Eigen::MatrixXd yaw_pts = info->yaw_traj_.getControlPoint();
      for (int i = 0; i < yaw_pts.rows(); ++i)
      {
        double yaw = yaw_pts(i, 0);
        bspline.yaw_pts.push_back(yaw);
      }
      bspline.yaw_dt = info->yaw_traj_.getInterval();

      bspline_pub_.publish(bspline);//将轨迹的位置信息、控制点、节点信息等转化为消息类型 Bspline 并发布

      /* visulization */
      auto plan_data = &planner_manager_->plan_data_;
      visualization_->drawGeometricPath(plan_data->kino_path_, 0.075, Eigen::Vector4d(1, 1, 0, 0.4));
      visualization_->drawBspline(info->position_traj_, 0.1, Eigen::Vector4d(1.0, 0, 0.0, 1), true, 0.2,
                                  Eigen::Vector4d(1, 0, 0, 1));//使用 visualization_ 对几何路径 (kino_path_) 和 B样条轨迹 (position_traj_) 进行可视化

      return true;
    }
    else
    {
      cout << "generate new traj fail." << endl;
      return false;//规划失败
    }
  }

规划好后，主要是创建bspline消息，按照消息格式填充消息内容，通过bspline_pub_发布者发布bspline消息。

1️⃣ 调用 `kinodynamicReplan()` 进行轨迹规划
   ├── 规划成功？→ 否 ❌ → 输出 "generate new traj fail."，返回 false
   │
   ├── 是 ✅ → 继续执行：
   │   2️⃣ 调用 `planYaw()` 计算航向角
   │   3️⃣ 获取 `local_data_` 轨迹数据
   │   4️⃣ 创建 `Bspline` 轨迹消息
   │   5️⃣ 填充轨迹控制点 `pos_pts`
   │   6️⃣ 填充 B-Spline 节点 `knots`
   │   7️⃣ 填充航向角 `yaw_pts`
   │   8️⃣ 发布轨迹 `bspline_pub_`
   │   9️⃣ 可视化 `kino_path_` & `Bspline`
   │  🔟 返回 `true`

2.traj_generator节点

另一个核心节点是traj_generator，用于接受bspline信息，发布轨迹（控制）信息，在这里我们可以与mavlink进行搭建，实现与PX4的联合。

该节点定义在poly_traj/traj_generator.cpp文件中，

/**
* This file is part of Fast-Planner.
*
* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, <uav.ust.hk>
* Developed by Boyu Zhou <bzhouai at connect dot ust dot hk>, <uv dot boyuzhou at gmail dot com>
* for more information see <https://github.com/HKUST-Aerial-Robotics/Fast-Planner>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* Fast-Planner is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Fast-Planner is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Fast-Planner. If not, see <http://www.gnu.org/licenses/>.
*/



#include "nav_msgs/Odometry.h"
#include "std_msgs/Empty.h"
#include "visualization_msgs/Marker.h"
#include <Eigen/Eigen>
#include <ros/ros.h>

#include <swarmtal_msgs/drone_onboard_command.h>
#include <traj_generator/polynomial_traj.hpp>

using namespace std;

ros::Publisher state_pub, pos_cmd_pub, traj_pub;

nav_msgs::Odometry odom;
bool have_odom;

void displayPathWithColor(vector<Eigen::Vector3d> path, double resolution, Eigen::Vector4d color,
                          int id) {
  visualization_msgs::Marker mk;
  mk.header.frame_id = "world";
  mk.header.stamp = ros::Time::now();
  mk.type = visualization_msgs::Marker::SPHERE_LIST;
  mk.action = visualization_msgs::Marker::DELETE;
  mk.id = id;

  traj_pub.publish(mk);

  mk.action = visualization_msgs::Marker::ADD;
  mk.pose.orientation.x = 0.0;
  mk.pose.orientation.y = 0.0;
  mk.pose.orientation.z = 0.0;
  mk.pose.orientation.w = 1.0;

  mk.color.r = color(0);
  mk.color.g = color(1);
  mk.color.b = color(2);
  mk.color.a = color(3);

  mk.scale.x = resolution;
  mk.scale.y = resolution;
  mk.scale.z = resolution;

  geometry_msgs::Point pt;
  for (int i = 0; i < int(path.size()); i++) {
    pt.x = path[i](0);
    pt.y = path[i](1);
    pt.z = path[i](2);
    mk.points.push_back(pt);
  }
  traj_pub.publish(mk);
  ros::Duration(0.001).sleep();
}

void drawState(Eigen::Vector3d pos, Eigen::Vector3d vec, int id, Eigen::Vector4d color) {
  visualization_msgs::Marker mk_state;
  mk_state.header.frame_id = "world";
  mk_state.header.stamp = ros::Time::now();
  mk_state.id = id;
  mk_state.type = visualization_msgs::Marker::ARROW;
  mk_state.action = visualization_msgs::Marker::ADD;
  mk_state.pose.orientation.w = 1.0;
  mk_state.scale.x = 0.1;
  mk_state.scale.y = 0.2;
  mk_state.scale.z = 0.3;
  geometry_msgs::Point pt;
  pt.x = pos(0);
  pt.y = pos(1);
  pt.z = pos(2);
  mk_state.points.push_back(pt);
  pt.x = pos(0) + vec(0);
  pt.y = pos(1) + vec(1);
  pt.z = pos(2) + vec(2);
  mk_state.points.push_back(pt);
  mk_state.color.r = color(0);
  mk_state.color.g = color(1);
  mk_state.color.b = color(2);
  mk_state.color.a = color(3);
  state_pub.publish(mk_state);
}

void odomCallbck(const nav_msgs::Odometry& msg) {
  if (msg.child_frame_id == "X" || msg.child_frame_id == "O") return;

  odom = msg;
  have_odom = true;
}

int main(int argc, char** argv) {
  /* ---------- initialize ---------- */
  ros::init(argc, argv, "traj_generator");
  ros::NodeHandle node;

  ros::Subscriber odom_sub = node.subscribe("/uwb_vicon_odom", 50, odomCallbck);

  traj_pub = node.advertise<visualization_msgs::Marker>("/traj_generator/traj_vis", 10);
  state_pub = node.advertise<visualization_msgs::Marker>("/traj_generator/cmd_vis", 10);

  // pos_cmd_pub =
  // node.advertise<quadrotor_msgs::PositionCommand>("/traj_generator/position_cmd",
  // 50);

  pos_cmd_pub =
      node.advertise<swarmtal_msgs::drone_onboard_command>("/drone_commander/onboard_command", 10);

  ros::Duration(1.0).sleep();

  /* ---------- wait for odom ready ---------- */
  have_odom = false;
  while (!have_odom && ros::ok()) {
    cout << "no odomeetry." << endl;
    ros::Duration(0.5).sleep();
    ros::spinOnce();
  }

  /* ---------- generate trajectory using close-form minimum jerk ---------- */
  Eigen::MatrixXd pos(9, 3);
  // pos.row(0) =
  //     Eigen::Vector3d(odom.pose.pose.position.x, odom.pose.pose.position.y,
  //     odom.pose.pose.position.z);

  pos.row(0) =
      Eigen::Vector3d(odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z);
  // pos.row(0) = Eigen::Vector3d(-2, 0, 1);
  pos.row(1) = Eigen::Vector3d(-0.5, 0.5, 1);
  pos.row(2) = Eigen::Vector3d(0, 0, 1);
  pos.row(3) = Eigen::Vector3d(0.5, -0.5, 1);
  pos.row(4) = Eigen::Vector3d(1, 0, 1);
  pos.row(5) = Eigen::Vector3d(0.5, 0.5, 1);
  pos.row(6) = Eigen::Vector3d(0, 0, 1);
  pos.row(7) = Eigen::Vector3d(-0.5, -0.5, 1);
  pos.row(8) = Eigen::Vector3d(-1, 0, 1);

  Eigen::VectorXd time(8);
  time(0) = 2.0;
  time(1) = 1.5;
  time(2) = 1.5;
  time(3) = 1.5;
  time(4) = 1.5;
  time(5) = 1.5;
  time(6) = 1.5;
  time(7) = 2.0;

  Eigen::MatrixXd poly = generateTraj(pos, time);
  cout << "poly:\n" << poly << endl;

  cout << "pos:\n" << pos << endl;

  cout << "pos 0 1 2: " << pos(0) << ", " << pos(1) << ", " << pos(2) << endl;

  /* ---------- use polynomials ---------- */
  PolynomialTraj poly_traj;
  for (int i = 0; i < poly.rows(); ++i) {
    vector<double> cx(6), cy(6), cz(6);
    for (int j = 0; j < 6; ++j) {
      cx[j] = poly(i, j), cy[j] = poly(i, j + 6), cz[j] = poly(i, j + 12);
    }
    reverse(cx.begin(), cx.end());
    reverse(cy.begin(), cy.end());
    reverse(cz.begin(), cz.end());
    double ts = time(i);
    poly_traj.addSegment(cx, cy, cz, ts);
  }
  poly_traj.init();
  vector<Eigen::Vector3d> traj_vis = poly_traj.getTraj();

  displayPathWithColor(traj_vis, 0.05, Eigen::Vector4d(1, 0, 0, 1), 1);

  /* ---------- publish command ---------- */
  ros::Time start_time = ros::Time::now();
  ros::Time time_now;

  ros::Duration(0.1).sleep();

  swarmtal_msgs::drone_onboard_command cmd;
  cmd.command_type = swarmtal_msgs::drone_onboard_command::CTRL_POS_COMMAND;
  cmd.param1 = 0;
  cmd.param2 = 0;
  cmd.param3 = 0;
  cmd.param4 = 666666;
  cmd.param5 = 0;
  cmd.param6 = 0;
  cmd.param7 = 0;
  cmd.param8 = 0;
  cmd.param9 = 0;
  cmd.param10 = 0;

  while (ros::ok()) {
    time_now = ros::Time::now();
    double tn = (time_now - start_time).toSec();
    Eigen::Vector3d pt = poly_traj.evaluate(tn);
    Eigen::Vector3d vel = poly_traj.evaluateVel(tn);
    Eigen::Vector3d acc = poly_traj.evaluateAcc(tn);

    cmd.param1 = int(pt(0) * 10000);
    cmd.param2 = int(pt(1) * 10000);
    cmd.param3 = int(pt(2) * 10000);

    cmd.param5 = int(vel(0) * 10000);
    cmd.param6 = int(vel(1) * 10000);

    cmd.param7 = 0;
    cmd.param8 = int(acc(0) * 10000);
    cmd.param9 = int(acc(1) * 10000);

    pos_cmd_pub.publish(cmd);

    drawState(pt, vel, 0, Eigen::Vector4d(0, 1, 0, 1));
    drawState(pt, acc, 1, Eigen::Vector4d(0, 0, 1, 1));
    ros::Duration(0.01).sleep();
  }

  ros::spin();
  return 0;
}

首先是变量定义，

ROS 话题发布者：
state_pub：发布当前状态 (位置 & 速度 & 加速度)
pos_cmd_pub：发布轨迹跟踪控制指令 (swarmtal_msgs::drone_onboard_command)
traj_pub：发布轨迹可视化 (visualization_msgs::Marker)
odom：存储无人机当前位姿
have_odom：标记 odom 是否接收到

odom_sub用于订阅里程计数据，在回调函数void odomCallbck(const nav_msgs::Odometry& msg)中，将里程计数据记录在odom中，标志位have_odom为true。

have_odom = false;
while (!have_odom && ros::ok())
{
  cout << "no odometry." << endl;
  ros::Duration(0.5).sleep();
  ros::spinOnce();
}

之后在循环内一直等待里程计数据就位。

利用displayPathWithColor绘制轨迹，displayPathWithColor(traj_vis, 0.05, Eigen::Vector4d(1, 0, 0, 1), 1);

swarmtal_msgs::drone_onboard_command cmd;
cmd.command_type = swarmtal_msgs::drone_onboard_command::CTRL_POS_COMMAND;

while (ros::ok()) {
  time_now = ros::Time::now();
  double tn = (time_now - start_time).toSec();
  Eigen::Vector3d pt = poly_traj.evaluate(tn);
  Eigen::Vector3d vel = poly_traj.evaluateVel(tn);
  Eigen::Vector3d acc = poly_traj.evaluateAcc(tn);

  cmd.param1 = int(pt(0) * 10000);
  cmd.param2 = int(pt(1) * 10000);
  cmd.param3 = int(pt(2) * 10000);
  pos_cmd_pub.publish(cmd);

  drawState(pt, vel, 0, Eigen::Vector4d(0, 1, 0, 1));
  drawState(pt, acc, 1, Eigen::Vector4d(0, 0, 1, 1));
  ros::Duration(0.01).sleep();
}

这里构建发布的控制指令消息cmd，在循环中不断的获得当前时刻位置、速度、加速度，填充到消息cmd中，再使用pos_cmd_pub发布者发布消息cmd，调用函数drawState绘制箭头。