PONTIFICIA UNIVERSIDAD CATLICA DEL PER

FACULTY OF SCIENCE AND ENGINEERING

MECHANICS ENGINEERING SECTION
MECHATRONIC ENGINEERING

TOPICS OF ADVANCE ROBOTICS

ING341
PROGRAMMING ASSIGNMENT 2
CLASS 09M2

Submitted for:
Franco Hidalgo Herencia

Submitted by:
Marta Larraaga Palacios
Paul Mendoza Yupanqui
Hugo Deudor Benavente
Jos Balbuena Galvn
Heser Len Reyes

San Miguel
May 22, 2016

20110113
20111271
20111683
20112077
20112270

I.

Introduction
Nowadays, there are a lot of applications where the mobile robots simplify our work
or keep us safe from some dangerous tasks. For example, mobile robots are used
for localizing mines and removing them or for inspections in places where a person
can go through like pipes. In all these activities mention before, there are many tasks
that mobile robots have to do. The most important is move efficiently in an
environment, in this project we are going to simulate the movement of a mobile robot
that is the first step in any kind of application where mobile robots are involved.

II.

Problem
The problem of this project is to create a complete navigation program that have as
input a robots pose (position and orientation). In order to achieve this objective we
have to create a kinematic model, a motion profile, a path algorithm and identify a
dynamic system.

III. Methodology
1. Kinematic Model
Model in Simulink the kinematic model of a mobile vehicle given v and w as inputs
and the pose (position and orientation) of the robot as output.

Figure 1. Dimension of Model

Figure 2. Equations of model

Where:
Ur: right wheel velocity
Ul: left wheel velocity
r: radius of Wheel
L: distance between wheels
v: linear velocity
w: angular velocity
Using the kinematic ecuations shown, we can obtain de model in simulink:

Figure 3. Simulink Model

Subsystem consist of 4 inputs (r, L, v and w) and four outputs (x, y and theta)

Figure 4. Subsystem Model Simulink

Simulation:
1) r=5; L=13; v=2; w=0

Figure 5. Simulation by w=0

Results show that the robot is moving streight and has no angular variation. This
result is correct because as input it was introduced w=0.

2) r=5; L=13; v=1; w=3

Figure 6. Simulation by w

0.

Results show that the robot is turning in circles and has angular variation. This
result is correct because as input it was introduced w 0.

2. System Identification
For this part of the project, the selected model is the same model used for the
PA1, the model is shown below in Figure 7.

Figure 7. Model used for system identification

To find the missing parameters, we used a grey-box model where the inputoutput information was given. The identification data is shown below in Figure 8.

Figure 8. Input-output data used in system identification

First of all, we created an ODE-file, this type of file allow us to create a nonlinear
state-space model, and the code is shown below:
function [dx,y] = NonlinearMobile(~,x,u, p1, p2, p3, p4, p5, p6,
varargin)
% Output equation.
y = [x(1);
% linear velocity
x(2)];
% angular velocity,.
% State equations.
% X(1) = v , X(2) = w
dx = [p1*x(2)^2 + p2*x(1) + p3*(u(1)+u(2));
% Linear velocity
p4*x(1)*x(2) + p5*x(2) + p6*(u(2)-u(1)) ];
% Angular velocity

Then, we create a code where we load the data and process it to get linear and
angular velocity as output. These outputs are filtered using a moving average
filter and median filter; the first one smooth the curve and the second eliminate

all kind of peaks. The comparison between the data with filtered data is shown
below in Figure 9 and Figure 10.

Figure 9. Comparison between linear velocity data (left) and filtered data (right)

Figure 10. Comparison between angular velocity data (left) and filtered data (right)

