Minimum snap matlab 代码(一)- 用优化solver求解参数

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

具体看Minimum Snap轨迹规划详解(一)

  • 最高阶7阶,共8个参数
    p ( t ) = p 0 + p 1 t + p 2 t 2 . . . + p n t n = ∑ i = 0 n = 7 p i t i v ( t ) = p ′ ( t ) = ∑ i ⩾ 1 n = 7 i ⋅ p i t i − 1 a ( t ) = p ′ ′ ( t ) = ∑ i ⩾ 2 n = 7 i ! ( i − 2 ) ! ⋅ p i t i − 2 j e r k ( t ) = p ( 3 ) ( t ) = ∑ i ⩾ 3 n = 7 i ! ( i − 3 ) ! ⋅ p i t i − 3 s n a p ( t ) = p ( 4 ) ( t ) = ∑ i ⩾ 4 n = 7 i ! ( i − 4 ) ! ⋅ p i t i − 4 \begin{aligned} p(t)&=p_0+p_1t+p_2t^2...+p_nt^n=\sum_{i=0}^{n=7}p_it^i\\ v(t) &= p^\prime(t) = \sum_{i\geqslant1}^{n=7}i\cdot p_it^{i-1}\\ a(t) &= p^{\prime \prime}(t) =\sum_{i\geqslant2}^{n=7}\frac{i!}{(i-2)!}\cdot p_it^{i-2} \\ jerk(t) &= p^{(3)}(t) =\sum_{i\geqslant3}^{n=7}\frac{i!}{(i-3)!}\cdot p_it^{i-3} \\ snap(t) &= p^{(4)}(t) =\sum_{i\geqslant4}^{n=7}\frac{i!}{(i-4)!}\cdot p_it^{i-4} \end{aligned} p(t)v(t)a(t)jerk(t)snap(t)=p0+p1t+p2t2...+pntn=i=0n=7piti=p(t)=i1n=7ipiti1=p(t)=i2n=7(i2)!i!piti2=p(3)(t)=i3n=7(i3)!i!piti3=p(4)(t)=i4n=7(i4)!i!piti4
  • 目标函数为 J = min ⁡ [ p 1 p 2 ⋮ p k ] T [ Q 1 Q 2 ⋱ Q k ] [ p 1 p 2 ⋮ p k ] J=\min \begin{bmatrix}p_1\\p_2\\\vdots\\p_k\end{bmatrix}^T \begin{bmatrix} Q_1 &&& \\ &Q_2 && \\ && \ddots &\\ &&& Q_k\end{bmatrix}\begin{bmatrix}p_1\\p_2\\\vdots\\p_k\end{bmatrix} J=minp1p2pkTQ1Q2Qkp1p2pk
  • 等式约束 A e q [ p 1 p 2 ⋮ p k ] = d e q A_{eq}\begin{bmatrix}p_1\\p_2\\\vdots\\p_k\end{bmatrix}=d_{eq} Aeqp1p2pk=deq

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矩阵

