(四)路径规划算法---QP解决Minimum Snap轨迹优化问题

QP解决Minimum Snap轨迹优化问题

大佬的代码： https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots
本文代码： https://github.com/tgj-maker/QP_Minimumn_Snap(gitee无法使用，转战GitHub)

1. 多项式的次数确定

对于单段轨迹，多项式的次数$N$确定如下

• Minimum Jerk: 假如优化变量为$(p,v,a)$,那么$N=2\ast 3(jerk)-1=5$
• Minimum Snap：假如优化变量为$(p,v,a,j)$,那么$N=2\ast 4(snap)-1=7$

**注意:**上述只是假设Minimum Snap的优化变量为$(p,v,a,j)$ 4个变量，并不是说Minimum Snap一定只能是$(p,v,a,j)$，它们之间是相互独立的。

对于多段轨迹，多项式次数$N$的确定如下

• Minimum Jerk:假如优化变量为$(p,v,a)$,那么对于$k$段轨迹，存在一个起点、一个终点、$k-1$个中间点的位置，那么固定的变量个数：
  $$3(jerk起点)+3(jerk终点)+(k-1)\ast 1(中间点只知道位置)=k+5$$
  ​ 对于$k$段轨迹，$N$次多项式至少需要
  $$(N+1)\ast k$$
  ​ 因此
  $$\begin{gathered} (N+1)\ast k\ge k+5 \\ \Rightarrow N\ge \frac{5}{k} \end{gathered}$$

• Minimum Snap 推导过程如上，本文就不赘述。

注意：实际编程过程中，为了简单起见，多段轨迹的每一段多项式次数均相同，因此常采用单段轨迹的多项式次数作为多段轨迹的多项式次数。

---

2. Minimum Snap案例分析

2.1 轨迹的多项式表达

$$f(t)=\{\begin{array}{c}{f}_1(t)={\sum }_{i=0}^N{p}_{1,i}{t}^{i}0\le t\le T_1-T_0=\Delta T_1\\ f_2(t)={\sum }_{i=0}^N{p}_{2,i}{t}^{i}0\le t\le T_2-T_1=\Delta T_2\\ ⋮\\ f_M(t)={\sum }_{i=0}^N{p}_{M,i}{t}^{i}0\le t\le T_M-T_{M-1}=\Delta T_M\end{array}$$

其中变量说明如下

M：表示总共$M$段轨迹
$p_{M,i}$：表示在第$M$段轨迹中，多项式的第$i$个参数
t：表示该段轨迹时间，注意在实际编程中，为了增加数据的稳定性，时间常采用$[0,T]$的格式，这点和之前的理论部分略微不同。

2.2 约束条件确定

导数约束

规定起始状态$p_{start}=(x_{start},v_{start},a_{start},j_{start})$,终止状态 $p_{stop}=(x_{stop},v_{stop},a_{stop},j_{stop})$，中间点$p$的位置。

连续性约束

规定为前后两段轨迹交点处状态$(p,v,a,j)$连续。

通过第一节和该节的说明，此时多段多项式的系数均为7。

---

3 Minimum Snap算法运行过程

通过前章得出求解的方程
$$\begin{gathered} minJ={\mathbf{P}}^T\mathbf{Q}\mathbf{P}=min{\left[\begin{array}{c}{\mathbf{p}}_1\\ ⋮\\ {\mathbf{p}}_M\end{array}\right]}^T\left[\begin{array}{ccc}{\mathbf{Q}}_1& 0& 0\\ 0& \ddots & 0\\ 0& 0& {\mathbf{Q}}_M\end{array}\right]\left[\begin{array}{c}{\mathbf{p}}_1\\ ⋮\\ {\mathbf{p}}_M\end{array}\right] \\ s.t.{\mathbf{A}}_{eq}\left[\begin{array}{c}{\mathbf{p}}_1\\ ⋮\\ {\mathbf{p}}_M\end{array}\right]={\mathbf{d}}_{eq} \end{gathered}$$
注意：这里的${\mathbf{p}}_1,{\mathbf{p}}_2,...{\mathbf{p}}_{\mathbf{M}}$为向量，均由该段轨迹的多项式系数组成，例如${\mathbf{p}}_{\mathbf{M}}$= $[p_{M,1},p_{M,2},p_{M,3},p_{M,4},p_{M,5},p_{M,6},p_{M,7},p_{M,8}]$

3.1 准备工作

1. 多项式系数确定
2. 通过交互信息获取起点、中间点和终点的位置坐标
3. 确定每段轨迹的时间这里通过轨迹两端的距离分配总时间T=25s,当然为了简单起见，也可以将每段时间设置一样。
4. 调用函数MinimumSnapQPSolver进行QP求解MInimum Snap问题。代码将X与Y轴单独分离开来，进行Minimum Snap算法，这也是该算法的一大优势。

主函数 (hw1_1.m)：

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);  %各路段所需要的时间，均是以0-ts
%根据两点之间的距离按比例计算时间分布
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;
% 简单化，时间间隔均设置为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);

