SHENLAN_路径规划_Chapter5_HW1_通过数值优化方法解Minimum Snap的轨迹生成问题

前言：
第一次写CSDN博客，用我家蛋蛋记录一下，谢谢！ 写博客的主要目的是想和大家相互交流学习

1.原理

求解Q的方法:

解目标函数为Minimum Snap，多项式表达式为七次多项式
$$f(t)=\sum _i{p}_i{t}^i=p_7{t}^7+p_6{t}^6+p_5{t}^5+p_4{t}^4+p_3{t}^3+p_2{t}^2+p_{1}t+p_0$$
Cost funciton为Minimum Snap，
每段轨迹的Cost funciton为f(t)的4次导数的平方 $$J_k(T)={\int }_{T_{k-1}}^{T_k}(f^4(t){)}^{2}dt$$
经过推导得出
$$J_k(T)={\left[\begin{array}{c}⋮\\ p_i\\ ⋮\end{array}\right]}^T\left[\begin{array}{ccc}& ⋮& \\ \cdots & \frac{i(i-1)(i-2)(i-3)l(l-1)(l-2)(l-3)}{i+l-7}{T}^{i+l-7}& \cdots \\ & ⋮& \end{array}\right]\left[\begin{array}{c}⋮\\ p_i\\ ⋮\end{array}\right]=p_k^T{Q}_k{p}_k$$
其中i和j=[0:n_order],n_order为每段多项式的阶数。Qk为(n_order+1)*n_order+1)的矩阵。
k代表第k段轨迹。
算出每一段轨迹的Qk矩阵,通过对角对阵构造的方法，构造出经过轨迹的总的Q矩阵：
$$Q=\left[\begin{array}{cccc}{Q}_1& 0& \cdots & 0\\ 0& Q_k& \cdots & 0\\ ⋮& ⋮& \ddots & 0\\ 0& 0& \dots & Q_{nseg}\end{array}\right]$$
则
$$J=p^{T}Qp={\left[\begin{array}{c}{p}_1\\ ⋮\\ p_k\\ ⋮\\ p_{nseg}\end{array}\right]}^T\left[\begin{array}{cccc}{Q}_1& 0& \cdots & 0\\ 0& Q_k& \cdots & 0\\ ⋮& ⋮& \ddots & 0\\ 0& 0& \dots & Q_{nseg}\end{array}\right]\left[\begin{array}{c}{p}_1\\ ⋮\\ p_k\\ ⋮\\ p_{nseg}\end{array}\right]$$
其中
$$p_k=\left[\begin{array}{c}⋮\\ p_i\\ ⋮\end{array}\right]$$

求解等式约束Aeq和beq的方法

