基于ROS的Minimun Snap闭式求解算法代码学习_minimum snap c++

本文是在学习路径规划算法中的一篇学习记录,由于对c++掌握粗浅,将整个代码进行了详细的梳理,方便后来者参考学习.代码需要和理论部分结合起来看更容易理解,理论部分: 《Minimun Snap轨迹优化学习记录》.

(代码注释中英夹杂,英文注释是原来作者的注释,中文注释是本人后来添加的)

1.过程演示

         1.进入到工作空间下运行catkin_make编译;
         2.运行source devel/setup.bash;
         3.打开一个新的终端运行roscore;
         4.回到原终端窗口运行roslaunch waypoint_trajectory_generator test.launch;
         5.rviz运行起来之后,按如下步骤操作:

         结束的操作

         最终的效果

2.trajectory_generator_waypoint.h解析

#ifndef _TRAJECTORY_GENERATOR_WAYPOINT_H_
#define _TRAJECTORY_GENERATOR_WAYPOINT_H_

//#include <Eigen/Eigen>
#include <eigen3/Eigen/Eigen>
#include <vector>

class TrajectoryGeneratorWaypoint {
    private:
    // 原作者给出的，闭式求解没有用到
		double _qp_cost{};
		Eigen::MatrixXd _Q;
		Eigen::VectorXd _Px, _Py, _Pz;
    public:
        TrajectoryGeneratorWaypoint();

        ~TrajectoryGeneratorWaypoint();

        // 生成多项式系数
        Eigen::MatrixXd PolyQPGeneration(
            int order,
            const Eigen::MatrixXd &Path,
            const Eigen::MatrixXd &Vel,
            const Eigen::MatrixXd &Acc,
            const Eigen::VectorXd &Time);
        
        // Factorial阶乘
        static int Factorial(int x);
        // 获得Ct矩阵
        Eigen::MatrixXd getCt(int n_seg, int d_order);
        // 获得M矩阵
        Eigen::MatrixXd getM(int n_seg, int  d_order, int p_num1d, const Eigen::VectorXd &ts);
        // 获得Q矩阵
        Eigen::MatrixXd getQ(int n_seg, int  d_order,  int p_num1d, const Eigen::VectorXd &ts);

};
        
#endif
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

3.trajectory_generator_waypoint.cpp解析

        3.1 Factorial函数

          实现阶乘;

int TrajectoryGeneratorWaypoint::Factorial(int x)
{
    int fac = 1;
    for(int i = x; i > 0; i--)
        fac = fac * i;
    return fac;
}
1
2
3
4
5
6
7

        3.2 getCt函数

          下面的代码是原作者给出的，我贴到这里，但是我并没有理解作者的思路，所以后面又自己写了两种获得Ct矩阵的方法便于后来者理解，对于Ct矩阵的理论部分可参考上一篇博文《Minimun Snap轨迹优化学习记录》中第三小节<3.Minimun Snap闭式求解(Closed-form Solution for Minimun Snap)>的第四张图片;

