Transcript Upravljanje mobilnim robotima

Upravljanje mobilnim robotima
Vježba 1
Local Incremental Planning for Nonholonomic Mobile
Robots (De Luca & Oriolo)
=u2=Feedforward control
%referentna trajektorija (u obliku Gaussiana, zaobilazenje prepreke)
Xref = Vx*t;
Yref = Ymax*exp(-sigg*(Xref-Xxc)*(Xref-Xxc));
% Gaussian
dXref = Vx;
ddXref = 0;
dYref = -2*sigg*Vx*(Xref-Xxc)*Yref;
ddYref = -2*sigg*Vx*dXref*Yref-2*sigg*Vx*(Xref-Xxc)*dYref;
%
%
%
%
Vd2 = dXref*dXref + dYref*dYref;
dTheta = (ddYref*dXref - ddXref*dYref)/Vd2;
% u1_ref^2
% prva derivacija od Theta_ref
u1 = k_T*(dXref*cos(y(3))+dYref*sin(y(3)));
u2 = dTheta;
prva derivacija od Xref
prva derivacija od Xref
prva derivacija od Yref
druga derivacija od Yref
% Local Incremental Planning for Nonholonomic Mobile Robots
%----Kinematika mobilnog robota---------------%
dy(1) = u1*cos(y(3));
dy(2) = u1*sin(y(3));
dy(3) = u2;
%---------------------------------------------%
clear;
T=10.0;
DT=0.01;
%vrijeme simulacije
%korak integracije
global Vx Ymax sigg Xxc k_T k_R
Vx=1;
Ymax=1;
sigg=2.0;
Xxc=Vx*T/2;
k_T = 1;
k_R = 40;
x10 = 0.0;
%initial conditions
x20 = 0.0;
%initial conditions
x30 = 0.0;
%initial conditions
tT = 0:DT:T;
options = odeset('RelTol',1e-8,'AbsTol',1e-8);
[t,y] = ode45('MobRobot',tT,[x10 x20 x30],options);
10
1.5
1.2
1
1
0.5
X
Xref
0
0
5
t, s
10
1
-0.5
0.8
Y
Yref
0
0
5
t, s
0.6
10
Y
5
0.4
0.5
0.2
0

-0.5
0
referentna trajektorija
feedback
ref
-1
0
5
t, s
10
-0.2
0
1
2
3
4
5
X
6
7
8
9
10
u2=Feedback control
u1 = k_T*(dXref*cos(y(3))+dYref*sin(y(3)));
u2 = k_R*(atan2(dYref, dXref)-y(3));
12
1
1.2
10
8
0.5
1
6
4
0
X
Xref
2
0
0
2
4
6
8
Y
Yref
10
t, s
0
2
4
6
8
10
t, s
0.6
Y
1
0.4
0.5
0
0.2

ref
-0.5
-1
-0.5
0.8
0
2
4
6
8
0
10
referentna trajektorija
feedback
t, s
-0.2
0
2
4
6
X
8
10
12