Models Examples

The Geometrical Model of the DR
The Kinematic model of the DR
The Inverse kinematic model of the DR

The Geometrical Model of the DR

Input data: The DR position a vector 2×1 t:=[tx;ty]; The DR orientation a scalar alfa; Output data: The DR graphical representation

Contents

DR parameters

The points coordinates

p0=[0;0;0;1];
p1=[0.3;0;0;1];
p2=[-0.2;0.2;0;1];
p3=[-0.2;-0.2;0;1];
p4=[0.4;0;0;1];
p5=[0;0.4;0;1];
% Contours and lines
%   Obs:for these simple shape we avoid the incident matrix
L1=[p1,p2,p3,p1];
L2=[p0,p4];
L3=[p0,p5];

Representing the DR in its referential frame

figure
hold
plot(L1(1,:),L1(2,:),'k','LineWidth',2)
plot(L2(1,:),L2(2,:),'r','LineWidth',2)
plot(L3(1,:),L3(2,:),'g','LineWidth',2)
grid
xlabel 'X'
ylabel 'Y'
title ('The differential robot graphical model')
axis([-0.5,0.5,-0.5,0.5])

Current plot held

First method: change the pose in the absolute referential frame

Input data:

t=[1;1];
alfa=pi/2;
T0_k=[cos(alfa),-sin(alfa),0,t(1,1);...
      sin(alfa),cos(alfa),0,t(2,1);...
      0,0,1,0;...
      0,0,0,1];
L1k=T0_k*L1;
L2k=T0_k*L2;
L3k=T0_k*L3;
% The new pose
figure
hold
plot(L1k(1,:),L1k(2,:),'k','LineWidth',2)
plot(L2k(1,:),L2k(2,:),'r','LineWidth',2)
plot(L3k(1,:),L3k(2,:),'g','LineWidth',2)
grid
xlabel 'X'
ylabel 'Y'
title ('The differential robot graphical model')
axis 'equal'

Current plot held

Second method: change the pose relative to the previous referential frame

Input data:

tk_k1=[0.5;0.5];
alfak_k1=pi/2;
Tk_k1=[cos(alfak_k1),-sin(alfak_k1),0,tk_k1(1,1);...
      sin(alfak_k1),cos(alfak_k1),0,tk_k1(2,1);...
      0,0,1,0;...
      0,0,0,1];
L1k_k1=Tk_k1*L1k;
L2k_k1=Tk_k1*L2k;
L3k_k1=Tk_k1*L3k;
% The new pose
plot(L1k_k1(1,:),L1k_k1(2,:),'k','LineWidth',2)
plot(L2k_k1(1,:),L2k_k1(2,:),'r','LineWidth',2)
plot(L3k_k1(1,:),L3k_k1(2,:),'g','LineWidth',2)
axis 'equal'

Trajectory and Working Volume animation

From the first method we have constructed the function DR_gm(t,alfa) Define a circular trajectory

beta=linspace(0,2*pi,100);
r=2;
t=r*[cos(beta);sin(beta)];
alfa=pi/2+beta;
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:length(beta)
    [L1k,L2k,L3k]=DR_gm( t(:,i),alfa(1,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.025)
end
traject=r*[cos(0:0.1:2*pi);sin(0:0.1:2*pi)];
comet(traject(1,:),traject(2,:))

Current plot held

Trajectory and Working Volume animation 2

From the first method we have constructed the function DR_gm(t,alfa) Define a circular trajectory

x=linspace(0,5,50);
y=sin(2*pi*x/5);
alfa=atan(2*pi/5*cos(2*pi*x/5));
t=[x;y];
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:length(x)
    [L1k,L2k,L3k]=DR_gm( t(:,i),alfa(1,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.025)
end

Current plot held

geoMod1_05

Trajectory and Working Volume animation 3

From the first method we have constructed the function DR_gm(t,alfa) Define a circular trajectory

x=linspace(0,5,50);
y=0*x;
alfa=y;
t=[x;y];
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:length(x)
    [L1k,L2k,L3k]=DR_gm( t(:,i),alfa(1,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.025)
end

Current plot held

geoMod1_06

Published with MATLAB® R2013a

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'

kimMod1_01

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

 
kimMod1_02

Published with MATLAB® R2013a

animation

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

kimMod2_01

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



kimMod2_02


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



kimMod2_03


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



kimMod2_04



Published with MATLAB® R2013a