Minimum snap matlab 代码（一）- 用优化solver求解参数

1.用优化solver求解参数算法概况

具体看Minimum Snap轨迹规划详解（一）

• 最高阶7阶,共8个参数
  $$\begin{array}{rl}p(t)& =p_0+p_{1}t+p_2{t}^2...+p_n{t}^n=\sum _{i=0}^{n=7}{p}_i{t}^i\\ v(t)& =p^{\prime }(t)=\sum _{i⩾1}^{n=7}i\cdot p_i{t}^{i-1}\\ a(t)& =p^{\prime \prime }(t)=\sum _{i⩾2}^{n=7}\frac{i!}{(i-2)!}\cdot p_i{t}^{i-2}\\ jerk(t)& =p^{(3)}(t)=\sum _{i⩾3}^{n=7}\frac{i!}{(i-3)!}\cdot p_i{t}^{i-3}\\ snap(t)& =p^{(4)}(t)=\sum _{i⩾4}^{n=7}\frac{i!}{(i-4)!}\cdot p_i{t}^{i-4}\end{array}$$
• 目标函数为$J=\min {\left[\begin{array}{c}{p}_1\\ p_2\\ ⋮\\ p_k\end{array}\right]}^T\left[\begin{array}{cccc}{Q}_1& & & \\ & Q_2& & \\ & & \ddots & \\ & & & Q_k\end{array}\right]\left[\begin{array}{c}{p}_1\\ p_2\\ ⋮\\ p_k\end{array}\right]$
• 等式约束$A_{eq}\left[\begin{array}{c}{p}_1\\ p_2\\ ⋮\\ p_k\end{array}\right]=d_{eq}$

2.用优化solver求解参数代码

代码中时间按照相对坐标轴取值，以免数值计算不稳定。

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

% n次项
n_order       = 7;
% 段数
n_seg         = size(path,1)-1;
% 每段参数数目
n_poly_perseg = (n_order+1);

% 每段轨迹的时间，按距离均匀分配时间，或直接赋值
ts = zeros(n_seg, 1);
for i = 1:n_seg
   ts(i) = 1.0;
end

% x,y轴分别计算参数
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
    % 取计算好的每段参数，此时参数根据阶数由低到高
    Pxi = poly_coef_x(i*n_poly_perseg +1:(i+1)*n_poly_perseg );
    % 阶数由高到低排列
    Pxi = flipud(Pxi);
    Pyi = poly_coef_y(i*n_poly_perseg +1:(i+1)*n_poly_perseg );
    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)
    % waypoints 点集
    % ts，每段的时间，相对时间轴坐标系
    % n_seg，段数
    % n_order ,最高阶数
    % 起点位置、速度、加速度、jerk 数值
    start_cond = [waypoints(1), 0, 0, 0];
    % 终点位置、速度、加速度、jerk 数值
    start_cond = [waypoints(1), 0, 0, 0];
    end_cond   = [waypoints(end), 0, 0, 0];
    % 计算Q矩阵
    Q = getQ(n_seg, n_order, ts);
    % 计算等式约束
    [Aeq, beq] = getAbeq(n_seg, n_order, waypoints, ts, start_cond, end_cond);
    % 用solver优化，得到优化后的所有参数poly_coef 
    f = zeros(size(Q,1),1);
    poly_coef = quadprog(Q,f,[],[],Aeq, beq);
end

2.1 计算Q矩阵

代价函数为
$$\begin{array}{rl}J& =\min {\int }_0^T(p^{(4)}(t){)}^{2}dt\\ & =\min \sum _{i=1}^k{\int }_{t_{i-1}}^{t_i}(p^{(4)}(t){)}^{2}dt\\ & =\min \sum _{i=1}^k{p}_i^T{Q}_i{p}_i\\ & =\min p^{T}Qp\end{array}$$

