五次多项式过渡的直线轨迹(Linear Trajectory with Polynomial Blends)

其他 2020-10-30 11:25:35 阅读次数: 0

文章目录

一、 经过所有中间点(MATLAB代码)

clc;
clear;
close all;

%{
    
    
syms ts te ps pe vs ve as ae T real;
a = [1, 0, 0, 0, 0, 0
    0, 1, 0, 0, 0, 0
    0, 0, 2, 0, 0, 0
    1, T, T^2, T^3, T^4, T^5
    0, 1, 2*T, 3*T^2, 4*T^3, 5*T^4
    0, 0, 2, 6*T, 12*T^2, 20*T^3
    ] \ [ps; vs; as; pe; ve; ae];
a = [simplify(a(1)), simplify(a(2)), simplify(a(3)), simplify(a(4)), simplify(a(5)), simplify(a(6))]'
%}

t = [0, 2, 4, 8, 10]';
pos = [10, 20, 0, 30, 40]';
dt = 0.001;
n = length(t);
accelerationTimeRatio = 0.4 * ones(n - 1, 1);    %加速段/减速段的比率，(0, 1/2)之间

tArray = [];
posArray = [];
velArray = [];
accArray = [];
tArray = [tArray; t(1)];
posArray = [posArray; pos(1)];
velArray = [velArray; 0.0];
accArray = [accArray; 0.0];
for i = 1 : n - 1
    Ta = accelerationTimeRatio(i) * (t(i + 1) - t(i));
    Ts = 0.5 * Ta;
    
    %第一段五次多项式
    ts = t(i);
    te = t(i) + Ta;
    ps = pos(i);
    pe = (pos(i + 1) - pos(i)) / (t(i + 1) - 2.0 * Ts - t(i)) * (t(i) + Ta - (t(i) + Ts)) + pos(i);
    vs = 0.0;
    ve = (pos(i + 1) - pos(i)) / (t(i + 1) - 2.0 * Ts - t(i));
    as = 0.0;
    ae = 0.0;
    h = pe - ps;
    T = te - ts;
    
    a0 = ps;
    a1 = vs;
    a2 = 0.5 * as;
    a3 = (20.0 * h - (8.0 * ve + 12.0 * vs) * T - (3.0 * as - ae) * T^2) / (2.0 * T^3);
    a4 = (-30.0 * h + (14.0 * ve + 16.0 * vs) * T + (3.0 * as - 2.0 * ae) * T^2) / (2.0 * T^4);
    a5 = (12.0 * h - 6.0 * (ve + vs) * T - (as - ae) * T^2) / (2.0 * T^5);
    
    tt = (ts + dt : dt : te)';
    if abs(tt(end) - te) > 1.0e-8
        tt = [tt; te];
    end
    
    tArray = [tArray; tt];
    posArray = [posArray; a0 + (tt - ts) .* (a1 + (tt - ts) .* (a2 + (tt - ts) .* (a3 + (tt - ts) .* (a4 + a5 .* (tt - ts)))))];
    velArray = [velArray; a1 + (tt - ts) .* (2.0 * a2 + (tt - ts) .* (3.0 * a3 + (tt - ts) .* (4.0 * a4 + 5.0 * a5 .* (tt - ts))))];
    accArray = [accArray; 2.0 * a2 + (tt - ts) .* (6.0 * a3 + (tt - ts) .* (12.0 * a4 + 20.0 * a5 .* (tt - ts)))];
    
    %第二段直线
    ts = te;
    te = t(i + 1) - Ta;
    ps = pe;
    pe = (pos(i + 1) - pos(i)) / (t(i + 1) - 2.0 * Ts - t(i)) * (t(i + 1) - Ta - (t(i) + Ts)) + pos(i);
    h = pe - ps;
    T = te - ts;
    
    tt = (ts + dt : dt : te)';
    if abs(tt(end) - te) > 1.0e-8
        tt = [tt; te];
    end
    
    tArray = [tArray; tt];
    posArray = [posArray; ps + h / T * (tt - ts)];
    velArray = [velArray; h / T * ones(size(tt))];
    accArray = [accArray; zeros(size(tt))];
    
    %第三段五次多项式
    ts = te;
    te = t(i + 1);
    ps = pe;
    pe = pos(i + 1);
    vs = ve;
    ve = 0.0;
    as = 0.0;
    ae = 0.0;
    h = pe - ps;
    T = te - ts;
    
    a0 = ps;
    a1 = vs;
    a2 = 0.5 * as;
    a3 = (20.0 * h - (8.0 * ve + 12.0 * vs) * T - (3.0 * as - ae) * T^2) / (2.0 * T^3);
    a4 = (-30.0 * h + (14.0 * ve + 16.0 * vs) * T + (3.0 * as - 2.0 * ae) * T^2) / (2.0 * T^4);
    a5 = (12.0 * h - 6.0 * (ve + vs) * T - (as - ae) * T^2) / (2.0 * T^5);
    
    tt = (ts + dt : dt : te)';
    if abs(tt(end) - te) > 1.0e-8
        tt = [tt; te];
    end
    
    tArray = [tArray; tt];
    posArray = [posArray; a0 + (tt - ts) .* (a1 + (tt - ts) .* (a2 + (tt - ts) .* (a3 + (tt - ts) .* (a4 + a5 .* (tt - ts)))))];
    velArray = [velArray; a1 + (tt - ts) .* (2.0 * a2 + (tt - ts) .* (3.0 * a3 + (tt - ts) .* (4.0 * a4 + 5.0 * a5 .* (tt - ts))))];
    accArray = [accArray; 2.0 * a2 + (tt - ts) .* (6.0 * a3 + (tt - ts) .* (12.0 * a4 + 20.0 * a5 .* (tt - ts)))];