The code used for this and also the creation of the data set (iddata) is shown
below:
clear all; close all; clc;
load E_R.mat;
load E_L.mat;
load PWM_R.mat;
load PWM_L.mat;
% -------------------- PART 1 -----------------------------% In this part we will prepare the data what was obtained from
the
% experiment (given by teacher)
sample_time = 0.03;
%Ticks/revolutions relation: 540 ticks = 1 rev
rel_T.R = 540;
%radius of the wheel (5cm)
r = 5/100;
%distance between wheels (13cm)
L = 13/100;

%time
t = E_L.time;
t = t';
%Data of the encoder R
Enco_R_Data = E_R.Data;
for i = 1:length(Enco_R_Data)-1
Enco_r(i) = (Enco_R_Data(i+1)-Enco_R_Data(i))/0.03;
end
Enco_r(750) = Enco_r(749);
%Conversion to rev
Rev_R = Enco_r/rel_T.R;
%Conversion to rad/s
vR = Rev_R*2*pi;
%Data of the encoder L
Enco_L_Data = E_L.Data;
for i = 1:length(Enco_L_Data)-1
Enco_l(i) = (Enco_L_Data(i+1)-Enco_L_Data(i))/0.03;
end
Enco_l(750) = Enco_l(749);
%Conversion to rev
Rev_L = Enco_l/rel_T.R;
%Conversion to rad/s
vL = Rev_L*2*pi;
Z = zeros(11,1);
%With the encoder data we find the v, w outputs values
v = r*(vR + vL)/2;
vf = medfilt1(v);
vf = [Z;vf];
vf = tsmovavg(vf,'s',12,1);
vf = vf(12:761);
v1 = vf';
w = r*(vR - vL)/L;
wf = medfilt1(w);
wf = [Z;wf];
wf = tsmovavg(wf,'s',12,1);
wf = wf(12:761);
w1 = wf';
%With the PWM lectures we have the inputs values
%Data of the
PWM_R_Data = PWM_R_ts.Data;
PWM_L_Data = PWM_L_ts.Data;
%We are gonna create a "iddata" with two inputs and two outputs
an ts =
%0.03, this is gonna help us to evaluate the model
data = iddata([vf,wf],[PWM_L_Data,PWM_R_Data],sample_time,
'Name','Mobile Dynamics');
data.InputName = {'% PWM Left Wheel','% PWM Right Wheel'};
data.InputUnit = {'%V', '%V'};
data.OutputName = {'Linear Velocity','Angular Velocity'};
data.OutputUnit = {'m/s','rad/s'};
data.Tstart = 0;
data.TimeUnit = 's';
%Plotting the data
figure; subplot(1,2,1);plot(v); title('Linear velocity');
subplot(1,2,2); plot(vf); title('Filtered Linear velocity');
figure; subplot(1,2,1);plot(w); title('Angular velocity');
subplot(1,2,2); plot(wf); title('Filtered angular velocity');

