Minimun Snap的matlab实现（含代码）

最新推荐文章于 2024-03-31 11:15:43 发布

itiszzh

最新推荐文章于 2024-03-31 11:15:43 发布

阅读量1.5k

点赞数 7

文章标签： numpy 算法 matlab

本文链接：https://blog.csdn.net/xiazhouzhou/article/details/126414262

版权

1、简介

使用N次多项式连接（M+1）个点完成轨迹生成。

Minimum Jerk：N=5 ；Minimum Snap：N=7

多项式的形式：

$f(t)_j=\sum_i p_{j,i}t^i$

其 k 阶微分：

$f(t)_j^{(k)}=\sum_{i\ge k}\frac{i!}{(i-k)!} p_{j,i}t^{(i-k)}$

构建起的所有cost function和约束都和上面两个密切相关。

2、QP问题

waypoints数：M+1

segment数：M

每个segment多项式阶数：N

每个segment多项式未知数个数：N+1

每个segment的时间段： $t\in[0,T_j]$

Snap阶数：K=4

1）、cost function

Minimum Snap：

定义欧式距离：

$f(t)_j^{(4)}=\sum_{i\ge 4}\frac{i!}{(i-k)!} p_{j,i}t^i=\sum_{i\ge 4}i(i-1)(i-2)(i-3)p_{j,i}t^{(i-4)}$

对于每一段的代价函数：

$J(T_j)_j=\int_0^{T_j} [f(t)_j^{(4)}]^2 dt=\sum_{i\ge4,l\ge4}\frac{i(i-1)(i-2)(i-3)l(l-1)(l-2)(l-3)}{i+l-7}T_j^{i+l-7}p_{j,i},p_{j,l}$

写成矩阵乘法：

$J(T_j)_j=P_j^TQ_jPj$

其中：Pj为size(N+1,1) Qj为size(N+1,N+1)

对于所有段的代价函数：

$J(T)=P^TQP$

其中：

$P=[P1;...P_j;....;P_M]$

$Q=diag([Q_1;...Q_j;...Q_M])$

注: 分块矩阵matlab命令为：blkdiag

2）、Derivate Constraint

包括：整个轨迹首、末两点的 0阶到 k-1阶微分必须等于给定的值和

每段轨迹首、末两点的 0 阶微分必须等于waypoints的值。（0阶即为位置）

实现方法：

一般约束：

对于每一段：

$f(0)_j=x_{j,0},f(T_j)_j=x_{j,T_j}$

额外的约束：

对于第一段：0时刻满足当前点的K阶微分约束

$f(0)_1^{(k)}=x_{1,0}^{(k)},k=0,1,...K-1$

对于最后一段：T时刻满足当前点的K阶微分约束

$f(T_M)_M^{(k)}=x_{M,T_M}^{(k)},k=0,1,...K-1$

上述两个约束很容易合写为：

$A_jP_j=D_j$

$AP=D$

其中：

$A=diag([A_1,...A_j,...A_M]), D=[D_1;...D_M]$

3）、Continuity Constraint

中间点（即每段的连接点）需要满足 K-1 阶微分的连续

$f(T_j)_j^{(k)}=f(0)_{j+1}^{(k)},k=0,1,...K-1$

即：

$f(T_j)_j^{(k)}-f(0)_{j+1}^{(k)}=0,k=0,1,...K-1$

写成矩阵形式为：

$[A_j(T_j) , -A_{j+1}(0)][P_j ;P_{j+1}]=0$

总的约束矩阵为：

$BP=0$

其中：B为上三角阶梯矩阵：其形式如下：

$B=\begin{bmatrix} - & - & 0 & 0&0 \\ 0 & - & - & 0&0 \\ 0& 0 & -& -& 0\\ 0&0 &0 & - & -\\ \end{bmatrix}$

4）、典型的QP问题

综上：

$min J(T)=P^TQP$

$AP=D$

$BP=0$

合并为：

$\min _p ~J(T)=P^TQP$

