minimum snap轨迹规划

本文代码以及其他概念可参考
https://blog.csdn.net/q597967420/article/details/76099491
本文仅对该博文程序部分做进一步解释

定义路径点、阶次

轨迹一般用n阶多项式(polynomial)来表示，即
p(t)=p0+p1∗t+p2∗t2……+pn∗tn=∑i=0npi∗tip0+p1*t+p2*t^2……+pn*t^n=\sum_{i=0}^{n}pi*t^ip0+p1∗t+p2∗t2……+pn∗tn=∑i=0npi∗ti
但整段轨迹通常难以用一个多项式表示，所以一般将其分为k段多项式表示。

机器人的平面运动路径也应该由x和y两个多项式构成，所以共有2k段多项式。
已知条件：起始点和终点的位置p、速度v、加速度a、加加速度j，每段多项式在连接点处的位置p已知，且在连接点处光滑（pvaj相等）。每一个分段都是多项式；每个分段的多项式都是相同的阶次，这样对于问题的求解比较简单；每一段的时间间隔都是已知的

限制条件数量: 4+4+（k-1)=k+7
首末pvaj加中间点位置
未知数数量：（N+1）*k (N为多项式阶数，k为路径的段数）
k+7<=（N+1）*k
则N>=7/k7/k7/k 表明段数越多，则提供的阶次越低。k最少是1，所以minisnap求解中每段多项式至少有7阶，每段有八个未知数。

clc;clear;close all;
path = ginput() * 100.0;    %  返回几个点的（x,y)坐标
n_order       = 7;% 多项式的阶次 mini_snap为7 ，mini_jerk为5
n_seg         = size(path,1)-1;  % n_seg 代表路径的段数
n_poly_perseg = (n_order+1); % 多项式方程的未知量个数

时间分配

时间分配一般有两种，一种是按路径长度分配时间，一种是按梯形运动方式分配时间，此处为图简便可直接将每段时间赋值1。