After, this we initialize the ODE-file, and run the program to estimate the value
of the missing parameters, the code and the results is shown below:
%----------------- PART 2 -------------------------------% Initializing the ODE model
FileName
= 'NonlinearMobile'; %File describing the model
structure.
Order
= [2 2 2];
% Model orders [ny nu nx].
Parameters
= [0;0;0;0;0;0]; %Initial random parameters.Np= 6.
InitialStates = [0;0];
% Initial initial states.
Ts
= 0.03;
% Time-continuous system.
nlgr = idnlgrey(FileName, Order, Parameters, InitialStates, Ts,
'Name', 'Mobile Robot');
set(nlgr, 'InputName', {'% PWM Left Wheel','% PWM Right Wheel'},
'InputUnit',{'%V', '%V'} , 'OutputName',{'Linear
Velocity','Angular Velocity'},'OutputUnit', {'m/s','rad/s'} ,
'TimeUnit', 's');
nlgr = setinit(nlgr, 'Name', {'Linear Velocity' 'Angular
Velocity'} );
nlgr = setinit(nlgr, 'Unit', {'m/s' 'rad/s'});
nlgr = setpar(nlgr, 'Name', {'p1' 'p2' 'p3' 'p4' 'p5' 'p6'});
nlgr = setpar(nlgr, 'Unit', {'m/rad' '1/s' 'm/V.s2' '1/s.m' '1/s'
'rad/V.s2'});
nlgr.Algorithm.SearchMethod = 'lsqnonlin';
nlgr.Algorithm.SimulationOptions.Solver = 'Auto';
nlgr.Algorithm.SimulationOptions.AbsTol = 1e-6;
nlgr.Algorithm.SimulationOptions.RelTol = 1e-5;
nlgr = setinit(nlgr, 'Fixed', {false false}); % Estimate the
initial state.
nlgr = pem(data, nlgr, 'Display', 'Full');
compare(data,nlgr)
present(nlgr)

Figure 11. Results of the system identification. Real data (grey) and simulated data (blue)

3. Creating a Motion Profile

In this part, we are going to design the motion profile, for this we use the next MATLAB
script. The specifications for this trapezoidal motion profile are acceleration equal to
0.5 rad/s^2, deceleration equal to -0.5 rad/s^2 and maximum angular velocity equal to
1 rad/s.
%We are gonna build a motion profile for an alngular position
controller,
%this controller will give us a velocity. This is the input of our
plant
%Parameter definition
a = 0.5;
%aceleration of the movement
d = -0.5;
%desaceleration of the movement
w = 1;
%max angular velocity of the movement in rad/s
theta_d = 5;
%desired angular position in rad/s
%Definition of the movement times
T1 = w/a;
T3 = -w/d;
T2 = (theta_d/w) - (T1 + T3)/2;

%aceleration time
%desaceleration time
%constang velocity time

%We create the profile of the angular position

%First we create a time vector
t=0:0.001:(T1+T2+T3);
%angular position
f1 = (t<=T1).*(w/(2*T1)).*(t.^2);
f2 = ((t>T1)&(t<=T1+T2)).*((w*T1/2)+w.*(t-T1));
f3 = ((t>T1+T2)&(t<=T1+T2+T3)).*(theta_d - w*T3/2 + w.*(t-(T1+T2)) (w/(2*T3)).*((t-(T1+T2)).^2));

%plotting the angular position

figure();
f = f1+f2+f3;
plot(t,f);
%angular velocity
w1 = (t<=T1).*(w/T1).*(t);
w2 = ((t>T1)&(t<=T1+T2)).*w;
w3 = ((t>T1+T2)&(t<=T1+T2+T3)).*(w - (w/T3).*(t-(T1+T2)));
%plotting the angular velocity
figure();
wd = w1+w2+w3;
plot(t,wd);

The result of this shown in the figures below:

Figure 12. Results of Angular Position (Angular Position vs t (s))

Figure 13. Results of Angular Velocity (Angular Velocity vs t (s))

4. Trajectory path algorithm

We worked with the Pure Pursuit Algorithm
Matlab code:
clc;
%Orientation of diferential wheeled mobile robot
orientationX=280;
orientationY=500;
%Init position
Xorigin=50;
Yorigin=150;
%Destiny
Xdestiny=300;
Ydestiny=500;
a=purepursuitfun(orientationX,orientationY,Xorigin,Yorigin,Xdestiny
,Ydestiny);

*The complete code of pureputsuitfun function is in the attached file and the principal
script is Pathalgorithm.m
The red dot is the circle and the blue line is its orientation, the green dot is the
reach point

The next picture shows us the trajectory followed by the mobile robot using the Pure
Pursuit Algorithm

IV. Analysis
The Pure Pursuit Algorithm is an algorithm that gives the robot a trajectory that it can
follow (a curvature).
Inside the algorithm there must be considered the limitation of the mobile robot, like the
maximum steering angle.

V.

Conclusions
-

First, is recommendable to filter the input-output data in order to obtain a better

model in the system identification procedure.
Pure Pursuit algorithm shows as a good choice to trace a path