移动机器人路径规划minimum_snap（MATLAB)笔记整理

minimum snap轨迹规划

本文代码以及其他概念可参考
https://blog.csdn.net/q597967420/article/details/76099491
本文仅对该博文程序部分做进一步解释

定义路径点、阶次

轨迹一般用n阶多项式(polynomial)来表示，即
p(t)=$p0+p1\ast t+p2\ast t^2\dots \dots +pn\ast t^n={\sum }_{i=0}^{n}pi\ast t^i$
但整段轨迹通常难以用一个多项式表示，所以一般将其分为k段多项式表示。

机器人的平面运动路径也应该由x和y两个多项式构成，所以共有2k段多项式。
已知条件：起始点和终点的位置p、速度v、加速度a、加加速度j，每段多项式在连接点处的位置p已知，且在连接点处光滑（pvaj相等）。每一个分段都是多项式；每个分段的多项式都是相同的阶次，这样对于问题的求解比较简单；每一段的时间间隔都是已知的

限制条件数量: 4+4+（k-1)=k+7
首末pvaj加中间点位置
未知数数量：（N+1）*k (N为多项式阶数，k为路径的段数）
k+7<=（N+1）*k
则N>=$7/k$ 表明段数越多，则提供的阶次越低。k最少是1， 所以minisnap求解中每段多项式至少有7阶，每段有八个未知数。

clc;clear;close all;
path = ginput() * 100.0;    %  返回几个点的（x,y)坐标
n_order       = 7;% 多项式的阶次 mini_snap为7 ，mini_jerk为5
n_seg         = size(path,1)-1;  % n_seg 代表路径的段数
n_poly_perseg = (n_order+1); % 多项式方程的未知量个数

时间分配

时间分配一般有两种，一种是按路径长度分配时间，一种是按梯形运动方式分配时间，此处为图简便可直接将每段时间赋值1。

% calculate time distribution in proportion to distance between 2 points
dist     = zeros(n_seg, 1);
dist_sum = 0;
T        = 25;
t_sum    = 0;

for i = 1:n_seg
    dist(i) = sqrt((path(i+1, 1)-path(i, 1))^2 + (path(i+1, 2) - path(i, 2))^2);
    dist_sum = dist_sum+dist(i);
end
for i = 1:n_seg-1
    ts(i) = dist(i)/dist_sum*T;  %前面n-1段路程都是按距离/总距离来计算时间
    t_sum = t_sum+ts(i);
end
ts(n_seg) = T - t_sum;  %而最后一段是总的时间减去前面所有时间之和

% or you can simply set all time distribution as 1
% ts=ones(5,1);

求解多项式系数

%求解x轴和y轴的多项式系数
poly_coef_x = MinimumSnapQPSolver(path(:, 1), ts, n_seg, n_order);
poly_coef_y = MinimumSnapQPSolver(path(:, 2), ts, n_seg, n_order);

function poly_coef = MinimumSnapQPSolver(waypoints, ts, n_seg, n_order)
    start_cond = [waypoints(1), 0, 0, 0];  %waypoints为 path(:, 1)即第一个点，而先对x轴求多项式系数，则waypoints(1)只提取第一个点的x轴坐标
    end_cond   = [waypoints(end), 0, 0, 0];
    
    % STEP 1: compute Q of p'Qp
    Q = getQ(n_seg, n_order, ts);
    
    % STEP 2: compute Aeq and beq 
    [Aeq, beq] = getAbeq(n_seg, n_order, waypoints, ts, start_cond, end_cond);
    f = zeros(size(Q,1),1);
    poly_coef = quadprog(Q,f,[],[],Aeq, beq); %poly_coef 为n段分轨迹多项式式子的系数的组合，长度为n_seg*8，每段8个系数
end

下面两图源自高飞老师PPT及上文参考博文中摘录

function Q = getQ(n_seg, n_order, ts)
    Q = [];
    for j = 1:n_seg  %一共有n_seg段
        Q_k = zeros(8,8); %minsnap阶数是7，系数为8

        % STEP 1.1: calculate Q_k of the k-th segment 
         for i=4:n_order
            for l=4:n_order
                L = factorial(l)/factorial(l-4);
                I = factorial(i)/factorial(i-4);             
                Q_k(i+1,l+1) = L*I/(i+l-7);
            end
         end
        Q = blkdiag(Q, Q_k);
    end
end

下为$min{\sum }_{i=1}^k{p}^{T}Qip$的等式约束，即1.起始点和终点的位置p、速度v、加速度a、加加速度j 已知。
2.每段多项式在连接点处的位置p已知。
3.在连接点处前后两段多项式pvaj相等


这里两个式子描述的是段与段之间的连接点，这个连接点在前后两段多项式中应该p v a j数值一样。两个式子时间Tj相同，但是却一个乘以系数pj,i，一个乘以系数pj+1,i ,在前一个多项式中，此连接点处于末尾点，时间为ts,而后一个多项式中，连接点处于起始点，时间为0。注意！不同的轨迹段享有不一样的多项式方程系数，所以时间计算可以不共享，每一段的起始点都可以视为这一段的时间0,这样计算每一段的p,v,a,j时，就会发现系数为阶乘。

