The Kinematic model of the DR

Input Data: v_r:= the right wheel speed v_l:= the left wheel speed x_0:=[t_x0;t_y0;aplha_0] the initial pos of the robot b:=the distance betwen the wheels [m] dt:= the sampling period [s] Output data: The trajectory coordinates: x=[t_x;t_y;aplha] The trajectory plot (Using the geometrical model)

Contents

Input Data

niT=900; % number of iterations
v_r=[2*ones(1,niT/3),ones(1,niT/3),1.5*ones(1,niT/3)];
v_l=[1.5*ones(1,niT/3),ones(1,niT/3),2*ones(1,niT/3)];
x_0=[0;0;0];
b=0.3;
dt=0.01;

Direct Kinematic

x=x_0;
for i=1:niT
    v=0.5*(v_r(i)+v_l(i));% the linear speed of the robot central point
    w=(v_r(i)-v_l(i))/b; % the angular speed of the robot
    alpha=x(3,i);
    delta_d=v*dt;
    delta_alpha=w*dt;
    x(:,i+1)=x(:,i)+[delta_d*cos(alpha+delta_alpha);...
                     delta_d*sin(alpha+delta_alpha);...
                     delta_alpha];
end

Trajectory representation

figure
plot(x(1,:),x(2,:),'LineWidth',2)
axis equal
grid
xlabel 'x'
ylabel 'y'
title 'The DR trajectory'

Using the geoMod for the robot repesentation

figure
hold
plot(0,0,'kd','MarkerSize',5)
xlabel 'X'
ylabel 'Y'
title ('The differential robot graphical model')
grid
% The DR representation
for i=1:15:niT
    [L1k,L2k,L3k]=DR_gm(x(1:2,i),x(3,i));
    plot(L1k(1,:),L1k(2,:),'k','LineWidth',2)
    plot(L2k(1,:),L2k(2,:),'r','LineWidth',2)
    plot(L3k(1,:),L3k(2,:),'g','LineWidth',2)
    axis 'equal'
    pause(0.25)
end
hold

Published with MATLAB® R2013a

The inverse kinematic model of the DR

The Input data: The desired trajectory defined by several points: points_d=[x1,x2,x3,…xn;y1,y2,y3,…yn]; The traveling time T; and the sampling time dt; b:=the distance betwen the wheels [m] The Output Data: The intermetdiary points: points=[x1,…,xm;y1,…,ym]; The DR velocities: v_l=[vl1,…,vlm], v_r=[vr1,…,vrm]; Graphical representations: The desired points The computed trajectory The wheels speed The DR direct kinematic simulation

Contents

Input data

points_d=[1,5,10;...
          2,5,3];% [m]
T=10;%[s]
dt=0.01;%[s]
time=0:dt:T; %time vector
m=length(time);
b=0.3; % [m]

The intermediary points

x=linspace(points_d(1,1),points_d(1,end),m);
y=spline(points_d(1,:),points_d(2,:),x);

Graphical representation of the desired points and the computed trajectory

figure(1)
hold
grid
xlabel 'X'
ylabel 'Y'
title 'The desired points 'o' and trajectory (blue), the simulated trajectory (red)'
plot(points_d(1,:),points_d(2,:),'or')
plot(x,y,'lineWidth',2)

Current plot held

The DR speeds

dx=(x(2:end)-x(1:end-1))/dt; % (1,m-1)
dy=(y(2:end)-y(1:end-1))/dt;
alfa=atan2(dy,dx); %(1,m-1)
dalfa=(alfa(2:end)-alfa(1:end-1))/dt; %(1,m-2)
v=dx./cos(alfa);%(1,m-1)
v=v(1:end-1);% %(1,m-2)
w=dalfa; %(1,m-2)
vl=0.5*(2*v-b*w); %left wheel speed
vr=0.5*(2*v+b*w); %right wheel speed

Graphical representation of the speeds

figure
subplot(2,1,1)
hold
xlabel 'Time [sec]'
ylabel 'v_r,(red) v_l (blue) [m/sec]'
grid
plot(time(1:end-2),vl,'b','lineWidth',2)
plot(time(1:end-2),vr,'r','lineWidth',2)
hold
subplot(2,1,2)
plot(time(1:end-2),180*alfa(1:end-1)/pi,'b','lineWidth',2)
xlabel 'Time [sec]'
ylabel 'alpha [dgr]'
grid

Simulate the DR direct kinematics

Trajectory=[];
pozition_0=[points_d(:,1);alfa(1,1)];
Trajectory(:,1)=pozition_0;
for i=1:m-2
    vi=0.5*(vr(i)+vl(i));% the linear speed of the robot central point
    wi=(vr(i)-vl(i))/b; % the angular speed of the robot
%     alpha=x(3,i);
    delta_d=vi*dt;
    delta_alpha=wi*dt;
    pozition=DR_km(Trajectory(:,i),[delta_d;delta_alpha]);
    Trajectory=[Trajectory,pozition];
end

Compare the two trajectories

figure(1)
plot(Trajectory(1,:),Trajectory(2,:),'r')
hold

Current plot released

Using the geoMod for the robot repesentation

figure
hold
plot(0,0,'kd','MarkerSize',5)
xlabel 'X'
ylabel 'Y'
title ('The differential robot graphical model')
grid
% The DR representation
for i=1:25:m-2
    [L1k,L2k,L3k]=DR_gm(Trajectory(1:2,i),Trajectory(3,i));
    plot(L1k(1,:),L1k(2,:),'k','LineWidth',2)
    plot(L2k(1,:),L2k(2,:),'r','LineWidth',2)
    plot(L3k(1,:),L3k(2,:),'g','LineWidth',2)
    axis 'equal'
    pause(0.25)
end
hold

Published with MATLAB® R2013a