simpid.m

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

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


%% This script initialize the simulation and uses
%% PID controller with the exact kinematics
function [Yd,Y2d,Ct,position_pts]=simPID(K,time,draw)
close all
%% vl,vr expressed in meters/seconds
%%the left wheel velocity
vl=0.02;
%%the maximum left speed 
vmax=0.08;
%%the right wheel velocity
vr=0.02;

%%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=time(1);
tend=time(2);
timestep=0.1;

%% The PID parameters
Pe=K(1);
Pi=K(2);
Pd=K(3);


%%now define the number of time steps
nsteps=(tend-tstart)/timestep;
%%this is the starting position for the robot
start_pos=[0,0.05,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
Yd=[];
%% the drift rate y 2th derivative
Y2d=[];

%%
figure(1);
if(draw==1)
DrawRobotPosition(old_pos,b);
end
for t=tstart:timestep:tend
    new_pos=deadReckonTimeVelocity3(old_pos,vr,vl,t,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
    %%this is the error function
    errorInt=dydt(vr,vl,theta0,t-tstart,b);
    errorDiff=d2ydt2(vr,vl,theta0,t-tstart,b);
    
    Yd=[Yd;errorInt];
    Y2d=[Y2d;errorDiff];
    %%yt is the integral error
    error=new_pos(2);    

    ct=(Pe*errorInt+Pi*error+Pd*errorDiff);
    Ct=[Ct;ct];
    if(1)

        %%use the error control on the steering system
        if(ct>0)
            if(ct>vmax)
                ct=vmax;
            end
        else
            if(ct<-vmax)
                ct=(-vmax);
            end
        end
        vr=vmax/2-ct/2;
        vl=vmax/2+ct/2;
    end
    if(draw==1)
    DrawRobotPosition(old_pos,b);
    end
  %%  pause(0.2);
end

%% Draw the traces for the left and right wheel
if(draw==1)
    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(Yd,'r--');
    plot(Y2d,'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');
end%%end draw
end