my code is this
clc, clear, close all
% ========================================================================
% ========================= INITIALIZATION PHASE =========================
% ========================================================================
iter = 0; T = 0; % initial iteration and time vector
% ========================================================================
% ======= All changes in the initial conditions of the trajectory ========
% ======== and the initial position of the robot are done here ===========
% ========================================================================
xr = 0; yr = 0; thr = 0; % ### initial real robot states
xest = 0; yest = 0; thest = 0; % ### initial estimated robot states
Xr = xr; Yr = yr; THr = thr; % initial real states vectors
Xest = xest; Yest = yest; THest = thest; % initial estimated robot states vectors
% ========================================================================
% ======================== SIMULATION PARAMETERS =========================
% ========================================================================
thetaplot = linspace(0,2*pi,180);
rlength = 0.06; rwidth = 0.04;
ts = 0.05; tstop = 60;
lamda = 0.1; % Distance between the two wheels. Needed for the simulation
% ========================================================================
% ============================ SIMULATION LOOP ===========================
% ========================================================================
while T(end)<=tstop % loop until current time exceeds stop time
figure(1), clf, hold on % close the previous and open a new figure
set(gcf,'color',[1,1,1]) % set figure properties
set(gca,'fontname','times','fontsize',14,'box','on')
xlabel('x [m]','fontname','times','fontsize',14)
ylabel('y [m]','fontname','times','fontsize',14)
% ====================================================================
% ========== For each case of velocity profile that needs ===========
% =================== to be simulated change them here ===============
% ====================================================================
v = 0.5; % current reference translational velocity
w = 0.5; % current reference rotational velocity
% ====================================================================
% ======================= REAL ROBOT =================================
% ====================================================================
% ====================================================================
% =========== This simulates the movement of the real robot ==========
% ======== according to the forward kinematics that are given ========
% ====================================================================
[tsolr,xsolr] = ode45(@DifferentialDriveKinematics,[iter*ts (iter+1)*ts],[xr,yr,thr]',[],[v,w]); % call ode45 to compute current reference states
xsolr = xsolr(end,:); tsolr = tsolr(end); % keep the last index
xr = xsolr(1); yr = xsolr(2); thr = xsolr(3); % these are the current real states
Xr = [Xr;xr]; Yr = [Yr;yr]; THr = [THr;thr]; % insert current real states into reference states vector
T = [T;(iter+1)*ts];
% ====================================================================
% ================ DEAD RECKONING ALGORITHM DESIGN ===================
% ====================================================================
% ====================================================================
% =========== At this point the implementation of the dead ===========
% ================= reckoning algorithm should be given ==============
% ====================================================================
% thest = ;
% xest = ;
% yest = ;
Xest = [Xest;xest]; Yest = [Yest;yest]; THest = [THest;thest]; % insert current reference states into reference states vector
% ====================================================================
% ============================ PLOT FRAME ============================
% ====================================================================
% ========================= REAL ROBOT PLOT FRAME ====================
plot(Xr,Yr,'b-','linewidth',2) % plot the robot path
fill(xr + cos(thr)*rlength*cos(thetaplot) - sin(thr)*rwidth*sin(thetaplot),...
yr + sin(thr)*rlength*cos(thetaplot) + cos(thr)*rwidth*sin(thetaplot),[0 0 0.8],'EdgeColor','none','FaceAlpha', 0.8);
plot([xr+cos(thr)*(-rlength/3)-sin(thr)*rwidth xr+cos(thr)*(rlength/3)-sin(thr)*rwidth],...
[yr+sin(thr)*(-rlength/3)+cos(thr)*rwidth yr+sin(thr)*(rlength/3)+cos(thr)*rwidth],'k','linewidth',5);
plot([xr+cos(thr)*(-rlength/3)-sin(thr)*(-rwidth) xr+cos(thr)*(rlength/3)-sin(thr)*(-rwidth)],...
[yr+sin(thr)*(-rlength/3)+cos(thr)*(-rwidth) yr+sin(thr)*(rlength/3)+cos(thr)*(-rwidth)],'k','linewidth',5);
% ===================== ESTIMATED ROBOT PLOT FRAME ===================
plot(Xest,Yest,'r-','linewidth',2) % plot the robot path
fill(xest + cos(thest)*rlength*cos(thetaplot) - sin(thest)*rwidth*sin(thetaplot),...
yest + sin(thest)*rlength*cos(thetaplot) + cos(thest)*rwidth*sin(thetaplot),[0.8 0 0],'EdgeColor','none','FaceAlpha', 0.8);
plot([xest+cos(thest)*(-rlength/3)-sin(thest)*rwidth xest+cos(thest)*(rlength/3)-sin(thest)*rwidth],...
[yest+sin(thest)*(-rlength/3)+cos(thest)*rwidth yest+sin(thest)*(rlength/3)+cos(thest)*rwidth],'k','linewidth',5);
plot([xest+cos(thest)*(-rlength/3)-sin(thest)*(-rwidth) xest+cos(thest)*(rlength/3)-sin(thest)*(-rwidth)],...
[yest+sin(thest)*(-rlength/3)+cos(thest)*(-rwidth) yest+sin(thest)*(rlength/3)+cos(thest)*(-rwidth)],'k','linewidth',5);
title(sprintf('Blue: Real Robot - Red: Estimated Robot\n Current time [sec]: %.2f',T(end)),...
'fontname','times','fontsize',14) % insert a title with current simulation time
axis tight, axis equal % set the axes to fill the plot and equalize the axes
pause(0.01); % a small pause
% ============================ NEXT LOOP =============================
% ====================================================================
iter=iter+1; % go to the next iteration
end % end the simulation loop
Errors output:
出错 ode45 (line 115)
odearguments(FcnHandlesUsed, solver_name, ode, tspan, y0, options, varargin);
出错 DeadReckoningTemplate (line 67)
[tsolr,xsolr] = ode45(@DifferentialDriveKinematics,[iter*ts (iter+1)*ts],[xr,yr,thr]',[],[v,w]); % call ode45 to compute current reference states
Someone tell where is wrong and how to correct it please

Antworten (1)

Walter Roberson
Walter Roberson am 4 Mai 2020

0 Stimmen

[tsolr,xsolr] = ode45(@DifferentialDriveKinematics,[iter*ts (iter+1)*ts],[xr,yr,thr]',[],[v,w]); % call ode45 to compute current reference states
| | | | |
ODEFUN TSPAN Y0 options ???
There is no documented input for ode45 after options; there has not been a documented input there for over 15 years. You cannot predict what will happen when it encounters that input.

Kategorien

Produkte

Tags

Gefragt:

am 3 Mai 2020

Community Treasure Hunt

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

Start Hunting!

Translated by