$$f(t)=p(t)=p_0+p_{1}t+p_2{t}^2+p_3{t}^3+p_4{t}^4+p_5{t}^5+p_6{t}^6+p_7{t}^7$$
$$f^{(1)}(t)=v(t)=0+p_1+p_2\ast 2t+p_3\ast 3t^2+p_4\ast 4t^3+p_5\ast 5t^4+p_6\ast 6t^5+p_7\ast 7t^6$$
$$f^{(2)}(t)=a(t)=0+0+p_2\ast 2+p_3\ast 6t+p_4\ast 12t^2+p_5\ast 20t^3+p_6\ast 30t^4+p_7\ast 42t^5$$
$$f^{(3)}(t)=j(t)=0+0+0+p_3\ast 6+p_4\ast 24t+p_5\ast 60t^2+p_6\ast 120t^3+p_7\ast 210t^4$$
写成矩阵形式，则：
$$\left[\begin{array}{c}p(t)\\ v(t)\\ a(t)\\ j(t)\end{array}\right]=\left[\begin{array}{cccccccc}1& t& t^2& t^3& t^4& t^5& t^6& t^7\\ 0& 1& 2t& 3t^2& 4t^3& 5t^4& 6t^5& 7t^6\\ 0& 0& 2& 6t& 12t^2& 20t^3& 30t^4& 42t^5\\ 0& 0& 0& 6& 24t& 60t^2& 120t^3& 210t^4\end{array}\right]\left[\begin{array}{c}{p}_0\\ p_1\\ p_2\\ p_3\\ p_4\\ p_5\\ p_6\\ p_7\end{array}\right]$$
对应着
$$beq=Aeq\left[\begin{array}{c}{p}_0\\ p_1\\ p_2\\ p_3\\ p_4\\ p_5\\ p_6\\ p_7\end{array}\right]$$
起始点和终止点指定了p,v,a,j
起始点的[p,v,a,j]=[point_start,0,0,0],v,a,j都为0，因为是从静止开始出发的。
则start_point点的$$beq=[poin{t}_{start},0,0,0{]}^T$$
把t=0代入Aeq表达式
则
$$Ae{q}_{startpoint}=\left[\begin{array}{ccccccccc}1& 0& 0& 0& 0& 0& 0& 0& \cdots \\ 0& 1& 0& 0& 0& 0& 0& 0& \cdots \\ 0& 0& 2& 0& 0& 0& 0& 0& \cdots \\ 0& 0& 0& 6& 0& 0& 0& 0& \cdots \end{array}\right]$$
终止点的[p,v,a,j]=[point_end,0,0,0],v,a,j都为0，因为到终点是需要静止。终点状态的t=ts_end
则
$$Ae{q}_{endpoint}=\left[\begin{array}{cccccccccc}0& \cdots & 1& t{s}_{end}& t{s}_{end}^2& t{s}_{end}^3& t{s}_{end}^4& t{s}_{end}^5& t{s}_{end}^6& t{s}_{end}^7\\ 0& \cdots & 0& 1& 2t{s}_{end}& 3t{s}_{end}^2& 4t{s}_{end}^3& 5t{s}_{end}^4& 6t{s}_{end}^5& 7t{s}_{end}^6\\ 0& \cdots & 0& 0& 2& 6t{s}_{end}& 12t{s}_{end}^2& 20t{s}_{end}^3& 30t{s}_{end}^4& 42t{s}_{end}^5\\ 0& \cdots & 0& 0& 0& 6& 24t{s}_{end}& 60t{s}_{end}^2& 120t{s}_{end}^3& 210t{s}_{end}^4\end{array}\right]$$
其中ts_end为最后一段轨迹的时间T，需要说明的是，每一段的T都是单独的[0,ts_n],如下图所示：

除了起始点和终止点，中间点都只限制了必须要经过该中间点，也就是限制了中间点的p，而v,a,j都是可变的优化项。
由此，先列出和中间点位置相关的Aeq和beq：
$$be{q}_{waypoint}=\left[\begin{array}{c}{p}_{seg(1)endpoint}\\ p_{seg(2)endpoint}\\ ⋮\\ p_{seg(n_{seg}-1)endpoint}\end{array}\right]$$
p_{seg(k)endpoint}代表第k段轨迹的末端。
$$Ae{q}_{waypoint}=\left[\begin{array}{ccccccccccc}1& t{s}_1& t{s}_1^2& t{s}_1^3& t{s}_{1d}^4& t{s}_1^5& t{s}_1^6& t{s}_1^7& 0& \cdots & \cdots \\ 0& \cdots & 1& t{s}_2& t{s}_2^2& t{s}_2^3& t{s}_2^4& t{s}_2^5& t{s}_2^6& t{s}_2^7& 0\\ 0& \cdots & ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ 0& \cdots & \cdots & 1& t{s}_{n_{seg}-1}& t{s}_{n_{seg}-1}^2& t{s}_{n_{seg}-1}^3& t{s}_{n_{seg}-1}^4& t{s}_{n_{seg}-1}^5& t{s}_{n_{seg}-1}^6& t{s}_{n_{seg}-1}^7\end{array}\right]$$
根据连续性约束，第一段轨迹的终点和第二段轨迹的起点相同，第二段轨迹的终点和第三段轨迹的起点相同：
$$f_1(t{s}_1)-f_2(0)=0,f_2(t{s}_2)-f_3(0)=0$$
，以此类推v,a,j，则可以得出下图通过Excel表格做出的表达式中第13-20行的形式。

2.代码分析

基于Matlab的代码实现，求解的工具是通过得出Aeq和beq，通过poly_coef = quadprog(Q,f,[],[],Aeq, beq)得到三段轨迹的七阶多项式的系数。
matlab中quadprog的用法：


