摆线/谐波过渡的匀速/匀加速度轨迹(Constant Velocity/Acceleration Trajectories with Cycloidal or Harmonic Blends)

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

文章目录

一、 Constraints on the velocity profile

clc;
clear;
close all;

t = [0, 1, 2, 3, 4, 6, 7, 8, 12]'; %单调递增时间序列
v = [0, 1, 1, 2, 2, -3, -3, 0, 0]';

% t = [0, 1, 2, 3]';
% v = [0, 1, 1, 0]';

% t = [0, 1, 2, 3, 4, 5, 6, 7]';
% v = [0, 1, 1, 0, 0, -1, -1, 0]';

dt = 0.001;
n = length(t);
V = zeros(n - 1, 1);
K = zeros(n - 1, 1);
omega = zeros(n - 1, 1);
phi = zeros(n - 1, 1);

for i = 1 : 2 : n - 1
    V(i) = 0.5 * (v(i + 1) - v(i));
    K(i) = 0.5 * (v(i + 1) + v(i));
    omega(i) = pi / (t(i + 1) - t(i));
    phi(i) = 3.0 * pi / 2.0;
end
for i = 2 : 2 : n - 1
    K(i) = v(i + 1);
end

tArray = [];
posArray = [];
velArray = [];
accArray = [];
jerkArray = [];
tArray = [tArray; t(1)];
posArray = [posArray; 0.0];
velArray = [velArray; -V(1) + K(1)];
accArray = [accArray; 0.0];
jerkArray = [jerkArray; V(1) * omega(1)^2];
for i = 1 : n - 1
    tt = (t(i) + dt : dt : t(i + 1))';
    if abs(tt(end) - t(i + 1)) > 1.0e-8
        tt = [tt; t(i + 1)];
    end
    
    q = -(V(i) / omega(i)) * sin(omega(i) * (tt - t(i))) + K(i) * (tt - t(i)) + posArray(end);
    dq = -V(i) * cos(omega(i) * (tt - t(i))) + K(i);
    ddq = V(i) * omega(i) * sin(omega(i) * (tt - t(i)));
    dddq = V(i) * omega(i)^2 * cos(omega(i) * (tt - t(i)));
    tArray = [tArray; tt];
    
    posArray = [posArray; q];
    velArray = [velArray; dq];
    accArray = [accArray; ddq];
    jerkArray = [jerkArray; dddq];
end
figure(1)
subplot(4, 1, 1)
plot(tArray, posArray);
xlabel('t');
ylabel('pos');

subplot(4, 1, 2)
plot(tArray, velArray);
hold on
plot(t, v, 'o')
xlabel('t');
ylabel('vel');

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

subplot(4, 1, 4)
plot(tArray, jerkArray);
xlabel('t');
ylabel('jerk');

在这里插入图片描述

二、Constraints on the acceleration profile

clc;
clear;
close all;

T = [0.125, 0.25, 0.125, 0, 0.125, 0.25, 0.125]';%7段时间
t = cumsum([0; T]);

dt = 0.001;
c = zeros(4, 1);
v = zeros(6, 1);
q = zeros(6, 1);

c(1) = 2.0 * T(1) / pi + T(2) + 2.0 * T(3) / pi;
c(2) = -2.0 * T(5) / pi - T(6) - 2.0 * T(7) / pi;
c(3) = (2.0 * T(1) / pi) * ((pi - 2) * T(1) / pi + T(2) / 2.0) + ...
   (2.0 * T(1) / pi + T(2)) * (T(2) / 2.0 + (pi - 2.0) * T(3) / pi) + ...
   (2.0 * T(1) / pi + T(2) + 2.0 * T(3) / pi) * (2.0 * T(3) / pi + T(4) + 2.0 * T(5) / pi);
c(4) = (2.0 * T(7) / pi + T(6)) * ((pi - 2.0) * T(5) / pi + T(6) / 2.0) + ...
   (2.0 * T(7) / pi) * (T(6) / 2.0 + (pi - 2.0) * T(7) / pi);

a2 = -c(2) / (c(1) * c(4) - c(2) * c(3));
a6 = c(1) / (c(1) * c(4) - c(2) * c(3));

v(1) = a2 * (2.0 * T(1) / pi);
v(2) = v(1) + a2 * T(2);
v(3) = v(2) + a2 * (2.0 * T(3) / pi);
v(4) = v(3);
v(5) = v(4) - a6 * (2.0 * T(5) / pi);
v(6) = v(5) - a6 * T(6);

q(1) = a2 * (2.0 * T(1)^2 / pi) * (1.0 - 2.0 / pi);
q(2) = q(1) + a2 * T(2) * (2.0 * T(1) / pi + T(2) / 2.0);
q(3) = q(2) + a2 * T(3) * (2.0 * T(1) / pi + T(2) + 4.0 * T(3) / pi^2);
q(4) = q(3) + a2 * T(4) * (2.0 * T(1) / pi + T(2) + 2.0 * T(3) / pi);
q(5) = q(4) + a2 * T(5) * (2.0 * T(1) / pi + T(2) + 2.0 * T(3) / pi) ...
      - a6 * (2.0 * T(5)^2 / pi) * (1.0 - 2.0 / pi);
