mobile robot follow path problem

4 views (last 30 days)
Mohammad Adeeb
Mohammad Adeeb on 12 May 2021
Edited: Mohammad Adeeb on 12 May 2021
hey all;
im trying to make a mobile robot follow a circular path and i did the follwoing :
clc;
close all;
clear all;
clc;
close all;
clear all;
%% define constant
Vd = 4.5; %head velocity in m/s
Wd = 3; %angula velocity in rad/s
R = Vd/Wd; %radius of the circular path
T = 0.01; % simulation time step
t = 0:T:(2*pi*R/Vd); %final time is the time for the robot to go through a full circul path
x0 = 0;
y0 = 1;
th0 = 0;
p = []; %desired path coordinates in cartesian space
u = []; %desired path speed in cartesian space
p(:,1) = [x0,y0,th0]'; %initial mobile robot position vector
u(:,1) = [Vd*cos(th0);Vd*sin(th0);Wd];
%% start simulation to get position and velocity vectors for all time steps
for k = 1:length(t)-1
p(:,k+1) = p(:,k) + T*[Vd*cos(p(3,k));Vd*sin(p(3,k));Wd];
p(3,k+1) = correct_angle(p(3,k+1));
u(:,1) = [Vd*cos(p(3,k+1));Vd*sin(p(3,k+1));Wd];
end
%% start real path inititalization
xr0 = -0.5;
yr0 = -1.5;
thr0 = pi/2;
b = 0.1;
pr0 = [xr0;yr0;thr0]; %actual initial position for mobile robot
pr = [];
pr(:,1) = get_position(pr0,b);
%% start IOSF controller
% p Desired position
% pr actual position
% u Desired speed
% gain controller gains
% b distance from center of mass , it will be always !=0
gain=20*[1,1];
u_controller = [];
for k = 1:length(t)-1
u_controller(:,k) = get_speed(p(:,k),pr(:,k),u(:,k),gain,b);
V_controller = u_controller(1,k);
W_controller = u_controller(2,k);
pr(:,k+1) = pr(:,k) + T*[V_controller*cos(p(3,k));V_controller*sin(p(3,k));W_controller];
draw_robot_follow_path(p,k)
end
function draw_robot_follow_path(p,k)
xmin = -1; %setting the figure limits
xmax = 5;
ymin = 0;
ymax = 5;
mob_l = 0.5; %the Mobile Robot Length
mob_W = 0.25; %The Mobile Robot Width
Tire_W = 0.125; %The Tire Width
Tire_l = mob_l/2; %The Tire Length
plot(p(1,1:k+1),p(2,1:k+1),'-r' ,'LineWidth' , 1.5)
axis square;
hold on
%Body
v1=[mob_l;-mob_W];
v2=[-mob_l/4;-mob_W];
v3=[-mob_l/4;mob_W];
v4=[mob_l;mob_W];
%Right Tier
v5=[Tire_l/2;-mob_W-0.02];
v6=[Tire_l/2;-mob_W-Tire_W-0.02];
v7=[-Tire_l/2;-mob_W-Tire_W-0.02];
v8=[-Tire_l/2;-mob_W-0.02];
%left Tire
v9=[Tire_l/2;mob_W+0.02];
v10=[Tire_l/2;mob_W+0.02+Tire_W];
v11=[-Tire_l/2;mob_W+0.02+Tire_W];
v12=[-Tire_l/2;mob_W+0.02];
%line
v13=[0;-mob_W-0.02];
v14=[0;mob_W+0.02];
%front Tire
v15=[mob_l+Tire_l/2;Tire_W/2];
v16=[mob_l+Tire_l/2;-Tire_W/2];
v17=[mob_l-Tire_l/2;-Tire_W/2];
v18=[mob_l-Tire_l/2;Tire_W/2];
%% calculate Rotational matrix assiciated with theat
x = p(1,k);
y = p(2,k);
th = p(3,k);
R = [cos(th) -sin(th) ;sin(th) cos(th)]; %rotational matrix
p = [x;y]; %position matrix
%% transform all mobile robot corners in accordance
v1=R*v1+p;
v2=R*v2+p;
v3=R*v3+p;
v4=R*v4+p;
v5=R*v5+p;
v6=R*v6+p;
v7=R*v7+p;
v8=R*v8+p;
v9=R*v9+p;
v10=R*v10+p;
v11=R*v11+p;
v12=R*v12+p;
v13=R*v13+p;
v14=R*v14+p;
v15=R*v15+p;
v16=R*v16+p;
v17=R*v17+p;
v18=R*v18+p;
% Body
Body= [v1 v2 v3 v4 v1];
Body_x = Body(1,:);
Body_y = Body(2,:);
plot(Body_x , Body_y ,'-k','LineWidth',2);
fill(Body_x , Body_y ,'k');
axis([xmin xmax ymin ymax]);
axis square;
% Right Tire
R_Tire = [v5 v6 v7 v8 v5];
R_Tire_x = R_Tire(1,:);
R_Tire_y = R_Tire(2,:);
plot(R_Tire_x , R_Tire_y ,'-k','LineWidth',2);
fill(R_Tire_x , R_Tire_y,'r');
axis([xmin xmax ymin ymax]);
axis square;
% Left Tire
L_Tire = [v9 v10 v11 v12 v9];
L_Tire_x = L_Tire(1,:);
L_Tire_y = L_Tire(2,:);
plot(L_Tire_x , L_Tire_y ,'-k','LineWidth',2);
fill(L_Tire_x , L_Tire_y,'r');
axis([xmin xmax ymin ymax]);
axis square;
%line
line = [v13 v14];
line_x = line(1,:);
line_y = line(2,:);
plot(line_x , line_y ,'-y' ,'LineWidth',2);
axis([xmin xmax ymin ymax]);
axis square;
%front Tire
F_Tire = [v15 v16 v17 v18 v15];
F_Tire_x = F_Tire(1,:);
F_Tire_y = F_Tire(2,:);
plot(F_Tire_x, F_Tire_y ,'-k' ,'LineWidth',2);
fill(F_Tire_x , F_Tire_y,'r');
axis([xmin xmax ymin ymax]);
axis square;
hold off;
drawnow;
end
function [ u_controller ] = get_speed(p,pr,u,gain,b)
gain1=gain(1);
gain2=gain(2);
xd_dot = u(1);
yd_dot = u(2);
xd = p(1);
yd = p(2);
thd = p(3);
x_b = pr(1); %for point b
y_b = pr(2);
th_b = pr(3);
ex = xd-x_b;
ey = yd-y_b;
xb_dot = xd_dot + gain1*ex;
yb_dot = yd_dot + gain2*ey;
vec1 = [xb_dot;yb_dot];
vec2 = [cos(th_b) -b*sin(th_b);sin(th_b) b*cos(th_b)];
u=vec2\vec1;
end
function pb = get_position(p,b)
xreal = p(1);
yreal = p(2);
threal = p(3);
xb = xreal+b*cos(threal);
yb = yreal+b*sin(threal);
pb=[xb;yb;threal];
end
function [ out ] = correct_angle( in )
%correct angle from -pi to pi
out = mod(in,2*pi);
out(out>pi) = out(out>pi)-2*pi;
end
and i get the follwoing error :
Output argument "u_controller" (and maybe others) not assigned during call to "follow_path_mobilrR>get_speed".
Error in follow_path_mobilrR (line 54)
u_controller(:,k) = get_speed(p(:,k),pr(:,k),u(:,k),gain,b);
what should i do , and am i writting the code correctly?
thanks in advance

Answers (0)

Products


Release

R2020b

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!