深蓝路径规划作业-第六章

最新推荐文章于 2024-11-14 23:29:08 发布

后厂村路钢铁侠

最新推荐文章于 2024-11-14 23:29:08 发布

阅读量295

点赞数

CC 4.0 BY-SA版权

文章标签：算法人工智能

本文链接：https://blog.csdn.net/weixin_47765522/article/details/133799402

作业要求：

在matlab中，写一个走廊约束的分段贝塞尔曲线生成。

•给出了贝塞尔到单项多项式之间的转换。

•该走廊已被预定义。

•只需要限制自己的位置。

作业结果：

一、边界条件：

设置地图和分段数，设置五个正方形区域的走廊，每个分为八段（即是7阶多项式）

path = [50, 50;
       100, 120;
       180, 150;
       250, 80;
       280, 0];             % 飞行走廊中各个矩形区域的中心点
x_length = 100;
y_length = 100;

首尾条件：其起始坐标为[50,50],终止坐标为[280,0]。

生成飞行走廊：

%% 显示规划出的轨迹和相应的飞行走廊
plot(path(:,1), path(:,2), '*r'); hold on;
for i = 1:n_seg
    plot_rect([corridor(1,i);corridor(2,i)], corridor(3, i), corridor(4,i));hold on;
end
hold on;
x_pos = [];y_pos = [];
idx = 1;
function plot_rect(center, x_r, y_r)
    p1 = center+[-x_r;-y_r];
    p2 = center+[-x_r;y_r];
    p3 = center+[x_r;y_r];
    p4 = center+[x_r;-y_r];
    plot_line(p1,p2);
    plot_line(p2,p3);
    plot_line(p3,p4);
    plot_line(p4,p1);
end

function plot_line(p1,p2)
    a = [p1(:),p2(:)];    
    plot(a(1,:),a(2,:),'b'

给每段轨迹设置固定时间为1

%% 定义每一段的时间，这里简单的将每段的时间都设置为1
ts = zeros(n_seg, 1);
for i = 1:n_seg
    ts(i,1) = 1;
end

调用minisnap函数解出多项式系数

% 获取x和y方向的多项式系数
poly_coef_x = MinimumSnapCorridorBezierSolver(1, path(:, 1), corridor, ts, n_seg, n_order, v_max, a_max);
poly_coef_y = MinimumSnapCorridorBezierSolver(2, path(:, 2), corridor, ts, n_seg, n_order, v_max, a_max);

显示规划出的轨迹和相应的飞行走廊

plot(path(:,1), path(:,2), '*r'); hold on;
for i = 1:n_seg
    plot_rect([corridor(1,i);corridor(2,i)], corridor(3, i), corridor(4,i));hold on;
end
hold on;
x_pos = [];y_pos = [];
idx = 1;

绘制相应的贝塞尔曲线

%% #####################################################
% STEP 4: 绘制相应的贝塞尔曲线
for k = 1:n_seg
    for t = 0:0.01:1
        x_pos(idx) = 0.0;
        y_pos(idx) = 0.0;
        for i = 0:n_order
            basis_p = nchoosek(n_order, i) * t^i * (1-t)^(n_order-i);   % 此处的nchoosek对应于C_n_i
             x_pos(idx) = x_pos(idx) + poly_coef_x((k-1)*(n_order + 1) + i + 1) * basis_p * ts(k) ;
             y_pos(idx) = y_pos(idx) + poly_coef_y((k-1)*(n_order + 1) + i + 1) * basis_p * ts(k) ;
        end
        idx = idx + 1;
    end
end
scatter(ts(1)*poly_coef_x(1:8),ts(1)*poly_coef_y(1:8),100,"r");
scatter(ts(2)*poly_coef_x(9:16),ts(2)*poly_coef_y(9:16),100,"g");
scatter(ts(3)*poly_coef_x(17:24),ts(3)*poly_coef_y(17:24),100,"b");
scatter(ts(4)*poly_coef_x(25:32),ts(4)*poly_coef_y(25:32),100,"c");
scatter(ts(5)*poly_coef_x(33:40),ts(5)*poly_coef_y(33:40),100,"m");
plot(x_pos',y_pos',"Linewidth",2.0,"color","k");

% 求解贝塞尔多项式系数
function poly_coef = MinimumSnapCorridorBezierSolver(axis, waypoints, corridor, ts, n_seg, n_order, v_max, a_max)