无人机避障——如何利用MinumSnap进行对速度、加速度进行优化的轨迹生成（附C++&python代码）

原创

已于 2025-05-13 17:01:40 修改 · 1.5k 阅读

25 ·

CC 4.0 BY-SA版权

文章标签：

#无人机 #规划控制 #Minmum-snap

于 2025-05-13 16:57:07 首次发布

🔥轨迹规划领域的 “YYDS”——minimum snap！作为基于优化的二次规划经典，它是无人机、自动驾驶轨迹规划论文必引的 “开山之作”。从优化目标函数到变量曲线表达，各路大神疯狂 “魔改”，衍生出无数创新方案。

💡深度剖析百度 Apollo 的迭代奥秘：早期五次多项式拼接，如今离散点 piecewise jerk 路径 / 速度规划，本质皆是 minimum snap 的 “变形记”。

🚀想知道这些 “神操作” 背后的数学逻辑与技术细节？想解锁轨迹规划的优化新姿势？速来围观，带你直击前沿技术内核，开启轨迹规划的 “脑洞大开” 之旅！

问题：局部路径在运行的过程中对当下的速度和加速度进行采样，得到一个经过速度和加速度优化的轨迹进行运行是非常必要的，可以减少无人机单单用基于位置的简单的路径在空中经过PID或者其他控制以后出现振荡与大量抖动，这部分与控制并无关系，因为是期望都会不稳定，所以需要MinumSnap采用最小加加加速度（snap）的方法进行优化轨迹生成。

Fast-planner的全局规划主函数：

// 全局轨迹规划主函数，输入参数为起始位置
bool FastPlannerManager::planGlobalTraj(const Eigen::Vector3d& start_pos) {
  // 清空之前的拓扑路径数据
  plan_data_.clearTopoPaths();

  /* ------------------------- 全局参考轨迹生成 ------------------------- */
  // 获取预定义的全局航点
  vector<Eigen::Vector3d> points = plan_data_.global_waypoints_;
  if (points.size() == 0) 
    std::cout << "no global waypoints!" << std::endl; // 空航点警告

  // 将起始点插入航点列表首部（确保轨迹从当前位置开始）
  points.insert(points.begin(), start_pos);

  /* ----------- 插入中间点（当航点间距过大时分段插入） ----------- */
  vector<Eigen::Vector3d> inter_points;
  const double dist_thresh = 4.0; // 最大分段距离阈值

  for (int i = 0; i < points.size() - 1; ++i) {
    inter_points.push_back(points.at(i)); // 添加当前航点
    double dist = (points.at(i + 1) - points.at(i)).norm();

    // 当两点间距超过阈值时插入中间点
    if (dist > dist_thresh) {
      int id_num = floor(dist / dist_thresh) + 1; // 计算需要插入的中间点数
      // 线性插值生成中间点（例如间距6米时插入1个中间点，形成3段）
      for (int j = 1; j < id_num; ++j) {
        Eigen::Vector3d inter_pt = points.at(i) * (1.0 - double(j)/id_num) 
                                 + points.at(i + 1) * double(j)/id_num;
        inter_points.push_back(inter_pt);
      }
    }
  }
  inter_points.push_back(points.back()); // 添加最后一个航点

  // 若处理后仅有起点和终点，在中间强制添加中点（确保轨迹有控制点）
  if (inter_points.size() == 2) {
    Eigen::Vector3d mid = (inter_points[0] + inter_points[1]) * 0.5;
    inter_points.insert(inter_points.begin() + 1, mid);
  }

  /* ------------------------- 轨迹生成 ------------------------- */
  // 构造位置矩阵，每行代表一个航点的3D坐标
  int pt_num = inter_points.size();
  Eigen::MatrixXd pos(pt_num, 3);
  for (int i = 0; i < pt_num; ++i) 
    pos.row(i) = inter_points[i];

  // 初始化速度/加速度边界条件为零（起点终点零速度、零加速度）
  Eigen::Vector3d zero(0, 0, 0);
  
  // 计算相邻航点间的预估时间（基于最大速度和距离）
  Eigen::VectorXd time(pt_num - 1);
  for (int i = 0; i < pt_num - 1; ++i) {
    time(i) = (pos.row(i+1) - pos.row(i)).norm() / pp_.max_vel_;
  }

  // 调整首尾段时间权重（确保时间不小于1秒）
  time(0) *= 2.0;
  time(0) = max(1.0, time(0));
  time(time.rows()-1) *= 2.0;
  time(time.rows()-1) = max(1.0, time(time.rows()-1));

  // 生成最小加加速度（Snap）多项式轨迹
  PolynomialTraj gl_traj = minSnapTraj(pos, zero, zero, zero, zero, time);

  // 设置全局轨迹数据并记录生成时间
  auto time_now = ros::Time::now();
  global_data_.setGlobalTraj(gl_traj, time_now);

  /* ------------------------- 局部轨迹截取 ------------------------- */
  double dt, duration;
  // 重新参数化局部轨迹（时间间隔dt和总时长duration会被计算）
  Eigen::MatrixXd ctrl_pts = reparamLocalTraj(0.0, dt, duration);
  // 基于控制点生成3阶非均匀B样条轨迹
  NonUniformBspline bspline(ctrl_pts, 3, dt);

  // 更新局部轨迹数据
  global_data_.setLocalTraj(bspline, 0.0, duration, 0.0);
  local_data_.position_traj_ = bspline;
  local_data_.start_time_ = time_now;
  ROS_INFO("global trajectory generated.");

  // 更新轨迹信息（如速度、加速度约束等）
  updateTrajInfo();

  return true;
}

