首先是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;
}