Minimum snap matlab 代码（二）- 闭式解求解参数

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

具体看Minimum Snap轨迹规划详解(二)- 闭式求解
$$\begin{array}{rl}J& =p^{T}Qp={\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]}^T{C}^T{M}^{-T}Q{M}^{-1}C\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]\\ & ={\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]}^{T}R\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]={\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]}^T\left[\begin{array}{cc}{R}_{FF}& R_{FP}\\ R_{PF}& R_{PP}\end{array}\right]\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]\\ & =d_F^T{R}_{FF}{d}_F+d_F^T{R}_{FP}{d}_P+d_P^T{R}_{PF}{d}_F+d_P^T{R}_{PP}{d}_P\end{array}$$
求解
$$\begin{array}{rl}{d}_P& =-R_{PP}^{-1}{R}_{FP}^T{d}_F\\ p& =M^{-1}C\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]\end{array}$$

2.闭式解求解参数代码

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

n_order = 7;
n_seg = size(path, 1) - 1;
n_poly_perseg = n_order + 1;

% 按照两点间距离均匀分配时间
ts = zeros(n_seg, 1);
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;

poly_coef_x = MinimumSnapCloseformSolver(path(:, 1), ts, n_seg, n_order);
poly_coef_y = MinimumSnapCloseformSolver(path(:, 2), ts, n_seg, n_order);

X_n = [];
Y_n = [];

