2011年，Vijay Kumar的论文提出了QP形式的Minimum-snap方法，2016年，Nicholas Roy的论文提出了closed-form minimum snap，2021年，Zhepei Wang与Fei Gao在论文Geometrically Constrained Trajectory Optimization for Multicopters中，提出了一种无代价函数的minimum-snap方法。本文使用最后这种方法，构建Minimum-jerk的轨迹。
BVIP其实就是根据最优性条件把问题变成Mc=b。M是关于c在t等于不同时刻的系数，c就是我们待求的多段多项式样条的系数矩阵。
函数的输入输出为
void minimumJerkTrajGen(
// Inputs:
const int pieceNum,
const Eigen::Vector3d &initialPos,
const Eigen::Vector3d &initialVel,
const Eigen::Vector3d &initialAcc,
const Eigen::Vector3d &terminalPos,
const Eigen::Vector3d &terminalVel,
const Eigen::Vector3d &terminalAcc,
const Eigen::Matrix3Xd &intermediatePositions,
const Eigen::VectorXd &timeAllocationVector,
// Outputs:
Eigen::MatrixX3d &coefficientMatrix)
{
由原文，
int s = 3; // minimum-jerk
int dim = timeAllocationVector.size() * 2 * s; // dimension of matrix M
Eigen::MatrixXd M = Eigen::MatrixXd::Zero(dim, dim);
Eigen::MatrixXd b = Eigen::MatrixXd::Zero(dim, 3);
// get F0 and D0
Eigen::MatrixXd F0(s, 2 * s);
Eigen::MatrixXd D0(s, 3);
F0 << 1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0,
0, 0, 2, 0, 0, 0;
D0 << initialPos(0), initialPos(1), initialPos(2),
initialVel(0), initialVel(1), initialVel(2),
initialAcc(0), initialAcc(1), initialAcc(2);
M.block(0, 0, s, 2 * s) << F0;
b.block(0, 0, s, 3) << D0;
// get Fi, Ei, and Di, use for loop to fill them into M and b
for (int i = 0; i < dim - 2 * s; i += 2 * s)
{
int index = i / (2 * s);
double t = timeAllocationVector(index);
Eigen::MatrixXd Fi(2 * s, 2 * s);
Eigen::MatrixXd Ei(2 * s, 2 * s);
Eigen::MatrixXd Di(1, 3);
Fi << 0, 0, 0, 0, 0, 0,
-1, 0, 0, 0, 0, 0,
**0, -1, 0, 0, 0, 0,
0, 0, -2, 0, 0, 0,
0, 0, 0, -6, 0, 0,
0, 0, 0, 0, -24, 0;
Ei << 1, t, pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5),
1, t, pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5),
0, 1, 2 * t, 3 * pow(t, 2), 4 * pow(t, 3), 5 * pow(t, 4),
0, 0, 2, 6 * t, 12 * pow(t, 2), 20 * pow(t, 3),
0, 0, 0, 6, 24 * t, 60 * pow(t, 2),
0, 0, 0, 0, 24, 120 * t;
Di << intermediatePositions.col(index).transpose();
M.block(s + i, 2 * s + i, 2 * s, 2 * s) << Fi;
M.block(s + i, i, 2 * s, 2 * s) << Ei;
b.block(s + i, 0, 1, 3) << Di;
}
// get E_M and D_M
Eigen::MatrixXd E_M(s, 2 * s);
Eigen::MatrixXd D_M(s, 3);
double t = timeAllocationVector(timeAllocationVector.size() - 1);
E_M << 1, t, pow(t, 2), pow(t, 3), pow(t, 4), pow(t, 5),
0, 1, 2 * t, 3 * pow(t, 2), 4 * pow(t, 3), 5 * pow(t, 4),
0, 0, 2, 6 * t, 12 * pow(t, 2), 20 * pow(t, 3);
D_M << terminalPos(0), terminalPos(1), terminalPos(2),
terminalVel(0), terminalVel(1), terminalVel(2),
terminalAcc(0), terminalAcc(1), terminalAcc(2);
M.block(dim - s, dim - 2 * s, s, 2 * s) << E_M;
b.block(dim - s, 0, s, 3) << D_M;