%%%% WHEELED MOBILE ROBOT %% clear all; close all; clc;
global r b d a mc mw Ic Iw Im L1 L2 % WMR paramters global xe ye rx ry ell_an start_an w % Trajectory information global Kp1 Kd1 Kp2 Kd2
d2r=pi/180; COMPUTE=1; %% SYSTEM PARAMETERS OF WMR r= 0.15; % Radius of each wheel b= 0.75; % Wheel Base d= 0.30; % Forward distance of Center of mass from center of rear axle a= 2.00; mc= 30.00; mw= 1.00; Ic= 15.625; Iw= 0.005; Im= 0.0025; L1 = 0.25; L2 =0.25; % Coord of Look Ahead point in WMR ref frame %% TRAJECTORY (ELLIPSE) INFORMATION xe=2; % x center of ellipse ye=0; % y center of ellipse rx=1.75;% Half-length of major axis ry=1.25; % Half-length of minor axis ell_an=45*d2r; % Angle subtended between major axis and horizontal start_an= 0*d2r; % Initial phase at t=0 w=72*d2r; %% INITIAL CONDITIONS t=0; dist=1; % Initial Disturbance % Position of look ahead point x_E0=xe+rx*cos(w*t + start_an)*cos(ell_an) + ry*sin(w*t + start_an)*(-sin(ell_an)); % Initial X on ellipse y_E0=ye+rx*cos(w*t + start_an)*sin(ell_an) + ry*sin(w*t + start_an)*(cos(ell_an)); % Initial Y on ellipse
% xd_0= w*( -rx*sin(w*t + start_an)*cos(ell_an) + ry*cos(w*t + start_an)*(-sin(ell_an)) ); % Initial X-vel on ellipse % yd_0= w*( -rx*sin(w*t + start_an)*sin(ell_an) + ry*cos(w*t + start_an)*(cos(ell_an)) ); % Initial Y-vel on ellipse % v=sqrt(xd_0^2+yd_0^2);
phi_0 = 180*d2r; v= 1; % Linear velocity of CM om= -32*d2r; % Ang velocity of WMR about CM th_R_0= 0*d2r; th_L_0= 0*d2r;
TR_C2O = [cos(phi_0), -sin(phi_0); sin(phi_0), cos(phi_0)]; % Transformation from CM cood system to Absolute cood sys X_C_0= [x_E0,y_E0]' - TR_C2O*[L1,L2]'; % Initial position of CM x_0= X_C_0(1)+dist; % Initial x position of center of mass with disturbance y_0= X_C_0(2)+dist; % Initial y position of center of mass with disturbance c= r/(2*b); S= [c*(b*cos(phi_0)-d*sin(phi_0)), c*(b*cos(phi_0)+d*sin(phi_0)); c*(b*sin(phi_0) + d*cos(phi_0)),c*(b*sin(phi_0) - d*cos(phi_0)); c,-c]; Nu= pinv(S)*([v*cos(phi_0), v*sin(phi_0), om]'); thd_R_0= Nu(1); thd_L_0= Nu(2); %
% SIMULATION PARAMETERS n=360; % total number of points in simulation sim_time=10; % total simulation time dt=sim_time/n; tspan=0:dt:sim_time; % tspan=[0,10]; temp=COMPUTE; SIMU=1; switch(SIMU) case 1 % Simulation 1 Kp1=5; Kp2=Kp1; % Position Level Control Weights Kd1=10; Kd2=Kd1; % Velocity Level Control Weights txt=['Kp',num2str(Kp1),'Kd',num2str(Kd1),'.mat']; if COMPUTE==1 options= odeset('RelTol',1e-3,'AbsTol',[1e-3, 1e-3, 1e-3, 1e-3, 1e-3,1e-3, 1e-3]); X0=[x_0, y_0, phi_0, th_R_0, th_L_0, thd_R_0, thd_L_0]; [t,Y]=ode45(@WMR_TRAJTRACK,tspan,X0,options,L1,L2); % Variable Time Step % Y=ode4(@WMR_TRAJTRACK,tspan,X0); t=tspan; % Fixed Time Step else load(txt); % Load the pre-existing file if no computation required end txt1=['Wheeled Mobile Robot Control;Kd1=',num2str(Kd1),'Kd2=',num2str(Kd2),'Kp1=',num2str(Kp1),'Kp2=',num2str(Kp2)]; plotbot_WMR(t,Y,1,txt1); %Plot trajectory and errors if temp==1 save(txt); % If computation is done, save to file for later use end otherwise disp('incorrect TRIG value'); end
%% FUNCTION FOR PLOTTING THE SIMULATED OUTPUT FOR A GIVEN TIME VECTOR AND %%% JOINT SPACE ANGLE VECTORS
% Course: Robotic Manipulation and Mobility % Advisor: Dr. V. Krovi % % Homework Number: 6 % % Names: Sourish Chakravarty % Hrishi Lalit Shah
function plotbot_WMR(t,X,index,txt1) global b d L1 L2%r a mc mw Ic Iw Im % WMR paramters global xe ye rx ry ell_an start_an w % Trajectory information aviobj = avifile([txt1,'.avi'],'compression','Cinepak'); % Declare an avi object close all; figure(index*3-2); cla('reset'); axis manual; axis([-1 5 -3 3]); hold on; grid on; % T=[0:0.1:20] x_E=xe+rx*cos(w*t + start_an)*cos(ell_an) + ry*sin(w*t + start_an)*(-sin(ell_an)); % Initial X on ellipse y_E=ye+rx*cos(w*t + start_an)*sin(ell_an) + ry*sin(w*t + start_an)*(cos(ell_an)); % Initial Y on ellipse plot(x_E,y_E,'-k'); x_E1=x_E; y_E1=y_E; title(txt1); %% Initial Sketch x_C=X(1,1); y_C=X(1,2); phi=X(1,3); wx=0.2; wy=wx*2; TR_C2O= [cos(phi), -sin(phi), x_C; sin(phi), cos(phi), y_C; 0,0,1]; % Transformation from CM cood system to Absolute cood sys WMR_COORD=[ -d -d -d+wy -d+wy d 1.5*d d -d+wy -d+wy -d; -b-wx b+wx b+wx b b 0 -b -b -b-wx -b-wx; 1 1 1 1 1 1 1 1 1 1]; WMR_PLOT=TR_C2O*WMR_COORD; WMR_PLOT1=TR_C2O*[L1;L2;1]; h1=plot(WMR_PLOT(1,:),WMR_PLOT(2,:),'r','Erasemode','xor');% WMR PLOTTING h3=plot(WMR_PLOT1(1),WMR_PLOT1(2),'bo','Erasemode','xor'); % PLot Look ahead point ctr=0; for i=1:4:length(t) x_C=X(i,1); y_C=X(i,2); phi=X(i,3); % thR=X(i,4); % thL=X(i,5); ctr=ctr+1; TR_C2O= [cos(phi), -sin(phi), x_C; sin(phi), cos(phi), y_C; 0,0,1]; % Transformation from CM cood system to Absolute cood sys %% ELLIPSE INFORMATION : DESIRED OUTPUTS x_E=xe+rx*cos(w*t(i) + start_an)*cos(ell_an) + ry*sin(w*t(i) + start_an)*(-sin(ell_an)); % Initial X on ellipse y_E=ye+rx*cos(w*t(i) + start_an)*sin(ell_an) + ry*sin(w*t(i) + start_an)*(cos(ell_an)); % Initial Y on ellipse
WMR_PLOT=TR_C2O*WMR_COORD; WMR_PLOT1=TR_C2O*[L1;L2;1]; x_L=WMR_PLOT1(1); y_L=WMR_PLOT1(2); WMR_Coord_List(ctr,:)=[WMR_PLOT1(1),WMR_PLOT1(2)]; %Store for later use
err1(ctr)= sqrt((x_E-x_L)^2 + (y_E-y_L)^2); time(ctr)= t(i); set(h1,'Xdata',WMR_PLOT(1,:),'Ydata',WMR_PLOT(2,:));%WMR PLOTTING set(h3,'Xdata',WMR_PLOT1(1),'Ydata',WMR_PLOT1(2)); % PLot Look ahead point plot(WMR_Coord_List(ctr,1),WMR_Coord_List(ctr,2),'yo');
pause(0.05); %Stop execution for 0.01 to make animation visible frame= getframe(gcf); %Step 2: Grab the frame aviobj = addframe(aviobj,frame); % Step 3: Add frame to avi object end aviobj = close(aviobj) % Close the avi object hold off % axis equal; % disp(max(err1)); figure(index*3-1); plot(time,err1); figure(index*3); plot(WMR_Coord_List(:,1),WMR_Coord_List(:,2),'bo'); hold on; plot(x_E1,y_E1,'-k');
function [dX]= WMR_TRAJTRACK(t,X,L1,L2)
global b d mc mw Ic Iw Im r%L1 L2%a %WMR paramters global xe ye rx ry ell_an start_an w % Trajectory information global Kp1 Kp2 Kd1 Kd2
% t %% CURRENT STATE INFORMATION x_C= X(1); y_C= X(2); phi= X(3); % th_R= X(4); % Not required since wheels are shown as stationary % th_L= X(5); thd_R= X(6); thd_L= X(7);
c= r/(2*b); S= [ c*(b*cos(phi)-d*sin(phi)), c*(b*cos(phi)+d*sin(phi)); c*(b*sin(phi) + d*cos(phi)), c*(b*sin(phi) - d*cos(phi)); c, -c; 1, 0; 0, 1];
Nu= [thd_R,thd_L]'; % Minimal coordinates
Q_dot = S*Nu; % Extended coordinates xd_C= Q_dot(1); yd_C=Q_dot(2); om= Q_dot(3); % Phi_dot=omega
Sd = [ c*om*(-b*sin(phi)-d*cos(phi)), c*om*(-b*sin(phi)+d*cos(phi)); c*om*(b*cos(phi) - d*sin(phi)),c*om*(b*cos(phi) + d*sin(phi)); 0, 0; 0, 0; 0, 0];
TR_C2O=[cos(phi), -sin(phi); sin(phi), cos(phi)]; % Transformation matrix from WMR to Global Ref %% ELLIPSE INFORMATION : DESIRED OUTPUTS
x_E=xe+rx*cos(w*t + start_an)*cos(ell_an) + ry*sin(w*t + start_an)*(-sin(ell_an)); % Initial X on ellipse y_E=ye+rx*cos(w*t + start_an)*sin(ell_an) + ry*sin(w*t + start_an)*(cos(ell_an)); % Initial Y on ellipse
xd_E= w*( -rx*sin(w*t + start_an)*cos(ell_an) + ry*cos(w*t + start_an)*(-sin(ell_an)) ); % Initial X-vel on ellipse yd_E= w*( -rx*sin(w*t + start_an)*sin(ell_an) + ry*cos(w*t + start_an)*(cos(ell_an)) ); % Initial Y-vel on ellipse
xdd_E= w*w*( -rx*cos(w*t + start_an)*cos(ell_an) - ry*sin(w*t + start_an)*(-sin(ell_an)) ); % Initial X-acc on ellipse ydd_E= w*w*( -rx*cos(w*t + start_an)*sin(ell_an) - ry*sin(w*t + start_an)*(cos(ell_an)) ); % Initial Y-acc on ellipse
%% ERROR MODELING X_L_O= TR_C2O*[L1;L2]+[x_C;y_C]; % Position of Look Ahead point in {O} frame= Xco + Xlc x_L=X_L_O(1); y_L=X_L_O(2);
Xd_C_O= [xd_C,yd_C]'; % Velocity of Look Ahead point in {O} frame Xd_L_C= (om*[L1,L2]*[0,1;-1,0]*TR_C2O')'; Xd_L_O= Xd_C_O + Xd_L_C; xd_L=Xd_L_O(1); yd_L=Xd_L_O(2);
xdd_L = xdd_E - Kp1*(x_L - x_E) - Kd1*(xd_L - xd_E); % X-Acc of Look Ahead point ydd_L = ydd_E - Kp2*(y_L - y_E) - Kd2*(yd_L - yd_E); % Y-Acc of Look Ahead point
%% INPUT MODELING p11= c*((b-L2)*cos(phi) - (d+L1)*sin(phi)); p12= c*((b+L2)*cos(phi) + (d+L1)*sin(phi)); p21= c*((b-L2)*sin(phi) + (d+L1)*cos(phi)); p22= c*((b+L2)*sin(phi) - (d+L1)*cos(phi)); P=[p11,p12; p21,p22];
pd11= c*(Nu(1) - Nu(2))*c*(-(b-L2)*sin(phi) - (d+L1)*cos(phi)); pd12= c*(Nu(1) - Nu(2))*c*(-(b+L2)*sin(phi) + (d+L1)*cos(phi)); pd21= c*(Nu(1) - Nu(2))*c*( (b-L2)*cos(phi) - (d+L1)*sin(phi)); pd22= c*(Nu(1) - Nu(2))*c*( (b+L2)*cos(phi) + (d+L1)*sin(phi)); Pd= [pd11,pd12; pd21,pd22];
v=[xdd_L, ydd_L]'; u = inv(P)*(v - Pd*Nu);
%% Dynamic Modeling m= mc+ 2*mw; I = Ic + 2*mw*(d^2 + b^2) + 2*Im;
M = [m, 0, 2*mw*d*sin(phi), 0 , 0; 0, m, -2*mw*d*cos(phi), 0, 0; 2*mw*d*sin(phi), -2*mw*d*cos(phi), I, 0, 0; 0, 0, 0, Iw, 0; 0, 0, 0, 0, Iw]; V = [2*mw*d*(om^2)*cos(phi); 2*mw*d*(om^2)*sin(phi); 0; 0; 0];
E = [0, 0; 0, 0; 0, 0; 1, 0; 0, 1];
f2= inv(S'*M*S)*(-S'*M*Sd*Nu - S'*V); Tau= pinv(inv(S'*M*S)*S'*E)*(u-f2); dX = [S*Nu; f2] + [zeros(5,2); inv(S'*M*S)*S'*E]*Tau;
