0. 题目
1. 解答
—
Listing1：
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)
{
// coefficientMatrix is a matrix with 6*piece num rows and 3 columes
// As for a polynomial c0+c1*t+c2*t^2+c3*t^3+c4*t^4+c5*t^5,
// each 6*3 sub-block of coefficientMatrix is
// -- --
// | c0_x c0_y c0_z |
// | c1_x c1_y c1_z |
// | c2_x c2_y c2_z |
// | c3_x c3_y c3_z |
// | c4_x c4_y c4_z |
// | c5_x c5_y c5_z |
// -- --
// Please computed coefficientMatrix of the minimum-jerk trajectory
// in this function
// ------------------------ Put your solution below ------------------------
//coefficientMatrix维度维度为(6*pieceNum, 3)，之前已经给出，不用操作
//起始和末状态的PVA约束分别是3行，加起来总共6行约束(s*2s)=(3*6)，中间状态有(pieceNum-1)组约束(2s*2s)=(6*6)，所以总约束仍为(2sM*2sM)=(6M*6M)
Eigen::MatrixXd M = Eigen::MatrixXd::Zero(6*pieceNum, 6*pieceNum);
Eigen::MatrixXd b = Eigen::MatrixXd::Zero(6*pieceNum, 3);
//初始条件PVA约束
Eigen::MatrixXd F_0(3, 6);
F_0.setZero();
F_0(0,0) = 1;
F_0(1,1) = 1;
F_0(2,2) = 2;
M.block(0,0,3,6) = F_0;
b.row(0) = initialPos.transpose();
b.row(1) = initialVel.transpose();
b.row(2) = initialAcc.transpose();
//终止条件条件PVA约束
Eigen::MatrixXd E_M(3, 6);
double T_M = timeAllocationVector(pieceNum-1);
double T_M_2 = T_M * T_M;
double T_M_3 = T_M_2 * T_M;
double T_M_4 = T_M_3 * T_M;
double T_M_5 = T_M_4 * T_M;
E_M << 1, T_M, T_M_2, T_M_3, T_M_4, T_M_5,
0, 1, 2*T_M, 3*T_M_2, 4*T_M_3, 5*T_M_4,
0, 0, 2, 6*T_M, 12*T_M_2, 20*T_M_3;
M.block(6*pieceNum-3,6*(pieceNum-1),3,6) = E_M;
b.row(6*pieceNum-3) = terminalPos.transpose();
b.row(6*pieceNum-2) = terminalVel.transpose();
b.row(6*pieceNum-1) = terminalAcc.transpose();
//M共pieceNum-1组中间状态约束，前面F_0的3*6 PVA约束，后面E_M的3*6 PVA约束，
//中间是pieceNum-1组中间状态约束，由waypoint，P，V，A，Jerk，Snap连续可导组成的E_i(6*6)，F_i(6*6)约束
for(int i = 1; i < pieceNum; ++i) {//这里使用的时间是左闭右开，中间点约束在左边点上，所以是从第[1]个而非第[0]个开始
double T = timeAllocationVector(i-1);
double T_2 = T * T;
double T_3 = T_2 * T;
double T_4 = T_3 * T;
double T_5 = T_4 * T;
Eigen::MatrixXd E_i(6, 6);
Eigen::MatrixXd F_i(6, 6);
E_i << 1, T, T_2, T_3, T_4, T_5,
1, T, T_2, T_3, T_4, T_5,
0, 1, 2*T, 3*T_2, 4*T_3, 5*T_4,
0, 0, 2, 6*T, 12*T_2, 20*T_3,
0, 0, 0, 6, 24*T, 60*T_2,
0, 0, 0, 0, 24, 120*T;
M.block(6*i-3, 6*(i-1), 6, 6) = E_i;
F_i.setZero();
F_i(1,0) = -1;
F_i(2,1) = -1;
F_i(3,2) = -2;
F_i(4,3) = -6;
F_i(5,4) = -24;
M.block(6*i-3, 6*i, 6, 6) = F_i;
Eigen::Vector3d D_i_transpose = intermediatePositions.block(0,i-1,3,1);
b.block(6*i-3, 0, 1, 3) << D_i_transpose(0), D_i_transpose(1), D_i_transpose(2);
}
clock_t time_stt = clock();
// 使用PartialPivLU进行分解
// Eigen::PartialPivLU<Eigen::MatrixXd> lu(M);
// Mc = b 解为c
std::cout << "use lu" <<std::endl;
coefficientMatrix = M.lu().solve(b);
/* // Solve Mc = b, using QR solver
for (int i = 0; i < 3; i++)
{
coefficientMatrix.col(i) = M.colPivHouseholderQr().solve(b.col(i));
// coefficientMatrix.col(i) = M.lu().solve(b.col(i));
}
coefficientMatrix = M.inverse() * b;*/
// std::cout << "C is " << coefficientMatrix << std::endl;
std::cout << "Time cost = " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << std::endl;
// ------------------------ Put your solution above ------------------------
