1、简介

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

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

多项式的形式：

其 k 阶微分：

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

2、QP问题

waypoints数：M+1

segment数：M

每个segment多项式阶数：N

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

每个segment的时间段：

Snap阶数：K=4

1）、cost function

Minimum Snap：

定义欧式距离：

对于每一段的代价函数：

写成矩阵乘法：

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

对于所有段的代价函数：

其中：

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

2）、Derivate Constraint

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

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

实现方法：

一般约束：

对于每一段：

额外的约束：

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

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

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

其中：

3）、Continuity Constraint

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

即：

写成矩阵形式为：

总的约束矩阵为：

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

4）、典型的QP问题

综上：

合并为：

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 onpathX=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:Mfor i=1:N+1for l=1:N+1if i<K+1 || l<K+1Q_bais(l,i)=0;elseQ_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);endendQ_extend(:,:,j)=Q_bais;end
endQ=zeros((N+1)*M);% 分块对角矩阵
index=0;
for j=1:Mfor i=1:N+1for l=1:N+1Q(i+index,l+index)=Q_extend(i,l,j);endendindex=index+N+1;
endQ(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:ML=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_maxT(j)=t_slip+(L-x_max)/V_max;elseT(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=0Aeq_bais_Tj(1)=path(j+1); %尾点0阶  t=TjAeq=[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==1for m=0:K-1      % m阶连续/相等  kfor n=m:N    % n阶对应的系数 iA_j_0(m+1,n+1)=factorial(n)/factorial(n-m)*T_begin^(n-m);endendelsem=0;for n=m:N    % n阶对应的系数 iA_j_0(m+1,n+1)=factorial(n)/factorial(n-m)*T_begin^(n-m);endend% j段的尾if j==Mfor m=0:K-1      % m阶连续/相等  kfor n=m:N    % n阶对应的系数 iA_j_Tj(m+1,n+1)=factorial(n)/factorial(n-m)*T_end^(n-m);endendelsem=0;for n=m:N    % n阶对应的系数 iA_j_Tj(m+1,n+1)=factorial(n)/factorial(n-m)*T_end^(n-m);endend% A=diag([Aj])for m=1:Kfor n=1:N+1A(m+m_index,n+n_index)=A_j_0(m,n);A(m+m_index+K,n+n_index)=A_j_Tj(m,n);endendm_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阶连续/相等  kfor n=m:N    % n阶对应的系数 iA_j_Tj(m+1,n+1)=factorial(n)/factorial(n-m)*T_end^(n-m);endend% j+1段的首for m=0:K-1      % m阶连续/相等  kfor n=m:N    % n阶对应的系数 iA_j1_0(m+1,n+1)=factorial(n)/factorial(n-m)*T_begin^(n-m);endendfor m=1:Kfor n=1:N+1A(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);endendm_index=m_index+K;n_index=n_index+N+1;
endB=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:MqXj=qX(:,j);qYj=qY(:,j);Tj=T(j);index=0;for t=0:0.001:Tjindex=index+1;time(index)=t;t_exp=zeros(N+1,1);for n=0:Nt_exp(n+1)=t^(n);endx(index)=qXj'*t_exp;y(index)=qYj'*t_exp;endplot(x,y,"r.")hold on
end
hold off

8、运行结果：

Minimun Snap的matlab实现（含代码）相关推荐

[图像]Canny检测的Matlab实现(含代码)
原创文章,欢迎转载.转载请注明:转载自祥的博客原文链接:http://blog.csdn.net/humanking7/article/details/46606791 文章目录 @[toc] C ...
分布时滞matlab,Matlab仿真含时滞多智体一致性分析，附代码
Matlab仿真含时滞多智体一致性分析,附代码 Matlab仿真含时滞多智体一致性分析,附代码 Matlab仿真含时滞多智体一致性分析,附代码系统结构如下图所示: clear; clc; % 201 ...
LL1分析构造法_数学建模算法--最优赋权法（含代码）
数学建模算法--最优赋权法(含代码) 作者:郑铿城本次介绍数学建模和科研写作的方法--最优赋权法最优赋权法经常用于分析评价类问题,从该算法的名称就可以看到,该算法首先要体现"最优" ...
基于matlab的64QAM,通信调制体制设计之64QAM性能分析MATLAB仿真及代码
通信调制体制设计之64QAM性能分析MATLAB仿真及代码通信调制体制设计之64QAM性能分析MATLAB仿真及代码任务背景弗雷泽岛旅游经理在审查您之前建立无线链路任务的解决方案时,正在研究使用 ...
基于小波变换的图像边缘检测（matlab祖传代码注释）
基于小波变换的图像边缘提取应用展示上图为针对png格式无背景原图的边缘检测,对比各种边缘检测算子,小波变化的优势体现并不明显. 上图为针对含背景图片的边缘检测,小波变化的优势这里体现的比较明显. m ...
2020研究生数学建模E题--AlexNet深度网络解法（大雾能见度估计与预测）（含代码）
2020研究生数学建模E题--AlexNet深度网络解法(大雾能见度估计与预测)(含代码) 一.E题赛题二.赛题分析三.解题 3.1 数据预处理 3.2 AlexNet深度网络 (1)AlexNe ...
数学建模中的ARMA模型和ARIMA模型的使用实例（含代码）
数学建模中的ARMA模型和ARIMA模型的使用实例(含代码) 原文地址:http://blog.csdn.net/qq_34861102/article/details/77659399 对于较少时间 ...
聚类算法（五）——层次聚类 linkage （含代码）
聚类算法相关: 聚类算法(一)--DBSCAN 聚类算法(二)-- 优缺点对比聚类算法(三)-- 评测方法1 聚类算法(三)-- 评测方法2 聚类算法(三)-- 评测方法3(代码) 聚类算法(四)- ...
【DSP教程】第36章 FIR滤波器的Matlab设计(含低通，高通，带通和带阻)
完整版教程下载地址:http://www.armbbs.cn/forum.php?mod=viewthread&tid=94547 第36章 FIR滤波器的Matlab设计(含低通 ...

Minimun Snap的matlab实现（含代码）