end

figure(1)
subplot(3, 1, 1)
plot(t, pos, 'r+');
hold on;
plot(tArray, posArray);
xlabel('t');
ylabel('pos');

subplot(3, 1, 2)
plot(tArray, velArray);
xlabel('t');
ylabel('vel');

subplot(3, 1, 3)
plot(tArray, accArray);
xlabel('t');
ylabel('acc');

在这里插入图片描述

二、 靠近但不经过所有中间点(MATLAB代码)

clc;
clear;
close all;

%{
    
    
syms ts te ps pe vs ve as ae T real;
a = [1, 0, 0, 0, 0, 0
   0, 1, 0, 0, 0, 0
   0, 0, 2, 0, 0, 0
   1, T, T^2, T^3, T^4, T^5
   0, 1, 2*T, 3*T^2, 4*T^3, 5*T^4
   0, 0, 2, 6*T, 12*T^2, 20*T^3
   ] \ [ps; vs; as; pe; ve; ae];
a = [simplify(a(1)), simplify(a(2)), simplify(a(3)), simplify(a(4)), simplify(a(5)), simplify(a(6))]'
%}

t = [0, 2, 4, 8, 10]';
pos = [10, 40, 0, 50, 10]';  
dt = 0.001;
n = length(t);
accelerationTimeRatio = 0.3 * ones(n - 1, 1);    %加速段/减速段的比率，(0, 1/2)之间

