基于minimum snap MATLAB求解二次规划QP问题

Minimum Snap 轨迹生成

算法思想:
1.给定起点 终点 经过点；
2.将所有点通过多项式连接，如图1；
3.起点终点（pvaj）限制；经过点（p）限制；
4.每一个segment多项式都不同，也就是每个segment的系数都是不同的，并不是用一条多项式将所有点相连，如图2所示，但是多项式的n_order相等；n_order=2×d_order-1;d_order表示优化目标的阶数；在此对snap进行优化，所以d_order=4；
5.这里采用分段计时，每个segment都有一个时间段T，如图3；

算法过程：
算法的过程难点就是再求解QP问题时，对QP二次型及限制条件的建立，也就是对QP中一些矩阵的建立
1.如图中4所示，Q矩阵为对称矩阵，Q中包含多个矩阵，每个小矩阵是每段多项式的的系数，有多少segmentQ就包含多少个Q_j;其中p为要求解优化的变量,p代表每个segment的系数，例如第一段的系数为p10,p11,…,p16;T为每个segment的时间；
2.手推一遍公式，Q的建立就很清楚了；
3.多项式的导数限制即为起点终点的pvaj,经过点的p；如图5
4.在建立矩阵时，因为Aj要与P相乘，前面说过p是所有多项式系数的一个组合，即p=（p10,p11…p16,p22…p26,…）,所以在建立Aj矩阵时，注意Aj中元素的位置，使得A与P相乘时对应自己的系数p。得到Aeq_start,beq_start，Aeq_end,beq_end,Aeq_wp,beq_wp;
5.连续性约束即为前一段segment的终点与后一段segment的起点p是相等的；
6.这里就是把前一段seg的t=T带入多项式等于后面seg将t=0带入；这里就是0阶微分；
7.建立矩阵时前面的为+，后面的为-，与p相乘时=0；得到Aeq_con,beq_con
8.Aeq = [Aeq_start; Aeq_end; Aeq_wp; Aeq_con];
beq = [beq_start; beq_end; beq_wp; beq_con];
9. 通过求解器得到最优的系数poly_coef：poly_coef = quadprog(Q,f,[],[],Aeq, beq);因为p包含所有segment的系数p，所以还要将p分开，得到一组组的p系数p0 p1…p6；通过flipud将系数反转一下，得到从最高到最低的系数，再通过画图将曲线画出来
10. x和y都是关于t的多项式，所以将想x y分开求得到x关于t y关于t的多项式

MATLAB实现代码

主函数 hw1_1.m

clc;clear;close all;
path = ginput() * 100.0;

n_order       = 7;% order of poly
n_seg         = size(path,1)-1;% segment number
n_poly_perseg = (n_order+1); % coef number of perseg

ts = zeros(n_seg, 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;
%     t_sum = t_sum+ts(i);
% end
% ts(n_seg) = T - t_sum;

% or you can simply set all time distribution as 1
for i = 1:n_seg
    ts(i) = 1.0;
end
testp = path(:,1);%path 的第一列
poly_coef_x = MinimumSnapQPSolver(path(:, 1), ts, n_seg, n_order);
poly_coef_y = MinimumSnapQPSolver(path(:, 2), ts, n_seg, n_order);


% 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)
        X_n(k)  = polyval(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));

function poly_coef = MinimumSnapQPSolver(waypoints, ts, n_seg, n_order)
    start_cond = [waypoints(1), 0, 0, 0];
    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);
end

建立Q对称矩阵

function Q = getQ(n_seg, n_order, ts)
    Q = [];
    for j = 0:n_seg-1
        Q_j = zeros(8,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_j(i+1,l+1) = L*I/(i+l-7);
            end
        end
        Q = blkdiag(Q, Q_j);
    end
end

建立Aeq矩阵和beq

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);
    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
            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);
    for i=0:n_seg-2
        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
    
    Aeq_con = zeros((n_seg-1)*4, n_all_poly);
    beq_con = zeros((n_seg-1)*4, 1);
    for k=0:3
        for j=0:n_seg-2
            for i = k:7
                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);
                if(i == k)
                    Aeq_con(start_idx_1+j+1,start_idx_2+(n_order+1)+i+1) = ...
                                                            -factorial(i);
                end
            end
        end
    end
    
    
    %#####################################################
    % combine all components to form Aeq and beq   
    Aeq = [Aeq_start; Aeq_end; Aeq_wp; Aeq_con];
    beq = [beq_start; beq_end; beq_wp; beq_con];
end

仿真结果