【深蓝学院】移动机器人运动规划--第5章 最优轨迹生成--作业

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 ------------------------