Error using plot Vectors must be the same length.

2020 11 25
1 个回答
更新时间：2020 11 25
7 次查看（30 天）

0 个投票

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 = -6; thr = 0; % ### initial reference states
x = -0.7; y = 0; th = -90*pi/180; % ### initial robot states
%x = -0.7; y = 0; th = -pi/2; % ### initial robot states 1
%x = -0.7; y = 0.5; th = -pi; % ### initial robot states 2
%x = -0.9; y = 0.6; th = -pi/180; % ### initial robot states 3
%xr = 0; yr = -0.6; thr = 0*pi/180; % ### initial reference states 4
%x = 1; y = 1; th = 0; % ### initial robot states 4
Xr = xr; Yr = yr; THr = thr; % initial reference states vectors
X = x; Y = y; TH = th; % initial robot states vectors
% ========================================================================
% ========================================================================
Vr = []; Wr = []; % initial reference velocities vectors
V = []; W = []; % initial robot velocities vectors
% ========================================================================
% ======================== SIMULATION PARAMETERS =========================
% ========================================================================
thetaplot = linspace(0,2*pi,180);
rlength = 0.06; rwidth = 0.04;
ts = 0.05; tstop = 20;
% ==================== Bounds on robot velocities ========================
vmax = 0.4; wmax = pi/2; % (A)
%vmax = 0.9; wmax = pi; % (B)
% ========================================================================
% ============================ SIMULATION LOOP ===========================
% ========================================================================
while T(end)<=tstop % loop until current time exceeds stop time
clf, figure(1), 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)
% ====================================================================
% ============ Two cases of simulations will be used: ================
% ====== either the reference velocities (vr,wr) are given ===========
% ======= or the reference trajectory (x(t),y(t) is given ============
% ====================================================================
% ====================================================================
% ======== In the case that the reference velocities are given =======
% =================== then insert them here ==========================
% ====================================================================
vr = 0.08+abs(0.05*sin(2*pi/tstop*iter*ts)); % current reference translational velocity
wr = 0.5*sin(2*pi/tstop*iter*ts); % current reference rotational velocity
% vr = 0.08+abs(0.08*sin(0.1*pi/tstop*iter*ts)); % current reference translational velocity 1 AND 2
% wr = 0.5*sin(0.1*pi/tstop*iter*ts); % current reference rotational velocity 1 AND 2
% vr = 0.3+abs(0.08*sin(0.1*pi/tstop*iter*ts)); % current reference translational velocity 3
% wr = 0.7*sin(0.1*pi/tstop*iter*ts); % current reference rotational velocity 3
% xr = (20-t)^3+(9*(20-t)^2*t)+(12*(20-t)*t^2+2*t^3) /8000; % 4
% yr = (20-t)^3+(3*(20-t)^2*t)+(9*(20-t)*t^2)/8000; % 4
% x = 1 ; y = 1 ; thr = 0 ; % 4
% ====================================================================
% ====== In the case that the reference position is given then =======
% =========== use the Inverse Kinematics formulas to calculate them ==
% =========== and comment out the previous lines of the code =========
% ====================================================================
% vr = ;
% wr = ;
% Vr = [Vr;vr]; Wr = [Wr;wr]; % insert current reference velocities into reference velocities vectors
% ====================================================================
% ==================== REFERENCE ROBOT ===============================
% ====================================================================
% ====================================================================
% ======== In the case that the reference velocities are given =======
% ====== then use this code to calculate the Forward Kinematics ======
% ====================================================================
[tsolr,xsolr] = ode45(@DifferentialDriveKinematics,[iter*ts (iter+1)*ts],[xr,yr,thr]',[],[vr,wr]); % 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 reference states
% ====================================================================
% === In the case that the reference position is given then insert ===
% =========== the formulas for the reference trajectory here =========
% =========== and comment out the previous lines of the code =========
% ====================================================================
% xr = ;
% yr = ;
% thr = ;
% Xr = [Xr;xr]; Yr = [Yr;yr]; THr = [THr;thr]; % insert current reference states into reference states vector
% ====================================================================
% ======================= CONTROLLER DESIGN ==========================
% ====================================================================
v = vr;
w = wr;
z = 0.5;
g = 20;
% =============== Application of velocity constraints ================
v = sign(vr)*min([abs(vr),vmax]);
w = sign(wr)*min([abs(wr),wmax]);
V = [V;v]; W = [W;w]; % insert the current velocities into the robot velocities vector
% ====================================================================
% ====================== REAL ROBOT ==================================
% ====================================================================
[tsol,xsol] = ode45(@DifferentialDriveKinematics,[iter*ts (iter+1)*ts],[x,y,th]',[],[v,w]); % call ode45 to compute current robot states
xsol = xsol(end,:); tsol = tsol(end); % keep the last index
x = xsol(1); y = xsol(2); th = xsol(3); % these are the current robot states
T = [T;tsol]; % insert current time into time vector
X = [X;x]; Y = [Y;y]; TH = [TH;th]; % insert current robot states into robot states vector
% ====================================================================
% ============================ PLOT FRAME ============================
% ====================================================================
plot(Xr,Yr,'b-','linewidth',2) % plot the reference path
plot(xr,yr,'bo','markersize',7,'markerfacecolor','b') % plot the current reference position
plot(X,Y,'r-','linewidth',2) % plot the robot path
fill(x + cos(th)*rlength*cos(thetaplot) - sin(th)*rwidth*sin(thetaplot),...
y + sin(th)*rlength*cos(thetaplot) + cos(th)*rwidth*sin(thetaplot),[0.8 0 0],'EdgeColor','none','FaceAlpha', 0.8);
plot([x+cos(th)*(-rlength/3)-sin(th)*rwidth x+cos(th)*(rlength/3)-sin(th)*rwidth],...
[y+sin(th)*(-rlength/3)+cos(th)*rwidth y+sin(th)*(rlength/3)+cos(th)*rwidth],'k','linewidth',5);
plot([x+cos(th)*(-rlength/3)-sin(th)*(-rwidth) x+cos(th)*(rlength/3)-sin(th)*(-rwidth)],...
[y+sin(th)*(-rlength/3)+cos(th)*(-rwidth) y+sin(th)*(rlength/3)+cos(th)*(-rwidth)],'k','linewidth',5);
title(sprintf('Blue: Reference Robot - Red: Real 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.04); % a small pause
% ============================ NEXT LOOP =============================
% ====================================================================
iter=iter+1; % go to the next iteration
end % end the simulation loop
% ========================================================================
% ================= PLOTS AFTER SIMULATION ENDS ==========================
% ========================================================================
% Plot the error between reference and real robot states
figure(2), hold on
set(gcf,'color',[1,1,1])
set(gca,'fontname','times','fontsize',14,'box','on')
xlabel('t [sec]','fontname','times','fontsize',14)
ylabel('errors','fontname','times','fontsize',14)
plot(T,Xr-X,'b-','linewidth',2), plot(T,Yr-Y,'r-','linewidth',2)
plot(T,THr-TH,'g-','linewidth',2), plot([0,T(end)],[0,0],'k:')
legend('e_x [m]','e_y [m]','e_\theta [rad]','steady state','location','best'), hold off
axis tight
% Plot the transformed errors
figure(3), hold on
set(gcf,'color',[1,1,1])
set(gca,'fontname','times','fontsize',14,'box','on')
xlabel('t [sec]','fontname','times','fontsize',14)
ylabel('transformed errors','fontname','times','fontsize',14)
E1=cos(TH).*(Xr-X)+sin(TH).*(Yr-Y);
E2=-sin(TH).*(Xr-X)+cos(TH).*(Yr-Y);
E3=THr-TH;
plot(T,E1,'b-','linewidth',2), plot(T,E2,'r-','linewidth',2)
plot(T,E3,'g-','linewidth',2), plot([0,T(end)],[0,0],'k:')
legend('e_1 [m]','e_2 [m]','e_3 [rad]','steady state','location','best'), hold off
axis tight
% Plot the reference and real robot translational velocities
figure(4), hold on
set(gcf,'color',[1,1,1])
set(gca,'fontname','times','fontsize',14,'box','on')
xlabel('t [sec]','fontname','times','fontsize',14)
ylabel('translational velocities','fontname','times','fontsize',14)
plot(T(1:end-1),V,'r-','linewidth',2), plot(T(1:end-1),Vr,'b-','linewidth',2)
legend('\upsilon [m/sec]','\upsilon^r [m/sec]','location','best'), hold off
axis tight
% Plot the reference and real robot rotational velocities
figure(5), hold on
set(gcf,'color',[1,1,1])
set(gca,'fontname','times','fontsize',14,'box','on')
xlabel('t [sec]','fontname','times','fontsize',14)
ylabel('rotational velocities','fontname','times','fontsize',14)
plot(T(1:end-1),W,'r-','linewidth',2), plot(T(1:end-1),Wr,'b-','linewidth',2)
legend('\omega [rad/sec]','\omega^r [rad/sec]','location','best'), hold off
axis tight
-------------------------------------------------------------------------------
the function (that is in a sperate .m file):
function Xdot=DifferentialDriveKinematics(t,X,U)
% This is a function used to simulate the Amigobot's kinodynamic model :
%
% xdot = v cos(theta)
% ydot = v sin(theta)
% xdot = w
%
% Its arguments are : U = (v, w) and X = (x, y, theta), along with the
% simulation time interval t.
Xdot=[U(1)*cos(X(3));
U(1)*sin(X(3));
U(2)];

2 个评论
显示 无 隐藏 无

Walter Roberson
Walter Roberson 2020-11-25
you posted over 300 lines of code but do not show us which line the error is on.
Kunal Kundalia
Kunal Kundalia 2020-11-25
the error occurs after it has ran as the code is displaying the trajectory of two robots one is the real the other is the refrence, they dont seem to ever meet which is why the error occurs about the plot length, it was code provided to me to learn about trajectroy control but i am not sure what i am doing wrong, probley something very simple.

请先登录，再进行评论。

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

回答（1 个）

Walter Roberson
Walter Roberson 2020-11-25

0 个投票

You initialize Vr with capital V, and you plot() Vr with capital V.
Inside your loop you assign values to vr with lower-case v .
Each value that you assign to vr with lower-case v is a scalar.
So probably you need to insert
Vr = [Vr; vr];

类别

在 帮助中心 和 File Exchange 中查找有关 Trajectory Generation 的更多信息

标签

Community Treasure Hunt

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

Start Hunting!

Translated by