Transcript Metoda potencijalnih polja

Reaktivna navigacija mobilnih robota primjenom
metode potencijalnih polja
Vježbe iz Opće teorije sustava
Reaktivna navigacija mobilnog robota primjenom
metode potencijalnih polja
Atraktivni potencijal
Suma potencijala
Repulzivni potencijal
Reaktivna navigacija u okolini s preprekama
Metoda potencijalnih polja
a) atraktivni potencijal (bez prepreke)
U a
x
U a
y
y
x
Ua 
1 
2
2
ka  x  xa    y  ya  

2 
b) atraktivni + repulzivni potencijal (sa preprekom)
U a U r

x
x
U
U r
y a 
y
y
x
 ( x  x )2  ( y  yr )2 
1

U r  kr e  r
2
c) atraktivni + repulzivni + rotacijski potencijal (sa preprekom)
U a U r
U r


x
x
y
U U
U r
y  a  r 
y
y
x
x
Dokaz stabilnosti po Lyapunovu za atraktivni potencijal
x  x  xa
y  y  ya
Ua 
1
ka  x 2  y 2 
2
Ua 
U a
 ka x
x
U
y   a   ka y
y
x
1
ka  2 xx  2 yy   ka xx  ka yy  ka2 x 2  ka2 y 2  0
2
Za općeniti atraktivni potencijal, Ua(x,y)>0
U a
U a
U a
U a ( x, y ) 
x
y
x
y
x
 U a

 x
 U a

 y
 U a

 y
2

 U a   U a 



 0

 

x

y





2
Lyapunovljeva funkcija rotacijskog potencijala Ur(x,y)
U r
y
U
y r
x
x
U r ( x, y) 
U r
U
U r  U r  U r
x r y 


x
y
x  y  y
 U r

 x

0

Zadatak
4
x0=0.0;
y0=0.0;
3.5
attr_x = 4.0;
attr_y = 4.0;
3
x_o=2.0;
y_o=2.0;
y
Radijus=0.5;
2.5
2
1.5
1
0.5
......
vel_xd = -Fx_attr + Fx_rep - m*Fy_rep;
vel_yd = -Fy_attr + Fy_rep +m*Fx_rep;
dy(1) = vel_xd;
dy(2) = vel_yd;
0
0
0.5
1
1.5
2
x
2.5
3
3.5
4
% main.m; metoda potencijalnih polja: atraktivni potencijal +
% + repulzivni potencijal + rotacijski potencijal
clear;
clc
T=10;
%vrijeme simulacije
Ndot=200;
%za crtanje krugova
global a1 Sigma attr_x attr_y x_o y_o b1 b2
x0=0.0; y0=0.0;
attr_x = 4.0; attr_y = 4.0;
x_o=2.0; y_o=2.0;
a1=1.0; b1=20.0; b2=1.0;
Sigma=2.0;
Radijus=0.5;
%-------------------------------------------------%
options = odeset('RelTol',1e-6,'AbsTol',1e-6);
[t,y] = ode45('Potential',[0 T],[x0 y0],options);
%-------------------------------------------------%
for i=1:Ndot,
xx01(i) = x_o + Radijus*cos(2*pi*i/Ndot);
yy01(i) = y_o + Radijus*sin(2*pi*i/Ndot);
end
figure(1)
plot(y(:,1),y(:,2),xx01,yy01,'r', 'linewidth',2.5),
xlabel('x'),ylabel('y')
function dy = Potential(t,y)
dy = zeros(2,1);
global a1 Sigma attr_x attr_y x_o y_o b1 b2
Fx_attr = (y(1)-attr_x);
Fy_attr = (y(2)-attr_y);
r2 = (y(1)-x_o)^2 + (y(2)-y_o)^2;
Fx_rep = (y(1)-x_o)*exp(-Sigma*r2);
Fy_rep = (y(2)-y_o)*exp(-Sigma*r2);
vel_xd = -a1*Fx_attr + b1*Fx_rep - b2*Fy_rep;
vel_yd = -a1*Fy_attr + b1*Fy_rep + b2*Fx_rep;
%--------------------------------------------%
dy(1) = vel_xd;
dy(2) = vel_yd;
Analitički fuzzy regulator
 re  r   k  ( yr  y) cos    ( xr  x)sin  