function [Aeq beq]= getAbeq(n_seg, n_order, waypoints, ts, start_cond, end_cond)
    n_all_poly = n_seg*(n_order+1);%未知量总数
    % p,v,a,j constraint in start, 
    Aeq_start = zeros(4, n_all_poly);
    beq_start = zeros(4, 1);
    % STEP 2.1: write expression of Aeq_start and beq_start
    for k= 0:3
        Aeq_start(k+1,k+1) = factorial(k);
        %求4个阶次的情况，起始点时间t为0，那么位置就是p0，速度就是p1,加速度就是2p2，多项式系数是我二次规划要求的未知数，
                                          %所以，此处起始点每个阶次，要求的p前的常数项系数为n的阶乘
    end
    beq_start = start_cond'; 
    % p,v,a constraint in end
    Aeq_end = zeros(4, n_all_poly);
    beq_end = zeros(4, 1);
    % STEP 2.2: write expression of Aeq_end and beq_end
    start_idx_2 = (n_order + 1)*(n_seg - 1);
    for k=0 : 3
        for i=k : 7   %总共0-3 4个阶次，k到7意思是八个系数求导之后，前面几个系数都消失了，
             Aeq_end(k+1,start_idx_2 + 1 + i ) = factorial(i)/factorial(i-k)*ts(n_seg)^(i-k);
        end
    end
    beq_end = end_cond';
    % position constrain in all middle waypoints
    Aeq_wp = zeros(n_seg-1, n_all_poly);
    beq_wp = zeros(n_seg-1, 1);
    % STEP 2.3: write expression of Aeq_wp and beq_wp
    for i=0:n_seg-2  %一共n_seg-1个中间点
        start_idx_2 = (n_order + 1)*(i+1);  %前面几段有多少个系数
        Aeq_wp(i+1,start_idx_2+1) = 1;%
        beq_wp(i+1,1) = waypoints(i+2);%右端为 本段分轨迹的末点的坐标（已知量）
    end
    % position continuity constrain between each 2
    % segments，连接点处前后两段在此点的位置、速度、加速度、加加速度相等
    Aeq_con = zeros((n_seg-1)*4, n_all_poly);
    beq_con = zeros((n_seg-1)*4, 1);
    % STEP 2.4: write expression of Aeq_con_p and beq_con_p
     for k=0:3
        for j=0:n_seg-2   %n_seg-1个中间点
            for i = k:7   %循环顺序不重要，只是遍历 n-1个中间点，的4个阶次pvaj，以及使i>=7的情况下遍历8次
                start_idx_1 = (n_seg-1)*k;
                start_idx_2 = (n_order+1)*j;
                Aeq_con(start_idx_1 + j + 1,start_idx_2 + i+1)=...
                    factorial(i)/factorial(i-k)*ts(j+1)^(i-k);   %代表同一个连接点在前一段的多项式方程的末尾点pvaj（求k阶导），此时时间在前一段中为ts
                if(i == k)   %因为后一段的起始点时间为0，所以求导后，除了i==k时，其他部分都为0，所以不用算
                    Aeq_con(start_idx_1+j+1,start_idx_2+(n_order+1)+i+1) = ...
                                                            -factorial(i);%代表同一个连接点在后一段的多项式方程中的计算,此时时间在后一段中为0
                                                            
                end  %两者相减等于0                                        % %因为在后一个连接点的多项式中，t为0，所以pi前的系数只有阶乘
            end
        end
    end
    
    Aeq = [Aeq_start; Aeq_end; Aeq_wp; Aeq_con];
    beq = [beq_start; beq_end; beq_wp; beq_con];
end

绘制minimum snap的路径

% display the trajectory
X_n = [];
Y_n = [];
k = 1;
tstep = 0.01;
for i=0:n_seg-1
    
    % STEP 3: get the coefficients of i-th segment of both x-axis
    % and y-axis
    start_idx = n_poly_perseg * i;
    Pxi = poly_coef_x(start_idx + 1 : start_idx + n_poly_perseg,1);
    Pxi = flipud(Pxi);%得到多项式系数
    Pyi = poly_coef_y(start_idx + 1 : start_idx + n_poly_perseg,1);
    Pyi = flipud(Pyi);   %多项式从阶数高的开头
    for t = 0:tstep:ts(i+1)  %以0.01的步长遍历所有时间   （ts(i+1)就是时间结尾）
        X_n(k)  = polyval(Pxi, t); %计算多项式Pxi在t点处的每一个值
        Y_n(k)  = polyval(Pyi, t);
        k = k + 1;
    end
end
 
plot(X_n, Y_n , 'Color', [0 1.0 0], 'LineWidth', 2);  %画出所有轨迹
hold on
scatter(path(1:size(path, 1), 1), path(1:size(path, 1), 2));