deadreckontimevelocity2.m

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

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


function new_position=deadReckonTimeVelocity2(position,vr,vl,t,bw)
%% This function calculate the new position of the robot
%% with the exact kinematics equations
%% from the left wheel speed, right wheel speed and simulation time
%% bw is the axe's length

x0=position(1);
y0=position(2);
theta0=position(3);

if(vr==vl)
    theta=theta0;
    x=x0+bw*(sin((vr-vl)*t/bw+theta0)-sin(theta0));
    y=y0-bw*(cos((vr-vl)*t/bw+theta0)-cos(theta0));
end
theta=theta0+(vr-vl)*t/bw;
x=x0+bw*(vr+vl)*(sin((vr-vl)*t/bw+theta0)-sin(theta0))/(2*(vr-vl));
y=y0-bw*(vr+vl)*(cos((vr-vl)*t/bw+theta0)-cos(theta0))/(2*(vr-vl));
new_position=[x,y,theta];

end