% calculate time distribution in proportion to distance between 2 points
dist     = zeros(n_seg, 1);
dist_sum = 0;
T        = 25;
t_sum    = 0;for i = 1:n_segdist(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-1ts(i) = dist(i)/dist_sum*T;  %前面n-1段路程都是按距离/总距离来计算时间t_sum = t_sum+ts(i);
end
ts(n_seg) = T - t_sum;  %而最后一段是总的时间减去前面所有时间之和% or you can simply set all time distribution as 1
% ts=ones(5,1);

求解多项式系数


%求解x轴和y轴的多项式系数
poly_coef_x = MinimumSnapQPSolver(path(:, 1), ts, n_seg, n_order);
poly_coef_y = MinimumSnapQPSolver(path(:, 2), ts, n_seg, n_order);function poly_coef = MinimumSnapQPSolver(waypoints, ts, n_seg, n_order)start_cond = [waypoints(1), 0, 0, 0];  %waypoints为 path(:, 1)即第一个点，而先对x轴求多项式系数，则waypoints(1)只提取第一个点的x轴坐标end_cond   = [waypoints(end), 0, 0, 0];% STEP 1: compute Q of p'QpQ = 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); %poly_coef 为n段分轨迹多项式式子的系数的组合，长度为n_seg*8，每段8个系数
end

下面两图源自高飞老师PPT及上文参考博文中摘录

function Q = getQ(n_seg, n_order, ts)Q = [];for j = 1:n_seg  %一共有n_seg段Q_k = zeros(8,8); %minsnap阶数是7，系数为8% STEP 1.1: calculate Q_k of the k-th segment for i=4:n_orderfor l=4:n_orderL = factorial(l)/factorial(l-4);I = factorial(i)/factorial(i-4);             Q_k(i+1,l+1) = L*I/(i+l-7);endendQ = blkdiag(Q, Q_k);end
end

下为min∑i=1kpTQipmin\sum_{i=1}^{k}p^TQipmin∑i=1kpTQip的等式约束，即1.起始点和终点的位置p、速度v、加速度a、加加速度j 已知。
2.每段多项式在连接点处的位置p已知。
3.在连接点处前后两段多项式pvaj相等

这里两个式子描述的是段与段之间的连接点，这个连接点在前后两段多项式中应该p v a j数值一样。两个式子时间Tj相同，但是却一个乘以系数pj,i，一个乘以系数pj+1,i ,在前一个多项式中，此连接点处于末尾点，时间为ts,而后一个多项式中，连接点处于起始点，时间为0。注意！不同的轨迹段享有不一样的多项式方程系数，所以时间计算可以不共享，每一段的起始点都可以视为这一段的时间0,这样计算每一段的p,v,a,j时，就会发现系数为阶乘。

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, Aeq_start = zeros(4, n_all_poly);beq_start = zeros(4, 1);% STEP 2.1: write expression of Aeq_start and beq_startfor k= 0:3Aeq_start(k+1,k+1) = factorial(k);%求4个阶次的情况，起始点时间t为0，那么位置就是p0，速度就是p1,加速度就是2p2，多项式系数是我二次规划要求的未知数，%所以，此处起始点每个阶次，要求的p前的常数项系数为n的阶乘endbeq_start = start_cond'; % p,v,a constraint in endAeq_end = zeros(4, n_all_poly);beq_end = zeros(4, 1);% STEP 2.2: write expression of Aeq_end and beq_endstart_idx_2 = (n_order + 1)*(n_seg - 1);for k=0 : 3for i=k : 7   %总共0-3 4个阶次，k到7意思是八个系数求导之后，前面几个系数都消失了，Aeq_end(k+1,start_idx_2 + 1 + i ) = factorial(i)/factorial(i-k)*ts(n_seg)^(i-k);endendbeq_end = end_cond';% position constrain in all middle waypointsAeq_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_wpfor i=0:n_seg-2  %一共n_seg-1个中间点start_idx_2 = (n_order + 1)*(i+1);  %前面几段有多少个系数Aeq_wp(i+1,start_idx_2+1) = 1;%beq_wp(i+1,1) = waypoints(i+2);%右端为 本段分轨迹的末点的坐标（已知量）end% position continuity constrain between each 2% segments，连接点处前后两段在此点的位置、速度、加速度、加加速度相等Aeq_con = zeros((n_seg-1)*4, n_all_poly);beq_con = zeros((n_seg-1)*4, 1);% STEP 2.4: write expression of Aeq_con_p and beq_con_pfor k=0:3for j=0:n_seg-2   %n_seg-1个中间点for i = k:7   %循环顺序不重要，只是遍历 n-1个中间点，的4个阶次pvaj，以及使i>=7的情况下遍历8次start_idx_1 = (n_seg-1)*k;start_idx_2 = (n_order+1)*j;Aeq_con(start_idx_1 + j + 1,start_idx_2 + i+1)=...factorial(i)/factorial(i-k)*ts(j+1)^(i-k);   %代表同一个连接点在前一段的多项式方程的末尾点pvaj（求k阶导），此时时间在前一段中为tsif(i == k)   %因为后一段的起始点时间为0，所以求导后，除了i==k时，其他部分都为0，所以不用算Aeq_con(start_idx_1+j+1,start_idx_2+(n_order+1)+i+1) = ...-factorial(i);%代表同一个连接点在后一段的多项式方程中的计算,此时时间在后一段中为0end  %两者相减等于0                                        % %因为在后一个连接点的多项式中，t为0，所以pi前的系数只有阶乘endendendAeq = [Aeq_start; Aeq_end; Aeq_wp; Aeq_con];beq = [beq_start; beq_end; beq_wp; beq_con];
end

绘制minimum snap的路径

% display the trajectory
X_n = [];
Y_n = [];
k = 1;
tstep = 0.01;
for i=0:n_seg-1% STEP 3: get the coefficients of i-th segment of both x-axis% and y-axisstart_idx = n_poly_perseg * i;Pxi = poly_coef_x(start_idx + 1 : start_idx + n_poly_perseg,1);Pxi = flipud(Pxi);%得到多项式系数Pyi = poly_coef_y(start_idx + 1 : start_idx + n_poly_perseg,1);Pyi = flipud(Pyi);   %多项式从阶数高的开头for t = 0:tstep:ts(i+1)  %以0.01的步长遍历所有时间   （ts(i+1)就是时间结尾）X_n(k)  = polyval(Pxi, t); %计算多项式Pxi在t点处的每一个值Y_n(k)  = polyval(Pyi, t);k = k + 1;end
endplot(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));