tArray = [];
posArray = [];
velArray = [];
accArray = [];
tArray = [tArray; t(1)];
posArray = [posArray; pos(1)];
velArray = [velArray; 0.0];
accArray = [accArray; 0.0];
if n == 2
   Ta = accelerationTimeRatio(1) * (t(2) - t(1));
   Ts = 0.5 * Ta;
   
   %第一段五次多项式
   ts = t(1);
   te = t(1) + Ta;
   ps = pos(1);
   pe = (pos(2) - pos(1)) / (t(2) - 2.0 * Ts - t(1)) * (t(1) + Ta - (t(1) + Ts)) + pos(1);
   vs = 0.0;
   ve = (pos(2) - pos(1)) / (t(2) - 2.0 * Ts - t(1));
   as = 0.0;
   ae = 0.0;
   h = pe - ps;
   T = te - ts;
   
   a0 = ps;
   a1 = vs;
   a2 = 0.5 * as;
   a3 = (20.0 * h - (8.0 * ve + 12.0 * vs) * T - (3.0 * as - ae) * T^2) / (2.0 * T^3);
   a4 = (-30.0 * h + (14.0 * ve + 16.0 * vs) * T + (3.0 * as - 2.0 * ae) * T^2) / (2.0 * T^4);
   a5 = (12.0 * h - 6.0 * (ve + vs) * T - (as - ae) * T^2) / (2.0 * T^5);
   
   tt = (ts + dt : dt : te)';
   if abs(tt(end) - te) > 1.0e-8
       tt = [tt; te];
   end
   
   tArray = [tArray; tt];
   posArray = [posArray; a0 + (tt - ts) .* (a1 + (tt - ts) .* (a2 + (tt - ts) .* (a3 + (tt - ts) .* (a4 + a5 .* (tt - ts)))))];
   velArray = [velArray; a1 + (tt - ts) .* (2.0 * a2 + (tt - ts) .* (3.0 * a3 + (tt - ts) .* (4.0 * a4 + 5.0 * a5 .* (tt - ts))))];
   accArray = [accArray; 2.0 * a2 + (tt - ts) .* (6.0 * a3 + (tt - ts) .* (12.0 * a4 + 20.0 * a5 .* (tt - ts)))];
   
   %第二段直线
   ts = te;
   te = t(2) - Ta;
   ps = pe;
   pe = (pos(2) - pos(1)) / (t(2) - 2.0 * Ts - t(1)) * (t(2) - Ta - (t(1) + Ts)) + pos(1);
   h = pe - ps;
   T = te - ts;
   
   tt = (ts + dt : dt : te)';
   if abs(tt(end) - te) > 1.0e-8
       tt = [tt; te];
   end
   
   tArray = [tArray; tt];
   posArray = [posArray; ps + h / T * (tt - ts)];
   velArray = [velArray; h / T * ones(size(tt))];
   accArray = [accArray; zeros(size(tt))];
   
   %第三段五次多项式
   ts = te;
   te = t(2);
   ps = pe;
   pe = pos(2);
   vs = ve;
   ve = 0.0;
   as = 0.0;
   ae = 0.0;
   h = pe - ps;
   T = te - ts;
   
   a0 = ps;
   a1 = vs;
   a2 = 0.5 * as;
   a3 = (20.0 * h - (8.0 * ve + 12.0 * vs) * T - (3.0 * as - ae) * T^2) / (2.0 * T^3);
   a4 = (-30.0 * h + (14.0 * ve + 16.0 * vs) * T + (3.0 * as - 2.0 * ae) * T^2) / (2.0 * T^4);
   a5 = (12.0 * h - 6.0 * (ve + vs) * T - (as - ae) * T^2) / (2.0 * T^5);
   
   tt = (ts + dt : dt : te)';
   if abs(tt(end) - te) > 1.0e-8
       tt = [tt; te];
   end
   
   tArray = [tArray; tt];
   posArray = [posArray; a0 + (tt - ts) .* (a1 + (tt - ts) .* (a2 + (tt - ts) .* (a3 + (tt - ts) .* (a4 + a5 .* (tt - ts)))))];
   velArray = [velArray; a1 + (tt - ts) .* (2.0 * a2 + (tt - ts) .* (3.0 * a3 + (tt - ts) .* (4.0 * a4 + 5.0 * a5 .* (tt - ts))))];
   accArray = [accArray; 2.0 * a2 + (tt - ts) .* (6.0 * a3 + (tt - ts) .* (12.0 * a4 + 20.0 * a5 .* (tt - ts)))];