q(6) = q(5) + v(5) * T(6) - a6 * T(6)^2 / 2.0;

posArray = [];
velArray = [];
accArray = [];
jerkArray = [];
tArray = (t(1) : dt : t(end))';
for i = 1 : length(tArray)
   tt = tArray(i);
   if tt < t(2)
       pos = a2 * (2.0 * T(1) / pi) * (tt - (2.0 * T(1) / pi) * sin((tt - t(1)) * pi / (2.0 * T(1))));
       vel = a2 * (2.0 * T(1) / pi) * (1 - cos((tt - t(1)) * pi / (2.0 * T(1))));
       acc = a2 * sin((tt - t(1)) * pi / (2.0 * T(1)));
       jerk = a2 * (pi / (2.0 * T(1))) * cos((tt - t(1)) * pi / (2.0 * T(1)));
   elseif tt < t(3)
       pos = q(1) + v(1) * (tt - t(2)) + a2 * (tt - t(2))^2 / 2.0;
       vel = v(1) + a2 * (tt - t(2));
       acc = a2;
       jerk = 0.0;
   elseif tt < t(4)
       pos = q(2) + v(2) * (tt - t(3)) + a2 * (2.0 * T(3) / pi)^2 * (1.0 - cos((tt - t(3)) * pi / (2.0 * T(3))));
       vel = v(2) + a2 * (2.0 * T(3) / pi) * sin((tt - t(3)) * pi / (2.0 * T(3)));
       acc = a2 * cos((tt - t(3)) * pi / (2.0 * T(3)));
       jerk = -a2 * (pi / (2.0 * T(3))) * sin((tt - t(3)) * pi / (2.0 * T(3)));
   elseif tt < t(5)
       pos = q(3) + v(3) * (tt - t(4));
       vel = v(3);
       acc = 0.0;
       jerk = 0.0;
   elseif tt < t(6)
       pos = q(4) + v(4) * (tt - t(5)) - a6 * (2.0 * T(5) / pi) * (tt - t(5) - (2.0 * T(5) / pi) * sin((tt - t(5)) * pi / (2.0 * T(5))));
       vel = v(4) - a6 * (2.0 * T(5) / pi) * (1 - cos((tt - t(5)) * pi / (2.0 * T(5))));
       acc = -a6 * sin((tt - t(5)) * pi / (2.0 * T(5)));
       jerk = -a6 * (pi / (2.0 * T(5))) * cos((tt - t(5)) * pi / (2.0 * T(5)));
   elseif tt < t(7)
       pos = q(5) + v(5) * (tt - t(6)) - a6 * (tt - t(6))^2 / 2.0;
       vel = v(5) - a6 * (tt - t(6));
       acc = -a6;
       jerk = 0.0;
   else
       pos = q(6) + v(6) * (tt - t(7)) - a6 * (2.0 * T(7) / pi)^2 * (1.0 - cos((tt - t(7)) * pi / (2.0 * T(7))));
       vel = v(6) - a6 * (2.0 * T(7) / pi) * sin((tt - t(7)) * pi / (2.0 * T(7)));
       acc = -a6 * cos((tt - t(7)) * pi / (2.0 * T(7)));
       jerk = a6 * (pi / (2.0 * T(7))) * sin((tt - t(7)) * pi / (2.0 * T(7)));
   end
   posArray = [posArray; pos];
   velArray = [velArray; vel];
   accArray = [accArray; acc];
   jerkArray = [jerkArray; jerk];
end

if abs(tArray(end) - t(end)) > 1.0e-8
   tArray = [tArray; t(end)];
   posArray = [posArray; q(6) + v(6) * (t(end) - t(7)) - a6 * (2.0 * T(7) / pi)^2 * (1.0 - cos((t(end) - t(7)) * pi / (2.0 * T(7))))];
   velArray = [velArray; v(6) - a6 * (2.0 * T(7) / pi) * sin((t(end) - t(7)) * pi / (2.0 * T(7)))];
   accArray = [accArray; -a6 * cos((t(end) - t(7)) * pi / (2.0 * T(7)))];
   jerkArray = [jerkArray; a6 * (pi / (2.0 * T(7))) * sin((t(end) - t(7)) * pi / (2.0 * T(7)))];
end

figure(1)
subplot(4, 1, 1)
plot(tArray, posArray);
xlabel('t');
ylabel('pos');

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

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

subplot(4, 1, 4)
plot(tArray, jerkArray);
xlabel('t');
ylabel('jerk');

在这里插入图片描述

三、Minimum-time trajectories

clc;
clear;
close all;

vmax = 10;
amax = 100;
h = 2;
dt = 0.001;

T = zeros(3, 1);
if h >= pi * vmax^2 / (2.0 * amax)
    T(1) = pi * vmax / (2.0 * amax);
    T(2) = h / vmax - T(1);
    T(3) = T(1);
    totalT = 2.0 * T(1) + T(2);
else
    T(1) = sqrt(pi * h / (2.0 * amax));
    T(3) = T(1);
    totalT = 2.0 * T(1);
