Scribd
Upload
182 views

Matlab

This document contains code for simulating the motion of various robotic systems over time including: 1. A mobile robot with equations of motion defined and plotted over time. 2. A robotic arm with 3 joints and forward kinematics equations to calculate end effector position over time using a PID controller. 3. An omnidirectional robot with equations of motion in x,y and orientation defined and plotted. 4. A drone model with equations of motion in x,y,z and orientation defined and controlled to follow a reference trajectory using a PID controller. The code defines system models, initial conditions, controllers, and simulations to plot the motion of each system as it changes over time. Pause

Uploaded by

Diana Ulloa
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
182 views

Matlab

This document contains code for simulating the motion of various robotic systems over time including: 1. A mobile robot with equations of motion defined and plotted over time. 2. A robotic arm with 3 joints and forward kinematics equations to calculate end effector position over time using a PID controller. 3. An omnidirectional robot with equations of motion in x,y and orientation defined and plotted. 4. A drone model with equations of motion in x,y,z and orientation defined and controlled to follow a reference trajectory using a PID controller. The code defines system models, initial conditions, controllers, and simulations to plot the motion of each system as it changes over time. Pause

Uploaded by

Diana Ulloa
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 14

clear all

close all

ts=0.1;
t=0:ts:20;

u=0*ones(1,length(t));
w=0.5*ones(1,length(t));

xr(1)=0;
yr(1)=0;
phi(1)=0;

for k=1:length(t)

xrp(k)=u(k)*cos(phi(k));
yrp(k)=u(k)*sin(phi(k));

xr(k+1)=xr(k)+ts*xrp(k);
yr(k+1)=yr(k)+ts*yrp(k);
phi(k+1)=phi(k)+ts*w(k);

end

pasos=20; fig=figure;
set(fig,'position',[10 60 980 600]);
axis square; cameratoolbar
axis([-8 10 -6 6 0 1]); grid on
MobileRobot;
M1=MobilePlot(xr(1),yr(1),phi(1));
hold on, plot(xr,yr);

for i=1:pasos:length(t)

delete (M1)
M1=MobilePlot(xr(i),yr(i),phi(i)); hold on

pause(0.5)

end
_________________________________________________________________________
_________________________________________________________________________
*************************************************************************
*************************************************************************
clc
clear all
close all

ts=0.1;
t=0:ts:20;

u=0*ones(1,length(t));
w=0.5*ones(1,length(t));

a=0.2;

xc(1)=0;
yc(1)=0;
phi(1)=0;

xr(1)=xc(1)+a*cos(phi(1));
yr(1)=yc(1)+a*sin(phi(1));

for k=1:length(t)
xrp(k)=u(k)*cos(phi(k))-a*w(k)*sin(phi(k));
yrp(k)=u(k)*sin(phi(k))+a*w(k)*cos(phi(k));

xr(k+1)=xr(k)+ts*xrp(k);
yr(k+1)=yr(k)+ts*yrp(k);
phi(k+1)=phi(k)+ts*w(k);

xc(k+1)=xr(k+1)-a*cos(phi(k+1));
yc(k+1)=yr(k+1)-a*sin(phi(k+1));

end

pasos=20; fig=figure;
set(fig,'position',[10 60 980 600]);
axis square; cameratoolbar
axis([-8 10 -6 6 0 1]); grid on
MobileRobot;
M1=MobilePlot(xr(1),yr(1),phi(1));
hold on, plot(xr,yr);

for i=1:pasos:length(t)

delete (M1)
M1=MobilePlot(xc(i),yc(i),phi(i)); hold on

pause(0.5)

end
clc
clear all
close all

ts=0.1;
t=0:ts:60;

% 1. Condiciones iniciales del manipulador


l1=0.5;
l2=0.65;
h=0.25;

q1(1) = 0*(pi/180);
q2(1) = 30*(pi/180);
q3(1) = 30*(pi/180);

xr(1)=l1*cos(q2(1))*cos(q1(1))+l2*cos(q2(1)+q3(1))*cos(q1(1));
yr(1)=l1*cos(q2(1))*sin(q1(1))+l2*cos(q2(1)+q3(1))*sin(q1(1));
zr(1)=h+l1*sin(q2(1))+l2*sin(q2(1)+q3(1));

% 3) Referencias deseadas
% xrd = 0.5;
% yrd = -0.8;
% zrd = 0.2;
xrd = .6*cos(0.2*t);
yrd = .6*sin(0.2*t);
zrd = 0.7*ones(1,length(t));
% zrd =0.7+0.01*t;