移动机器人路径规划minimum_snap（MATLAB)笔记整理相关推荐

【路径规划】基于D*算法的移动机器人路径规划（Matlab代码实现）
基于遗传算法的移动机器人路径规划
之前在网上找基于遗传算法的移动机器人路径规划资料的时候,没有找到理想的开源代码.最后参照论文,用matlab写了代码.最近开了公众号--Joe学习笔记,会不定期更新一些文章,主要是自己平时学到的知 ...
3 移动机器人路径规划（5- DWA路径规划算法）
3 移动机器人路径规划 5.1 DWA路径规划基本原理 5.2 DWA路径规划流程 5.3 栅格地图上绘制XY图像 5.3.1 栅格地图和XY坐标系关系 5.3.2 栅格行列位置转坐标系函数sub2c ...
【路径规划】基于A星算法结合floyd和动态窗口法实现机器人栅格地图路径规划附matlab代码
1 简介针对移动机器人在静态环境中的特点,为了提高路径规划效率和精度,设计了A~*算法与Floyd算法结合的路径规划.我们根据实际环境,在栅格地图的基础上,利用A~*算法进行初步路径规划,找到了一条 ...
rrt算法流程图_RRT算法移动机器人路径规划(快速扩展随机树).pdf
( ) 第 34 卷第 5期南京理工大学学报自然科学版 Vo l. 34 No. 5 20 10年 10 月 Journal of N anj ing Un iversity of Scienc ...
移动机器人路径规划算法及思考——A*算法
A*算法原理 A算法是一种经典的路径搜索算法,A算法的原理初学者可以去网上搜索算法原理详解,讲得很好链接:http://www.gamedev.net/reference/articles/arti ...
面向多任务的仓储移动机器人路径规划与调度
文章着重对移动机器人路径规划和多机器人调度问题展开研究. 1.针对移动机器人路径规划问题,在蚁群算法基础上做出了巨大改进,设计了基于独狼蚁群混合算法的路径规划,算法在路径选择方向.信息素控制和路径停滞 ...
基于人工势场法的二维平面内无人机的路径规划的matlab仿真,并通过对势场法改进避免了无人机陷入极值的问题
目录 1.算法描述 2.matlab算法仿真效果 3.MATLAB核心程序 4.完整MATLAB 1.算法描述人工势场法原理是:首先构建一个人工虚拟势场,该势场由两部分组成,一部分是目标点对移动机器 ...
基于人工势场法和果蝇优化算法的路径规划（Matlab代码实现）
目录 1 概述 2 运行结果 2.1 算例1 2.2 算例2 3 Matlab代码实现 4 参考文献 1 概述近年来,智能机器人逐渐应用于医疗服务﹑航空等众多领域.路径规划作为机器人实现智能自 ...
基于粒子群优化算法的最优机器人路径规划（Matlab代码实现）
目录 1 概述 2 运行结果 3 Matlab代码实现 1 概述近年来,智能机器人逐渐应用于医疗.服务.航空等众多领域.路径规划作为机器人实现智能自主规划的关键技术,受到众多学者的广泛关注2'.其具 ...

移动机器人路径规划minimum_snap（MATLAB)笔记整理