代价函数为
J = min ⁡ ∫ 0 T ( p ( 4 ) ( t ) ) 2 d t = min ⁡ ∑ i = 1 k ∫ t i − 1 t i ( p ( 4 ) ( t ) ) 2 d t = min ⁡ ∑ i = 1 k p i T Q i p i = min ⁡ p T Q p \begin{aligned} J &=\min \int^T_0(p^{(4)}(t))^2dt\\ &=\min\sum_{i=1}^k \int^{t_i}_{t_{i−1}}(p^{(4)}(t))^2dt \\ &= \min\sum_{i=1}^kp_i^TQ_ip_i \\ &=\min p^TQp \end{aligned} J=min0T(p(4)(t))2dt=mini=1kti1ti(p(4)(t))2dt=mini=1kpiTQipi=minpTQp

  • 其中 Q i Q_i Qi [ 0 4 × 4 0 4 × ( n − 3 ) 0 ( n − 3 ) × 4 r ! ( r − 4 ) ! c ! ( c − 4 ) ! 1 ( r − 4 ) + ( c − 4 ) + 1 ( t i ( r + c − 7 ) − t i − 1 ( r + c − 7 ) ) ] ( n + 1 ) × ( n + 1 ) \begin{bmatrix}0_{4\times4}&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^{(r+c−7)}_i−t^{(r+c−7)}_{i−1})\end{bmatrix}_{(n+1)\times(n+1)} [04×40(n3)×404×(n3)(r4)!r!(c4)!c!(r4)+(c4)+11(ti(r+c7)ti1(r+c7))](n+1)×(n+1)
  • Q Q Q [ Q 1 Q 2 ⋱ Q k ] k ( n + 1 ) × k ( n + 1 ) \begin{bmatrix} Q_1 &&& \\ &Q_2 && \\ && \ddots &\\ &&& Q_k\end{bmatrix}_{k(n+1)\times k(n+1)} Q1Q2Qkk(n+1)×k(n+1)
  • p i p_i pi [ p i , 0 , p i , 1 , ⋯   , p i , 7 ] T [p_{i,0},p_{i,1},\cdots,p_{i,7}]^T [pi,0,pi,1,,pi,7]T共7个参数,列向量
  • p p p [ p 1 p 2 ⋮ p k ] k ( n + 1 ) × 1 \begin{bmatrix}p_1\\p_2\\\vdots\\p_k\end{bmatrix}_{k(n+1)\times 1} p1p2pkk(n+1)×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
    p 1 ( 0 ) = ∑ i = 0 n = 7 p 1 , i ∗ 0 i v 1 ( 0 ) = p 1 ′ ( 0 ) = ∑ i ⩾ 1 n = 7 i ⋅ p 1 , i ∗ 0 i − 1 a 1 ( 0 ) = p 1 ′ ′ ( 0 ) = ∑ i ⩾ 2 n = 7 i ! ( i − 2 ) ! ⋅ p 1 , i ∗ 0 i − 2 j e r k 1 ( 0 ) = p 1 ( 3 ) ( 0 ) = ∑ i ⩾ 3 n = 7 i ! ( i − 3 ) ! ⋅ p 1 , i ∗ 0 i − 3 \begin{aligned} p_1(0)&=\sum_{i=0}^{n=7}p_{1,i}*0^i\\ v_1(0) &= p_1^\prime(0) = \sum_{i\geqslant1}^{n=7}i\cdot p_{1,i}*0^{i-1}\\ a_1(0) &= p_1^{\prime \prime}(0) =\sum_{i\geqslant2}^{n=7}\frac{i!}{(i-2)!}\cdot p_{1,i}*0^{i-2} \\ jerk_1(0) &= p_1^{(3)}(0) =\sum_{i\geqslant3}^{n=7}\frac{i!}{(i-3)!}\cdot p_{1,i}*0^{i-3} \end{aligned} p1(0)v1(0)a1(0)jerk1(0)=i=0n=7p1,i0i=p1(0)=i1n=7ip1,i0i1=p1(0)=i2n=7(i2)!i!p1,i0i2=p1(3)(0)=i3n=7(i3)!i!p1,i0i3
  • 终点位置、速度、加速度、jerk限制,时间为T
    p k ( T ) = ∑ i = 0 n = 7 p k , i T i v k ( T ) = p k ′ ( T ) = ∑ i ⩾ 1 n = 7 i ⋅ p k , i T i − 1 a k ( T ) = p k ′ ′ ( T ) = ∑ i ⩾ 2 n = 7 i ! ( i − 2 ) ! ⋅ p k , i T i − 2 j e r k k ( T ) = p k ( 3 ) ( T ) = ∑ i ⩾ 3 n = 7 i ! ( i − 3 ) ! ⋅ p k , i T i − 3 \begin{aligned} p_k(T)&=\sum_{i=0}^{n=7}p_{k,i}T^i\\ v_k(T) &= p_k^\prime(T) = \sum_{i\geqslant1}^{n=7}i\cdot p_{k,i}T^{i-1}\\ a_k(T) &= p_k^{\prime \prime}(T) =\sum_{i\geqslant2}^{n=7}\frac{i!}{(i-2)!}\cdot p_{k,i}T^{i-2} \\ jerk_k(T) &= p_k^{(3)}(T) =\sum_{i\geqslant3}^{n=7}\frac{i!}{(i-3)!}\cdot p_{k,i}T^{i-3} \end{aligned} pk(T)vk(T)ak(T)jerkk(T)=i=0n=7pk,iTi=pk(T)=i1n=7ipk,iTi1=pk(T)=i2n=7(i2)!i!pk,iTi2=pk(3)(T)=i3n=7(i3)!i!pk,iTi3
  • 前后段连接点的位置限制
  • 前后段连接点高阶项连续,第 j j j段, m m m阶,在 T j T_j Tj处的极限值,等于第 j + 1 j+1 j+1段, m m m阶,在 j + 1 j+1 j+1段初始时时间0处的极限值
    p j ( m ) ( T j ) − p j + 1 ( m ) ( 0 ) = 0 p_{j}^{(m)}(T_j) - p_{j+1}^{(m)}(0) =0 pj(m)(Tj)pj+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
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值