xrd_p= -.6*0.2*sin(0.2*t);
yrd_p= .6*0.2*cos(0.2*t);
% zrd_p = 0.01*ones(1,length(t));

zrd_p= 0*ones(1,length(t));

for k=1:length(t)

%a) Errores de control


xre(k) = xrd(k) - xr(k);
yre(k) = yrd(k) - yr(k);
zre(k) = zrd(k) - zr(k);

e = [xre(k);yre(k);zre(k)];

%b) Matriz Jacobiana


J=[ -l1*cos(q2(k))*sin(q1(k))-l2*cos(q2(k)+q3(k))*sin(q1(k)) -
l1*cos(q1(k))*sin(q2(k))-l2*cos(q1(k))*sin(q2(k)+q3(k)) -
l2*cos(q1(k))*sin(q2(k)+q3(k));...
l1*cos(q2(k))*cos(q1(k))+l2*cos(q2(k)+q3(k))*cos(q1(k)) -
l1*sin(q1(k))*sin(q2(k))-l2*sin(q1(k))*sin(q2(k)+q3(k)) -
l2*sin(q1(k))*sin(q2(k)+q3(k));...
0 l1*cos(q2(k))+l2*cos(q2(k)+q3(k)) l2*cos(q2(k)+q3(k))];

%c) Matriz de ganancia


K = [0.2 0 0;...
0 0.2 0;...
0 0 0.2];

%d) Ley de control


% v = inv(J)*K*e;

hd_p=[xrd_p(k) yrd_p(k) zrd_p(k)]';

%d) Ley de control


v = inv(J)*(hd_p+K*tanh(e));

q1p_ref(k)=v(1);
q2p_ref(k)=v(2);
q3p_ref(k)=v(3);

q1(k+1)=q1(k)+q1p_ref(k)*ts;
q2(k+1)=q2(k)+q2p_ref(k)*ts;
q3(k+1)=q3(k)+q3p_ref(k)*ts;
xr(k+1)=l1*cos(q2(k+1))*cos(q1(k+1))+l2*cos(q2(k+1)+q3(k+1))*cos(q1(k+1))
;

yr(k+1)=l1*cos(q2(k+1))*sin(q1(k+1))+l2*cos(q2(k+1)+q3(k+1))*sin(q1(k+1))
;
zr(k+1)=h+l1*sin(q2(k+1))+l2*sin(q2(k+1)+q3(k+1));
end

pasos=5; fig=figure;
set(fig,'position',[10 60 980 600]);
axis square; cameratoolbar
grid on;
axis([-1 1 -1 1 0 3]);

Arm_Parameters;

M1=Arm_Plot3D(0,0,0,0,0,q1(1),q2(1),q3(1),0);

for i=1:pasos:length(t)

delete (M1)
M1=Arm_Plot3D(0,0,0,0,0,q1(i),q2(i),q3(i),0),hold on;
plot3(xr(1:i),yr(1:i),zr(1:i),'r','linewidth',3);
pause(0.1)

end

=============================================================
clc % Clear Command Window
clear all %Remove items from workspace, freeing up system memory
close all % Remove specified figure

ts=0.1; % sample time


t=0:ts:30; % vector time

a=0.3;
% initial center position of robot
hx(1)=0; % X[m]
hy(1)=0; % Y[m]
phi(1)=0; % [rad]

% 3) Desired reference
hxd = 2*cos(0.2*t);
hyd = 2*sin(0.2*t);

hxdp = -2*0.2*sin(0.2*t);
hydp = 2*0.2*cos(0.2*t);

K=1;

hx(1)=hx(1)+a*cos(phi(1));
hy(1)=hy(1)+a*sin(phi(1));
for k=1:length(t)

% Following trayectory Control

% Error
hxe(k)=hxd(k)-hx(k);
hye(k)=hyd(k)-hy(k);

e=[hxe(k) hye(k)]';

% Jacobian Matrix

J=[cos(phi(k)) -sin(phi(k));...
sin(phi(k)) cos(phi(k))];

% Desired velocities
hdp=[hxdp(k) hydp(k)]';

v=inv(J)*(hdp+(K*e));

uf(k)=v(1);
ul(k)=v(2);

% Set action control in robot(real) or model

hxp(k)=uf(k)*cos(phi(k))-ul(k)*sin(phi(k));
hyp(k)=uf(k)*sin(phi(k))+ul(k)*cos(phi(k));

% Position in k+1
hx(k+1)=hx(k)+ts*hxp(k);
hy(k+1)=hy(k)+ts*hyp(k);
phi(k+1)=phi(k);
end

%% Simulacion

fig=figure; % new figure


