【RTB机器人工具箱学习记录】轨迹规划函数

轨迹：时间、位置、速度、加速度

给定时间0-2s，位置0-3

五次多项式轨迹：tpoly()

[S,SD,SDD] = tpoly(S0, SF, T, QD0, QD1)

​ S0：初始位置

​ SF：结束位置

​ T：时间序列

​ QD0：初始速度

​ QD1：结束速度

t = linspace(0,2,51); %// 时间 0:0.04:2
[P,dP,ddP] = tpoly(0,3,t); %// 位置，速度，加速度
plot(t,P);grid on;hold on;plot(t,dP);hold on;plot(t,ddP);legend('pos','vel','acc')

% //指定初末速度
[P,dP,ddP] = tpoly(0,3,t,0.01，0.05); % //位置，速度，加速度，初始速度末尾速度默认0

---

混合曲线轨迹：lspb()

[S,SD,SDD] = lspb(S0, SF, T, V)

​ S0：初始位置

​ SF：结束位置

​ T：时间序列

​ V：最大速度

t = linspace(0,2,51); % //时间 0:0.04:2
[P,dP,ddP] = lspb(0,3,t); % //位置，速度，加速度
plot(t,P);grid on;hold on;plot(t,dP);hold on;plot(t,ddP);legend('pos','vel','acc')

% //指定最大速度
[P,dP,ddP] = lspb(0,3,t,1.8); % //位置，速度，加速度，最大速度

---

多维轨迹：mtraj()

[Q,QD,QDD] = mtraj(TFUNC, Q0, QF, T)

​ TFUNC：轨迹函数，可选@tpoly或@lspb

​ Q0：初始位置

​ QF：结束位置

​ T：时间序列

t = linspace(0,2,51); %// 时间 0:0.04:2
[P,dP,ddP] = mtraj(@tpoly,[0,0],[2,3],t);
figure(1) % //画在同一个坐标系下
plot(t,P(:,1));grid on;hold on;plot(t,P(:,2));hold on;
plot(t,dP(:,1));grid on;hold on;plot(t,dP(:,2));hold on;
plot(t,ddP(:,1));grid on;hold on;plot(t,ddP(:,2));hold on;
legend('posx','posy','velx','vely','accx','accy')

figure(2) %// 绘制在同一个图窗的三个坐标系下
sgtitle('多维轨迹规划') % sgtitleg
subplot(1,3,1);plot(t,P(:,1));grid on;hold on;plot(t,P(:,2));hold on;   
title('位置');legend('posx','posy')
subplot(1,3,2);plot(t,dP(:,1));grid on;hold on;plot(t,dP(:,2));hold on;
title('速度');legend('velx','vely')
subplot(1,3,3);plot(t,ddP(:,1));grid on;hold on;plot(t,ddP(:,2));hold on;
title("加速度");legend('accx','accy')

---

多段多维轨迹：mstraj()

TRAJ=mstraj(WP,QDMAX,TSEG,QO,DT,TACC,OPTIONS)

​ WP：路径点

​ QDMAX：最大速度，和TSEG为同一个自由度，若设置了时间间隔，则最大速度置空。

​ TSEG：每段的时间间隔，和QDMAX为同一个自由度，若设置了最大速度，则时间间隔置空。

​ QO：初始位置，置为空时，WP第一个点必须为[0,0]

​ DT：插值间隔

​ TACC：加速时间

​ OPTIONS：

wp=[0,0;3,4;1,2];
t=0.04; % //时间间隔
P1=mstraj(wp,[],[2,1],[],t,0.1);
P2=mstraj(wp,[],[2,1],[],t,0.8);

figure(1) % //绘制在同一个图窗的三个坐标系下
sgtitle('多段多维轨迹') % //sgtitleg
subplot(1,2,1);
plot(linspace(0,t*length(P1(:,1)),length(P1(:,1))),P1(:,1));
grid on;hold on;
plot(linspace(0,t*length(P1(:,1)),length(P1(:,1))),P1(:,2));
hold on;   
title('P1');legend('posx','posy')
subplot(1,2,2);
plot(linspace(0,t*length(P2(:,1)),length(P2(:,1))),P2(:,1));
grid on;hold on;
plot(linspace(0,t*length(P2(:,1)),length(P2(:,1))),P2(:,2));
hold on;
title('P2');legend('posx','posy')

---