re  r   k

vp 
r 
no  r  ( xr  x) 2  ( yr  y ) 2
u1  Vmax  tanh(k2  no)  e k3 ( vp )
....
Xref=y(1); Yref=y(2);
X=y(3); Y=y(4); Theta=y(5);
2
norma=sqrt((X-Xref)^2+(Y-Yref)^2);
vecprod=(Yref-Y)*cos(Theta)-(Xref-X)*sin(Theta);
vecnorm=vecprod/(norma+0.0001);
u2  max  tanh  k1  vp 
x  u1 cos  
y  u1 sin  
  u2
v_max = 1;
w_max = 3;
k1 = 20;
k2 = 20;
k3 = 20;
u1 = v_max*tanh(k2*norma)*exp(-k3*vecnorm^2);
u2 = w_max*tanh(k1*vecnorm);
....
referentna
trajektorija
y
r
rr
......
re
dy(1) = (v_max/sqrt(2))*tanh(vel_xd);
dy(2) = (v_max/sqrt(2))*tanh(vel_yd);

dy(3) = u1*cos(Theta);
dy(4) = u1*sin(Theta);
dy(5) = u2;
rm
x
Simulacijski rezultati – trajektorije u ravnini
4
referentna trajektorija
trajektorija mobilnog robota
prepreka
3.5
3
Y [m]
2.5
2
1.5
1
0.5
0
0
0.5
1
1.5
2
X [m]
2.5
3
3.5
4
4
3
3
2
1
0
1.5
1
 [rad]
4
Y [m]
X [m]
Simulacijski rezultati – varijable stanja i upravljanja
2
1
0
5
0
10
0
10
-1
0
5
10
t [s]
4
2
u2 [rad/s]
u1 [m/s]
5
t [s]
0.8
0.6
0.4
referentna trajektorija
trajektorija mobilnog robota
0
-2
0.2
0
0
-0.5
t [s]
1
0.5
0
5
t [s]
10
-4
0
5
t [s]
10
Formacije mobilnih robota
Referentne trajektorije grupe od tri mobilna robota:
xj  
yj  
V
x j
V
y j
j  1, 2,3
r12  ( x1  x2 ) 2  ( y1  y2 ) 2
Udaljenosti2 između mobilnih robota:
r23  ( x2  x3 ) 2  ( y2  y3 ) 2
r31  ( x3  x1 ) 2  ( y3  y1 ) 2
Potencijalna funkcija konfiguracije mobilnih robota
(s minimumom za željenu konfiguraciju jednakostraničnog trokuta, duljine stranice R):
V
2
2
2
1 2
1
1
r12  R 2    r232  R 2    r312  R 2 

4
4
4
Referentne trajektorije formacije mobilnih robota
r
r
V
1
1
x1  
   r12  R 2  12   r31  R 2  31
x1
2
x1 2
x1
x1  
V
   r12  R 2  ( x1  x2 )   r31  R 2  ( x3  x1 )
x1
r
r
V
1
1
y1  
   r12  R 2  12   r31  R 2  31
y1
2
y1 2
y1
y1  
V
   r12  R 2  ( y1  y2 )   r31  R 2  ( y3  y1 )
y1
x2  
r
r
V
1
1
   r12  R 2  12   r23  R 2  23
x2
2
x2 2
x2
x2  
y2  
r
r
V
1
1
   r12  R 2  12   r23  R 2  23
y2
2
y2 2
y2
V
  r12  R 2  ( x1  x2 )   r23  R 2  ( x2  x3 )
x2
y2  
V
  r12  R 2  ( y1  y2 )   r23  R 2  ( y2  y3 )
y2
x3  
r
r
V
1
1
   r23  R 2  23   r31  R 2  31
