Code covered by the BSD License

Paul Lambrechts (view profile)

13 Sep 2007 (Updated )

Create and simulate up to fourth order trajectories for motion systems doing point-to-point moves.

[dj,tx,d,j,a,v,p,tt]=profile4(t,d,acc,plt)
```function [dj,tx,d,j,a,v,p,tt]=profile4(t,d,acc,plt)

% function [dj,tx,d,j,a,v,p,tt]=profile4(t,d,acc)
%
% Calculate symmetrical fourth order profiles from times:
%
%  Inputs:
%
%      t(1) = constant djerk phase duration
%      t(2) = constant jerk phase duration (default 0)
%      t(3) = constant acceleration phase duration (default 0)
%      t(4) = constant velocity phase duration (default 0)
%
%      d    = bound on djerk
%      acc  = continuous time: accuracy for profiles: t(1)*acc = minimal timestep
%             discrete time:   sample time
%
%  Outputs:
%
%      dj  = derivative of jerk profile suitable for simulink
%
%      tx  = time sequence for plotting profiles
%      d   = derivative of jerk profile
%      j   = jerk profile
%      a   = acceleration profile
%      v   = velocity profile
%      p   = position profile
%
%      tt  = 16 switching times for profile:
%
%       0 1              6 7             10 11  12 13
%       .-.              .-.              .-.    .-.
%       | |              | |              | |    | |
%       | |  2 3    4 5  | |         8 9  | |    | |  14 15
%       '-'--.-.----.-.--' '---------.-.--'-'----'-'--.-.--
%            | |    | |              | |              | |
%            | |    | |              | |              | |
%            '-'    '-'              '-'              '-'
%
%  Note: coinciding switching times are not removed

%
% Copyright 2004, Paul Lambrechts, The MathWorks, Inc.
%

if nargin~=3 && nargin~=4
help profile4
return
end
if nargin==3
plt=1;
end

if length(t)==1  % min distance with max djerk
tt=   [0 1 1 2 2 3 3 4 4 5 5 6 6 7 7 8]*t;
%    tt=   [0 1 1 2 2 3 4 4 4 4 4 5 5 6 6 7]*t;

elseif length(t)==2 % constant jerk phase
tt=   [0 1 1 2 2 3 3 4 4 5 5 6 6 7 7 8]*t(1) ...
+ [0 0 1 1 1 1 2 2 2 2 3 3 3 3 4 4]*t(2);

elseif length(t)==3 % constant acceleration phase
tt=   [0 1 1 2 2 3 3 4 4 5 5 6 6 7 7 8]*t(1) ...
+ [0 0 1 1 1 1 2 2 2 2 3 3 3 3 4 4]*t(2) ...
+ [0 0 0 0 1 1 1 1 1 1 1 1 2 2 2 2]*t(3) ;

elseif length(t)==4 % constant velocity phase
tt=   [0 1 1 2 2 3 3 4 4 5 5 6 6 7 7 8]*t(1) ...
+ [0 0 1 1 1 1 2 2 2 2 3 3 3 3 4 4]*t(2) ...
+ [0 0 0 0 1 1 1 1 1 1 1 1 2 2 2 2]*t(3) ...
+ [0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1]*t(4) ;

else
return
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dt=[];
for i=1:16
dt =  [dt   [1 1]*tt(i) ];
end
dt  = [dt 1.5*tt(16)];

dd = [0 d d 0];
dd = [ dd -dd -dd dd -dd dd dd -dd 0];

dj = [dt ; dd] ;

if plt==0  % no plot required
tx=[];d=[];j=[];a=[];v=[];p=[];    % dummy outputs
return
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Generate profiles for plotting

% Determine continuous or discrete
if max(abs( round(t/acc)-t/acc )) > 1e-12 % continuous
disp('Calculating continuous time profiles')
step = t(1)*acc;
tx=(0:step:1.2*tt(16))';
x=[];
for i=0:step:1.2*tt(16)
j=find(i<=dj(1,:));
x=[x ; dj(2,j(1))];
end
d=x;
j=cumsum(d)*step;
a=cumsum(j)*step;
v=cumsum(a)*step;
p=cumsum(v)*step;

else % discrete

disp('Calculating discrete time profiles')
Ts=acc;
ttest=[tt 1.5*tt(16)];
len = round(1.2*tt(16)/Ts + 1); % length of profiles
xd = zeros(len,1);
xj = xd;
xa = xd;
xv = xd;
xp = xd;
xd(1) = d;
tx=(0:Ts:1.2*tt(16)+Ts/2)';
for time=Ts:Ts:1.2*tt(16)+Ts/2
j = find( (time + Ts/2) <= ttest ); j = j(1)-1;
k = round(time/Ts);
if j==1 || j==7 || j==11 || j==13
xd(k+1) =  d;
elseif j==3 || j==5 || j==9 || j==15
xd(k+1) = -d;
else
xd(k+1) =  0;
end
xj(k+1) = xj(k) + xd(k)*Ts;
xa(k+1) = xa(k) + xj(k)*Ts;
xv(k+1) = xv(k) + xa(k)*Ts;
xp(k+1) = xp(k) + xv(k)*Ts;
end
d=xd;j=xj;a=xa;v=xv;p=xp;

dj(1,:)=dj(1,:)+Ts/2 ; % add Ts/2 to avoid numerical problems
end
dj=dj'; % to be compatible with Simulink 'From Workspace' block

% figure
%close all
subplot(511);plot(tx,d,'k','LineWidth',1.5);hold on;plot([0 0],[-1 1]*max(d),'k--',[1 1]*max(tt),[-1 1]*max(d),'k--','LineWidth',1.5); grid on; axis([ [-0.01 1]*max(tx) [-1.1 1.1]*max(d)]);
title('Fourth order trajectory profiles');ylabel('d [m/s4]');
subplot(512);plot(tx,j,'k','LineWidth',1.5);hold on;plot([0 0],[-1 1]*max(j),'k--',[1 1]*max(tt),[-1 1]*max(j),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-1.1 1.1]*max(j)]);
ylabel('j [m/s3]');
subplot(513);plot(tx,a,'k','LineWidth',1.5);hold on;plot([0 0],[-1 1]*max(a),'k--',[1 1]*max(tt),[-1 1]*max(a),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-1.1 1.1]*max(a)]);
ylabel('a [m/s2]');
subplot(514);plot(tx,v,'k','LineWidth',1.5);hold on;plot([0 0],[0 1]*max(v),'k--',[1 1]*max(tt),[0 1]*max(v),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-0.1 1.1]*max(v)]);
ylabel('v [m/s]');
subplot(515);plot(tx,p,'k','LineWidth',1.5);hold on;plot([0 0],[0 1]*max(p),'k--',[1 1]*max(tt),[0 1]*max(p),'k--','LineWidth',1.5);grid on; axis([ [-0.01 1]*max(tx) [-0.1 1.1]*max(p)]);
xlabel('time [s]');ylabel('x [m]');
set(1,'position',[700 100 500 860])
set(1,'paperposition',[0 0 5 8.6])

return
subplot(511);
text(tt( 1)+tt(16)/200,-max(d)/5,'t_0');
text(tt( 2)-tt(16)/200,-max(d)/5,'t_1');
text(tt( 3)-tt(16)/200, max(d)/5,'t_2');
text(tt( 4)-tt(16)/200, max(d)/5,'t_3');
text(tt( 5)-tt(16)/200, max(d)/5,'t_4');
text(tt( 6)-tt(16)/200, max(d)/5,'t_5');
text(tt( 7)-tt(16)/200,-max(d)/5,'t_6');
text(tt( 8)-tt(16)/200,-max(d)/5,'t_7');
text(tt( 9)-tt(16)/200, max(d)/5,'t_8');
text(tt(10)-tt(16)/200, max(d)/5,'t_9');
text(tt(11)-tt(16)/100,-max(d)/5,'t_1_0');
text(tt(12)-tt(16)/200,-max(d)/5,'t_1_1');
text(tt(13)-tt(16)/100,-max(d)/5,'t_1_2');
text(tt(14)-tt(16)/200,-max(d)/5,'t_1_3');
text(tt(15)-tt(16)/100, max(d)/5,'t_1_4');
text(tt(16)-tt(16)/200, max(d)/5,'t_1_5');
```