k = 1;
tstep = 0.01;
for i=0:n_seg-1
    % 取第i段参数，参数为阶数从低到高排列
    Pxi = poly_coef_x(i*(n_order+1)+1:(i+1)*(n_order+1));
    Pxi = flipud(Pxi);
    Pyi = poly_coef_y(i*(n_order+1)+1:(i+1)*(n_order+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',4);
hold on
scatter(path(1:size(path,1),1),path(1:size(path,1),2));

function poly_coef = MinimumSnapCloseformSolver(waypoints, ts, n_seg, n_order)
    start_cond = [waypoints(1), 0, 0, 0];
    end_cond =   [waypoints(end), 0, 0, 0];
    Q = getQ(n_seg, n_order, ts);
    % 计算M矩阵
    M = getM(n_seg, n_order, ts);
    % 计算C矩阵
    Ct = getCt(n_seg, n_order);
    C = Ct';
    % 计算R
    R = C * inv(M)' * Q * inv(M) * Ct;
    R_cell = mat2cell(R, [n_seg+7 3*(n_seg-1)], [n_seg+7 3*(n_seg-1)]);
    R_pp = R_cell{2, 2};
    R_fp = R_cell{1, 2};
    % 已知参数，有起点终点的位置、速度、加速度、jerk；及中间点的位置
    dF = zeros(8+n_seg-1,1);
     %起点
    dF(1:4)=start_cond;
    %终点
    dF(4+n_seg-1+1:8+n_seg-1)=end_cond;
    %中间点 位置
    if n_seg~=1
        dF(4+1:4+n_seg-1)=waypoints(2:2+n_seg-2);
    end
	% 根据极值点求解未知数据，如中间点的速度、加速度、jerk
    dP = -inv(R_pp) * R_fp' * dF;
    % 反向求解所有轨迹段的参数
    poly_coef = inv(M) * Ct * [dF;dP];
end

2.1 得到M矩阵

$M$矩阵为$Mp=d$计算，$\left[\begin{array}{cccc}{M}_1& & & \\ & M_2& & \\ & & \ddots & \\ & & & M_k\end{array}\right]\left[\begin{array}{c}{p}_1\\ p_2\\ ⋮\\ p_k\end{array}\right]=\left[\begin{array}{c}{d}_1\\ d_2\\ ⋮\\ d_k\end{array}\right]$
对于每一段轨迹
$$M_j=\left[\begin{array}{cccccc}1& 0& 0^2& 0^3& \cdots & 0^7\\ 0& 1& \frac{2!}{1!}\ast 0^1& \frac{3!}{2!}\ast 0^2& \cdots & \frac{7!}{6!}\ast 0^6\\ 0& 0& 2\ast 0^0& \frac{3!}{1!}\ast 0^1& \cdots & \frac{7!}{5!}{0}^5\\ 0& 0& 0& \frac{3!}{0!}\ast 0^0& \cdots & \frac{7!}{4!}{0}^4\\ 1& T& T^2& T^3& \cdots & T^7\\ 0& 1& \frac{2!}{1!}T& \frac{3!}{2!}{T}^2& \cdots & \frac{7!}{6!}{T}^6\\ 0& 0& 2& \frac{3!}{1!}T& \cdots & \frac{7!}{5!}{T}^5\\ 0& 0& 0& \frac{3!}{0!}& \cdots & \frac{7!}{4!}{T}^4\end{array}\right]\left[\begin{array}{c}{p}_1\\ p_2\\ ⋮\\ p_k\end{array}\right]=\left[\begin{array}{c}{p}_j(0)\\ v_j(0)\\ a_j(0)\\ j_j(0)\\ p_j(T)\\ v_j(T)\\ a_j(T)\\ j_j(T)\end{array}\right]$$

function M = getM(n_seg, n_order, ts)
    M = [];
    for k = 1:n_seg
        M_k = zeros(8, n_order+1);
        %#####################################################
        % STEP 1.1: calculate M_k of the k-th segment 
        for r=0:3
            for c=r:n_order
                M_k(r+1,c+1)=(factorial(c)/factorial(c-r))*0^(c-r);
                M_k(r+1+4,c+1)=(factorial(c)/factorial(c-r))*ts(k)^(c-r);
            end
        end
        M = blkdiag(M, M_k);
    end
end

2.2计算C矩阵

C矩阵为$d=C\left[\begin{array}{c}{d}_F\\ d_P\end{array}\right]$;

• d为每段轨迹中起点终点的位置、速度、加速度、jerk，维度为$8\ast 段数\times 1$；
• 而$d_F$为已知数据，包括整体轨迹的起点终点的位置、速度、加速度、jerk，及中间点的位置，维度为$4+4+(段数-1)\times 1$
• $d_P$为未知数据，中间点的速度、加速度、jerk，维度为$(段数-1)\ast 3\times 1$
• C矩阵的维度为$(8\ast 段数)\times (8+(段数-1)\ast 4)$；且矩阵中数据为0/1，目的为选择$d_F,d_P$中数据。

function Ct = getCt(n_seg, n_order)
    % C矩阵维度为 (8*段数 )X (4+k-1+4+(k-1)*3)
    Ct = zeros((n_order+1)*n_seg,8+(n_seg-1)*4);
    % start,p,v,a,j，已知的起点数据
    for i=0:3
        Ct(i+1,i+1)=1;
    end
    % end,p,v,a,j，已知的终点数据
    for i = 0:3
        Ct(8*(n_seg-1)+4+i+1,4+(n_seg-1)+i+1)=1;
    end
    if n_seg~=1
        for i=0:n_seg-2
            % p，已知的位置
            Ct(8*i+4+1,4+i+1)=1;
            Ct(8*(i+1)+1,4+i+1)=1;
            % v，未知的速度
            Ct(8*i+4+2,8+(n_seg-1)+i*3+1)=1;
            Ct(8*(i+1)+2,8+(n_seg-1)+i*3+1)=1;
            % a，未知的加速度
            Ct(8*i+4+3,8+(n_seg-1)+i*3+2)=1;
            Ct(8*(i+1)+3,8+(n_seg-1)+i*3+2)=1;
            % j，未知的jerk
            Ct(8*i+4+4,8+(n_seg-1)+i*3+3)=1;
            Ct(8*(i+1)+4,8+(n_seg-1)+i*3+3)=1;
        end
    end
end