3.2 MinimumSnapQPSolver函数

1. 规定起始状态:$p_{start}=(x_{start},v_{start},a_{start},j_{start})=(x_{start},0,0,0)$
2. 规定终止状态 $p_{stop}=(x_{stop},v_{stop},a_{stop},j_{stop})=(x_{stop},0,0,0)$
3. 通过调用getQ函数获取最小化目标函数的Q矩阵，通过getAbeq函数获取QP解法中的等式方程(仿射变换)的矩阵A和变量b
4. 通过matlab中quadprog函数进行QP问题的求解。有关quadprog函数说明参考quadprog

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

3.2.1 getQ函数

getQ函数目的是获取$Q$矩阵，首先大矩阵$Q$矩阵是对角矩阵，是由每一段的小矩阵$Q_i$拼凑而成的，matlab代码如下

function Q = getQ(n_seg, n_order, T)
    Q = [];
    fac = @(x) x*(x-1)*(x-2)*(x-3);
    for k = 1:n_seg
        %#####################################################
        % 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 l=0:n_order
                if (i<4) || (l<4)
                    continue;
                else
                    Q_k(i+1,l+1)=fac(i)*fac(l)/(i+l-7)*T(k)^(i+l-7);
                end
            end
        end
        Q = blkdiag(Q, Q_k);
    end
end

1. 其中函数参数说明如下

   n_seg: 路段个数
   n_order:多项式最高次幂
   T:各个路段的时间总集

2. 各个路段的小矩阵Q的求解公式如下，代码也是按照以下公式进行的，参考前文。观察该矩阵发现，如果$i=0，1，2，3$或者$l=0,1,2,3$，矩阵会为0，这也是代码中出现if (i<4) || (l<4) continue 的原因。

$$\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]$$

3. 其中blkdig为matlab内置函数，主要功能用于拼接矩阵

$$\begin{gathered} blkdig(a,b,c,d,...) \\ \to \left[\begin{array}{ccccc}a& 0& 0& 0& 0\\ 0& b& 0& 0& 0\\ 0& 0& c& 0& 0\\ 0& 0& 0& d& 0\\ 0& 0& 0& 0& ...\end{array}\right] \end{gathered}$$

3.2.2 getAbeq函数

代码如下，有点复杂，不过不要急，后面有相关代码解释和原理推导。

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, 
    % Note: p = [p0, p1, p2,...pn-1]'           
    Aeq_start = zeros(4, n_all_poly);
    beq_start =  start_cond';
    Aeq_start(1:4,1:8) = getCoeff(0); 

    %#####################################################
    % p,v,a constraint in end
    Aeq_end = zeros(4, n_all_poly);
    beq_end =  end_cond';
    t = ts(end);
    Aeq_end(1:4, end-7:end) = getCoeff(t);
    
    %#####################################################
    % 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 k = 0:1:n_seg-2 % here k is the index of segments
        beq_wp(k+1, 1) = waypoints(k+2);
        coeff = getCoeff(ts(k+1));
        Aeq_wp(k+1, 1+k*8:8+k*8) = coeff(1, :);  
    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);
    % 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);
    % 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);
    % 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);
    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];
    % 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
            Aeq_con(1+4*k:4+4*k,1+8*k:8+8*k) = getCoeff(ts(k+1));
            Aeq_con(1+4*k:4+4*k,1+8*(k+1):8+8*(k+1)) = -getCoeff(0);            
    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

1. 函数参数说明

n_seg: 路段个数
n_order:多项式最高次幂
waypoints:交互信息的所有点的坐标值，包括起点、终点、中间点
ts:各个路段的时间总集
start_cond:起点的开始状态（x, v,a,j）
end_cond:起点的开始状态（x, v,a,j）