$s.t.~A*P=Aeq=[D;0]$

matlab 解决QP问题：help quadprog

即可求解。

3、MATLAB代码

框架如下：运行MiniSnap即可，ginput鼠标左键选取数个点后回车即可运行。

1、MiniSnap.m主函数

clear;close all;clc;

path=ginput()*100;
close all;

figure;
plot(path(:,1),path(:,2),"r*");
hold on
plot(path(:,1),path(:,2),"b");
axis([0 100  0 100]);
hold on

pathX=path(:,1);
pathY=path(:,2);

M=length(path)-1;   % m+1个waypoints m段
N=7;                %7阶多项式
K=4;                %minimum snap的阶数


P=M*(N+1)   %未知数个数
N_derivate=M*K*2    %微分约束方程个数：行数
N_continuy=(M-1)*K  %连续约束方程个数：行数



T=getT(path,M);  %算总时间
T_factor=max(T);
T=T/T_factor;


qX=com_q(N,K,T,M,pathX);
qY=com_q(N,K,T,M,pathY);

plot_path(qX,qY,M,T,N)

2、getQ.m 获取Q矩阵

function Q=getQ(N,K,T,M)

% T=size(M,1)
% blkdiag 分块对角矩阵

Q_bais=zeros(N+1);
for j=1:M
    for i=1:N+1
        for l=1:N+1
            if i<K+1 || l<K+1
                Q_bais(l,i)=0;
            else
                Q_bais(l,i)=i*(i-1)*(i-2)*(i-3)*l*(l-1)*(l-2)*(l-3)/(i+l-7)*T(j)^(i+l-7);
            end
        end
        Q_extend(:,:,j)=Q_bais;
    end
end

Q=zeros((N+1)*M);

% 分块对角矩阵
index=0;
for j=1:M
    for i=1:N+1
        for l=1:N+1
              Q(i+index,l+index)=Q_extend(i,l,j);
        end
    end
    index=index+N+1;
end

Q(isnan(Q))=0;

3、getT.m 获取T向量

使用梯形速度获取大致的每段时间。

function T=getT(path,M)

% 梯形加速

T=zeros(M,1);

V_max=10;
a_max=10;
x_max=a_max*(V_max/a_max)^2;
t_slip=(V_max/a_max)*2;

for j=1:M
    L=sqrt((path(j,1)-path(j+1,1))^2+(path(j,2)-path(j+1,2))^2);  %sqrt(dx^2+dy^2)
    if L>=x_max
        T(j)=t_slip+(L-x_max)/V_max;
    else
        T(j)=sqrt(L/a_max);
    end
end

4、getAeq.m

得到Derivate Constraint矩阵

function [A,Aeq]=getAeq(path,K,N,M,T)

% waypoints 的 [1-th ... (k-1)-th] 连续

% 0的阶乘为1

% B的维度为 (m-1)*m
% j的尾和j+1的首

A_j_0=zeros(K,N+1);   % K阶最小snap，则 0th-(K-1)th总共K个已知或连续
A_j_Tj=zeros(K,N+1);  % K个数

Aeq=[ ];
A=[ ];

m_index=0;  %行
n_index=0;  %列

A=zeros(K*M,(N+1)*M);  % A的维度

for j=1:M

    %初始化
    Aeq_bais_0=zeros(K,1);   %up ：segment的首
    Aeq_bais_Tj=zeros(K,1); %down ：segment的尾
    

    Aeq_bais_0(1)=path(j);     %首点0阶  t=0
    Aeq_bais_Tj(1)=path(j+1); %尾点0阶  t=Tj
    Aeq=[Aeq;Aeq_bais_0;Aeq_bais_Tj];

    T_end=T(j);
    T_begin=0;

    %初始化
    A_j_0=zeros(K,N+1);
    A_j_Tj=zeros(K,N+1);
    % j段的首
    if j==1
        for m=0:K-1      % m阶连续/相等  k
            for n=m:N    % n阶对应的系数 i
                A_j_0(m+1,n+1)=factorial(n)/factorial(n-m)*T_begin^(n-m);
            end
        end
    else
        m=0;
        for n=m:N    % n阶对应的系数 i
            A_j_0(m+1,n+1)=factorial(n)/factorial(n-m)*T_begin^(n-m);
        end
    end

    % j段的尾
    if j==M
        for m=0:K-1      % m阶连续/相等  k
            for n=m:N    % n阶对应的系数 i
                A_j_Tj(m+1,n+1)=factorial(n)/factorial(n-m)*T_end^(n-m);
            end
        end
    else
        m=0;
        for n=m:N    % n阶对应的系数 i
            A_j_Tj(m+1,n+1)=factorial(n)/factorial(n-m)*T_end^(n-m);
        end
    end

    
