MinimumSnap 代码细节

(MATLAB)

基于高飞老师的移动机器人运动规划课程hw5作业写的代码。因为过程中在对Q,M,Ct函数的构造上花了很多功夫去理解，因此记下来。

注意：多项式的排列方式很重要，我们这里采用的是升幂排序，也就是

1. Q矩阵

下面的矩阵是不完全的，因为它只是Q矩阵中的一小部分，记为，实际上是

 

 然后还要积分，Q矩阵中对应的有值项还要乘上

最后写为：

 代码写成这样，

function Q = getQ(n_seg, n_order,t_order,ts)
    Q = [];
    for k = 1:n_seg
        % 7阶，所以有8个自由度
        Q_k = zeros(n_order+1,n_order+1);
        %#####################################################
        % STEP 1.1: calculate Q_k of the k-th segment 
        for i=0:n_order
            for j=0:n_order
                if((i<t_order)||(j<t_order))
                    continue;
                else
                    Q_k(i+1,j+1) = factorial(i)/ factorial(i-t_order) * factorial(j) / factorial(j-t_order) /(i+j-n_order) * ts(k) ^ (i+j-n_order);
                end
            end
        end
        Q = blkdiag(Q, Q_k);
    end
end

 最终结果（单段轨迹）是：

2. M矩阵

M矩阵是将原来的轨迹上的p参数，转换为p,v,a,j的状态变量，找到对应关系，即可。

实际上就是根据下图的关系式转换，M的每一行就是去掉t以后的等式参数化

代码

 结果是

3. Ct矩阵

Ct矩阵比较麻烦，要摸透下面矩阵的含义。

（1）列：

        橘色部分是df，前4个是起始的四个状态变量，后四个是终止的四个状态变量，中间是每两段交点的位置变量。

        白色部分是dp，每三个为一组，分别是每个航路点的除位置量以外的三个变量。

（2）行：

        就是原始的每个点的四个状态量的排列，注意一段轨迹有2*t_order个（起始和末尾）。

        剩下的每8行为一组，都是些重复的，也就是图3.2 t1交界处的8个量。

2.数值解法

结果展示