改变最大分段距离阈值dist_thresh：

改变前：

dist_thresh = 4.0

改变后：

dist_thresh = 20.0

【注意】：运行之后发现并没有太大的区别。

最小化Sanp轨迹生成函数（核心实现）解读——函数minSnapTraj：

// 最小Snap轨迹生成函数（核心实现）
PolynomialTraj minSnapTraj(const Eigen::MatrixXd& Pos, const Eigen::Vector3d& start_vel,
                           const Eigen::Vector3d& end_vel, const Eigen::Vector3d& start_acc,
                           const Eigen::Vector3d& end_acc, const Eigen::VectorXd& Time) {

  // --- 初始化阶段 --- 
  int seg_num = Time.size(); // 轨迹段数（航点间分段）
  Eigen::MatrixXd poly_coeff(seg_num, 3 * 6); // 存储XYZ三轴各6阶多项式系数
  Eigen::VectorXd Px(6 * seg_num), Py(6 * seg_num), Pz(6 * seg_num); // 各段多项式系数向量

  int num_f, num_p;  // number of fixed and free variables
  int num_d;         // number of all segments' derivatives

  // 阶乘计算lambda（用于导数项系数计算）
  const static auto Factorial = [](int x) {
    int fac = 1;
    for (int i = x; i > 0; i--)
      fac = fac * i;
    return fac;
  };

  /* ---------- end point derivative ---------- */
  // --- 端点导数约束设置 ---
  Eigen::VectorXd Dx = Eigen::VectorXd::Zero(seg_num * 6); // X轴各段导数约束
  Eigen::VectorXd Dy = Eigen::VectorXd::Zero(seg_num * 6);
  Eigen::VectorXd Dz = Eigen::VectorXd::Zero(seg_num * 6);

  for (int k = 0; k < seg_num; k++) {
    /* position to derivative */
    // 设置位置约束（每段起点和终点）
    Dx(k * 6) = Pos(k, 0);
    Dx(k * 6 + 1) = Pos(k + 1, 0);
    Dy(k * 6) = Pos(k, 1);
    Dy(k * 6 + 1) = Pos(k + 1, 1);
    Dz(k * 6) = Pos(k, 2);
    Dz(k * 6 + 1) = Pos(k + 1, 2);

    // 首段设置初始速度/加速度约束
    if (k == 0) {
      Dx(k * 6 + 2) = start_vel(0);
      Dy(k * 6 + 2) = start_vel(1);
      Dz(k * 6 + 2) = start_vel(2);

      Dx(k * 6 + 4) = start_acc(0);
      Dy(k * 6 + 4) = start_acc(1);
      Dz(k * 6 + 4) = start_acc(2);
    } 
    // 末段设置终止速度/加速度约束
    else if (k == seg_num - 1) {
      Dx(k * 6 + 3) = end_vel(0);
      Dy(k * 6 + 3) = end_vel(1);
      Dz(k * 6 + 3) = end_vel(2);

      Dx(k * 6 + 5) = end_acc(0);
      Dy(k * 6 + 5) = end_acc(1);
      Dz(k * 6 + 5) = end_acc(2);
    }
  }

  /* ---------- Mapping Matrix A ---------- */
  // --- 构建映射矩阵A ---
  Eigen::MatrixXd Ab;
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(seg_num * 6, seg_num * 6);

  for (int k = 0; k < seg_num; k++) {
    Ab = Eigen::MatrixXd::Zero(6, 6);
    for (i