Ego_planner_swarm之minimum snap(jerk)代码解释

首先是minimum snap的理论推导过程 https://blog.csdn.net/u011341856/article/details/121861930
我对他的博客的一些笔记 https://pan.quark.cn/s/8549109ff930#/list/share
下面就是对高飞老师ego planner中的minimum snap(jerk)的注释解析

#include <iostream>
#include <traj_utils/polynomial_traj.h>

PolynomialTraj 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();
  // 这里是xyz分开讨论的，所以每个轨迹段的系数矩阵是3*6的
  Eigen::MatrixXd poly_coeff(seg_num, 3 * 6);
  // 一共有多少个变量，这里是6*seg_num，因为每个轨迹段有6个系数，这是个向量
  Eigen::VectorXd Px(6 * seg_num), Py(6 * seg_num), Pz(6 * seg_num);

  // num_f是固定的变量个数，num_p是自由变量个数，num_d是所有段的导数个数
  int num_f, num_p; // number of fixed and free variables
  int num_d;        // number of all segments' derivatives

  // 这段代码定义了一个名为Factorial的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 ---------- */
  // 端点和端点的微分约束，每一个轴的约束都是6个，分别是每一中的位置、速度、加速度，起始点和终止点
  Eigen::VectorXd Dx = Eigen::VectorXd::Zero(seg_num * 6);
  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 */
    // k*6是因为每个轨迹段有6个系数，k是轨迹段的索引
    // 初始位置
    Dx(k * 6) = Pos(0, k);
    // 末端位置
    Dx(k * 6 + 1) = Pos(0, k + 1);
    Dy(k * 6) = Pos(1, k);
    Dy(k * 6 + 1) = Pos(1, k + 1);
    Dz(k * 6) = Pos(2, k);
    Dz(k * 6 + 1) = Pos(2, k + 1);

    // 对速度进行约束
    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 对角矩阵---------- */
  // Ab存储fixed的值，包括起始点和终止点的位置、速度、加速度
  Eigen::MatrixXd Ab;
  // 对于一个多端的轨迹的话，A是个对角矩阵，对角线上的元素是Ab
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(seg_num * 6, seg_num * 6);
/*
对角矩阵
 A=[A1,0,0
 0........0
 0,......,Ak]

 Ab=[1,0, 0, 0, 0, 0;
 1, dt, dt2,dt3,dt4,dt5;
 0, 1, 0,0,0,0；
 0，1, 2 * dt, 3 * dt2, 4* dt3, 5* dt4;
 0, 0, 2, 0,0,0;
 0, 0, 2, 6* dt, 12 * dt2, 20 * dt3];
 注:dtn=t*t*..*t,t的个数为n
*/

  for (int k = 0; k < seg_num; k++)
  {
    Ab = Eigen::MatrixXd::Zero(6, 6);
    for (int i = 0; i < 3; i++)
    {
      // 2*i 偶数项，其实就是终止点的约束，包括位置速度加速度
      Ab(2 * i, i) = Factorial(i);
      for (int j = i; j < 6; j++)
      // 对应的下一项就是起始点的约束，包括速度和加速度
        Ab(2 * i + 1, j) = Factorial(j) / Factorial(j - i) * pow(Time(k), j - i);
    }
    /*
    block函数的参数是(startRow, startCol, blockRows, blockCols)，表示子块的起始行、起始列、行数和列数。
    在这个例子中，子块的起始行和起始列都是k * 6，行数和列数都是6，所以这个子块是一个6x6的方块，位于矩阵A的(k * 6, k * 6)位置。
    */
    A.block(k * 6, k * 6, 6, 6) = Ab;
  }

  /* ---------- Produce Selection Matrix C' ---------- */
  Eigen::MatrixXd Ct, C;

  // fixed点（6个）始末点的pva，中间点（segment-1）的位置，中间点的位置（segment-1）连续
  num_f = 2 * seg_num + 4; // 3 + 3 + (seg_num - 1) * 2 = 2m + 4，
  // 中间点（segmen-1）的速度，加速度连续约束，这个是主要优化的对象，微分不确定性
  num_p = 2 * seg_num - 2; //(seg_num - 1) * 2 = 2m - 2
  num_d = 6 * seg_num;
  // 置换矩阵，C来将连续性约束进行引入到代价函数中
  Ct = Eigen::MatrixXd::Zero(num_d, num_f + num_p);
  Ct(0, 0) = 1;
  Ct(2, 1) = 1;
  Ct(4, 2) = 1; // stack the start point
  Ct(1, 3) = 1;
  Ct(3, 2 * seg_num + 4) = 1;
  Ct(5, 2 * seg_num + 5) = 1;

  Ct(6 * (seg_num - 1) + 0, 2 * seg_num + 0) = 1;
  Ct(6 * (seg_num - 1) + 1, 2 * seg_num + 1) = 1; // Stack the end point
  Ct(6 * (seg_num - 1) + 2, 4 * seg_num + 0) = 1;
  Ct(6 * (seg_num - 1) + 3, 2 * seg_num + 2) = 1; // Stack the end point
  Ct(6 * (seg_num - 1) + 4, 4 * seg_num + 1) = 1;
  Ct(6 * (seg_num - 1) + 5, 2 * seg_num + 3) = 1; // Stack the end point

  for (int j = 2; j < seg_num; j++)
  {
    Ct(6 * (j - 1) + 0, 2 + 2 * (j - 1) + 0) = 1;
    Ct(6 * (j - 1) + 1, 2 + 2 * (j - 1) + 1) = 1;
    Ct(6 * (j - 1) + 2, 2 * seg_num + 4 + 2 * (j - 2) + 0) = 1;
    Ct(6 * (j - 1) + 3, 2 * seg_num + 4 + 2 * (j - 1) + 0) = 1;
    Ct(6 * (j - 1) + 4, 2 * seg_num + 4 + 2 * (j - 2) + 1) = 1;
    Ct(6 * (j - 1) + 5, 2 * seg_num + 4 + 2 * (j - 1) + 1) = 1;
  }

  C = Ct.transpose();

  Eigen::VectorXd Dx1 = C * Dx;
  Eigen::VectorXd Dy1 = C * Dy;
  Eigen::VectorXd Dz1 = C * Dz;

  /* ---------- minimum snap matrix ---------- */
  Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(seg_num * 6, seg_num * 6);

  for (int k = 0; k < seg_num; k++)
  {
    for (int i = 3; i < 6; i++)
    {
      for (int j = 3; j < 6; j++)
      {
        Q(k * 6 + i, k * 6 + j) =
            i * (i - 1) * (i - 2) * j * (j - 1) * (j - 2) / (i + j - 5) * pow(Time(k), (i + j - 5));
      }
    }
  }

  /* ---------- R matrix ---------- */
  Eigen::MatrixXd R = C * A.transpose().inverse() * Q * A.inverse() * Ct;

  // 固定的变量个数2 * seg_num + 4
  Eigen::VectorXd Dxf(2 * seg_num + 4), Dyf(2 * seg_num + 4), Dzf(2 * seg_num + 4);

  // segment()函数的参数是(startIndex, length)，表示从startIndex开始，长度为length的子向量。
  Dxf = Dx1.segment(0, 2 * seg_num + 4);
  Dyf = Dy1.segment(0, 2 * seg_num + 4);
  Dzf = Dz1.segment(0, 2 * seg_num + 4);

  // 分块矩阵的四个元素
  Eigen::MatrixXd Rff(2 * seg_num + 4, 2 * seg_num + 4);
  Eigen::MatrixXd Rfp(2 * seg_num + 4, 2 * seg_num - 2);
  Eigen::MatrixXd Rpf(2 * seg_num - 2, 2 * seg_num + 4);
  Eigen::MatrixXd Rpp(2 * seg_num - 2, 2 * seg_num - 2);

  // 赋值给分块矩阵的四个元素
  Rff = R.block(0, 0, 2 * seg_num + 4, 2 * seg_num + 4);
  Rfp = R.block(0, 2 * seg_num + 4, 2 * seg_num + 4, 2 * seg_num - 2);
  Rpf = R.block(2 * seg_num + 4, 0, 2 * seg_num - 2, 2 * seg_num + 4);
  Rpp = R.block(2 * seg_num + 4, 2 * seg_num + 4, 2 * seg_num - 2, 2 * seg_num - 2);

  /* ---------- close form solution ---------- */
  // 闭式求解

  Eigen::VectorXd Dxp(2 * seg_num - 2), Dyp(2 * seg_num - 2), Dzp(2 * seg_num - 2);
  // 求偏导
  Dxp = -(Rpp.inverse() * Rfp.transpose()) * Dxf;
  Dyp = -(Rpp.inverse() * Rfp.transpose()) * Dyf;
  Dzp = -(Rpp.inverse() * Rfp.transpose()) * Dzf;

  Dx1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dxp;
  Dy1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dyp;
  Dz1.segment(2 * seg_num + 4, 2 * seg_num - 2) = Dzp;

  /*
  P = A.inverse()* Dx =
    = A.inverse()*C.transpose().Dx1
  这样我们就求出来了所有的系数,共有2m+4+2m-2=4m+2个系数
  */
  Px = (A.inverse() * Ct) * Dx1;
  Py = (A.inverse() * Ct) * Dy1;
  Pz = (A.inverse() * Ct) * Dz1;

  for (int i = 0; i < seg_num; i++)
  {
    // 每一行代表一段轨迹,0-5是x轴的系数，6-11是y轴的系数，12-17是z轴的系数
    poly_coeff.block(i, 0, 1, 6) = Px.segment(i * 6, 6).transpose();
    poly_coeff.block(i, 6, 1, 6) = Py.segment(i * 6, 6).transpose();
    poly_coeff.block(i, 12, 1, 6) = Pz.segment(i * 6, 6).transpose();
  }

  /* ---------- use polynomials ---------- */
  PolynomialTraj poly_traj;
  for (int i = 0; i < poly_coeff.rows(); ++i)
  {
    vector<double> cx(6), cy(6), cz(6);
    for (int j = 0; j < 6; ++j)
    {
      cx[j] = poly_coeff(i, j), cy[j] = poly_coeff(i, j + 6), cz[j] = poly_coeff(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);
  }

  return poly_traj;
}