H是二阶的相关矩阵，f是一阶的相关矩阵，在这里H就是我们求的Q，由于没有一阶项，f则为0向量。
且没有不等式约束和输入变量约束，则A、b、lb、ub都为0，若有相关约束则可以加上。

分别独立求出x(t)和y(t)，就能得到y-x轨迹图。

// hw1_1.m
% 通过ginput()函数获取通过鼠标点击的路径中间点
clc;clear;close all;
path = ginput() * 100.0;
%load("path.mat");

% 定义每段多项式的阶数、通过输入的路径点数算出有几段、每段多项式的系数个数
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，用于存储n_seg段，每段轨迹的时间T
ts = zeros(n_seg, 1);
% calculate time distribution in proportion to distance between 2 points
% 通过计算两个点之间的欧拉距离（两点之间的直线距离）来分配每段轨迹的时间T
dist     = zeros(n_seg, 1);
dist_sum = 0;
T        = 1; % 通过所有段轨迹的总时间T
t_sum    = 0;
% 算出序号临近的i和i+1点之间的距离总和dist_sum
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
% 计算出每段轨迹的时间ts
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

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
    %提取出每一段7次多项式的系数
    Pxi = poly_coef_x((n_order+1)*(i)+1:(n_order+1)*(i)+n_order+1);
    Pyi = poly_coef_y((n_order+1)*(i)+1:(n_order+1)*(i)+n_order+1);
    for t = 0:tstep:ts(i+1)
        X_n(k)  = polyval(flip(Pxi), t);  %得到的多项式系数是前后倒置，需要顺序反过来
        Y_n(k)  = polyval(flip(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));

% MinimumSnapQPSolver函数用于构建求解QP问题，构建Q矩阵和等式约束Aeq及beq
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矩阵是关键一步。每段轨迹的Qk矩阵主要是和每一段轨迹分配的时间Ts有关。

%getQ.m,用于计算二次型的Q矩阵
%这里的i和j代表行和列，在这里行和列的数量相同为n_order+1，计算出每一段轨迹的Qj矩阵，再通过对角矩阵的方法把所有轨迹的Q矩阵连接成对角矩阵为总的Q。
function Q = getQ(n_seg, n_order, ts)
    Q = [];
    fac = @(x) x*(x-1)*(x-2)*(x-3);
    for k = 1:n_seg
        Q_k = [];
        %#####################################################
        % STEP 1.1: calculate Q_k of the k-th segment 
       Q_k = zeros(n_order+1,n_order+1);

        for i = 0:n_order
            for j = 0:n_order
                if (i < 4) || (j < 4)
                    continue;
                else
                    Q_k(i + 1,j + 1) = fac(i) * fac(j)/(i + j - 7) * ts(k) ^ (i + j -7); 
                end
            end
        end
        Q = blkdiag(Q, Q_k);
    end
end

求beq