• 其中$Q_i$为${\left[\begin{array}{cc}{0}_{4\times 4}& 0_{4\times (n-3)}\\ 0_{(n-3)\times 4}& \frac{r!}{(r-4)!}\frac{c!}{(c-4)!}\frac{1}{(r-4)+(c-4)+1}(t_i^{(r+c-7)}-t_{i-1}^{(r+c-7)})\end{array}\right]}_{(n+1)\times (n+1)}$
• $Q$为${\left[\begin{array}{cccc}{Q}_1& & & \\ & Q_2& & \\ & & \ddots & \\ & & & Q_k\end{array}\right]}_{k(n+1)\times k(n+1)}$
• $p_i$为$[p_{i,0},p_{i,1},\cdots ,p_{i,7}{]}^T$共7个参数，列向量
• $p$为 ${\left[\begin{array}{c}{p}_1\\ p_2\\ ⋮\\ p_k\end{array}\right]}_{k(n+1)\times 1}$

function Q = getQ(n_seg, n_order, ts)
    % ts，每段的时间，相对时间轴坐标系
    % n_seg，段数
    % n_order ,最高次项
    Q = [];
    % 每段分别计算Q
    for k = 1:n_seg
    	% 初始化，对于7次多项式，维度为8*8
        Q_k = zeros(n_order+1, n_order+1);
        %行
        for r = 4:n_order
            %列
            for c= 4:n_order
            	% t_i为第i段的时间，t_{i-1}为第i段的起始时间，取相对时间轴，值为0
                Q_k(r+1,c+1) = (factorial(r)/factorial(r-4))*(factorial(c)/factorial(c-4))*(1/(r+c-7))*(ts(k)^(r+c-7));
            end
        end
        Q = blkdiag(Q, Q_k);
    end
end

2.2 计算等式约束

• 起点位置、速度、加速度、jerk限制，时间为0
  $$\begin{array}{rl}{p}_1(0)& =\sum _{i=0}^{n=7}{p}_{1,i}\ast 0^i\\ v_1(0)& =p_1^{\prime }(0)=\sum _{i⩾1}^{n=7}i\cdot p_{1,i}\ast 0^{i-1}\\ a_1(0)& =p_1^{\prime \prime }(0)=\sum _{i⩾2}^{n=7}\frac{i!}{(i-2)!}\cdot p_{1,i}\ast 0^{i-2}\\ jer{k}_1(0)& =p_1^{(3)}(0)=\sum _{i⩾3}^{n=7}\frac{i!}{(i-3)!}\cdot p_{1,i}\ast 0^{i-3}\end{array}$$
• 终点位置、速度、加速度、jerk限制，时间为T
  $$\begin{array}{rl}{p}_k(T)& =\sum _{i=0}^{n=7}{p}_{k,i}{T}^i\\ v_k(T)& =p_k^{\prime }(T)=\sum _{i⩾1}^{n=7}i\cdot p_{k,i}{T}^{i-1}\\ a_k(T)& =p_k^{\prime \prime }(T)=\sum _{i⩾2}^{n=7}\frac{i!}{(i-2)!}\cdot p_{k,i}{T}^{i-2}\\ jer{k}_k(T)& =p_k^{(3)}(T)=\sum _{i⩾3}^{n=7}\frac{i!}{(i-3)!}\cdot p_{k,i}{T}^{i-3}\end{array}$$
• 前后段连接点的位置限制
• 前后段连接点高阶项连续,第$j$段，$m$阶，在$T_j$处的极限值，等于第$j+1$段，$m$阶，在$j+1$段初始时时间0处的极限值
  $$p_j^{(m)}(T_j)-p_{j+1}^{(m)}(0)=0$$

