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

这篇具有很好参考价值的文章主要介绍了(四)路径规划算法---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 N N确定如下

  • Minimum Jerk: 假如优化变量为 ( p , v , a ) (p,v,a) (p,v,a),那么 N = 2 ∗ 3 ( j e r k ) − 1 = 5 N=2*3(jerk)-1=5 N=23(jerk)1=5
  • Minimum Snap:假如优化变量为 ( p , v , a , j ) (p,v,a,j) (p,v,a,j),那么 N = 2 ∗ 4 ( s n a p ) − 1 = 7 N=2*4(snap)-1=7 N=24(snap)1=7

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

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

  • Minimum Jerk:假如优化变量为 ( p , v , a ) (p,v,a) (p,v,a),那么对于 k k k段轨迹,存在一个起点、一个终点、 k − 1 k-1 k1个中间点的位置,那么固定的变量个数:
    3 ( j e r k 起 点 ) + 3 ( j e r k 终 点 ) + ( k − 1 ) ∗ 1 ( 中 间 点 只 知 道 位 置 ) = k + 5 3(jerk \quad起点)+3(jerk \quad 终点)+(k-1)*1(中间点只知道位置)=k+5 3(jerk)+3(jerk)+(k1)1()=k+5
    ​ 对于 k k k段轨迹, N N N次多项式至少需要
    ( N + 1 ) ∗ k (N+1)*k (N+1)k
    ​ 因此
    ( N + 1 ) ∗ k ≥ k + 5 ⇒ N ≥ 5 k (N+1)*k \ge k+5 \\ \Rightarrow N \ge \frac{5}{k} (N+1)kk+5Nk5

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

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


2. Minimum Snap案例分析

2.1 轨迹的多项式表达