set(fig,'position',[10 60 980 600]); % position window
axis equal; % Set axis aspect ratios
axis([-3 3 -3 3 -1 1]); % Set axis limits
grid on; % Display axes grid lines

Dimension_Omni; % Parameters of robot


M1=Plot_Omni(hx(1),hy(1),phi(1)); % Plot robot in initial position hx,hy
and phi orientation
hold on; % Retain current plot when adding new plot
plot(hxd,hyd,'r'); % plot trayectory.
xlabel('x(m)'); ylabel('y(m)'); zlabel('z(x)'); % Label axis
camlight('rigth');
%%
step=5;

for i=1:step:length(t) % Loop emulation

delete (M1)
M1=Plot_Omni(hx(i),hy(i),phi(i)); hold on
hold on; % Retain current plot when adding new plot
plot(hx(1:i),hyd(1:i),'b'); % plot trayectory.
pause(ts)
end
=========================================================================
0==

OMNIDIRECCIONAL_____CON VELOCIDAD RUEDAS


clc
clear all
close all

% Time
ts=0.1;
t=0:ts:20;

uf=-0.3*ones(1,length(t));
ul=0.3*ones(1,length(t));
w=0.0*ones(1,length(t));

a=0.15;
b=0.10;
R=0.02;

T=(1/R)*[1 -1 -(a+b);...
1 1 (a+b);...
1 1 -(a+b);...
1 -1 (a+b)];

xr(1)=0;
yr(1)=0;
phi(1)=0;

for k=1:length(t)

xrp(k)=uf(k)*cos(phi(k))-ul(k)*sin(phi(k));
yrp(k)=uf(k)*sin(phi(k))+ul(k)*cos(phi(k));

Wd=T*([uf(k);ul(k);w(k)]);

Wd1(k)=Wd(1);
Wd2(k)=Wd(2);
Wd3(k)=Wd(3);
Wd4(k)=Wd(4);

xr(k+1)=xr(k)+ts*xrp(k);
yr(k+1)=yr(k)+ts*yrp(k);
phi(k+1)=phi(k)+ts*w(k);

end

pasos=10; fig=figure;
set(fig,'position',[10 60 980 600]);
axis square; cameratoolbar
grid on;
% axis([-3 3 -3 3 0 1]); grid on
DimensionOmni;
M1=Plot_Omni(xr(1),yr(1),phi(1));
hold on, plot(xr,yr,'g','linewidth',2);

for i=1:pasos:length(t)

delete (M1)
M1=Plot_Omni(xr(i),yr(i),phi(i)); hold on

pause(0.1)

end

OMNIDIRECCIONAL
=============================================================
==========
DRONE
clc
clear all
close all

ts=0.1;
t=0:ts:10;
a=0.1;

% 1. Condiciones iniciales
xc(1) = 0; %pocisión en el eje x en (m)
yc(1) = 0; %pocisión en el eje y en (m)
zr(1) = 0; %pocisión en el eje z en (m)

phi(1)= 0*(pi/180); %orientación en radianes

xr(1)=xc(1)+a*cos(phi(1));
yr(1)=yc(1)+a*sin(phi(1));

% 3) Referencias deseadas
xrd = -2;
yrd = 2;
zrd = 5;
phid=90*(pi/180);

for k=1:length(t)

%a) Errores de control


xre(k) = xrd - xr(k);
yre(k) = yrd - yr(k);
zre(k) = zrd - zr(k);
phie(k) = phid-phi(k);
e = [xre(k);yre(k);zre(k);phie(k)];

%b) Matriz Jacobiana


J=[cos(phi(k)) -sin(phi(k)) 0 -a*sin(phi(k));...
sin(phi(k)) cos(phi(k)) 0 a*cos(phi(k));...
0 0 1 0;...
0 0 0 1];

%c) Matriz de ganancia


K = [0.1 0 0 0;...
0 0.1 0 0;...
0 0 0.5 0;...
0 0 0 0.5];

%d) Ley de control


v = inv(J)*K*e;

uf(k)=v(1);
ul(k)=v(2);
uz(k)=v(3);
w(k)=v(4);

xrp(k)=uf(k)*cos(phi(k))-ul(k)*sin(phi(k))-a*w(k)*sin(phi(k));
yrp(k)=uf(k)*sin(phi(k))+ul(k)*cos(phi(k))+a*w(k)*cos(phi(k));
zrp(k)=uz(k);

xr(k+1)=xr(k)+ts*xrp(k);
yr(k+1)=yr(k)+ts*yrp(k);
zr(k+1)=zr(k)+ts*zrp(k);

phi(k+1)=phi(k)+ts*w(k);