MatrixXd TrajectoryGeneratorWaypoint::getCt(int const n_seg, int const d_order){ // d_order is 4
     /*
     * get Selection Matrix
     * args:
     *      n_seg: the number of segments
     *      d_order: the deferential order: if minimal jerk. it is 3,
     * return:
     *      Ct: a matrix,
     *      row corresponds to original variable vector d ( 2 * d_order * n_seg )
     *      column corresponds to [ dF, dP ]' ( d_order*2*n_seg - (n_seg-1)*d_order )
     *      Note: the variables implicitly eliminates same variables""
     */
    int ct_rows = d_order*2*n_seg;
    int ct_cols = d_order*2*n_seg - (n_seg-1)*d_order;
    MatrixXd Ct = MatrixXd::Zero(ct_rows, ct_cols);

    vector<int> d_vector;
    for(int k = 0; k < n_seg; k ++){
        // 
        for(int t = 0; t < 2; t++){
            for(int d = 0; d < d_order; d++){
                d_vector.push_back(k*100+t*10+d);
            }
        }
    }
    // cout << "---------------" << endl;
    // cout << "d_vector:" << endl;
    // for(vector<int>::iterator it = d_vector.begin(); it != d_vector.end(); it++)
    //     cout << *it << endl;
    // cout << "---------------" << endl;
    int val, row;
    int col = 0; // column of one in Ct

    // fixed starting point at segment 0 in [ dF, dP ]'
    int k = 0;
    int t = 0;
    int d = 0;  
    for(d = 0; d < d_order; d++){
        val = k * 100 + t * 10 + d;
        auto it = std::find(d_vector.begin(), d_vector.end(), val);
        // std::distance,返回两个迭代器之间的距离
        row = std::distance(d_vector.begin(), it);
        Ct(row, col) = 1;
        col += 1;
    }
    // fixed final point at segment 0, 1, 2, n_seg-2 in [ dF, dP ]'
    t = 1;
    d = 0;
    for(k = 0; k < n_seg - 1; k++){
        val = k * 100 + t * 10 + d;
        auto it = std::find(d_vector.begin(), d_vector.end(), val);
        row = std::distance(d_vector.begin(), it);
        Ct(row, col) = 1;

        val = (k + 1) * 100 + (t - 1) * 10 + d;
        it= std::find(d_vector.begin(), d_vector.end(), val);
        row = std::distance(d_vector.begin(), it);
        Ct(row, col) = 1;

        col += 1;
    }

    // fixed final point at segment n_seg-1 in [ dF, dP ]'
    k = n_seg - 1;
    t = 1;
    d = 0;
    for(d = 0; d < d_order; d++){
        val = k * 100 + t * 10 + d;
        auto it = std::find(d_vector.begin(), d_vector.end(), val);
        row = std::distance(d_vector.begin(), it);
        Ct(row, col) = 1;
        col += 1;
    }

    // free variables at segment 0, 1, 2, n_seg-1 in [ dF, dP ]'
    k = 0;
    t = 1;
    d = 1;
    for(k = 0; k < n_seg - 1; k++){
        for(d = 1; d < d_order; d++){
            val = k * 100 + t * 10 + d;
            auto it = std::find(d_vector.begin(), d_vector.end(), val);
            row = std::distance(d_vector.begin(), it);
            Ct(row, col) = 1;

            val = (k + 1) * 100 + (t - 1) * 10 + d;
            it = std::find(d_vector.begin(), d_vector.end(), val);
            row = std::distance(d_vector.begin(), it);
            Ct(row, col) = 1;

            col += 1;
        }

    }
    return Ct;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96

          比较容易理解的方法一：

MatrixXd TrajectoryGeneratorWaypoint::getCt(int const n_seg, int const d_order){ // d_order is 4
    /*
     * get Selection Matrix
     * args:
     *      n_seg: the number of segments
     *      d_order: the deferential order: if minimal jerk. it is 3,
     * return:
     *      Ct: a matrix,
     *      row corresponds to original variable vector d ( 2 * d_order * n_seg )
     *      column corresponds to [ dF, dP ]' ( d_order*2*n_seg - (n_seg-1)*d_order )
     *      Note: the variables implicitly eliminates same variables""
    */
    /*
     * 阅读代码之前，请仔细理解 d = Ct*[dF dP]t，以及[dF dP]和矩阵d的内容.
     * 对于 d_order=4 n_seg=3 的情况：
     * [dF dP]t = [p0 v0 a0 j0 p1 p2 p3 v3 a3 j3, v1 a1 j1 v2 a2 j2]
     * dt = [p0 v0 a0 j0 p1 v1 a1 j1 p1 v1 a1 j1 p2 v2 a2 j2 p2 v2 a2 j2 p3 v3 a3 j3]
    */

    // 确定Ct矩阵尺寸，可参考https://blog.csdn.net/weixin_44558122/article/details/116173197#t3中<<3.Minimun Snap闭式求
    // 解(Closed-form Solution for Minimun Snap)>>部分第四张图片的例子
    int ct_rows = d_order*2*n_seg;
    int ct_cols = d_order*2*n_seg - (n_seg-1)*d_order;
    MatrixXd Ct = MatrixXd::Zero(ct_rows, ct_cols);
    string p("p");
    string v("v");
    string a("a");
    string j("j");

    // 构造dfdp
    vector<string> dfdp;
    for(int k = 0; k < n_seg+1; k++){
        if(k == 0 || k == n_seg){
            dfdp.push_back(p+to_string(k));
            dfdp.push_back(v+to_string(k));
            dfdp.push_back(a+to_string(k));
            dfdp.push_back(j+to_string(k));
        }
        else
        {
            dfdp.push_back(p+to_string(k));
        }
    }
    for(int i = 1; i < n_seg; i++){
        dfdp.push_back(v+to_string(i));
        dfdp.push_back(a+to_string(i));
        dfdp.push_back(j+to_string(i));
    }
    // for(auto it = dfdp.begin(); it != dfdp.end(); it++)
    //     cout << *it << " ";
    // cout << endl;

    // 构造d
    vector<string> d;
    for(int k = 0; k < n_seg+1; k++){
        if(k == 0 || k == n_seg){
            d.push_back(p+to_string(k));
            d.push_back(v+to_string(k));
            d.push_back(a+to_string(k));
            d.push_back(j+to_string(k));
        }
        else
        {
            d.push_back(p+to_string(k));
            d.push_back(v+to_string(k));
            d.push_back(a+to_string(k));
            d.push_back(j+to_string(k));
            d.push_back(p+to_string(k));
            d.push_back(v+to_string(k));
            d.push_back(a+to_string(k));
            d.push_back(j+to_string(k));
        }
    }
    // for(auto it = d.begin(); it != d.end(); it++)
    //     cout << *it << " ";

    // 通过查找约束(如p0、a1)在d和dfdp中的位置，对应Ct矩阵的行和列，对对应的位置赋1
    for(auto it = d.begin(); it != d.end(); it++){
        int row = distance(d.begin(),it);
        // 在dfdp中取找当前it迭代对象对应的d中的值
        auto it1 = std::find(dfdp.begin(), dfdp.end(), *it);
        int col = distance(dfdp.begin(),it1);
        Ct(row,col) = 1;
        // cout << row << " " << col << endl;
    }
    return Ct;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87

          稍微有些绕的方法二：

MatrixXd TrajectoryGeneratorWaypoint::getCt(int const n_seg, int const d_order){ // d_order is 4
     /*
     * get Selection Matrix
     * args:
     *      n_seg: the number of segments
     *      d_order: the deferential order: if minimal jerk. it is 3,
     * return:
     *      Ct: a matrix,
     *      row corresponds to original variable vector d ( 2 * d_order * n_seg )
     *      column corresponds to [ dF, dP ]' ( d_order*2*n_seg - (n_seg-1)*d_order )
     *      Note: the variables implicitly eliminates same variables""
     */
    // 阅读代码之前，请仔细理解 d = Ct*[dF dP]t，以及[dF dP]和矩阵d的内容.
    // 对于 d_order=4 n_seg=3 的情况：
    // [dF dP]t = [p0 v0 a0 j0 p1 p2 p3 v3 a3 j3, v1 a1 j1 v2 a2 j2]
    // dt = [p0 v0 a0 j0 p1 v1 a1 j1 p1 v1 a1 j1 p2 v2 a2 j2 p2 v2 a2 j2 p3 v3 a3 j3]
    int ct_rows = d_order*2*n_seg;
    int ct_cols = d_order*2*n_seg - (n_seg-1)*d_order;
    MatrixXd Ct = MatrixXd::Zero(ct_rows, ct_cols);
    // p0,v0,a0,j0
    for(int i = 0; i < d_order; i++)
        Ct(i,i) = 1;
    // middle point p1,p2,...p(n-1)
    for(int i = 1; i < n_seg; i++){
        // d_order+(i-1)*8-1+1:
        //      d_order,起点约束个数;
        //      i-1,表示这个中间点之前还有几个中间点;
        //      *8,表示每个中间点的p,v,a,j都有两套;
        //      -1,是因为在c++中,矩阵的下标表示是从0开始的,我们在下面推倒的时候用到矩阵下标都是从1开始;
        //      +1,表示当前操作的是p，举个例子，比如对p1操作的时候，前面有4个约束，p0,v0,a0,j0,即d_order数量
        //          的约束个数，+1就当前表示对p1操作，也就是第[dF dP]中的第五个;
        // d_order+(i-1)*1:                     (注:这里写成这样没做化简只是为了方便理解过程和思路)
        //      d_order,起点约束个数;
        //      i-1,表示这个中间点之前还有几个中间点;
        //      *1表示每个中间点的p;
        Ct(d_order+(i-1)*8-1+1,d_order+(i-1)*1) = 1;
        Ct(d_order+(i-1)*8+4-1+1,d_order+(i-1)*1) = 1;
    }
    // pn,vn,an,jn
    for(int i = 0; i < d_order; i++)
        Ct(d_order+2*d_order*(n_seg-1)+i,d_order+n_seg-1+i) = 1;
    // v1,a1,j1,...,v(n-1),a(n-1),j(n-1)
    // k表示第几个中间点
    for(int k = 1; k < n_seg; k++){
        // i表示中间点需要求的dP,d_order=4时表示v,a,j,而p是需要给定的,属于dF;
        for(int i = 1; i < d_order; i++){
            // ((k-1)*2+1)*d_order+1-1+i:       (注:这里写成这样没做化简只是为了方便理解过程和思路)
            //      k-1表示这个中间点之前还有几个中间点,比如k=1,表示第一个中间点,前面就是起点,没有中间点了,即k-1=0;
            //      (k-1)*2,*2表示每个中间点的p,v,a,j都有两套(上一段轨迹的终点一套,下一段轨迹的起点一套);
            //      ((k-1)*2+1)*d_order,*d_order表示每个点都有d_order个约束,+1表示起点的约束;
            //      ((k-1)*2+1)*d_order+1,+1代表约束p,因为我们现在要处理的是约束vaj,p是给定的不需要在这里处理;
            //      ((k-1)*2+1)*d_order+1-1,-1是因为在c++中,矩阵的下标表示是从0开始的,我们在下面推倒的时候用到矩阵下标都是从1开始;
            //      ((k-1)*2+1)*d_order+1-1+i,+i表示当前是对vaj的哪一个约束操作
            // (2*d_order+n_seg-1)-1+(k-1)*3+i):
            //      2*d_order,表示起点和终点;
            //      n_seg-1,表示中间点的个数,中间点有几个,就有几个约束p;
            //      (2*d_order+n_seg-1)-1,-1是因为在c++中,矩阵的下标表示是从0开始的,我们在下面推倒的时候用到矩阵下标都是从1开始;
            //      (k-1)*3,k-1表示这个中间点之前还有几个中间点,*3表示每个点的vaj约束;
            // ((k-1)*2+1)*d_order+1-1+4+i: +4表示现在处理的是中间点两套pvaj约束的后面一套.
            Ct(((k-1)*2+1)*d_order+1-1+i,(2*d_order+n_seg-1)-1+(k-1)*3+i) = 1;
            Ct(((k-1)*2+1)*d_order+1-1+4+i,(2*d_order+n_seg-1)-1+(k-1)*3+i) = 1;
        }
    }
    return Ct;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65

        3.3 getM函数

          对于M矩阵的理论部分可参考上一篇博文《Minimun Snap轨迹优化学习记录》中第三小节<3.Minimun Snap闭式求解(Closed-form Solution for Minimun Snap)>的第一张图片,请结合后面的图片理解,代码如下：

MatrixXd TrajectoryGeneratorWaypoint::getM(int const n_seg, int const d_order, int const p_num1d, const Eigen::VectorXd &ts){
    /*
     * get Mapping Matrix
     * args:
     *      n_seg: the number of segments
     *      d_order: the deferential order: if minimal jerk. it is 3,
     *      p_num1d: the number of variables in each segment, if minimal jerk, it is 6
     *      ts: time allocation as a column vector, size: n_seg x 1,
     * return:
     *      M: a matrix, size: n_seg * p_num1d x n_seg * p_num1d
     */
//    MatrixXd M, M_temp, zeros;
    MatrixXd M = MatrixXd::Zero(n_seg * p_num1d, n_seg * p_num1d);
    MatrixXd coeff(d_order, p_num1d);
    if(d_order == 4){
        coeff << 1,  1,  1,  1,  1,  1,  1,  1,
                 0,  1,  2,  3,  4,  5,  6,  7,
                 0,  0,  2,  6,  12, 20, 30, 42,
                 0,  0,  0,  6,  24, 60, 120,210;
    }
    else if(d_order == 3){
        coeff << 1,  1,  1,  1,  1,  1,
                 0,  1,  2,  3,  4,  5,
                 0,  0,  2,  6,  12, 20;
    }
    else{
        cout << "This derivatieve order is not provided getM!!!" << endl;
    }

    double t;
    for(int k = 0; k < n_seg; k++){
        // calculate M_k of the k-th segment
        MatrixXd M_k = MatrixXd::Zero(p_num1d, p_num1d); // Matrix size: p_num1d x p_num1d
        // time of the k-th segment
        t = ts(k);
        for(int i = 0; i < d_order; i++){
            M_k(i, i) = coeff(i, i);
        }

        for(int i = 0; i < d_order; i++){
            for(int j = i; j < p_num1d; j++){
                if( i == j){
                    M_k(i+d_order, j) = coeff(i, j) ;
                }
                else{
                    M_k(i+d_order, j) = coeff(i, j) * pow(t, j - i);
                }
            }
        }
        // cout << "---------------" << endl;
        // cout << "M_old:" << endl;
        // cout << M << endl;
        M.block(k*p_num1d, k*p_num1d, p_num1d, p_num1d) = M_k;
        // cout << "---------------" << endl;
        // cout << "M_new:" << endl;
        // cout << M << endl;
    }
    return M;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59

         循环之前原始M矩阵：


         第一次循环后添加的部分：

         上面M_new图中上面的红色框部分的解释：

         上面M_new图中下面的红色框部分的解释：

         选取M_new图中的箭头指向的315.182作为例子计算一下：

         最终的到的M矩阵

        3.4 getQ函数

          对于Q矩阵的理论部分可参考上一篇博文《Minimun Snap轨迹优化学习记录》中第三小节<2.Minimun Snap轨迹生成(Minimun Snap Optimization))>的第二张和第三张图片,请结合后面的图片理解,代码如下：

Eigen::MatrixXd TrajectoryGeneratorWaypoint::getQ(int const n_seg, int const d_order, int const p_num1d, const Eigen::VectorXd &ts){
    /*
     * get Q matrix
     * args:
     *      n_seg: the number of segments
     *      d_order: the deferential order: if minimal snap. it is 4
     *      p_num1d: the number of variables in each segment
     *      ts: time allocation as a column vector, size: n_seg x 1,
     * return:
     *      Q: a matrix, size: n_seg * p_num1d x n_seg * p_num1d
     *      Note: p = [p0, p1, p2,...pn-1]'
     */

    MatrixXd Q = MatrixXd::Zero(n_seg * p_num1d, n_seg * p_num1d);
    for(int k = 0; k < n_seg; k++) {
        Eigen::MatrixXd Q_k = Eigen::MatrixXd::Zero(p_num1d, p_num1d); // Matrix size: p_num1d x p_num1d
        for (int i = 0; i < p_num1d; i++)
        {
            for (int j = 0; j < p_num1d; j++)
            {
                // 只有当i和j都同时大于等于4的时候(i,j是从0开始的，正常我们表示矩阵的时候下标是从1开始算的，要注意这点)，才会到else里执行计算
                if (i < d_order || j < d_order)
                    continue;
                else
                    // Factorial,表示阶乘;下面公式参考:https://blog.csdn.net/weixin_44558122/article/details/116173197#t3中<<2.Minimun Snap轨迹
                    // 生成(Minimun Snap Optimization)>>部分第三张图片,对于pow(ts(k), i + j - 2 * d_order + 1),这里为什么没有 （t下标i-1）^(r+c-7) 
                    // 这一项了？是因为我们采用的时间分配方法是相对时间,每一段轨迹的开头都是0.
                    Q_k(i, j) = Factorial(i) / Factorial(i - d_order) * Factorial(j) / Factorial(j - d_order) *
                                1 / (i + j - d_order * 2 + 1) * pow(ts(k), i + j - 2 * d_order + 1);
            }
        }
        // 矩阵块操作,可参考:https://gaoyichao.com/Xiaotu/?book=eigen&title=chapter4
        // cout << "---------------" << endl;
        // cout << "Q_old:" << endl;
        // cout << Q << endl;
        Q.block(k * p_num1d, k * p_num1d, p_num1d, p_num1d) = Q_k;
        // cout << "Q_new:" << endl;
        // cout << Q << endl;
        // cout << "---------------" << endl;
    }
    return Q;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

         对于Q1的求解：

         最终得到的Q矩阵

        3.5 PolyQPGeneration函数

          作用：求解多项式系数;

Eigen::MatrixXd TrajectoryGeneratorWaypoint::PolyQPGeneration(
            const int d_order,                    // the order of derivative
            const Eigen::MatrixXd &Path,          // waypoints coordinates (3d)
            const Eigen::MatrixXd &Vel,           // boundary velocity
            const Eigen::MatrixXd &Acc,           // boundary acceleration
            const Eigen::VectorXd &Time)          // time allocation in each segment
{
    // enforce initial and final velocity and acceleration, for higher order derivatives, just assume them be 0;
    int p_order   = 2 * d_order - 1;              // the order of polynomial
    int p_num1d   = p_order + 1;                  // the number of variables in each segment

    int n_segment = Time.size();                              // the number of segments
    // 3 * p_num1d, 三维坐标x,y,z, 分别对每一个维度去求解
    MatrixXd PolyCoeff = MatrixXd::Zero(n_segment, 3 * p_num1d); // position(x,y,z), so we need (3 * p_num1d) coefficients
    // 这部分后面没有用到
    VectorXd Px(p_num1d * n_segment), Py(p_num1d * n_segment), Pz(p_num1d * n_segment);

    // get Q
    MatrixXd Q = getQ(n_segment, d_order,  p_num1d, Time);
    cout << "Matrix Q is:\n"<< endl << Q << endl;

    /* Produce Mapping Matrix  to the entire trajectory,
     * M is a mapping matrix that maps polynomial coefficients to derivatives.   */
    MatrixXd M = getM(n_segment, d_order,  p_num1d, Time);
    cout << "Mapping matrix M is:\n" << endl << M << endl;


    // compute Ct
    MatrixXd Ct = getCt(n_segment, d_order);
    cout <<"Ct: \n"<< endl << Ct << endl;

    MatrixXd C = Ct.transpose();
    cout << "C is:\n" << endl << C << endl;

    MatrixXd M_inv = M.inverse();
    MatrixXd M_inv_t = M_inv.transpose();

    cout << "M_inv is:" << endl << M_inv << endl;
    cout << "M_inv_transp is:" << endl << M_inv_t << endl;
    cout << "size of C is: " << C.rows() << ", " << C.cols() << endl;
    cout << "size of M_inv_t is: " << M_inv_t.rows()  << ", " <<  M_inv_t.cols() << endl;
    cout << "size of Q  is: " << Q.rows()  << ", " << Q.cols()  << endl;
    cout << "size of M_inv  is: " << M_inv.cols() << ", " << M_inv.rows() << endl;
    cout << "size of Ct  is: " << Ct.rows() << ", " << Ct.cols() << endl;

    MatrixXd R = C * M_inv_t * Q * M_inv * Ct; // M is not changed
    cout << "R is:\n" << endl << R << endl;

    int num_d_F = 2 * d_order + n_segment - 1;
    int num_d_P = (n_segment - 1) * (d_order - 1);

    // 矩阵的角相关操作:https://blog.csdn.net/qq_33160678/article/details/112567335
    MatrixXd R_pp = R.bottomRightCorner(num_d_P, num_d_P); // 6*6
    cout << "R_pp is:\n" << endl << R_pp << endl;

    MatrixXd R_fp = R.topRightCorner(num_d_F, num_d_P);    // 10*6
    cout << "R_fp is:\n" << endl << R_fp << endl;

    // STEP 3: compute dF for x, y, z respectively
    for(int dim = 0; dim < 3; dim++){
        VectorXd wayPoints = Path.col(dim);
        cout << "waypoints: " << endl << wayPoints << endl;
        VectorXd d_F = VectorXd::Zero(num_d_F);

        d_F(0) = wayPoints(0); //p0
        // v0,0 a0,0 ... are 0

        // pT,0, pT,1, ,,PT,n_seg-2
        for(int i = 0; i < n_segment - 1; i++ ){
            d_F(d_order + i) = wayPoints(i + 1);
        }
        // pT,n_seg-1
        d_F(d_order + n_segment - 1) = wayPoints(n_segment);
        cout << "d_F is:" << endl << d_F << endl;

        VectorXd d_P = -1.0 * R_pp.inverse() * R_fp.transpose() * d_F;
        cout << "d_P is:" << endl << d_P << endl;

        VectorXd d_total(d_F.rows() + d_P.rows());
        d_total << d_F, d_P;
        cout << "d_total is:" << endl << d_total << endl;

        // M.inverse() * Ct * d_total也即QP问题中的P矩阵，P矩阵就是x(t)多项式中各项的系数
        VectorXd poly_coef_1d = M.inverse() * Ct * d_total;
        cout<< "Dimension " << dim <<" coefficients: "<< endl << poly_coef_1d << endl;

        MatrixXd poly_coef_1d_t = poly_coef_1d.transpose();
        cout << "poly_coef_1d.transpose() :" << endl << poly_coef_1d_t << endl;
        // PolyCoeff(n_segment, 3 * p_num1d)
        for(int k = 0; k < n_segment; k++){
            PolyCoeff.block(k, dim*p_num1d, 1, p_num1d) = poly_coef_1d_t.block(0,k*p_num1d, 1, p_num1d);
        }
    }
    cout << "PolyCoeff :" << endl << PolyCoeff << endl;
    return PolyCoeff;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96

4.trajectory_generator_node.cpp解析

        4.1 rcvWaypointsCallBack函数

         作用：获得输入的所有点坐标，即演示过程中的第五部分;

void rcvWaypointsCallBack(const nav_msgs::Path & wp)
{   
    vector<Vector3d> wp_list;
    wp_list.clear();

    for (int k = 0; k < (int)wp.poses.size(); k++)
    {
        // 记录所有路标点
        Vector3d pt( wp.poses[k].pose.position.x, wp.poses[k].pose.position.y, wp.poses[k].pose.position.z);
        wp_list.push_back(pt);

        // 如果路标点的Z小于0,退出循环
        if(wp.poses[k].pose.position.z < 0.0)
            break;
    }

    // MatrixXd：表示任意大小的元素类型为double的矩阵变量，其大小只有在运行时被赋值之后才能知道。
    MatrixXd waypoints(wp_list.size() + 1, 3);
    // 将所有路标点的坐标放到waypoints矩阵里
    waypoints.row(0) = _startPos;
    
    for(int k = 0; k < (int)wp_list.size(); k++)
        waypoints.row(k+1) = wp_list[k];

    //Trajectory generation: use minimum snap trajectory generation method
    //waypoints is the result of path planning (Manual in this homework)
    trajGeneration(waypoints);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

        4.2 trajGeneration函数

         作用：轨迹生成;

void trajGeneration(Eigen::MatrixXd path)
{
    TrajectoryGeneratorWaypoint  trajectoryGeneratorWaypoint;
    
    MatrixXd vel = MatrixXd::Zero(2, 3); 
    MatrixXd acc = MatrixXd::Zero(2, 3);

    vel.row(0) = _startVel;

    // give an arbitraty time allocation, all set all durations as 1 in the commented function.
    _polyTime  = timeAllocation(path);

    // generate a minimum-snap piecewise monomial polynomial-based trajectory
    _polyCoeff = trajectoryGeneratorWaypoint.PolyQPGeneration(_dev_order, path, vel, acc, _polyTime);

    // 可视化path，即将所有点用直线连接起来
    visWayPointPath(path);

    //After you finish your homework, you can use the function visWayPointTraj below to visulize your trajectory
    // 可视化优化过的轨迹
    visWayPointTraj( _polyCoeff, _polyTime);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

        4.3 visWayPointTraj函数

         作用：可视化优化过的轨迹;

void visWayPointTraj( MatrixXd polyCoeff, VectorXd time)
{
    // visualization_msgs相关参考:https://blog.csdn.net/u013834525/article/details/80447931
    // 或者直接参考官网:http://wiki.ros.org/rviz/DisplayTypes/Marker#The_Marker_Message
    visualization_msgs::Marker _traj_vis;

    _traj_vis.header.stamp       = ros::Time::now();
    _traj_vis.header.frame_id    = "/map";

    _traj_vis.ns = "traj_node/trajectory_waypoints";
    _traj_vis.id = 0;
    _traj_vis.type = visualization_msgs::Marker::SPHERE_LIST;
    _traj_vis.action = visualization_msgs::Marker::ADD;
    _traj_vis.scale.x = _vis_traj_width;
    _traj_vis.scale.y = _vis_traj_width;
    _traj_vis.scale.z = _vis_traj_width;
    _traj_vis.pose.orientation.x = 0.0;
    _traj_vis.pose.orientation.y = 0.0;
    _traj_vis.pose.orientation.z = 0.0;
    _traj_vis.pose.orientation.w = 1.0;

    _traj_vis.color.a = 1.0;
    _traj_vis.color.r = 1.0;
    _traj_vis.color.g = 0.0;
    _traj_vis.color.b = 0.0;

    double traj_len = 0.0;
    int count = 0;
    Vector3d cur, pre;
    cur.setZero();
    pre.setZero();

    _traj_vis.points.clear();
    Vector3d pos;
    // 这个是在序列、点集中才会用到，指明序列中每个点的位置
    geometry_msgs::Point pt;

    for(int i = 0; i < time.size(); i++ )
    {   
        for (double t = 0.0; t < time(i); t += 0.01, count += 1)
        {
            // 得到当前t对应的三维空间坐标位置
            pos = getPosPoly(polyCoeff, i, t);
            cur(0) = pt.x = pos(0);
            cur(1) = pt.y = pos(1);
            cur(2) = pt.z = pos(2);
            _traj_vis.points.push_back(pt);

            if (count) traj_len += (pre - cur).norm();
            pre = cur;
        }
    }
    _wp_traj_vis_pub.publish(_traj_vis);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

        4.4 getPosPoly函数

         作用：获得当前时间t对应的三维空间点坐标;

Vector3d getPosPoly( MatrixXd polyCoeff, int k, double t )
{
    Vector3d ret;

    // 3 dimension: x,y,z
    for ( int dim = 0; dim < 3; dim++ )
    {
        // 获取当前维度对应的polyCoeff，polyCoeff即公式中的M.inverse()*Ct*d,也即QP问题中的P矩阵，P矩阵就是x(t)多项式中各项的系数
        VectorXd coeff = (polyCoeff.row(k)).segment( dim * _poly_num1D, _poly_num1D );
        VectorXd time  = VectorXd::Zero( _poly_num1D );
//        for(int j = _poly_num1D-1; j >= 0; j--)
        cout << "------------" << endl;
        cout << polyCoeff << endl; 
        cout << coeff << endl;
        cout << "------------" << endl;

        // x(t) = p0 + p1*t^1 + p2*t^2 + p3*t^3 + p4*t^4 + p5*t^5 + p6*t^6 + p7*t^7
        for(int j = 0; j < _poly_num1D; j ++)
          if(j==0)
              // t的0次=1
              time(j) = 1.0;
          else
              time(j) = pow(t, j);

        // 得到当前time下,各个维度对应的x(time)的值,即当前时间下对应的坐标点位置
        ret(dim) = coeff.dot(time);
        cout << "dim:" << dim << " coeff:" << coeff << endl;
    }

    return ret;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

        4.5 timeAllocation函数

         作用：时间分配，理论部分可参考:https://blog.csdn.net/q597967420/article/details/77623235,下面会给出原作者的代码，不过我并没完全理解，所以采用了另一种更好理解的解法;

         原作者代码：

VectorXd timeAllocation( MatrixXd Path)
{ 
    VectorXd time(Path.rows() - 1);
    
    /*

    STEP 1: Learn the "trapezoidal velocity" of "TIme Allocation" in L5, then finish this timeAllocation function

    variable declaration: _Vel, _Acc: _Vel = 1.0, _Acc = 1.0 in this homework, you can change these in the test.launch

    You need to return a variable "time" contains time allocation, which's type is VectorXd

    The time allocation is many relative timeline but not one common timeline

    */
    double _Vel, _Acc;
    _Vel = 1.0, _Acc = 1.0;
    double t_slope = _Vel / _Acc;
    double max_s_triangle =  t_slope * _Vel;
    std::cout <<"Path: "<< Path << std::endl;

    time = VectorXd::Ones(Path.rows() - 1);
    cout << "----------" << time << endl;
    for(int k = 0;k < Path.rows()-1;k++){
        Vector3d p1_2 = Path.row(k) - Path.row(k+1);
        double s = std::sqrt(p1_2.dot(p1_2));

        if(s <= max_s_triangle){
            time(k) = 2.0 * s / _Acc;
        }
        else{
            time(k) = (s - max_s_triangle) / _Vel + t_slope;
        }
    }
    std::cout <<"time: "<< time << std::endl;
    return time;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

         另一种解法：

VectorXd timeAllocation( MatrixXd Path)
{
    VectorXd time(Path.rows() - 1);

    double _Vel, _Acc;
    _Vel = 1.0, _Acc = 1.0;
    double accInv = 1.0 / _Acc;
    double velInv = 1.0 / _Vel;

    // 加速时间与距离，减速需要耗费相同的资源
    double accTime = _Vel * accInv;            // v = v0 + at, v0 = 0
    // accDist = 0.5*_Vel*accTime = 1/2*_Vel*_Vel*accInv = 1/2*_Vel*_Vel*1/_Acc = 2/1 * _Vel^2 / _Acc
    double accDist = 0.5 * _Vel * accTime;     // v^2 - (v0)^2 = 2ax, v0 = 0
    double accTime2 = 2.0 * accTime;
    double accDist2 = 2.0 * accDist;           // 包含加速和减速的总距离

    for(int i = 0; i < Path.rows()-1; i++)
    {
        double t = 0.0;
        // n = norm(v) 返回向量 v 的欧几里德范数。此范数也称为 2-范数、向量模或欧几里德长度。
        double dist = (Path.row(i+1) - Path.row(i)).norm();

        if(dist <= accDist2)
            t = 2.0 * std::sqrt(dist*accInv);
        else
            t = accTime2 + (dist - accDist2)*velInv;
        
        time(i) = t;
    }
    cout << "--------------------" << time << endl;
    return time;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

5.参考引用

         代码来源：https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots/tree/master/hw_5（从该地址下载的代码是原作者的代码，其中并未包含我后来自己写的解法部分，需要用这部分代码请从本文中直接复制替换）.

         相关知识来源：深蓝学院<<移动机器人运动规划>>