else
   t_ = zeros(2 * n, 1);
   p = zeros(2 * n, 1);
   v = zeros(2 * n, 1);
   
   Ta = accelerationTimeRatio(1) * (t(2) - t(1));
   Ts = 0.5 * Ta;
   t_(1) = t(1);
   p(1) = pos(1);
   v(1) = 0.0;
   t_(2) = t(1) + Ta;
   p(2) = (pos(2) - pos(1)) / (t(2) - (t(1) + Ts)) * (t_(2) - t(2)) + pos(2);
   v(2) = (pos(2) - pos(1)) / (t(2) - (t(1) + Ts));
   t_(3) = t(2) - Ts;
   p(3) = (pos(2) - pos(1)) / (t(2) - (t(1) + Ts)) * (t_(3) - t(2)) + pos(2);
   v(3) = (pos(2) - pos(1)) / (t(2) - (t(1) + Ts));
   
   Ta = accelerationTimeRatio(n - 1) * (t(n) - t(n - 1));
   Ts = 0.5 * Ta;
   t_(2 * n) = t(n);
   p(2 * n) = pos(n);
   v(2 * n) = 0.0;
   t_(2 * n - 1) = t(n) - Ta;
   p(2 * n - 1) = (pos(n) - pos(n - 1)) / (t(n) - Ts - t(n - 1)) * (t_(2 * n - 1) - t(n - 1)) + pos(n - 1);
   v(2 * n - 1) = (pos(n) - pos(n - 1)) / (t(n) - Ts - t(n - 1));
   t_(2 * n - 2) = t(n - 1) + Ts;
   p(2 * n - 2) = (pos(n) - pos(n - 1)) / (t(n) - Ts - t(n - 1)) * (t_(2 * n - 2) - t(n - 1)) + pos(n - 1);
   v(2 * n - 2) = (pos(n) - pos(n - 1)) / (t(n) - Ts - t(n - 1));
   
   k = 4;
   for i = 2 : n - 2
       Ta = accelerationTimeRatio(i) * (t(i + 1) - t(i));
       Ts = 0.5 * Ta;
       t_(k) = t(i) + Ts;
       p(k) = (pos(i + 1) - pos(i)) / (t(i + 1) - t(i)) * (t_(k) - t(i)) + pos(i);
       v(k) = (pos(i + 1) - pos(i)) / (t(i + 1) - t(i));
       
       t_(k + 1) = t(i + 1) - Ts;
       p(k + 1) = (pos(i + 1) - pos(i)) / (t(i + 1) - t(i)) * (t_(k + 1) - t(i)) + pos(i);
       v(k + 1) = (pos(i + 1) - pos(i)) / (t(i + 1) - t(i));
       
       k = k + 2;
   end
   
   for i = 1 : 2 * n - 1
       ts = t_(i);
       te = t_(i + 1);
       ps = p(i);
       pe = p(i + 1);
       vs = v(i);
       ve = v(i + 1);
       as = 0.0;
       ae = 0.0;
       h = pe - ps;
       T = te - ts;
       
       tt = (ts + dt : dt : te)';
       if abs(tt(end) - te) > 1.0e-8
           tt = [tt; te];
       end
       tArray = [tArray; tt];
       
       if mod(i, 2) == 1  %五次多项式
           a0 = ps;
           a1 = vs;
           a2 = 0.5 * as;
           a3 = (20.0 * h - (8.0 * ve + 12.0 * vs) * T - (3.0 * as - ae) * T^2) / (2.0 * T^3);
           a4 = (-30.0 * h + (14.0 * ve + 16.0 * vs) * T + (3.0 * as - 2.0 * ae) * T^2) / (2.0 * T^4);
           a5 = (12.0 * h - 6.0 * (ve + vs) * T - (as - ae) * T^2) / (2.0 * T^5);
           posArray = [posArray; a0 + (tt - ts) .* (a1 + (tt - ts) .* (a2 + (tt - ts) .* (a3 + (tt - ts) .* (a4 + a5 .* (tt - ts)))))];
           velArray = [velArray; a1 + (tt - ts) .* (2.0 * a2 + (tt - ts) .* (3.0 * a3 + (tt - ts) .* (4.0 * a4 + 5.0 * a5 .* (tt - ts))))];
           accArray = [accArray; 2.0 * a2 + (tt - ts) .* (6.0 * a3 + (tt - ts) .* (12.0 * a4 + 20.0 * a5 .* (tt - ts)))];
       else   %直线
           posArray = [posArray; ps + h / T * (tt - ts)];
           velArray = [velArray; h / T * ones(size(tt))];
           accArray = [accArray; zeros(size(tt))];
       end
   end
end

figure(1)
subplot(3, 1, 1)
plot(t, pos, 'r+');
hold on;

plot(tArray, posArray);
xlabel('t');
ylabel('pos');

subplot(3, 1, 2)
plot(tArray, velArray);
xlabel('t');
ylabel('vel');

subplot(3, 1, 3)
plot(tArray, accArray);
xlabel('t');
ylabel('acc');

在这里插入图片描述

三、参考文献

Trajectory Planning for Automatic Machines and Robots中章节：3.3 Linear Trajectory with Polynomial Blends

猜你喜欢

转载自blog.csdn.net/maple_2014/article/details/106594760
今日推荐
TIOBE 5 月榜单：Fortran “复活”进入 Top 10
GCC 14.1 发布
面壁智能发布 Eurux-8x22B 开源大模型 —— 堪称「理科状元」
开源日报 | 谷歌扶持鸿蒙上位；开源Rabbit R1；Docker加持的安卓手机；微软的焦虑和野心；海尔电器把开放平台关了
中国码农的“35岁魔咒”
蘭雅 CorelDRAW 插件 2024.5.1 国际劳动节版，免费下载
Arc Browser for Windows 1.0 正式 GA
90后程序员开发视频搬运软件、不到一年获利超 700 万，结局很刑！
周排行
Java自定义时间格式
同步整形电路
在开发中最最最常用的字符串的属性大集合
Linux 查看端口占用并杀掉
Java基础四：ArrayList
多线程之死锁就是这么简单
mysql 基础命令集
awk 命令详解
Centos6.3编译安装nginx+php步骤
OCR （Optical Character Recognition，光学字符识别）
每日归档
更多
2024-05-08(42)
2024-05-07(14)
2024-05-06(40)
2024-05-05(0)
2024-05-04(7)
2024-05-03(19)
2024-05-02(0)
2024-05-01(4)
2024-04-30(1)
2024-04-29(40)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022