PolynomialTraj PolynomialTraj::one_segment_traj_gen(const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
                                                    const Eigen::Vector3d &end_pt, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc,
                                                    double t)
{
  // 这行代码创建了两个Eigen库中的MatrixXd类型的对象，分别命名为C和Crow。
  // MatrixXd是一个动态大小的矩阵，可以存储任意大小的二维数据。
  // C矩阵被用于存储多项式轨迹生成过程中的系数。
  // Crow(1, 6)创建了一个1行6列的矩阵，但没有初始化。这个矩阵被赋值给了Crow。
  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(6, 6), Crow(1, 6);
  Eigen::VectorXd Bx(6), By(6), Bz(6);

  // 在多项式轨迹生成中，我们通常会有一个6阶的多项式来描述轨迹，形如 a*t^5 + b*t^4 + c*t^3 + d*t^2 + e*t + f
  // C(0, 5) = 1; 这行代码将矩阵C的第1行第6列的元素设置为1，这是为了满足初始位置的约束，因为在多项式中，t^5的系数对应于初始位置。
  // C(1, 4) = 1; 这行代码将矩阵C的第2行第5列的元素设置为1，这是为了满足初始速度的约束，因为在多项式中，t^4的系数对应于初始速度。
  // C(2, 3) = 2; 这行代码将矩阵C的第3行第4列的元素设置为2，这是为了满足初始加速度的约束，因为在多项式中，2*t^3的系数对应于初始加速度。
  C(0, 5) = 1;
  C(1, 4) = 1;
  C(2, 3) = 2;

  // pow(t, 5), pow(t, 4), pow(t, 3), pow(t, 2), t, 1是6个值，分别是t的5次方、4次方、3次方、2次方、1次方和0次方（即1）。
  Crow << pow(t, 5), pow(t, 4), pow(t, 3), pow(t, 2), t, 1;
  // t时刻的位置约束
  C.row(3) = Crow;
  // t时刻的速度约束
  Crow << 5 * pow(t, 4), 4 * pow(t, 3), 3 * pow(t, 2), 2 * t, 1, 0;
  C.row(4) = Crow;
  // t时刻的加速度约束
  Crow << 20 * pow(t, 3), 12 * pow(t, 2), 6 * t, 2, 0, 0;
  C.row(5) = Crow;
  // x轴的位置速度加速度结果,这个是已知的
  Bx << start_pt(0), start_vel(0), start_acc(0), end_pt(0), end_vel(0), end_acc(0);
  By << start_pt(1), start_vel(1), start_acc(1), end_pt(1), end_vel(1), end_acc(1);
  Bz << start_pt(2), start_vel(2), start_acc(2), end_pt(2), end_vel(2), end_acc(2);

  Eigen::VectorXd Cofx = C.colPivHouseholderQr().solve(Bx);
  Eigen::VectorXd Cofy = C.colPivHouseholderQr().solve(By);
  Eigen::VectorXd Cofz = C.colPivHouseholderQr().solve(Bz);

  vector<double> cx(6), cy(6), cz(6);
  for (int i = 0; i < 6; i++)
  {
    cx[i] = Cofx(i);
    cy[i] = Cofy(i);
    cz[i] = Cofz(i);
  }

  PolynomialTraj poly_traj;
  poly_traj.addSegment(cx, cy, cz, t);

  return poly_traj;
}