end
t = cumsum([0; T]);

V = [vmax / 2.0, 0.0, vmax / 2.0]';
K = [vmax / 2.0, vmax, vmax / 2.0]';
omega = [pi/ T(1), 0.0, pi / T(3)]';
phi = [3.0 * pi / 2.0, 0.0, pi / 2.0]';
q = zeros(3, 1);
q(1) = 0.0;
q(2) = q(1) - (V(1) / omega(1)) * cos(omega(1) * T(1) + phi(1)) + K(1) * T(1);
q(3) = q(2) + K(2) * T(2);

posArray = [];
velArray = [];
accArray = [];
jerkArray = [];
tArray = (0.0 : dt : totalT)';
for i = 1 : length(tArray)
    tt = tArray(i);
    if tt < T(1)
        pos = q(1) - (V(1) / omega(1)) * cos(omega(1) * (tt - t(1)) + phi(1)) + K(1) * (tt - t(1));
        vel = V(1) * sin(omega(1) * (tt - t(1)) + phi(1)) + K(1);
        acc = V(1) * omega(1) * cos(omega(1) * (tt - t(1)) + phi(1));
        jerk = -V(1) * omega(1)^2 * sin(omega(1) * (tt - t(1)) + phi(1));
    elseif tt < T(1) + T(2)
        pos = q(2) + K(2) * (tt - t(2));
        vel = K(2);
        acc = 0.0;
        jerk = 0.0;
    else
        pos = q(3) - (V(3) / omega(3)) * cos(omega(3) * (tt - t(3)) + phi(3)) + K(3) * (tt - t(3));
        vel = V(3) * sin(omega(3) * (tt - t(3)) + phi(3)) + K(3);
        acc = V(3) * omega(3) * cos(omega(3) * (tt - t(3)) + phi(3));
        jerk = -V(3) * omega(3)^2 * sin(omega(3) * (tt - t(3)) + phi(3));
    end
    posArray = [posArray; pos];
    velArray = [velArray; vel];
    accArray = [accArray; acc];
    jerkArray = [jerkArray; jerk];
end

if abs(tArray(end) - t(end)) > 1.0e-8
    tArray = [tArray; t(end)];
    posArray = [posArray; q(3) - (V(3) / omega(3)) * cos(omega(3) * (t(end) - t(3)) + phi(3)) + K(3) * (t(end) - t(3))];
    velArray = [velArray; V(3) * sin(omega(3) * (t(end) - t(3)) + phi(3)) + K(3)];
    accArray = [accArray; V(3) * omega(3) * cos(omega(3) * (t(end) - t(3)) + phi(3))];
    jerkArray = [jerkArray; -V(3) * omega(3)^2 * sin(omega(3) * (t(end) - t(3)) + phi(3))];
end

figure(1)
subplot(4, 1, 1)
plot(tArray, posArray);
xlabel('t');
ylabel('pos');

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

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

subplot(4, 1, 4)
plot(tArray, jerkArray);
xlabel('t');
ylabel('jerk');

在这里插入图片描述

四、参考文献

Trajectory Planning for Automatic Machines and Robots中章节：3.10 Constant Velocity/Acceleration Trajectories with Cycloidal or Harmonic Blends

猜你喜欢

转载自blog.csdn.net/maple_2014/article/details/106729358
今日推荐
基于大语言模型的开源知识库问答系统 MaxKB GitHub Star 数量突破 5,000 个！
美国拟限制 AI 大模型出口中国和俄罗斯
苹果将与 OpenAI 达成协议，将 ChatGPT 应用于 iPhone
openKylin 社区生态委员会第六次会议圆满召开
阿里云正式发布通义千问 2.5
Python 3.13 发布首个 Beta：实验性自由线程模式和 JIT、改进交互式解释器
Stack Overflow 拿我的代码去训练 AI 大模型，还封了我的账号
Pop!_OS 的 COSMIC 桌面完成 App Store 上架工作
《2024 年一季度互联网投融资运行情况》研究报告
报告：Django 仍然是 74% 开发者的首选
15 年前上了“FFmpeg 耻辱柱”，今天他还得谢谢咱——腾讯QQPlayer一雪前耻？
TIOBE 5 月榜单：Fortran “复活”进入 Top 10
周排行
记一下去大梅沙的准备（2018-05-26）
Spring 注解 事务
基于HTTP协议的客户端缓存
阿里云rds 备份和还原
[PHP] 几个拖慢 PHP 程序/API 运行速度的点
python 代码风格------------PEP8规则
js控制json生成菜单——自制菜单（一）
将字符串: 'k:1|k1:2|k2:3|k3:4 ' ,处理成 python 字典: {'k':1, 'k1':2, ...}
微信小程序转支付宝小程序
Qt551.窗口滚动条
每日归档
更多
2024-05-13(18)
2024-05-12(0)
2024-05-11(38)
2024-05-10(38)
2024-05-09(35)
2024-05-08(42)
2024-05-07(14)
2024-05-06(40)
2024-05-05(0)
2024-05-04(7)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022