xc(k+1)=xr(k+1)-a*cos(phi(k+1));
yc(k+1)=yr(k+1)-a*sin(phi(k+1));

end

pasos=5; fig=figure;
set(fig,'position',[10 60 980 600]);
axis square; cameratoolbar
grid on;
axis([-3 3 -3 3 0 7]); grid on

M1=Drone6(xc(1),yc(1),zr(1),0,0,phi(1),1);

hold on, plot3(xr,yr,zr);


for i=1:pasos:length(t)

delete (M1)
M1=Drone6(xc(i),yc(i),zr(i),0,0,phi(i),1);

pause(0.1)

end

=============================================================
============ BRAZO ROBOTICO
clc
clear all
close all

ts=0.1;
t=0:ts:60;

% 1. Condiciones iniciales del manipulador


l1=0.5;
l2=0.65;
h=0.25;

q1(1) = 0*(pi/180);
q2(1) = 30*(pi/180);
q3(1) = 30*(pi/180);

xr(1)=l1*cos(q2(1))*cos(q1(1))+l2*cos(q2(1)+q3(1))*cos(q1(1));
yr(1)=l1*cos(q2(1))*sin(q1(1))+l2*cos(q2(1)+q3(1))*sin(q1(1));
zr(1)=h+l1*sin(q2(1))+l2*sin(q2(1)+q3(1));

% 3) Referencias deseadas
% xrd = 0.5;
% yrd = -0.8;
% zrd = 0.2;

xrd = .6*cos(0.2*t);
yrd = .6*sin(0.2*t);
zrd = 0.7*ones(1,length(t));
% zrd =0.7+0.01*t;

xrd_p= -.6*0.2*sin(0.2*t);
yrd_p= .6*0.2*cos(0.2*t);
% zrd_p = 0.01*ones(1,length(t));

zrd_p= 0*ones(1,length(t));

for k=1:length(t)

%a) Errores de control


xre(k) = xrd(k) - xr(k);
yre(k) = yrd(k) - yr(k);
zre(k) = zrd(k) - zr(k);

e = [xre(k);yre(k);zre(k)];

%b) Matriz Jacobiana


J=[ -l1*cos(q2(k))*sin(q1(k))-l2*cos(q2(k)+q3(k))*sin(q1(k)) -
l1*cos(q1(k))*sin(q2(k))-l2*cos(q1(k))*sin(q2(k)+q3(k)) -
l2*cos(q1(k))*sin(q2(k)+q3(k));...
l1*cos(q2(k))*cos(q1(k))+l2*cos(q2(k)+q3(k))*cos(q1(k)) -
l1*sin(q1(k))*sin(q2(k))-l2*sin(q1(k))*sin(q2(k)+q3(k)) -
l2*sin(q1(k))*sin(q2(k)+q3(k));...
0 l1*cos(q2(k))+l2*cos(q2(k)+q3(k)) l2*cos(q2(k)+q3(k))];

%c) Matriz de ganancia


K = [0.2 0 0;...
0 0.2 0;...
0 0 0.2];

%d) Ley de control


% v = inv(J)*K*e;

hd_p=[xrd_p(k) yrd_p(k) zrd_p(k)]';

%d) Ley de control


v = inv(J)*(hd_p+K*tanh(e));

q1p_ref(k)=v(1);
q2p_ref(k)=v(2);
q3p_ref(k)=v(3);

q1(k+1)=q1(k)+q1p_ref(k)*ts;
q2(k+1)=q2(k)+q2p_ref(k)*ts;
q3(k+1)=q3(k)+q3p_ref(k)*ts;

xr(k+1)=l1*cos(q2(k+1))*cos(q1(k+1))+l2*cos(q2(k+1)+q3(k+1))*cos(q1(k+1))
;

yr(k+1)=l1*cos(q2(k+1))*sin(q1(k+1))+l2*cos(q2(k+1)+q3(k+1))*sin(q1(k+1))
;
zr(k+1)=h+l1*sin(q2(k+1))+l2*sin(q2(k+1)+q3(k+1));
end

pasos=5; fig=figure;
set(fig,'position',[10 60 980 600]);
axis square; cameratoolbar
grid on;
axis([-1 1 -1 1 0 3]);

Arm_Parameters;
M1=Arm_Plot3D(0,0,0,0,0,q1(1),q2(1),q3(1),0);

for i=1:pasos:length(t)

delete (M1)
M1=Arm_Plot3D(0,0,0,0,0,q1(i),q2(i),q3(i),0),hold on;
plot3(xr(1:i),yr(1:i),zr(1:i),'r','linewidth',3);
pause(0.1)

end

You might also like