f ( t ) = { f 1 ( t ) = ∑ i = 0 N p 1 , i t i 0 ≤ t ≤ T 1 − T 0 = Δ T 1 f 2 ( t ) = ∑ i = 0 N p 2 , i t i 0 ≤ t ≤ T 2 − T 1 = Δ T 2 ⋮ f M ( t ) = ∑ i = 0 N p M , i t i 0 ≤ t ≤ T M − T M − 1 = Δ T M f(t) = \left\{ {\begin{matrix} {{f_1}(t) = \sum\nolimits_{i = 0}^N {{p_{1,i}}{t^i}0 \le t \le {T_1} - {T_0} = \Delta {T_1}} }\\ {{f_2}(t) = \sum\nolimits_{i = 0}^N {{p_{2,i}}{t^i}0 \le t \le {T_2} - {T_1} = \Delta {T_2}} }\\ \vdots \\ {{f_M}(t) = \sum\nolimits_{i = 0}^N {{p_{M,i}}{t^i}0 \le t \le {T_M} - {T_{M - 1}} = \Delta {T_M}} } \end{matrix}} \right. f(t)=f1(t)=i=0Np1,iti0tT1T0=ΔT1f2(t)=i=0Np2,iti0tT2T1=ΔT2fM(t)=i=0NpM,iti0tTMTM1=ΔTM

其中变量说明如下

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

2.2 约束条件确定

导数约束

规定起始状态 p s t a r t = ( x s t a r t , v s t a r t , a s t a r t , j s t a r t ) p_{start}=(x_{start},v_{start},a_{start},j_{start}) pstart=(xstart,vstart,astart,jstart),终止状态 p s t o p = ( x s t o p , v s t o p , a s t o p , j s t o p ) p_{stop}=(x_{stop},v_{stop},a_{stop},j_{stop}) pstop=(xstop,vstop,astop,jstop),中间点 p p p的位置。

连续性约束

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

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


3 Minimum Snap算法运行过程

通过前章得出求解的方程
m i n J = P T Q P = m i n [ p 1 ⋮ p M ] T [ Q 1 0 0 0 ⋱ 0 0 0 Q M ] [ p 1 ⋮ p M ] s . t . A e q [ p 1 ⋮ p M ] = d e q min \quad J = {{\bf{P}}^T}{\bf{QP}} = min \quad {\left[ {\begin{matrix} {{{\bf{p}}_1}}\\ \vdots \\ {{{\bf{p}}_M}} \end{matrix}} \right]^T}\left[ {\begin{matrix} {{{\bf{Q}}_1}}&0&0\\ 0& \ddots &0\\ 0&0&{{{\bf{Q}}_M}} \end{matrix}} \right]\left[ {\begin{matrix} {{{\bf{p}}_1}}\\ \vdots \\ {{{\bf{p}}_M}} \end{matrix}} \right] \\ s.t.{{\bf{A}}_{eq}}\left[ {\begin{matrix} {{{\bf{p}}_1}}\\ \vdots \\ {{{\bf{p}}_M}} \end{matrix}} \right] = {{\bf{d}}_{eq}} minJ=PTQP=minp1pMTQ1000000QMp1pMs.t.Aeqp1pM=deq
注意:这里的 p 1 , p 2 , . . . p M \bf p_1,\bf p_2,...\bf p_M p1,p2,...pM为向量,均由该段轨迹的多项式系数组成,例如 p M \bf {p_M} pM= [ p M , 1 , p M , 2 , p M , 3 , p M , 4 , p M , 5 , p M , 6 , p M , 7 , p M , 8 ] [p_{M,1},p_{M,2},p_{M,3},p_{M,4},p_{M,5},p_{M,6},p_{M,7},p_{M,8}] [pM,1,pM,2,pM,3,pM,4,pM,5,pM,6,pM,7,pM,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 s t a r t = ( x s t a r t , v s t a r t , a s t a r t , j s t a r t ) = ( x s t a r t , 0 , 0 , 0 ) p_{start}=(x_{start},v_{start},a_{start},j_{start})=(x_{start},0,0,0) pstart=(xstart,vstart,astart,jstart)=(xstart,0,0,0)
  2. 规定终止状态 p s t o p = ( x s t o p , v s t o p , a s t o p , j s t o p ) = ( x s t o p , 0 , 0 , 0 ) p_{stop}=(x_{stop},v_{stop},a_{stop},j_{stop})=(x_{stop},0,0,0) pstop=(xstop,vstop,astop,jstop)=(xstop,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矩阵,首先大矩阵 Q Q Q矩阵是对角矩阵,是由每一段的小矩阵 Q i Q_i Qi拼凑而成的,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 i=0,1,2,3 i=0123或者 l = 0 , 1 , 2 , 3 l=0,1,2,3 l=0,1,2,3,矩阵会为0,这也是代码中出现if (i<4) || (l<4) continue 的原因。

[ ⋮ ⋯ i ( i − 1 ) ( i − 2 ) ( i − 3 ) l ( l − 1 ) ( l − 2 ) ( l − 3 ) i + l − 7 T i + l − 7 ⋯ ⋮ ] \left[ {\begin{matrix} {}& \vdots &{}\\ \cdots &{\frac{{i(i - 1)(i - 2)(i - 3)l(l - 1)(l - 2)(l - 3)}}{{i + l - 7}}{T^{i + l - 7}}}& \cdots \\ {}& \vdots &{} \end{matrix}} \right] i+l7i(i1)(i2)(i3)l(l1)(l2)(l3)Ti+l7

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

b l k d i g ( a , b , c , d , . . . ) → [ a 0 0 0 0 0 b 0 0 0 0 0 c 0 0 0 0 0 d 0 0 0 0 0 . . . ] blkdig(a,b,c,d,...) \\ \to \left[ {\begin{matrix} 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{matrix}} \right] blkdig(a,b,c,d,...)a00000b00000c00000d00000...

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)

  1. 其中getCoeff函数,命名为G(t),目的是获取t时刻下,机器人的状态 x , v , a , j x,v,a,j x,v,a,j,计算公式如下
    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 [ 1 t t 2 t 3 t 4 t 5 t 6 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 ] ⏟ G ( t ) [ p 0 p 1 p 2 p 3 p 4 p 5 p 6 p 7 ] = [ X ( t ) X ( t ) ( 1 ) X ( t ) ( 2 ) X ( t ) ( 3 ) ] = [ x v a j ] X(t)=p_7t^7+p_6t^6+p_5t^5+p_4t^4+p_3t^3+p_2t^2+p_1t^1+p_0 \\ \underbrace {\left[ {\begin{matrix} 1&t&{{t^2}}&{{t^3}}&{{t^4}}&{{t^5}}&{{t^6}}&{{t^7}}\\ 0&1&{2t}&{3{t^2}}&{4{t^3}}&{5{t^4}}&{6{t^5}}&{7{t^6}}\\ 0&0&2&{6t}&{12{t^2}}&{20{t^3}}&{30{t^4}}&{42{t^5}}\\ 0&0&0&6&{24t}&{60{t^2}}&{120{t^3}}&{210{t^4}} \end{matrix}} \right]}_{G(t)}\left[ {\begin{matrix} {{p_0}}\\ {{p_1}}\\ {{p_2}}\\ {{p_3}}\\ {{p_4}}\\ {{p_5}}\\ {{p_6}}\\ {{p_7}} \end{matrix}} \right] = \left[ {\begin{matrix} {X(t)}\\ {X(t)^{(1)}}\\ { X(t)^{(2)}}\\ { X(t)^{(3)}} \end{matrix}} \right] = \left[ {\begin{matrix} x\\ v\\ a\\ j \end{matrix}} \right] X(t)=p7t7+p6t6+p5t5+p4t4+p3t3+p2t2+p1t1+p0G(t) 1000t100t22t20t33t26t6t44t312t224tt55t420t360t2t66t530t4120t3t77t642t5210t4p0p1p2p3p4p5p6p7=X(t)X(t)(1)X(t)(2)X(t)(3)=xvaj
    代码框架如下
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 s t a r t , x 1 , x 2 , x s t o p x_{start},x_1,x_2,x_{stop} xstart,x1,x2,xstop,即3条路段 P 1 , P 2 , P 3 P_1,P_2,P_3 P1,P2,P3,如图所示,每段时间均是从0开始
(四)路径规划算法---QP解决Minimum Snap轨迹优化问题

  1. 导数约束

    由前文可知,导数约束满足 A j p j = d j {{\bf{A}}_j}{{\bf{p}}_j} = {{\bf{d}}_j} Ajpj=dj,并且初始条件为

    { x s t a r t , v s t a r t , a s t a r t , j s t a r t x s t o p , v s t o p , a s t o p , j s t o p x 1 , x 2 \left\{ {\begin{matrix} {{x_{start}},v_{start},a_{start},j_{start}}\\ {{x_{stop}},v_{stop},a_{stop},j_{stop}}\\ {{x_1},{x_2}} \end{matrix}} \right. xstart,vstart,astart,jstartxstop,vstop,astop,jstopx1,x2

​ 若向量 d j \bf d_j dj表示形式如下(当然表示形式并不唯一)
[ x s t a r t v s t a r t a s t a r t j s t a r t x s t o p v s t o p a s t o p j s t o p x 1 x 2 ] \left[ {\begin{matrix} {{x_{start}}}\\ {{v_{start}}}\\ {{a_{start}}}\\ {{j_{start}}}\\ \hdashline {{x_{stop}}}\\ {{v_{stop}}}\\ {{a_{stop}}}\\ {{j_{stop}}}\\ \hdashline {{x_1}}\\ {{x_2}} \end{matrix}} \right] xstartvstartastartjstartxstopvstopastopjstopx1x2
​ 由于
G ( 0 ) ∙ p 1 = [ x s t a r t v s t a r t a s t a r t j s t a r t ] G(0) \bullet \bf p_1=\left[ {\begin{matrix} {{x_{start}}}\\ {{v_{start}}}\\ {{a_{start}}}\\ {{j_{start}}} \end{matrix}} \right] G(0)p1=xstartvstartastartjstart
G ( T ) ∙ p M = G ( T 3 ) ∙ p 3 = [ x s t o p v s t o p a s t o p j s t o p ] G(T) \bullet \bf p_M=G(T_3) \bullet \bf p_3=\left[ {\begin{matrix} {{x_{stop}}}\\ {{v_{stop}}}\\ {{a_{stop}}}\\ {{j_{stop}}} \end{matrix}} \right] G(T)pM=G(T3)p3=xstopvstopastopjstop
G _ 0 ( T 1 ) ∙ p 1 = x 1 G _ 0 ( T 2 ) ∙ p 2 = x 2 G\_0(T_1) \bullet p_1=x_1 \\ G\_0(T_2) \bullet p_2=x_2 G_0(T1)p1=x1G_0(T2)p2=x2
其中,G(t)表示getCoeff函数, G _ 0 ( t ) G\_0(t) G_0(t)表示 t t t时刻方程 G ( t ) G(t) G(t)的第一行。

​ 化成矩阵形式如下
[ G ( 0 ) 4 × 8 0 4 × 8 0 4 × 8 0 4 × 8 0 4 × 8 G ( T 3 ) 4 × 8 G _ 0 ( T 1 ) 1 × 8 0 1 × 8 0 1 × 8 0 1 × 8 G _ 0 ( T 2 ) 1 × 8 0 1 × 8 ] [ p 1 , 0 ⋮ p 1 , 7 p 2 , 0 ⋮ p 2 , 7 p 3 , 0 ⋮ p 3 , 7 ] = [ x s t a r t v s t a r t a s t a r t j s t a r t x s t o p v s t o p a s t o p j s t o p x 1 x 2 ] \left[ {\begin{matrix} {G{{(0)}_{4 \times 8}}}&{\bf {0_{4 \times 8}}}&{\bf{0_{4 \times 8}}}\\ {\bf{0_{4 \times 8}}}&{\bf{0_{4 \times 8}}}&{G{{({T_3})}_{4 \times 8}}}\\ \hdashline {G\_0{{({T_1})}_{1 \times 8}}}&{\bf{0_{1 \times 8}}}&{\bf{0_{1 \times 8}}}\\ {\bf{0_{1 \times 8}}}&{G\_0{{({T_2})}_{1 \times 8}}}&{\bf{0_{1 \times 8}}} \end{matrix}} \right]\left[ {\begin{matrix} {{p_{1,0}}}\\ \vdots \\ {{p_{1,7}}}\\ \hdashline {{p_{2,0}}}\\ \vdots \\ {{p_{2,7}}}\\ \hdashline {{p_{3,0}}}\\ \vdots \\ {{p_{3,7}}} \end{matrix}} \right] = \left[ {\begin{matrix} {{x_{start}}}\\ {{v_{start}}}\\ {{a_{start}}}\\ {{j_{start}}}\\ \hdashline {{x_{stop}}}\\ {{v_{stop}}}\\ {{a_{stop}}}\\ {{j_{stop}}}\\ \hdashline {{x_1}}\\ {{x_2}} \end{matrix}} \right] G(0)4×804×8G_0(T1)1×801×804×804×801×8G_0(T2)1×804×8G(T3)4×801×801×8p1,0p1,7p2,0p2,7p3,0p3,7=xstartvstartastartjstartxstopvstopastopjstopx1x2

  1. 连续性约束

​ 由前章可知,连续性约束为
[ A j − A j + 1 ] [ p j p j + 1 ] = 0 \left[ {\begin{matrix} {{{\bf{A}}_j}}&{ - {{\bf{A}}_{{\rm{j + }}1}}} \end{matrix}} \right]\left[ {\begin{matrix} {{{\bf{p}}_j}}\\ {{{\bf{p}}_{j + 1}}} \end{matrix}} \right] = 0 [AjAj+1][pjpj+1]=0
​ 以节点 x 1 x_1 x1为例,连续性约束表示在 p 1 p_1 p1末端( x 1 x_1 x1的左端)的 v , a , j v,a,j v,a,j p 2 p_2 p2首端( x 1 x_1 x1的右端)相等,数学表示如下
左 端 = [ x 1 ( T 1 ) v 1 ( T 1 ) a 1 ( T 1 ) j 1 ( T 1 ) ] = [ x 2 ( 0 ) v 2 ( 0 ) a 2 ( 0 ) j 2 ( 0 ) ] = 右 端 G ( T 1 ) ∙ p 1 = G ( 0 ) ∙ p 2 → [ G ( T 1 ) G ( 0 ) ] [ p 1 p 2 ] = 0 左端=\left[ {\begin{matrix} {{x_1}({T_1})}\\ {{v_1}({T_1})}\\ {{a_1}({T_1})}\\ {{j_1}({T_1})} \end{matrix}} \right] = \left[ {\begin{matrix} {{x_2}(0)}\\ {{v_2}(0)}\\ {{a_2}(0)}\\ {{j_2}(0)} \end{matrix}} \right]=右端 \\ G(T_1)\bullet \bf p_1=G(0) \bullet \bf p_2 \\ \to \left[ {\begin{matrix} {G({T_1})}&{G(0)} \end{matrix}} \right]\left[ {\begin{matrix} {{{\bf{p}}_1}}\\ {{{\bf{p}}_2}} \end{matrix}} \right] = {\bf{0}} =x1(T1)v1(T1)a1(T1)j1(T1)=x2(0)v2(0)a2(0)j2(0)=G(T1)p1=G(0)p2[G(T1)G(0)][p1p2]=0
​ 化为矩阵形式为
[ G ( T 1 ) 4 × 8 − G ( 0 ) 4 × 8 0 4 × 8 0 4 × 8 G ( T 2 ) 4 × 8 − G ( 0 ) 4 × 8 ] [ p 1 , 0 ⋮ p 1 , 7 p 2 , 0 ⋮ p 2 , 7 p 3 , 0 ⋮ p 3 , 7 ] = [ 0 ( 4 × 2 ) × 1 ] \left[ {\begin{matrix} {G{{({T_1})}_{4 \times 8}}}&{ - G{{(0)}_{4 \times 8}}}&{{{\bf{0}}_{4 \times 8}}}\\ {{{\bf{0}}_{4 \times 8}}}&{G{{({T_2})}_{4 \times 8}}}&{ - G{{(0)}_{4 \times 8}}}\\ \end{matrix}} \right]\left[ {\begin{matrix} {{p_{1,0}}}\\ \vdots \\ {{p_{1,7}}}\\ \hdashline {{p_{2,0}}}\\ \vdots \\ {{p_{2,7}}}\\ \hdashline {{p_{3,0}}}\\ \vdots \\ {{p_{3,7}}} \end{matrix}} \right] = \left[ {\bf{0}_{(4\times2)\times1}} \right] [G(T1)4×804×8G(0)4×8G(T2)4×804×8G(0)4×8]p1,0p1,7p2,0p2,7p3,0p3,7=[0(4×2)×1]

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

​ 由前文可知,Minimum Snap下QP的等式约束是导数约束与和连续性约束的合并,得到
A e q = [ G ( 0 ) 4 × 8 0 4 × 8 0 4 × 8 0 4 × 8 0 4 × 8 G ( T 3 ) 4 × 8 G _ 0 ( T 1 ) 1 × 8 0 1 × 8 0 1 × 8 0 1 × 8 G _ 0 ( T 2 ) 1 × 8 0 1 × 8 G ( T 1 ) 4 × 8 − G ( 0 ) 4 × 8 0 4 × 8 0 4 × 8 G ( T 2 ) 4 × 8 − G ( 0 ) 4 × 8 ] [ p 1 , 0 ⋮ p 1 , 7 p 2 , 0 ⋮ p 2 , 7 p 3 , 0 ⋮ p 3 , 7 ] = [ x s t a r t v s t a r t a s t a r t j s t a r t x s t o p v s t o p a s t o p j s t o p x 1 x 2 0 ( 4 × 2 ) × 1 ] = d e q \bf A_{eq}=\left[ {\begin{matrix} {G{{(0)}_{4 \times 8}}}&{{{\bf{0}}_{4 \times 8}}}&{{{\bf{0}}_{4 \times 8}}}\\ {{{\bf{0}}_{4 \times 8}}}&{{{\bf{0}}_{4 \times 8}}}&{G{{({T_3})}_{4 \times 8}}}\\ \hdashline {G\_0{{({T_1})}_{1 \times 8}}}&{{{\bf{0}}_{1 \times 8}}}&{{{\bf{0}}_{1 \times 8}}}\\ {{{\bf{0}}_{1 \times 8}}}&{G\_0{{({T_2})}_{1 \times 8}}}&{{{\bf{0}}_{1 \times 8}}}\\ \hdashline {G{{({T_1})}_{4 \times 8}}}&{ - G{{(0)}_{4 \times 8}}}&{{{\bf{0}}_{4 \times 8}}}\\ {{{\bf{0}}_{4 \times 8}}}&{G{{({T_2})}_{4 \times 8}}}&{ - G{{(0)}_{4 \times 8}}}\\ \end{matrix}} \right]\left[ {\begin{matrix} {{p_{1,0}}}\\ \vdots \\ {{p_{1,7}}}\\ \hdashline {{p_{2,0}}}\\ \vdots \\ {{p_{2,7}}}\\ \hdashline {{p_{3,0}}}\\ \vdots \\ {{p_{3,7}}} \end{matrix}} \right] = \left[ {\begin{matrix} {{x_{start}}}\\ {{v_{start}}}\\ {{a_{start}}}\\ {{j_{start}}}\\ \hdashline {{x_{stop}}}\\ {{v_{stop}}}\\ {{a_{stop}}}\\ {{j_{stop}}}\\ \hdashline {{x_1}}\\ {{x_2}}\\ \hdashline {{{\bf{0}}_{(4 \times 2) \times 1}}} \end{matrix}} \right]=\bf d_{eq} Aeq=G(0)4×804×8G_0(T1)1×801×8G(T1)4×804×804×804×801×8G_0(T2)1×8G(0)4×8G(T2)4×804×8G(T3)4×801×801×804×8G(0)4×8p1,0p1,7p2,0p2,7p3,0p3,7=xstartvstartastartjstartxstopvstopastopjstopx1x20(4×2)×1=deq

注意:上述描述仅表示3条轨迹、2个中间点的 A e q , d e q \bf A_{eq},\bf d_{eq} Aeq,deq的求解情况。对于任意的轨迹个数,矩阵 A e q , d e q \bf A_{eq},\bf d_{eq} Aeq,deq的计算过程类似,这里给出矩阵和向量的维数情况:
对 于 M i n i m u m S n a p 问 题 , 优 化 变 量 为 ( p , v , a , j ) 4 个 , 那 么 最 高 次 幂 为 7 。 对 于 M 段 轨 迹 , 那 么 矩 阵 A e q 的 行 数 为 : 4 ( s n a p ) + 4 ( s n a p ) + ( M − 1 ) + 4 × ( M − 1 ) = 5 × M + 3 矩 阵 A e q 的 列 数 为 : ( 7 + 1 ) × M = 8 × M 向 量 d e q 的 行 数 为 : 5 × M + 3 对于Minimum Snap问题,优化变量为(p,v,a,j)4个,那么最高次幂为7。对于M段轨迹,那么\\ 矩阵{\bf A_{eq}}的行数为:4(snap)+4 (snap)+(M-1)+4 \times (M-1)=5\times M+3 \\ 矩阵{\bf A_{eq}}的列数为:(7+1)\times M=8\times M \\ 向量{\bf d_{eq}}的行数为:5\times M+3 MinimumSnap(p,v,a,j)47MAeq4snap+4snap+(M1)+4×(M1)=5×M+3Aeq(7+1)×M=8×Mdeq5×M+3

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_0t^7+p_1t^6+...+p_7 p0t7+p1t6+...+p7,这与本文多项式的表示( p 7 t 7 + p 6 t 6 + . . . + p 0 p_7t^7+p_6t^6+...+p_0 p7t7+p6t6+...+p0)不符,因此需要使用flip函数进行多项式系数翻转。

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

原图
3段轨迹
4段轨迹
效果
(四)路径规划算法---QP解决Minimum Snap轨迹优化问题
(四)路径规划算法---QP解决Minimum Snap轨迹优化问题
(四)路径规划算法---QP解决Minimum Snap轨迹优化问题

注意:上述是看大佬代码并结合算法原理所得出自己的理解,可能出现错误,欢迎批评指证。码字不易,转载请注明出处,谢谢。,文章来源地址https://www.toymoban.com/news/detail-400322.html

到了这里,关于(四)路径规划算法---QP解决Minimum Snap轨迹优化问题的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 路径规划算法:基于鸽群优化的路径规划算法- 附代码

    摘要:本文主要介绍利用智能优化算法鸽群算法来进行路径规划。 鸽群算法原理请参考:https://blog.csdn.net/u011835903/article/details/109774886 1.1 环境设定 在移动机器人的路径优化中,每个优化算法的解代表机器人的一条运动路径。优化算法会通过优化计算在众多路径中找出一条最

    2024年02月08日
    浏览(46)
  • 路径规划算法:基于和声优化的路径规划算法- 附代码

    摘要:本文主要介绍利用智能优化算法和声算法来进行路径规划。 和声算法原理请参考:https://blog.csdn.net/u011835903/article/details/118724731 1.1 环境设定 在移动机器人的路径优化中,每个优化算法的解代表机器人的一条运动路径。优化算法会通过优化计算在众多路径中找出一条最

    2024年02月08日
    浏览(46)
  • 路径规划算法:基于蝠鲼觅食优化的路径规划算法- 附代码

    摘要:本文主要介绍利用智能优化算法蝠鲼觅食算法来进行路径规划。 蝠鲼觅食算法原理请参考:https://blog.csdn.net/u011835903/article/details/112390588 1.1 环境设定 在移动机器人的路径优化中,每个优化算法的解代表机器人的一条运动路径。优化算法会通过优化计算在众多路径中找

    2024年02月07日
    浏览(46)
  • 路径规划算法:基于引力搜索优化的路径规划算法- 附代码

    摘要:本文主要介绍利用智能优化算法引力搜索算法来进行路径规划。 引力搜索算法原理请参考:https://blog.csdn.net/u011835903/article/details/108518992 1.1 环境设定 在移动机器人的路径优化中,每个优化算法的解代表机器人的一条运动路径。优化算法会通过优化计算在众多路径中找

    2024年02月06日
    浏览(45)
  • 路径规划算法:基于花授粉优化的路径规划算法- 附代码

    摘要:本文主要介绍利用智能优化算法花授粉算法来进行路径规划。 花授粉算法原理请参考:https://blog.csdn.net/u011835903/article/details/108346554 1.1 环境设定 在移动机器人的路径优化中,每个优化算法的解代表机器人的一条运动路径。优化算法会通过优化计算在众多路径中找出一

    2024年02月06日
    浏览(42)
  • 路径规划算法:基于黄金正弦优化的路径规划算法- 附代码

    摘要:本文主要介绍利用智能优化算法黄金正弦算法来进行路径规划。 黄金正弦算法原理请参考:https://blog.csdn.net/u011835903/article/details/111699194 1.1 环境设定 在移动机器人的路径优化中,每个优化算法的解代表机器人的一条运动路径。优化算法会通过优化计算在众多路径中找

    2024年02月07日
    浏览(47)
  • 路径规划算法:基于静电放电优化的路径规划算法- 附代码

    摘要:本文主要介绍利用智能优化算法静电放电算法来进行路径规划。 静电放电算法原理请参考:https://blog.csdn.net/u011835903/article/details/118755197 1.1 环境设定 在移动机器人的路径优化中,每个优化算法的解代表机器人的一条运动路径。优化算法会通过优化计算在众多路径中找

    2024年02月08日
    浏览(42)
  • 路径规划算法:基于适应度相关优化的路径规划算法- 附代码

    摘要:本文主要介绍利用智能优化算法适应度相关算法来进行路径规划。 适应度相关算法原理请参考:https://blog.csdn.net/u011835903/article/details/119946003 1.1 环境设定 在移动机器人的路径优化中,每个优化算法的解代表机器人的一条运动路径。优化算法会通过优化计算在众多路径

    2024年02月09日
    浏览(48)
  • 路径规划算法:基于哈里斯鹰优化的路径规划算法- 附代码

    摘要:本文主要介绍利用智能优化算法哈里斯鹰算法来进行路径规划。 哈里斯鹰算法原理请参考:https://blog.csdn.net/u011835903/article/details/108528147 1.1 环境设定 在移动机器人的路径优化中,每个优化算法的解代表机器人的一条运动路径。优化算法会通过优化计算在众多路径中找

    2024年02月07日
    浏览(49)
  • 路径规划算法:基于帝国主义竞争优化的路径规划算法- 附代码

    摘要:本文主要介绍利用智能优化算法帝国主义竞争算法来进行路径规划。 帝国主义竞争算法原理请参考:https://blog.csdn.net/u011835903/article/details/108517210 1.1 环境设定 在移动机器人的路径优化中,每个优化算法的解代表机器人的一条运动路径。优化算法会通过优化计算在众多

    2024年02月06日
    浏览(58)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包