АЛГОРИТМ УПРАВЛЕНИЯ - БУРИДАНОВ ОСЕЛ

Целевая точка, это пчелка Bee с ведерком пыльцы, которую преследует робот. Угол поворота колес явно не задается, а колеса поворачиваются в сторону целевой точки, двигающейся над площадкой по кругу (программатор решает уравнение маятника).


%toolbox runge, %toolbox traffic, 

% THE ROBOT CONTROL,
if tick=0, C=robot(10), 
P=iniFG('dot.gif' 10 10),  
moveXat(C 220), moveYat(C 150), 
moveXat(P 220+100), moveYat(P 150), 
V=70, dt=0.3, Version=1, % 1 2 3,
% CAR dX/dt=B(V),
X=[0 0 0], 
% PROGRAMER dZ/dt=H*Z, 
h=0.4, H=[0 -h*dt 0; h*dt 0 0; 0 0 0], 
Z=[100 0 0],
end, 
moveXat(C 220+X[0]), 
moveYat(C 150+X[1]), turnat(C X[2]),
moveXat(P 220+Z[0]), 
moveYat(P 150+Z[1]), Z=Runge2(Z dt), X=Runge(X dt),
show(C P), 
call ticker(10), 

% SOLVE DIFFERENTIAL EQUATIONS dZ/dt=Fun(X) dX/dt=Fun2(X),

function: Fun(X),
% F aim angle | f car angle | s wheel angle,
dx=Z[0]-X[0],  dy=Z[1]-X[1], 
F=atan(dy/dx), if dx<0,
if dy<0, F=F-PI, else, F=PI+F, end, end,
f=X[2],  % f=Pi2Pi(X[2]), 
if f>PI,  f=f-2*PI, end,
if f<-PI, f=f+2*PI, end,
X[2]=f, s=F-f, % s=sat(s 0.8), 
if Version=1, 
B=[V*cos(f) V*sin(f) 0.1*V*tan(s) ], else,
B=[V*cos(s)*cos(f)  V*cos(s)*sin(f) 0.1*V*sin(s)], 
end,
X={B*dt}, return X,
end,

function: Fun2(Z),
if Version=3, dx=Z[0]-X[0], dy=Z[1]-X[1], 
R=30*dt/(abs(dx)+abs(dy)),  H[1][0]=-R,   H[0][1]=R, end, 
Z={H*Z}, return Z,
end,

Если указатель попадает в зону за спиной, то возникает неопределенность в повороте (буриданов осел), которую простой алгоритм не разрешает. Робот не может выбрать, куда именно свернуть, и удаляется (демонстрируется).

СОРЕВНОВАНИЕ АЛГОРИТМОВ УПРАВЛЕНИЯ

Тот же пример, изменим АЛГОРИТМ управления. Все хорошо до тех пор, пока при опережающей скорости автомобиль не обгонит целевую точку. Скорость гасится любопытным образом, вилянием на дороге. В вариациях бывают движения кругами вокруг медленной целевой точки. Точь в точь мамаша и нетерпеливый сынок.


%toolbox runge, %toolbox traffic, 

% THE ROBOT CONTROL,
if tick=0, C=robot(10), 
P=iniFG('dot.gif' 10 10),  
moveXat(C 220), moveYat(C 150), 
moveXat(P 220+100), moveYat(P 150), 
V=70, dt=0.3, Version=2, % 1 2 3,
% CAR dX/dt=B(V),
X=[0 0 0], 
% PROGRAMER dZ/dt=H*Z, 
h=0.4, H=[0 -h*dt 0; h*dt 0 0; 0 0 0], 
Z=[100 0 0],
end, 
moveXat(C 220+X[0]), 
moveYat(C 150+X[1]), turnat(C X[2]),
moveXat(P 220+Z[0]), 
moveYat(P 150+Z[1]), Z=Runge2(Z dt), X=Runge(X dt),
show(C P), 
call ticker(10), 

% SOLVE DIFFERENTIAL EQUATIONS dZ/dt=Fun(X) dX/dt=Fun2(X),

function: Fun(X),
% F aim angle | f car angle | s wheel angle,
dx=Z[0]-X[0],  dy=Z[1]-X[1], 
F=atan(dy/dx), if dx<0,
if dy<0, F=F-PI, else, F=PI+F, end, end,
f=X[2],  % f=Pi2Pi(X[2]), 
if f>PI,  f=f-2*PI, end,
if f<-PI, f=f+2*PI, end,
X[2]=f, s=F-f, % s=sat(s 0.8), 
if Version=1, 
B=[V*cos(f) V*sin(f) 0.1*V*tan(s) ], else,
B=[V*cos(s)*cos(f)  V*cos(s)*sin(f) 0.1*V*sin(s)], 
end,
X={B*dt}, return X,
end,

function: Fun2(Z),
if Version=3, dx=Z[0]-X[0], dy=Z[1]-X[1], 
R=30*dt/(abs(dx)+abs(dy)),  H[1][0]=-R,   H[0][1]=R, end, 
Z={H*Z}, return Z,
end,

АДАПТИВНОЕ УПРАВЛЕНИЕ

Адаптивный АЛГОРИТМ управления. Если не адаптировать движение целевой точки, то автомобиль будет срезать углы, не успевая отрабатывать траекторию или, наоборот, обгонять точку, попадая в мертвую зону. Выход прост. Пусть целевая точка тормозит или, наоборот, прибавляет скорость движения в зависимости от положения автомобиля. В момент старта такая управляющая точка ждет автомобиль.


%toolbox runge, %toolbox traffic, 

% THE ROBOT CONTROL,
if tick=0, C=robot(10), 
P=iniFG('dot.gif' 10 10),  
moveXat(C 220), moveYat(C 150), 
moveXat(P 220+100), moveYat(P 150), 
V=70, dt=0.3, Version=3, % 1 2 3,
% CAR dX/dt=B(V),
X=[0 0 0], 
% PROGRAMER dZ/dt=H*Z, 
h=0.4, H=[0 -h*dt 0; h*dt 0 0; 0 0 0], 
Z=[100 0 0],
end, 
moveXat(C 220+X[0]), 
moveYat(C 150+X[1]), turnat(C X[2]),
moveXat(P 220+Z[0]), 
moveYat(P 150+Z[1]), Z=Runge2(Z dt), X=Runge(X dt),
show(C P), 
call ticker(10), 

% SOLVE DIFFERENTIAL EQUATIONS dZ/dt=Fun(X) dX/dt=Fun2(X),

function: Fun(X),
% F aim angle | f car angle | s wheel angle,
dx=Z[0]-X[0],  dy=Z[1]-X[1], 
F=atan(dy/dx), if dx<0,
if dy<0, F=F-PI, else, F=PI+F, end, end,
f=X[2],  % f=Pi2Pi(X[2]), 
if f>PI,  f=f-2*PI, end,
if f<-PI, f=f+2*PI, end,
X[2]=f, s=F-f, % s=sat(s 0.8), 
if Version=1, 
B=[V*cos(f) V*sin(f) 0.1*V*tan(s) ], else,
B=[V*cos(s)*cos(f)  V*cos(s)*sin(f) 0.1*V*sin(s)], 
end,
X={B*dt}, return X,
end,

function: Fun2(Z),
if Version=3, dx=Z[0]-X[0], dy=Z[1]-X[1], 
R=30*dt/(abs(dx)+abs(dy)),  H[1][0]=-R,   H[0][1]=R, end, 
Z={H*Z}, return Z,
end,



Rambler's Top100