How to create Matlab Simulation for Kinematics of Differential Drive

Question

I studied the forward and inverse Kinematics of the robot and got a clear understanding. I am in the progress of developing my matlab simulation for a two wheeled differential drive robot. The robot moves in a straight line and has been integrated with PID. I want to show by the animation the movement of the robot.

Equation is,

The Vector in Initial Frame = Rotation Matrix Inverse x Vector in Robot Frame

My Rotational Matrix is, [0 -1 0; 1 0 0; 0 0 1] since the angle is 90.

The Robot Frame is [a; b; c] 
where a = Total translational Speed = (r x w1)/2 + (r x w2)/2
      b = In y direction = 0
      c = Total Rotational Speed = (r x w1)/2l + (r x w2)/2l

where l = 0.12 and r = 0.033 and w1 and w2 are angular velocities of wheel 1 and 2.

I have w1 and w2 data in a file as

w1 1 2 3 4 5 6 8 9
w2 1 3 4 5 6 7 8 9

I want to run an algorithm in such a way, Mat lab runs the equation and calculate the values of Total translational Speed and Total angular speed in the world frame and plot the graph. I also want to make an animation in such a way a box moves according to this equation. How can I do that? I can run it for one time if I input one value for w1 and w2, But not continuously. Help. Thanks Much.

Answer 1

Making a plot in Matlab can be done using the plot(data) command where data is a vector containing the series of points to plot. Further refinements to the plot can include the correct timeseries for example plot(data, time) where time is a vector with the same length as data as contains the timestesp at which data has been sampled.

Repetitive tasks in programming are solved be [loops][2]. Common ones are for-loops or while-loops. For loops (in Matlab) are useful when you know how many times do you want to execute something. While loops are useful when you want to satisfy a condition for stopping (e.g. robot reached its target).

In each loop cycle you have to know what the simulation time of your algorithm is. E.g. you can choose to increment the time variable with 0.1 seconds in each cycle. this will be one timestep. You have to store the value of the positions of the robot (e.g. in a vector) in each timestep (please note that you only posted the velocity calculations, which for the constant input of w1 and w2 will be always constant, so expect a straight line as a velocity plot).

A simple example would be:

x_init = 0; %[m]
y_init = 0; %[m]

x_target = 5; %[m]
y_target = 5; %[m]

x_vel = 1; %[m/s]
y_vel = 1; %[m/s]

target_reached = 0;
timestep = 0.1; %[s]

sample_no = 1;
x_robot(sample_no) = x_init;
y_robot(sample_no) = y_init;
timestamps(sample_no) = 0;
tol = 0.1; %[m]

while ~target_reached 
    %increase timestep
    sample_no =  sample_no + 1;

    %calculate new position
    x_robot(sample_no) = x_robot(sample_no - 1) + x_vel * timestep;
    y_robot(sample_no) = y_robot(sample_no - 1) + y_vel * timestep;
    %store timestep
    timestamps(sample_no) = timestamps(sample_no - 1) + timestep;    

    %check if target is reached
    if ((abs(x_robot(sample_no) - x_target) < tol) || (abs(y_robot(sample_no) - y_target) < tol))
        target_reached = 1;
    end

    %insert animation here
    %...
end

figure
plot(x_robot, timestamps);
figure 
plot(y_robot, timestamps);

Please note that this is a simple example to show how to generate a position plot and requires a number of improvements for making generally applicable.

For the animation, at each timestep the a figure should be updated with the current position values of the robot. A tutorial on making animations in Matlab can be found here.