x3
2
x3 2
x3
x3  
y3  
r
r
V
1
1
   r23  R 2  23   r31  R 2  31
y3
2
y3 2
y3
V
  r23  R 2  ( x2  x3 )   r31  R 2  ( x3  x1 )
x3
y3  
V
  r23  R 2  ( y2  y3 )   r31  R 2  ( y3  y1 )
y3
% main.m
clear
clc
T=1.0;
global R
R=2.0
xx0=[-3 2 7 -1 4 -5]
options = odeset('RelTol',1e-8,'AbsTol',1e-8);
[t,y] = ode45('multirob',[0 T],xx0,options);
figure(1)
for i=1:3
hold on
plot(y(:,2*i-1),y(:,2*i), 'linewidth',2)
xlabel('X [m]','FontSize',16,'FontName','Times'), ylabel('Y [m]','FontSize',16,'FontName','Times'),
end
x1=y(:,1); y1=y(:,2); x2=y(:,3); y2=y(:,4); x3=y(:,5); y3=y(:,6);
r12=sqrt((x1-x2).^2+(y1-y2).^2); r23=sqrt((x2-x3).^2+(y2-y3).^2); r31=sqrt((x3-x1).^2+(y3-y1).^2);
figure(2)
subplot(2,2,1), plot(t,r12,'r','linewidth',2)
xlabel('t [s]','FontSize',16,'FontName','Times'), ylabel('r_{12} [m]','FontSize',16,'FontName','Times'),
subplot(2,2,2), plot(t,r23,'b','linewidth',2)
xlabel('t [s]','FontSize',16,'FontName','Times'), ylabel('r_{23} [m]','FontSize',16,'FontName','Times'),
subplot(2,2,3), plot(t,r31,'g','linewidth',2)
xlabel('t [s]','FontSize',16,'FontName','Times'), ylabel('r_{31} [m]','FontSize',16,'FontName','Times'),
%multirob.m
function dy = multirob(t,y)
dy = zeros(6,1);
global R
x1=y(1); y1=y(2);
x2=y(3); y2=y(4);
x3=y(5); y3=y(6);
r12=sqrt((x1-x2)^2+(y1-y2)^2);
r23=sqrt((x2-x3)^2+(y2-y3)^2);
r31=sqrt((x3-x1)^2+(y3-y1)^2);
dy(1) = -(r12^2-R^2)*(x1-x2) + (r31^2-R^2)*(x3-x1);
dy(2) = -(r12^2-R^2)*(y1-y2) + (r31^2-R^2)*(y3-y1);
dy(3) = (r12^2-R^2)*(x1-x2) - (r23^2-R^2)*(x2-x3);
dy(4) = (r12^2-R^2)*(y1-y2) - (r23^2-R^2)*(y2-y3);
dy(5) = (r23^2-R^2)*(x2-x3) - (r31^2-R^2)*(x3-x1);
dy(6) = (r23^2-R^2)*(y2-y3) - (r31^2-R^2)*(y3-y1);
Simulacijski rezultati
2
1
-1
[m]
5
0
0.5
1
t [s]
[m]
10
8
6
31
-4
4
2
-5
-4
-2
0
2
X [m]
4
6
8
3
0
0.5
t [s]
2
1
0
0.5
t [s]
-2
-3
4
23
10
0
r
Y [m]
r
12
0
5
r
[m]
15
1
1
Zadatak
•
Treba dodati četvrtog robota koji slijedi zadanu referentnu trajektoriju
(x4,ref = 0.2*t; y4,ref = 0.2*t) dok jedan od preostala tri robota slijedi
četvrtog na udaljenosti R
8
6
2.5
4
2
2
0
0
2
20
1.5
1
40
0
-2
8
5
6
4
r43 [m]
0
4
0
-2
0
2
X [m]
xx0=[-3 2 2 -1 3 -3 0 0]
4
6
8
40
3
2
2
-4
-4
20
t [s]
t [s]
r31 [m]
Y [m]
4
r23 [m]
r12 [m]
6
0
20
t [s]
40
1
0
20
t [s]
40