% A=diag([Aj])
    for m=1:K
        for n=1:N+1
            A(m+m_index,n+n_index)=A_j_0(m,n);
            A(m+m_index+K,n+n_index)=A_j_Tj(m,n);
        end
    end
    m_index=m_index+2*K;
    n_index=n_index+N+1;
end

5、getBeq.m

Continuity Constraint矩阵

function [B,Beq]=getBeq(K,N,M,T)

% waypoints 的 [1-th ... (k-1)-th] 连续

% 0的阶乘为1

% B的维度为 (m-1)*m
% j的尾和j+1的首

A_j1_0=zeros(K,N+1);   % K阶最小snap，则 0th-(K-1)th总共K个已知或连续
A_j_Tj=zeros(K,N+1);  % K个数


Aeq=[ ];
A=[ ];

m_index=0;  %行
n_index=0;  %列

A=zeros(K*(M-1),(N+1)*M);  % A的维度

for j=1:M-1
    
    %初始化
    Aeq_bais_0=zeros(K,1);   %up ：segment的首
    Aeq=[Aeq;Aeq_bais_0];

    T_end=T(j);
    T_begin=0;

    %初始化
    A_j1_0=zeros(K,N+1);
    A_j_Tj=zeros(K,N+1);

    % j段的尾
    for m=0:K-1      % m阶连续/相等  k
        for n=m:N    % n阶对应的系数 i
            A_j_Tj(m+1,n+1)=factorial(n)/factorial(n-m)*T_end^(n-m);
        end
    end

    % j+1段的首
    for m=0:K-1      % m阶连续/相等  k
        for n=m:N    % n阶对应的系数 i
            A_j1_0(m+1,n+1)=factorial(n)/factorial(n-m)*T_begin^(n-m);
        end
    end
    
    for m=1:K
        for n=1:N+1
            A(m+m_index,n+n_index)=A_j_Tj(m,n);
            A(m+m_index,n+n_index+N+1)=-A_j1_0(m,n);
        end
    end
    m_index=m_index+K;
    n_index=n_index+N+1;
end

B=A;
Beq=Aeq;

6、com_q.m

x，y两个维度独立求解各自的qp问题

function q=com_q(N,K,T,M,pathX)


Q=getQ(N,K,T,M);

[A,Aeq]=getAeq(pathX,K,N,M,T);

[B,Beq]=getBeq(K,N,M,T);

A_total=[A;B];
Deq_total=[Aeq;Beq];

q=quadprog(Q,[],[],[],A_total,Deq_total);

7、plot_path.m

画图函数

function plot_path(qX,qY,M,T,N)

Roll=N+1;
qX=reshape(qX,Roll,M);   %每一列代表一段的qX
qY=reshape(qY,Roll,M);   %每一列代表一段的qX

%figure
hold on
for j=1:M
    qXj=qX(:,j);
    qYj=qY(:,j);
    Tj=T(j);

    index=0;
    for t=0:0.001:Tj
        index=index+1;
        time(index)=t;

        t_exp=zeros(N+1,1);
        for n=0:N
            t_exp(n+1)=t^(n);
        end
         x(index)=qXj'*t_exp;
         y(index)=qYj'*t_exp;
    end
    plot(x,y,"r.")
    hold on
end
hold off