mobile robot follow path problem

6 次查看（过去 30 天）
Mohammad Adeeb
Mohammad Adeeb 2021-5-12
编辑： Mohammad Adeeb 2021-5-12
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

请先登录，再回答此问题。

回答（0 个）

类别

在 Help Center 和 File Exchange 中查找有关 Robotics 的更多信息

标签

产品


版本

R2020b

Community Treasure Hunt

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

Start Hunting!

Translated by