% getAbeq.m
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 = start_cond'; %起始点的约束的等式值，p有数，v,a,j都是0
    % STEP 2.1: write expression of Aeq_start and beq_start
    Aeq_start(1:4,1:8) = Coeff(0);  %起始点的约束方程的多项式系数的矩阵，放到最前面（第一段曲线）
    %
    %
    %
    
    %#####################################################
    % p,v,a,j constraint in end
    Aeq_end = zeros(4, n_all_poly);
    beq_end = end_cond';%终止点的约束的等式值，p有数，v,a,j都是0
    % STEP 2.2: write expression of Aeq_end and beq_end
    t = ts(end);
    Aeq_end(1:4, end-7:end) = Coeff(t); %终止点的约束方程的多项式系数的矩阵，放到最后（最后一段曲线）
    %
    %
    %
    
    %#####################################################
    % position constrain in all middle waypoints
    Aeq_wp = zeros(n_seg-1, n_all_poly); %中间点的个数是n_seg(曲线段数)-1个，只需要约束p，所以v,a,j不写出来
    beq_wp = zeros(n_seg-1, 1);
    % STEP 2.3: write expression of Aeq_wp and beq_wp
    for k = 0:1:n_seg-2
        beq_wp(k+1,1) = waypoints(k+2); %中间点的约束为对应的p
        tempcoeff = Coeff(ts(k+1));
        Aeq_wp(k+1,1+k*8:8+k*8) = tempcoeff(1,:);  %中间点只有p约束，v, a,j不做约束
    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);
    % STEP 2.4: write expression of Aeq_con_p and beq_con_p
    for k = 0:1:n_seg-2 % here k is the index of segments
        tempcoeff = Coeff(ts(k+1));
        Aeq_con_p(1+k,1+8*k:8+8*k) = tempcoeff(1,:); %中间点左侧曲线该点的 p v a j的系数矩阵
        tempcoeff = Coeff(0);
        Aeq_con_p(1+k,1+8*(k+1):8+8*(k+1)) = -tempcoeff(1,:); %中间点右侧曲线该点的 p v a j的系数矩阵，由于每一段曲线的时间是用的相对时间，所以该点右侧的t是0；
    end
    % beq_con_p,v,a,j都是0,因为有约束：p_left-p_right=0,v_left-v_right=0,..,
    
    %#####################################################
    % velocity continuity constrain between each 2 segments
    Aeq_con_v = zeros(n_seg-1, n_all_poly);
    beq_con_v = zeros(n_seg-1, 1);
    % STEP 2.5: write expression of Aeq_con_v and beq_con_v
    for k = 0:1:n_seg-2 % here k is the index of segments
        tempcoeff = Coeff(ts(k+1));
        Aeq_con_v(1+k,1+8*k:8+8*k) = tempcoeff(2,:); %中间点左侧曲线该点的 p v a j的系数矩阵
        tempcoeff = Coeff(0);
        Aeq_con_v(1+k,1+8*(k+1):8+8*(k+1)) = -tempcoeff(2,:); %中间点右侧曲线该点的 p v a j的系数矩阵，由于每一段曲线的时间是用的相对时间，所以该点右侧的t是0；
    end

    %#####################################################
    % acceleration continuity constrain between each 2 segments
    Aeq_con_a = zeros(n_seg-1, n_all_poly);
    beq_con_a = zeros(n_seg-1, 1);
    % STEP 2.6: write expression of Aeq_con_a and beq_con_a
    for k = 0:1:n_seg-2 % here k is the index of segments
        tempcoeff = Coeff(ts(k+1));
        Aeq_con_a(1+k,1+8*k:8+8*k) = tempcoeff(3,:); %中间点左侧曲线该点的 p v a j的系数矩阵
        tempcoeff = Coeff(0);
        Aeq_con_a(1+k,1+8*(k+1):8+8*(k+1)) = -tempcoeff(3,:); %中间点右侧曲线该点的 p v a j的系数矩阵，由于每一段曲线的时间是用的相对时间，所以该点右侧的t是0；
    end
    
    %#####################################################
    % jerk continuity constrain between each 2 segments
    Aeq_con_j = zeros(n_seg-1, n_all_poly);
    beq_con_j = zeros(n_seg-1, 1);
    % STEP 2.7: write expression of Aeq_con_j and beq_con_j
    for k = 0:1:n_seg-2 % here k is the index of segments
        tempcoeff = Coeff(ts(k+1));
        Aeq_con_j(1+k,1+8*k:8+8*k) = tempcoeff(4,:); %中间点左侧曲线该点的 p v a j的系数矩阵
        tempcoeff = Coeff(0);
        Aeq_con_j(1+k,1+8*(k+1):8+8*(k+1)) = -tempcoeff(4,:); %中间点右侧曲线该点的 p v a j的系数矩阵，由于每一段曲线的时间是用的相对时间，所以该点右侧的t是0；
    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

function coeff = Coeff(t)
% Get the coefficient for Aeq
    coeff = [1,  1*t,  1*t^2,  1*t^3,  1*t^4,  1*t^5,  1*t^6,  1*t^7;
             0,  1,    2*t,    3*t^2,  4*t^3,  5*t^4,  6*t^5,  7*t^6;
             0,  0,    2,      6*t,    12*t^2, 20*t^3, 30*t^4, 42*t^5;
             0,  0,    0,      6,      24*t,   60*t^2, 120*t^3,210*t^4];
end