function [Aeq beq]= getAbeq(n_seg, n_order, waypoints, ts, start_cond, end_cond)
    % n_seg 段数
    % n_order， 最高阶数
    % waypoints，点集
    % ts每段时间
    % start_cond = [waypoints(1), 0, 0, 0];起始时位置，速度，加速度，jerk
    % end_cond   = [waypoints(end), 0, 0, 0];终止时位置，速度，加速度，jerk
    
	% 所有参数的个数
	n_all_poly = n_seg*(n_order+1);
    %#####################################################
    % p,v,a,j constraint in start, 起点位置，速度，加速度，jerk的限制，时间为0，矩阵只剩4项
    Aeq_start = zeros(4, n_all_poly);
    beq_start = start_cond ;
    for r=1:4
        Aeq_start(r,r) = factorial(r-1);
    end
    %#####################################################
    % p,v,a constraint in end，终点位置、速度、加速度、jerk的限制
    Aeq_end = zeros(4, n_all_poly);
    beq_end =end_cond;
    for r = 0:3
        for c=r:n_order
            Aeq_end(r+1,(n_seg-1)*(n_order+1)+c+1) =  (factorial(c)/ factorial(c-r))*(ts(n_seg)^(c-r));
        end
    end;
    %#####################################################
    % position constrain in all middle waypoints，中间路标点对位置的限制
    Aeq_wp = zeros(n_seg-1, n_all_poly);
    beq_wp = zeros(n_seg-1, 1);
    for r=0:n_seg-2
        for c=0:n_order
            Aeq_wp(r+1,r*(n_order+1)+c+1)= (ts(r+1)^c);
        end
        beq_wp(r+1) = waypoints(r+2);
    end
            
    %#####################################################
    % position continuity constrain between each 2
    % segments，前后两段对位置连续性的限制
    Aeq_con_p = zeros(n_seg-1, n_all_poly);
    beq_con_p = zeros(n_seg-1, 1);
    for r=0:n_seg-2
        for c=0:n_order
        	%第r段的位置
            Aeq_con_p(r+1,r*(n_order+1)+c+1)= ts(r+1)^c;
            %第r+1段的位置
            Aeq_con_p(r+1,(r+1)*(n_order+1)+c+1) = -0^c;
        end
    end
    %#####################################################
    % velocity continuity constrain between each 2 segments
    % segments，前后两段对速度连续性的限制
    Aeq_con_v = zeros(n_seg-1, n_all_poly);
    beq_con_v = zeros(n_seg-1, 1);
    for r=0:n_seg-2
        for c=1:n_order
            Aeq_con_v(r+1,r*(n_order+1)+c+1)= c*ts(r+1)^(c-1);
            Aeq_con_v(r+1,(r+1)*(n_order+1)+c+1) = -c*0^(c-1);
        end
    end
    %#####################################################
    % acceleration continuity constrain between each 2 segments
    % segments，前后两段对加速度连续性的限制
    Aeq_con_a = zeros(n_seg-1, n_all_poly);
    beq_con_a = zeros(n_seg-1, 1);
    for r=0:n_seg-2
        for c=2:n_order
            Aeq_con_a(r+1,r*(n_order+1)+c+1)= c*(c-1)*ts(r+1)^(c-2);
            Aeq_con_a(r+1,(r+1)*(n_order+1)+c+1) = -c*(c-1)*0^(c-2);
        end
    end
    %#####################################################
    % jerk continuity constrain between each 2 segments
    % segments，前后两段对jerk连续性的限制
    Aeq_con_j = zeros(n_seg-1, n_all_poly);
    beq_con_j = zeros(n_seg-1, 1);
    for r=0:n_seg-2
        for c=3:n_order
            Aeq_con_j(r+1,r*(n_order+1)+c+1)= c*(c-1)*(c-2)*ts(r+1)^(c-3);
            Aeq_con_j(r+1,(r+1)*(n_order+1)+c+1) = -c*(c-1)*(c-2)*0^(c-3);
        end
    end
    %#####################################################
    % combine all components to form Aeq and beq   
    Aeq_con = [Aeq_con_p; Aeq_con_v; Aeq_con_a; Aeq_con_j];
    beq_con = [beq_con_p; beq_con_v; beq_con_a; beq_con_j];
    Aeq = [Aeq_start; Aeq_end; Aeq_wp; Aeq_con];
    beq = [beq_start; beq_end; beq_wp; beq_con];
end