Minimum Snap轨迹规划相关作业

最新推荐文章于 2023-03-22 16:51:33 发布

Phillweston

最新推荐文章于 2023-03-22 16:51:33 发布

阅读量525

点赞数

分类专栏： SLAM 程序设计文章标签： matlab 开发语言

本文链接：https://blog.csdn.net/qq_26550927/article/details/126267356

版权

程序设计同时被 2 个专栏收录

17 篇文章 2 订阅

订阅专栏

SLAM

2 篇文章 0 订阅

订阅专栏

Minimum Snap

二次规划（QP）问题：

对于带约束的求解器，采用Matlab中的函数：quadprog()

参考链接：

(四)路径规划算法—QP解决Minimum Snap轨迹优化问题_月夕花晨TS的博客-CSDN博客

Bezier曲线和B样条曲线差异：

参考链接：

Bezier曲线和B-spline曲线

总结：总的来说，两种曲线拥有相似的基本公式，都可以把曲线点理解为已知控制点的加权平均值。区别在于，B-spline曲线的权重和Bezier曲线的权重计算公式不同，B-spline曲线更好的提供了局部支持，是Bezier曲线的拓展版。

第五周——minimum snap作业代码解析：

clc;clear;close all;
path = ginput() * 100.0; % [x0,y0;x1,y1; ... ;xn,yn]

n_order       = 7;% order of poly
n_seg         = size(path,1)-1;% segment number
n_poly_perseg = (n_order+1); % coef number of perseg

ts = zeros(n_seg, 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_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;

% or you can simply set all time distribution as 1
% for i = 1:n_seg
%     ts(i) = 1.0;
% end

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
    %#####################################################
    % STEP 3: get the coefficients of i-th segment of both x-axis
    % and y-axis
    Pxi = flipud(poly_coef_x(1+i*(n_order+1):(i+1)*(n_order+1)));
    Pyi = flipud(poly_coef_y(1+i*(n_order+1):(i+1)*(n_order+1)));
%     Pxi = (poly_coef_x(1+i*(n_order+1):(i+1)*(n_order+1)));
%     Pyi = (poly_coef_y(1+i*(n_order+1):(i+1)*(n_order+1)));
    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));
axis equal

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);
    assignin('base','Q',Q);
    %#####################################################
    % STEP 2: compute Aeq and beq 
    [Aeq, beq] = getAbeq(n_seg, n_order, waypoints, ts, start_cond, end_cond);
    assignin('base','Aeq',Aeq);
    assignin('base','beq',beq);
    f = zeros(size(Q,1),1);
    fprintf(1,'Aeq size: %d, %d\n',size(Aeq,1),size(Aeq,2));
    fprintf(1,'beq size: %d, %d\n',size(beq,1),size(beq,2));
    fprintf(1,'Q size: %d, %d\n',size(Q,1),size(Q,2));
    poly_coef = quadprog(Q,f,[],[],Aeq, beq);
end

第一步：设置路径点，多项式的阶数

第二步：得到每段轨迹的时间，这里有两种策略：第一种策略是给定总的时长，根据路径点的距离来分配时间，第二种策略是给每段轨迹分配固定的时间。

第三步：在X和Y方向上分别调用Minimum Snap的QP求解器解出。这一步主要的是需要解出Q矩阵，再传递给quadprog函数

getQ()函数求解Q矩阵，矩阵的维数等于变量 $Q_{j}$ ，该函数内调用blkdiag()矩阵输出a.b.c.d为对角的分块矩阵。

getAbeq()函数获取QP解法中的等式约束，计算两个矩阵 $A_{eq}$ 和 $B_{eq}$ 。