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

Current plot held
Current plot released

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

Current plot held
Current plot released

Published with MATLAB® R2013a