2. 其中getCoeff函数,命名为G(t)，目的是获取t时刻下，机器人的状态$x,v,a,j$,计算公式如下
   $$\begin{gathered} X(t)=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}^1+p_0 \\ \underset{G(t)}{\underbrace{\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]=\left[\begin{array}{c}X(t)\\ X(t{)}^{(1)}\\ X(t{)}^{(2)}\\ X(t{)}^{(3)}\end{array}\right]=\left[\begin{array}{c}x\\ v\\ a\\ j\end{array}\right] \end{gathered}$$
   代码框架如下

function coeff = getCoeff(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

为了方便描述，假设从起点到终点存在4个点$x_{start},x_1,x_2,x_{stop}$，即3条路段$P_1,P_2,P_3$，如图所示,每段时间均是从0开始。

3. 导数约束

   由前文可知，导数约束满足${\mathbf{A}}_j{\mathbf{p}}_j={\mathbf{d}}_j$,并且初始条件为

   $$\{\begin{array}{c}{x}_{start},v_{start},a_{start},j_{start}\\ x_{stop},v_{stop},a_{stop},j_{stop}\\ x_1,x_2\end{array}$$

​ 若向量${\mathbf{d}}_{\mathbf{j}}$表示形式如下(当然表示形式并不唯一)
$$\left[\begin{array}{c}{x}_{start}\\ v_{start}\\ a_{start}\\ j_{start}\\ x_{stop}\\ v_{stop}\\ a_{stop}\\ j_{stop}\\ x_1\\ x_2\end{array}\right]$$
​ 由于
$$G(0)\bullet {\mathbf{p}}_1=\left[\begin{array}{c}{x}_{start}\\ v_{start}\\ a_{start}\\ j_{start}\end{array}\right]$$
$$G(T)\bullet {\mathbf{p}}_{\mathbf{M}}=\mathbf{G}({\mathbf{T}}_3)\bullet {\mathbf{p}}_3=\left[\begin{array}{c}{x}_{stop}\\ v_{stop}\\ a_{stop}\\ j_{stop}\end{array}\right]$$
$$\begin{gathered} G\_0(T_1)\bullet p_1=x_1 \\ G\_0(T_2)\bullet p_2=x_2 \end{gathered}$$
​ 其中，G(t)表示getCoeff函数， $G\_0(t)$表示$t$时刻方程$G(t)$的第一行。

​ 化成矩阵形式如下
$$\left[\begin{array}{ccc}G{(0)}_{4\times 8}& 0_{4\times 8}& 0_{4\times 8}\\ 0_{4\times 8}& 0_{4\times 8}& G{(T_3)}_{4\times 8}\\ G\_0{(T_1)}_{1\times 8}& 0_{1\times 8}& 0_{1\times 8}\\ 0_{1\times 8}& G\_0{(T_2)}_{1\times 8}& 0_{1\times 8}\end{array}\right]\left[\begin{array}{c}{p}_{1,0}\\ ⋮\\ p_{1,7}\\ p_{2,0}\\ ⋮\\ p_{2,7}\\ p_{3,0}\\ ⋮\\ p_{3,7}\end{array}\right]=\left[\begin{array}{c}{x}_{start}\\ v_{start}\\ a_{start}\\ j_{start}\\ x_{stop}\\ v_{stop}\\ a_{stop}\\ j_{stop}\\ x_1\\ x_2\end{array}\right]$$

4. 连续性约束

​ 由前章可知，连续性约束为
$$\left[\begin{array}{cc}{\mathbf{A}}_j& -{\mathbf{A}}_{\mathrm{j}+1}\end{array}\right]\left[\begin{array}{c}{\mathbf{p}}_j\\ {\mathbf{p}}_{j+1}\end{array}\right]=0$$
​ 以节点$x_1$为例，连续性约束表示在$p_1$末端($x_1$的左端)的$v,a,j$与$p_2$首端($x_1$的右端)相等,数学表示如下
$$\begin{gathered} 左端=\left[\begin{array}{c}{x}_1(T_1)\\ v_1(T_1)\\ a_1(T_1)\\ j_1(T_1)\end{array}\right]=\left[\begin{array}{c}{x}_2(0)\\ v_2(0)\\ a_2(0)\\ j_2(0)\end{array}\right]=右端 \\ G(T_1)\bullet {\mathbf{p}}_1=\mathbf{G}(0)\bullet {\mathbf{p}}_2 \\ \to \left[\begin{array}{cc}G(T_1)& G(0)\end{array}\right]\left[\begin{array}{c}{\mathbf{p}}_1\\ {\mathbf{p}}_2\end{array}\right]=0 \end{gathered}$$
​ 化为矩阵形式为
$$\left[\begin{array}{ccc}G{(T_1)}_{4\times 8}& -G{(0)}_{4\times 8}& 0_{4\times 8}\\ 0_{4\times 8}& G{(T_2)}_{4\times 8}& -G{(0)}_{4\times 8}\end{array}\right]\left[\begin{array}{c}{p}_{1,0}\\ ⋮\\ p_{1,7}\\ p_{2,0}\\ ⋮\\ p_{2,7}\\ p_{3,0}\\ ⋮\\ p_{3,7}\end{array}\right]=\left[0_{(4\times 2)\times 1}\right]$$

5. 合并导数与连续性约束

​ 由前文可知，Minimum Snap下QP的等式约束是导数约束与和连续性约束的合并，得到
$${\mathbf{A}}_{\mathbf{e}\mathbf{q}}=\left[\begin{array}{ccc}G{(0)}_{4\times 8}& 0_{4\times 8}& 0_{4\times 8}\\ 0_{4\times 8}& 0_{4\times 8}& G{(T_3)}_{4\times 8}\\ G\_0{(T_1)}_{1\times 8}& 0_{1\times 8}& 0_{1\times 8}\\ 0_{1\times 8}& G\_0{(T_2)}_{1\times 8}& 0_{1\times 8}\\ G{(T_1)}_{4\times 8}& -G{(0)}_{4\times 8}& 0_{4\times 8}\\ 0_{4\times 8}& G{(T_2)}_{4\times 8}& -G{(0)}_{4\times 8}\end{array}\right]\left[\begin{array}{c}{p}_{1,0}\\ ⋮\\ p_{1,7}\\ p_{2,0}\\ ⋮\\ p_{2,7}\\ p_{3,0}\\ ⋮\\ p_{3,7}\end{array}\right]=\left[\begin{array}{c}{x}_{start}\\ v_{start}\\ a_{start}\\ j_{start}\\ x_{stop}\\ v_{stop}\\ a_{stop}\\ j_{stop}\\ x_1\\ x_2\\ 0_{(4\times 2)\times 1}\end{array}\right]={\mathbf{d}}_{\mathbf{e}\mathbf{q}}$$

注意：上述描述仅表示3条轨迹、2个中间点的${\mathbf{A}}_{\mathbf{e}\mathbf{q}},{\mathbf{d}}_{\mathbf{e}\mathbf{q}}$的求解情况。对于任意的轨迹个数，矩阵${\mathbf{A}}_{\mathbf{e}\mathbf{q}},{\mathbf{d}}_{\mathbf{e}\mathbf{q}}$的计算过程类似，这里给出矩阵和向量的维数情况:
$$\begin{gathered} 对于MinimumSnap问题，优化变量为(p,v,a,j)4个，那么最高次幂为7。对于M段轨迹，那么 \\ 矩阵{\mathbf{A}}_{\mathbf{e}\mathbf{q}}的行数为：4（snap）+4（snap）+(M-1)+4\times (M-1)=5\times M+3 \\ 矩阵{\mathbf{A}}_{\mathbf{e}\mathbf{q}}的列数为：(7+1)\times M=8\times M \\ 向量{\mathbf{d}}_{\mathbf{e}\mathbf{q}}的行数为：5\times M+3 \end{gathered}$$

3.3 显示功能

主函数 hw1_1.m

% 显示轨迹
X_n = [];
Y_n = [];
k = 1;
tstep = 0.01;
for i=0:n_seg-1
    %#####################################################
    % STEP 3: 获得各路段的多项式系数
    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)
        %flip表示翻转Pxi中的元素
        %polyval（[P0,P1,P2],t）生成P0t^2+P1^t+P2,因此需要翻转原始数据
        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));

这里需要注意的一点就是，matlab中的polyval函数所生成的多项式函数为$p_0{t}^7+p_1{t}^6+...+p_7$,这与本文多项式的表示($p_7{t}^7+p_6{t}^6+...+p_0$)不符，因此需要使用flip函数进行多项式系数翻转。

效果如下：
运行QP/hw1_1.m程序，通过鼠标在原图上点击几个关键点，然后通过键盘上的"Enter"键进行输入结束，稍等片刻，就会出现上述结果。

	原图	3段轨迹	4段轨迹
效果

注意:上述是看大佬代码并结合算法原理所得出自己的理解，可能出现错误，欢迎批评指证。码字不易，转载请注明出处，谢谢。，