simlegopid.m

该程序用PID系统实现差动转向车辆控制

%% Author: epokh
%% Website: www.epokh.org/drupy
%% This software is under GPL


%% This script initialize the simulation and uses
%% the PID controller with the approximated equations

close all
%% vl,vr expressed in meters/seconds
%%the left wheel velocity
vl=0.01;
%%the maximum left speed 
vlmax=0.1;
%%the right wheel velocity
vr=0.01;
%%the maximum right speed
vrmax=0.1;
%%the initial orientation of the robot
%%the angle is counter clockwise from the x axis
theta0=0;
%%b is the axis length of the robot that connect the 2 wheels expressed in
%%meters
b=0.5;
%%the run time
tstart=0;
tend=50;
timestep=0.01;

%% The PID parameters
Pe=2;
Pi=0.1;
Pd=1;

%%now define the number of time steps
nsteps=(tend-tstart)/timestep;
%%this is the starting position for the robot
start_pos=[0,0.005,theta0];
position_pts=[start_pos];
old_pos=start_pos;
lwheel_trace=LeftWheelLoc(start_pos,b);
rwheel_trace=RightWheelLoc(start_pos,b);
%%the correction control
Ct=[];
%% the drift y derivative
YdTaylor=[start_pos(2)];
%% the drift rate y 2th derivative
Y2dTaylor=[start_pos(2)];

%%
figure(1);
DrawRobotPosition(old_pos,b);
%% fix a reset time limit
%% this is really important for the Taylor aprrossimation!
treset=5 %%2 seconds
t=tstart;
tglobal=tstart;
while (tglobal<tend)
    if(t>treset)
        t=tstart;
    end
    
    new_pos=deadReckonTimeVelocityTaylor(old_pos,vr,vl,t-tstart,b);
    position_pts=[position_pts; new_pos];
    lwheel_trace=[lwheel_trace; LeftWheelLoc(new_pos,b)];
    rwheel_trace=[rwheel_trace; RightWheelLoc(new_pos,b)];
    old_pos=new_pos;
    %%compute the PID controller with the Taylor approssimator
    dyt=dydtTaylor(vr,vl,theta0,t-tstart,b,YdTaylor);
    d2yt=d2ydt2Taylor(vr,vl,t-tstart,b,Y2dTaylor);
    
    YdTaylor=[YdTaylor;dyt];
    Y2dTaylor=[Y2dTaylor;d2yt];
    yt=new_pos(2);
    
    ct=Pe*dyt+Pi*yt+Pd*d2yt;
    Ct=[Ct;ct];
    %%use the error control on the steering system
    if(ct>0)
        if(ct>(vlmax+vrmax))
            ct=(vlmax+vrmax);
        end
    else
        if(ct<(-vlmax-vrmax))
            ct=(-vlmax-vrmax);
        end
    end
   
    vr=vrmax/2-ct/2;
    vl=vrmax/2+ct/2;
    
    DrawRobotPosition(old_pos,b);
    t=t+1;
    tglobal=tglobal+1;
end

%% Draw the traces for the left and right wheel
figure(2)
DrawWheelTrace(lwheel_trace,'b');
DrawWheelTrace(rwheel_trace,'b');

%%now define the space boundaries
%%becareful to plan  the trajectory well or the robot will
%%go outside
figure(1)
xmin=min(position_pts(:,1))-b
xmax=max(position_pts(:,1))+b
ymin=min(position_pts(:,2))-b
ymax=max(position_pts(:,2))+b
box_space=[xmin xmax ymin ymax];
axis(box_space);

title('Differential steering robot simulation');
xlabel('X coordinate');
ylabel('Y coordinate');
figure(3);
title('Drift term and drift rate')
hold on;
grid on;
plot(YdTaylor,'r--');
plot(Y2dTaylor,'b--');
plot(Ct,'g--');
legend('dy/dt','d2y/dt2','Control error');
xlabel('Time');
ylabel('y');
hold off;

figure(4);
title('Trajectories');
grid on;
hold on;
plot(position_pts(:,1),'r--');
plot(position_pts(:,2),'b--');
legend('x','y');