EE 5329 Homework 4

Mobile Robot Control & Potential Fields

1. Potential Field. Use MATLAB to make a 3-D plot of the potential fields described below.
You will need to use plot commands and maybe the mesh function. The work area is a
square from (0,0) to (12,12) in the (x,y) plane. The goal is at (10,10). There are obstacles at
(4,6) and (6,4). Use a repulsive potential of K i / ri for each obstacle, with ri the vector to the
i-th obstacle. For the target use an attractive potential of KT rT , with rT the vector to the
target. Adjust the gains to get a decent plot. Plot the sum of the three force fields in 3-D.

2. Potential Field Navigation. For the same scenario as in Problem 1, a mobile robot starts at
(0,0). The front wheel steered mobile robot has dynamics
x  V cos  sin 
y  V cos  cos
V
  sin 
L
with (x,y) the position,  the heading angle, V the wheel speed, L the wheel base, and  the
steering angle. Set L= 2.
a. Compute forces due to each obstacle and goal. Compute total force on the vehicle at
point (x,y).
b. Design a feedback control system for force-field control. Sketch your control system.
c. Use MATLAB to simulate the nonlinear dynamics assuming a constant velocity V and a
steerable front wheel. The wheel should be steered so that the vehicle always goes
downhill in the force field plot. Plot the resulting trajectory in the (x,y) plane.

3. Swarm/Platoon/Formation. Do what you want to for this problem. The intent is to focus
on some sort of swarm or platoon or formation behavior, not the full dynamics. Therefore,
take 5 vehicles each with the simple point mass (Newton’s law) dynamics
x  Fx / m

y  Fy / m

with (x,y) the position of the vehicle and Fx , Fy the forces in the x and y direction respectively.
The forces might be the sums of attractive forces to goals, repulsive forces from obstacles, and
repulsive forces between the agents.
Make some sort of interesting plots or movies showing the leader going to a desired goal
or moving along a prescribed trajectory and the followers staying close to him, or in a prescribed
formation. Obstacle avoidance by a platoon or swarm is interesting.

5
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

Question 1:
Potential Field:

To generate a repulsive potential for obstacles,

𝐾𝑖
𝑉(𝑟) = where 𝑟𝑖 = ((𝑥 − 𝑥𝑖 )2 + (𝑦 − 𝑦𝑖 )2 )0.5
𝑟𝑖

To generate an attractive potential for target,

𝑉(𝑟) = 𝐾𝑇 𝑟𝑇 where 𝑟𝑇 = ((𝑥 − 𝑥𝑇 )2 + (𝑦 − 𝑦𝑇 )2 )0.5

In this case, there are 2 obstacles and one target considered. Target position is chosen as
(10,10). One of obstacles is placed at (4,6). Another obstacle is placed at (6,4). Total potential is
obtained as

𝐾1 𝐾2
𝑉𝑡𝑜𝑡𝑎𝑙 = 𝐾𝑇 𝑟𝑇 + +
𝑟1 𝑟2

Gains are chosen as

𝐾𝑇 = 1; 𝐾1 = 3; 𝐾2 = 5;

3D mesh plot for total potential is depicted below.

1
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

Potential Field on 3D Plot

60

40
V

20

0
12
10
8
6 12
10
4 8
6
2 4
y 0 2
0 x

Contour plot for total potential is also depicted below.

Potential Field on Contour Plot

12

10

6
y

0
0 2 4 6 8 10 12
x

2
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

 MATLAB code for this problem is depicted below.

clear all; clc;
%% Gains
K_G = 1;
K_1 = 3;
K_2 = 5;

%% Position of Obstacle 1
x_1 = 4;
y_1 = 6;
%% Position of Obstacle 2
x_2 = 6;
y_2 = 4;
%% Position of Goal
x_g = 10;
y_g = 10;
%% Calculate Potential Fields
x = 0:0.1:12;
y = 0:0.1:12;
n = length(x);

for i = 1:n
for j = 1:n

V_1(j,i) = (K_1) / sqrt((x(i)-x_1)^2 + (y(j)-y_1)^2);

V_2(j,i) = (K_2) / sqrt((x(i)-x_2)^2 + (y(j)-y_2)^2);
V_G(j,i) = (K_G) * sqrt((x(i)-x_g)^2 + (y(j)-y_g)^2);

V = V_1 + V_2 + V_G;

end
end
%% Plot Figures
figure;
mesh(x,y,V)
xlabel('\bfx')
ylabel('\bfy')
zlabel('\bfV')
title('\bfPotential Field on 3D Plot')

figure;
contour(x,y,V)
xlabel('\bfx')
ylabel('\bfy')
title('\bfPotential Field on Contour Plot')

3
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

Question 2:
Potential Field Navigation:

A mobile robot dynamics are

𝑥̇ = 𝑉𝑐𝑜𝑠(𝜙) cos(𝜃)

𝑦̇ = 𝑉𝑐𝑜𝑠(𝜙)sin𝜃)

𝑉
𝜃̇ = 𝑠𝑖𝑛(𝜙)
𝐿
To compute the forces,

𝜕𝑉
𝐹𝑥 = −
𝜕𝑥
𝜕𝑉
𝐹𝑦 = −
𝜕𝑦

a.) Computed forces for each obstacles and goal are depicted below.

 Forces for x-component are depicted below.

Force X-component (Fx)

600

400

200
x

0
F

-200

-400

-600
15 10 15
5 5 10
0
y x

4
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

 Forces for y-component are depicted below.

Force Y-component (Fy)

4
x 10

1.5

0.5
y
F

-0.5

-1
15
10 10 12
5 6 8
2 4
0 0
y x

b.) Design feedback control system for force field control can be chosen as

In this example, proportional controller is implemented as

𝐹𝑦
𝜙 = 𝐾(𝛼 − 𝜃) where 𝛼 = tan−1 (𝐹 )
𝑥

Controller K is chosen as 3 for this implementation.

5
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

c.) Velocity of robot is chosen as 5. Also, the wheel base L is chosen as 2.

Computed forces for target are

𝜕𝑉 𝐾𝑇 (𝑥𝑇 − 𝑥)
𝐹𝑥 = − =
𝜕𝑥 √(𝑥 − 𝑥𝑇 )2 + (𝑦 − 𝑦𝑇 )2

𝜕𝑉 𝐾𝑇 (𝑦𝑇 − 𝑦)
𝐹𝑦 = − =
𝜕𝑦 √(𝑥 − 𝑥𝑇 )2 + (𝑦 − 𝑦𝑇 )2

Computed forces for obstacles are

𝜕𝑉 −𝐾𝑖 (𝑥𝑖 − 𝑥)
𝐹𝑥𝑖 = − =
𝜕𝑥𝑖 ((𝑥 − 𝑥𝑖 )2 + (𝑦 − 𝑦𝑖 )2 )1.5

𝜕𝑉 −𝐾𝑖 (𝑦𝑖 − 𝑦)
𝐹𝑦 = − =
𝜕𝑦𝑖 ((𝑥 − 𝑥𝑖 )2 + (𝑦 − 𝑦𝑖 )2 )1.5

Total forces for this implementation is obtained as

𝐹𝑥 = 𝐹𝑥𝑇 + 𝐹𝑥𝑂1 + 𝐹𝑥𝑂2

𝐹𝑦 = 𝐹𝑦𝑇 + 𝐹𝑦𝑂1 + 𝐹𝑦𝑂2

 x-y trajectories for this robot is depicted below.

Single Mobile Robot Trajectory

12
Robot
Target
10 Obstacles

6
y

0
0 2 4 6 8 10 12
x

6
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

 x-y trajectories on contour plot for this robot is depicted below.

Single Mobile Robot Trajectory on Contour Plot

12

10

6
y

0
0 2 4 6 8 10 12
x

 3D mesh trajectories plot for this robot is depicted below.

Single Mobile Robot Trajectory on 3D Plot

60

40
V

20

0
12
10
8
12
6 10
8
4
6
2 4
y 2
0 0 x

7
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

 MATLAB code for this problem is depicted below.

clear all; clc;
global K_1 K_2 K_G
%% Gains
K_G = 1;
K_1 = 3;
K_2 = 5;

%% Position of Obstacle 1
x_1 = 4;
y_1 = 6;
%% Position of Obstacle 2
x_2 = 6;
y_2 = 4;
%% Position of Goal
x_g = 10;
y_g = 10;
%% Calculate Potential Fields
x = 0:0.1:12;
y = 0:0.1:12;
n = length(x);

for i = 1:n
for j = 1:n

V_1(j,i) = (K_1) / sqrt((x(i)-x_1)^2 + (y(j)-y_1)^2);

V_2(j,i) = (K_2) / sqrt((x(i)-x_2)^2 + (y(j)-y_2)^2);
V_G(j,i) = (K_G) * sqrt((x(i)-x_g)^2 + (y(j)-y_g)^2);

V = V_1 + V_2 + V_G;

F_1x(j,i) = ((-K_1)*((x_1)-x(i))) / ((x(i)-x_1)^2 + (y(j)-y_1)^2)^(1.5);

F_2x(j,i) = ((-K_2)*((x_2)-x(i))) / ((x(i)-x_2)^2 + (y(j)-y_2)^2)^(1.5);
F_Gx(j,i) = ((K_G)*((x_g)-x(i))) / sqrt((x(i)-x_g)^2 + (y(j)-y_g)^2);

F_x = F_1x + F_2x + F_Gx;

F_1y(j,i) = ((-K_1)*((y_1)-y(i))) / ((x(i)-x_1)^2 + (y(j)-y_1)^2)^(1.5);

F_2y(j,i) = ((-K_2)*((y_2)-y(i))) / ((x(i)-x_2)^2 + (y(j)-y_2)^2)^(1.5);
F_Gy(j,i) = ((K_G)*((y_g)-y(i))) / sqrt((x(i)-x_g)^2 + (y(j)-y_g)^2);

F_y = F_1y + F_2y + F_Gy;

end
end
%% Plot Forces
figure;
mesh(x,y,F_x)
xlabel('\bfx')
ylabel('\bfy')
zlabel('\bfF_x')
title('\bfForce X-component (F_x)')

figure;
mesh(x,y,F_y)
xlabel('\bfx')
ylabel('\bfy')
zlabel('\bfF_y')
title('\bfForce Y-component (F_y)')

%% Mobile Robot Dynamics

Initials = [0; 0; pi/3];

options = odeset('events', @StopSimulation);

[t,States] = ode45(@singleRobotQ2,[0 100],Initials,options);

x_robot = States(:,1);
y_robot = States(:,2);

8
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

V_11 = (K_1) ./ sqrt((x_robot-x_1).^2 + (y_robot-y_1).^2);

V_22 = (K_2) ./ sqrt((x_robot-x_2).^2 + (y_robot-y_2).^2);
V_GG = (K_G) .* sqrt((x_robot-x_g).^2 + (y_robot-y_g).^2);

VV = V_11 + V_22 + V_GG;

%% Plot Trajectories
figure;
plot(x_robot,y_robot,'b.')
hold on
plot(x_g,y_g,'r-s','LineWidth',2)
hold on
plot(x_1,y_1,'k-s','LineWidth',2)
hold on
plot(x_2,y_2,'k-s','LineWidth',2)
grid on
xlim([0 12])
ylim([0 12])
xlabel('\bfx')
ylabel('\bfy')
title('\bfSingle Mobile Robot Trajectory')
legend('Robot','Target','Obstacles','Location','NorthWest');

figure;
contour(x,y,V)
hold on
plot(x_robot,y_robot,'k','LineWidth',2)
hold on
plot(x_g,y_g,'r-s','LineWidth',2)
xlabel('\bfx')
ylabel('\bfy')
title('\bfSingle Mobile Robot Trajectory on Contour Plot')

figure;
mesh(x,y,V)
hold on
plot3(x_robot,y_robot,VV,'k','Linewidth',10)
xlabel('\bfx')
ylabel('\bfy')
zlabel('\bfV')
title('\bfSingle Mobile Robot Trajectory on 3D Plot')

function dS = singleRobotQ2(t,state)
global K_1 K_2 K_G
%% States
x = state(1);
y = state(2);
Theta = state(3);
%% Position of Obstacle 1
x_1 = 4;
y_1 = 6;
%% Position of Obstacle 2
x_2 = 6;
y_2 = 4;
%% Position of Goal
x_g = 10;
y_g = 10;
%% Parameters
V = 5;
K_p = 3;
L = 2;
%% Calculating Forces for a Mobile Robot
F_1x = ((-K_1)*((x_1)-x)) / ((x-x_1)^2 + (y-y_1)^2)^(1.5);
F_2x = ((-K_2)*((x_2)-x)) / ((x-x_2)^2 + (y-y_2)^2)^(1.5);
F_Gx = ((K_G)*((x_g)-x)) / sqrt((x-x_g)^2 + (y-y_g)^2);

F_x = F_1x + F_2x + F_Gx;

F_1y = ((-K_1)*((y_1)-y)) / ((x-x_1)^2 + (y-y_1)^2)^(1.5);

9
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

F_2y = ((-K_2)*((y_2)-y)) / ((x-x_2)^2 + (y-y_2)^2)^(1.5);

F_Gy = ((K_G)*((y_g)-y)) / sqrt((x-x_g)^2 + (y-y_g)^2);

F_y = F_1y + F_2y + F_Gy;

Alpha = atan2(F_y,F_x);
Phi = K_p*(Alpha-Theta);

%% Mobile Robot Dynamics

dS(1) = V*cos(Phi)*cos(Theta);
dS(2) = V*cos(Phi)*sin(Theta);
dS(3) = (V/L)*sin(Phi);

dS = [dS(1);dS(2);dS(3)];

end

Simulation is stopped when the robot reaches the target. The function is,
function [Val,Ister,Dir] = StopSimulation(t,st)
x = st(1);
y = st(2);

Val(1) = (10^2+10^2) - (x^2+y^2);

Ister(1) = 1;
Dir(1) = 0;

10
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

Question 3:
Swarm/Platoon/Formation:

A mobile robot dynamics as a simple point mass dynamics for this problem are

𝑥̇1 = 𝑥̇ 𝑖

𝑥̇2 = 𝑦̇ 𝑖

𝑥̇3 = 𝐹𝑥𝑖

𝑥̇4 = 𝐹𝑦𝑖

Attracting forces for leader by target are obtained as

𝜕𝑉 𝐾𝑇 (𝑥𝑇 − 𝑥)
𝐹𝑥 = − =
𝜕𝑥 √(𝑥 − 𝑥𝑇 )2 + (𝑦 − 𝑦𝑇 )2

𝜕𝑉 𝐾𝑇 (𝑦𝑇 − 𝑦)
𝐹𝑦 = − =
𝜕𝑦 √(𝑥 − 𝑥𝑇 )2 + (𝑦 − 𝑦𝑇 )2

Repulsive forces for leader by obstacles are obtained as

𝜕𝑉 −𝐾𝑖 (𝑥𝑖 − 𝑥)
𝐹𝑥𝑖 = − =
𝜕𝑥𝑖 ((𝑥 − 𝑥𝑖 )2 + (𝑦 − 𝑦𝑖 )2 )1.5

𝜕𝑉 −𝐾𝑖 (𝑦𝑖 − 𝑦)
𝐹𝑦 = − =
𝜕𝑦𝑖 ((𝑥 − 𝑥𝑖 )2 + (𝑦 − 𝑦𝑖 )2 )1.5

Total forces for this implementation is obtained as

𝐹𝑥 = 𝐹𝑥𝑇 + 𝐹𝑥𝑂1 + 𝐹𝑥𝑂2 − 𝐾 𝑥̇𝐿

𝐹𝑦 = 𝐹𝑦𝑇 + 𝐹𝑦𝑂1 + 𝐹𝑦𝑂2 − 𝐾 𝑦̇𝐿

Controller K is chosen as 3 for this implementation.

11
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

 Trajectory of leader robot is depicted below.

Single Mobile Robot Trajectory (Point-Mass Dynamics)

12

10

6
y

0
0 2 4 6 8 10 12
x

Repulsive potential between followers is calculated from

𝐾𝑖
𝑉𝑖𝑗 =
𝑟𝑖𝑗2

where

2 2
𝑟𝑖𝑗 = √(𝑥𝑖 − 𝑥𝑗 ) + (𝑦𝑖 − 𝑦𝑗 )

and i ≠ j between followers.

For the potential to the leader,

1
𝑉𝑖𝐿 = (𝑟 − 𝑟𝐷 )2
2 𝑖𝐿
where

𝑟𝑖𝐿 = √(𝑥𝑖 − 𝑥𝐿 )2 + (𝑦𝑖 − 𝑦𝐿 )2

12
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

In addition to attracting forces by target and repulsive forces by obstacles on followers,

repulsive forces between followers can be found out as

𝜕𝑉𝑖𝑗 2𝐾𝐹 (𝑥𝑖 − 𝑥𝑗 )

𝐹𝑥 𝑖𝑗 = − =
𝜕𝑥 𝑟𝑖𝑗4

𝜕𝑉𝑖𝑗 2𝐾𝐹 (𝑦𝑖 − 𝑦𝑗 )

𝐹𝑦 𝑖𝑗 = − =
𝜕𝑦 𝑟𝑖𝑗4

where

2 2
𝑟𝑖𝑗 = √(𝑥𝑖 − 𝑥𝑗 ) + (𝑦𝑖 − 𝑦𝑗 )

Furthermore, attractive force by leader to followers can be found from

𝜕𝑉𝑖𝐿 (𝑥𝐿 − 𝑥𝑖 )(𝑟𝑖𝐿 − 𝑟𝐷 )

𝐹𝑥 𝑖𝐿 = − =
𝜕𝑥 𝑟𝑖𝐿

𝜕𝑉𝑖𝐿 (𝑦𝐿 − 𝑦𝑖 )(𝑟𝑖𝐿 − 𝑟𝐷 )

𝐹𝑦 =− =
𝑖𝐿 𝜕𝑦 𝑟𝑖𝐿

where

𝑟𝑖𝐿 = √(𝑥𝑖 − 𝑥𝐿 )2 + (𝑦𝑖 − 𝑦𝐿 )2

Finally, total forces for each followers can be found as

𝐹𝑖𝑥 = 𝐹𝑥𝐿 + 𝐹𝑥𝑂1 + 𝐹𝑥𝑂2 + 𝐹𝑥𝑖𝑗 − 𝐾𝐹 𝑥̇ 𝑖

𝐹𝑖𝑦 = 𝐹𝑦𝐿 + 𝐹𝑦𝑂1 + 𝐹𝑦𝑂2 + 𝐹𝑦𝑖𝑗 − 𝐾𝐹 𝑦̇ 𝑖

and i ≠ j between followers.

Controller K is chosen as 3 for this implementation.

13
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

Simulation results are depicted below. Gain between followers is chosen as 𝐾𝐹 = 0.1 and
desired separation is chosen as 𝑟𝐷 = 0.5. Simulation is stopped when leader reaches the target
point.

Platoon of Mobile Robots Trajectories

12

10

6
y

Leader
2 Follow er 1
Follow er 2
Follow er 3
0 Follow er 4
Target
Obstacles
-2
-2 0 2 4 6 8 10 12
x

 MATLAB code for this problem is depicted below.

clear all; clc;
global K_1 K_2 K_G K_F rD K_p K_L
%% Gains
K_G = 1;
K_1 = 3;
K_2 = 5;
K_F = 0.1;
rD = 0.5;
K_p = 3;
K_L = 3;
%% Position of Obstacle 1
x_1 = 4;
y_1 = 6;
%% Position of Obstacle 2
x_2 = 6;
y_2 = 4;
%% Position of Goal
x_g = 10;
y_g = 10;
%% Single Mobile Robot Without Full Dynamics
Initials = [0; 0; 0; 0];

options = odeset('events', @StopSimulation);

[t,States] = ode45(@singleRobotQ3,[0 300],Initials,options);

14
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

x_robot = States(:,1);
y_robot = States(:,2);
%% Plot Single Robot Trajectory
figure;
plot(x_robot,y_robot,'k.')
hold on
plot(x_1,y_1,'k-s','LineWidth',2)
hold on
plot(x_2,y_2,'k-s','LineWidth',2)
hold on
plot(x_g,y_g,'r-s','LineWidth',2)
grid on
xlim([0 12])
ylim([0 12])
xlabel('\bfx')
ylabel('\bfy')
title('\bfSingle Mobile Robot Trajectory (Point-Mass Dynamics)')
%% Swarm/Platoon/Formation
Initial_L = [0; 0; 0; 0];
Initial_F1 = [0; 0.2; 0; 0];
Initial_F2 = [0.2; 0; 0; 0];
Initial_F3 = [0.2; 0.1; 0; 0];
Initial_F4 = [0.1; 0.2; 0; 0];
All_initials = [Initial_L;Initial_F1;Initial_F2;Initial_F3;Initial_F4];

options = odeset('events', @StopSimulation);

[tPR,StatesPR] = ode45(@PlatoonRobots,[0 300],All_initials,options);

x_L = StatesPR(:,1);
y_L = StatesPR(:,2);
x_F1 = StatesPR(:,5);
y_F1 = StatesPR(:,6);
x_F2 = StatesPR(:,9);
y_F2 = StatesPR(:,10);
x_F3 = StatesPR(:,13);
y_F3 = StatesPR(:,14);
x_F4 = StatesPR(:,17);
y_F4 = StatesPR(:,18);
%% Plot Swarm Trajectories
figure;
plot(x_L,y_L,'k.')
hold on
plot(x_F1,y_F1,'b.')
hold on
plot(x_F2,y_F2,'r.')
hold on
plot(x_F3,y_F3,'g.')
hold on
plot(x_F4,y_F4,'c.')
hold on
plot(x_g,y_g,'r-s','LineWidth',2)
hold on
plot(x_1,y_1,'k-s','LineWidth',2)
hold on
plot(x_2,y_2,'k-s','LineWidth',2)
grid on
xlim([-2 12])
ylim([-2 12])
xlabel('\bfx')
ylabel('\bfy')
title('\bfPlatoon of Mobile Robots Trajectories')
hh = legend('Leader','Follower 1','Follower 2','Follower 3','Follower
4','Target','Obstacles','Location','SouthEast');
set(hh,'FontSize',8);

function dS = singleRobotQ3(t,state)
global K_1 K_2 K_G
%% States
x = state(1);

15
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

y = state(2);
xdot = state(3);
ydot = state(4);
%% Gains
K_p = 3;
%% Position of Obstacle 1
x_1 = 4;
y_1 = 6;
%% Position of Obstacle 2
x_2 = 6;
y_2 = 4;
%% Position of Goal
x_g = 10;
y_g = 10;
%% Calculating Forces for a Mobile Robot
F_1x = ((-K_1)*((x_1)-x)) / ((x-x_1)^2 + (y-y_1)^2)^(1.5);
F_2x = ((-K_2)*((x_2)-x)) / ((x-x_2)^2 + (y-y_2)^2)^(1.5);
F_Gx = ((K_G)*((x_g)-x)) / sqrt((x-x_g)^2 + (y-y_g)^2);

F_x = F_1x + F_2x + F_Gx - K_p*xdot;

F_1y = ((-K_1)*((y_1)-y)) / ((x-x_1)^2 + (y-y_1)^2)^(1.5);

F_2y = ((-K_2)*((y_2)-y)) / ((x-x_2)^2 + (y-y_2)^2)^(1.5);
F_Gy = ((K_G)*((y_g)-y)) / sqrt((x-x_g)^2 + (y-y_g)^2);

F_y = F_1y + F_2y + F_Gy - K_p*ydot;

%% Mobile Robot Dynamics

dS(1) = xdot;
dS(2) = ydot;
dS(3) = F_x;
dS(4) = F_y;

dS = [dS(1);dS(2);dS(3);dS(4)];

end

function dS = PlatoonRobots(t,state)
global K_1 K_2 K_G K_F rD K_p K_L
%% Leader States
xL = state(1);
yL = state(2);
xLdot = state(3);
yLdot = state(4);
%% Follower 1 States
xF1 = state(5);
yF1 = state(6);
xF1dot = state(7);
yF1dot = state(8);
%% Follower 2 States
xF2 = state(9);
yF2 = state(10);
xF2dot = state(11);
yF2dot = state(12);
%% Follower 3 States
xF3 = state(13);
yF3 = state(14);
xF3dot = state(15);
yF3dot = state(16);
%% Follower 4 States
xF4 = state(17);
yF4 = state(18);
xF4dot = state(19);
yF4dot = state(20);
%% Position of Obstacle 1
x_1 = 4;
y_1 = 6;
%% Position of Obstacle 2
x_2 = 6;

16
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

y_2 = 4;
%% Position of Goal
x_g = 10;
y_g = 10;
%% Calculating Forces for Leader:
F_1xL = ((-K_1)*((x_1)-xL)) / ((xL-x_1)^2 + (yL-y_1)^2)^(1.5);
F_2xL = ((-K_2)*((x_2)-xL)) / ((xL-x_2)^2 + (yL-y_2)^2)^(1.5);
F_GxL = ((K_G)*((x_g)-xL)) / sqrt((xL-x_g)^2 + (yL-y_g)^2);

F_xL = F_1xL + F_2xL + F_GxL - K_L*xLdot;

F_1yL = ((-K_1)*((y_1)-yL)) / ((xL-x_1)^2 + (yL-y_1)^2)^(1.5);

F_2yL = ((-K_2)*((y_2)-yL)) / ((xL-x_2)^2 + (yL-y_2)^2)^(1.5);
F_GyL = ((K_G)*((y_g)-yL)) / sqrt((xL-x_g)^2 + (yL-y_g)^2);

F_yL = F_1yL + F_2yL + F_GyL - K_p*yLdot;

%% Calculating Forces for Follower 1:
%%% Obstacle Forces %%%
F_1xF1 = ((-K_1)*((x_1)-xF1)) / ((xF1-x_1)^2 + (yF1-y_1)^2)^(1.5);
F_2xF1 = ((-K_2)*((x_2)-xF1)) / ((xF1-x_2)^2 + (yF1-y_2)^2)^(1.5);

F_1yF1 = ((-K_1)*((x_1)-yF1)) / ((xF1-x_1)^2 + (yF1-y_1)^2)^(1.5);

F_2yF1 = ((-K_2)*((x_2)-yF1)) / ((xF1-x_2)^2 + (yF1-y_2)^2)^(1.5);

%%% Leader Forces %%%

rF1L = sqrt((xF1-xL)^2 + (yF1-yL)^2);
Fx_F1L = ((xL-xF1)*(rF1L-rD))/rF1L;
Fy_F1L = ((yL-yF1)*(rF1L-rD))/rF1L;

%%% Follower Forces %%%

rF1F2 = sqrt((xF1-xF2)^2 + (yF1-yF2)^2);
Fx_F1F2 = ((2*K_F)*(xF1-xF2))/(rF1F2^4);
Fy_F1F2 = ((2*K_F)*(yF1-yF2))/(rF1F2^4);

rF1F3 = sqrt((xF1-xF3)^2 + (yF1-yF3)^2);

Fx_F1F3 = ((2*K_F)*(xF1-xF3))/(rF1F3^4);
Fy_F1F3 = ((2*K_F)*(yF1-yF3))/(rF1F3^4);

rF1F4 = sqrt((xF1-xF4)^2 + (yF1-yF4)^2);

Fx_F1F4 = ((2*K_F)*(xF1-xF4))/(rF1F4^4);
Fy_F1F4 = ((2*K_F)*(yF1-yF4))/(rF1F4^4);

%%% Total Forces for Follower 1 %%%

F_xF1 = F_1xF1 + F_2xF1 + Fx_F1L + Fx_F1F2 + Fx_F1F3 + Fx_F1F4 - K_p*xF1dot;
F_yF1 = F_1yF1 + F_2yF1 + Fy_F1L + Fy_F1F2 + Fy_F1F3 + Fy_F1F4 - K_p*yF1dot;

%% Calculating Forces for Follower 2:

%%% Obstacle Forces %%%
F_1xF2 = ((-K_1)*((x_1)-xF2)) / ((xF2-x_1)^2 + (yF2-y_1)^2)^(1.5);
F_2xF2 = ((-K_2)*((x_2)-xF2)) / ((xF2-x_2)^2 + (yF2-y_2)^2)^(1.5);

F_1yF2 = ((-K_1)*((x_1)-yF2)) / ((xF2-x_1)^2 + (yF2-y_1)^2)^(1.5);

F_2yF2 = ((-K_2)*((x_2)-yF2)) / ((xF2-x_2)^2 + (yF2-y_2)^2)^(1.5);

%%% Leader Forces %%%

rF2L = sqrt((xF2-xL)^2 + (yF2-yL)^2);
Fx_F2L = ((xL-xF2)*(rF2L-rD))/rF2L;
Fy_F2L = ((yL-yF2)*(rF2L-rD))/rF2L;

%%% Follower Forces %%%

rF2F1 = sqrt((xF2-xF1)^2 + (yF2-yF1)^2);
Fx_F2F1 = ((2*K_F)*(xF2-xF1))/(rF2F1^4);
Fy_F2F1 = ((2*K_F)*(yF2-yF1))/(rF2F1^4);

rF2F3 = sqrt((xF2-xF3)^2 + (yF2-yF3)^2);

Fx_F2F3 = ((2*K_F)*(xF2-xF3))/(rF2F3^4);
Fy_F2F3 = ((2*K_F)*(yF2-yF3))/(rF2F3^4);

rF2F4 = sqrt((xF2-xF4)^2 + (yF2-yF4)^2);

Fx_F2F4 = ((2*K_F)*(xF2-xF4))/(rF2F4^4);
Fy_F2F4 = ((2*K_F)*(yF2-yF4))/(rF2F4^4);

17
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

%%% Total Forces for Follower 2 %%%

F_xF2 = F_1xF2 + F_2xF2 + Fx_F2L + Fx_F2F1 + Fx_F2F3 + Fx_F2F4 - K_p*xF2dot;
F_yF2 = F_1yF2 + F_2yF2 + Fy_F2L + Fy_F2F1 + Fy_F2F3 + Fy_F2F4 - K_p*yF2dot;

%% Calculating Forces for Follower 3:

%%% Obstacle Forces %%%
F_1xF3 = ((-K_1)*((x_1)-xF3)) / ((xF3-x_1)^2 + (yF3-y_1)^2)^(1.5);
F_2xF3 = ((-K_2)*((x_2)-xF3)) / ((xF3-x_2)^2 + (yF3-y_2)^2)^(1.5);

F_1yF3 = ((-K_1)*((x_1)-yF3)) / ((xF3-x_1)^2 + (yF3-y_1)^2)^(1.5);

F_2yF3 = ((-K_2)*((x_2)-yF3)) / ((xF3-x_2)^2 + (yF3-y_2)^2)^(1.5);

%%% Leader Forces %%%

rF3L = sqrt((xF3-xL)^2 + (yF3-yL)^2);
Fx_F3L = ((xL-xF3)*(rF3L-rD))/rF3L;
Fy_F3L = ((yL-yF3)*(rF3L-rD))/rF3L;

%%% Follower Forces %%%

rF3F1 = sqrt((xF3-xF1)^2 + (yF3-yF1)^2);
Fx_F3F1 = ((2*K_F)*(xF3-xF1))/(rF3F1^4);
Fy_F3F1 = ((2*K_F)*(yF3-yF1))/(rF3F1^4);

rF3F2 = sqrt((xF3-xF2)^2 + (yF3-yF2)^2);

Fx_F3F2 = ((2*K_F)*(xF3-xF2))/(rF3F2^4);
Fy_F3F2 = ((2*K_F)*(yF3-yF2))/(rF3F2^4);

rF3F4 = sqrt((xF3-xF4)^2 + (yF3-yF4)^2);

Fx_F3F4 = ((2*K_F)*(xF3-xF4))/(rF3F4^4);
Fy_F3F4 = ((2*K_F)*(yF3-yF4))/(rF3F4^4);

%%% Total Forces for Follower 3 %%%

F_xF3 = F_1xF3 + F_2xF3 + Fx_F3L + Fx_F3F1 + Fx_F3F2 + Fx_F3F4 - K_p*xF3dot;
F_yF3 = F_1yF3 + F_2yF3 + Fy_F3L + Fy_F3F1 + Fy_F3F2 + Fy_F3F4 - K_p*yF3dot;

%% Calculating Forces for Follower 4:

%%% Obstacle Forces %%%
F_1xF4 = ((-K_1)*((x_1)-xF4)) / ((xF4-x_1)^2 + (yF4-y_1)^2)^(1.5);
F_2xF4 = ((-K_2)*((x_2)-xF4)) / ((xF4-x_2)^2 + (yF4-y_2)^2)^(1.5);

F_1yF4 = ((-K_1)*((x_1)-yF4)) / ((xF4-x_1)^2 + (yF4-y_1)^2)^(1.5);

F_2yF4 = ((-K_2)*((x_2)-yF4)) / ((xF4-x_2)^2 + (yF4-y_2)^2)^(1.5);

%%% Leader Forces %%%

rF4L = sqrt((xF4-xL)^2 + (yF4-yL)^2);
Fx_F4L = ((xL-xF4)*(rF4L-rD))/rF4L;
Fy_F4L = ((yL-yF4)*(rF4L-rD))/rF4L;

%%% Follower Forces %%%

rF4F1 = sqrt((xF4-xF1)^2 + (yF4-yF1)^2);
Fx_F4F1 = ((2*K_F)*(xF4-xF1))/(rF4F1^4);
Fy_F4F1 = ((2*K_F)*(yF4-yF1))/(rF4F1^4);

rF4F2 = sqrt((xF4-xF2)^2 + (yF4-yF2)^2);

Fx_F4F2 = ((2*K_F)*(xF4-xF2))/(rF4F2^4);
Fy_F4F2 = ((2*K_F)*(yF4-yF2))/(rF4F2^4);

rF4F3 = sqrt((xF4-xF3)^2 + (yF4-yF3)^2);

Fx_F4F3 = ((2*K_F)*(xF4-xF3))/(rF4F3^4);
Fy_F4F3 = ((2*K_F)*(yF4-yF3))/(rF4F3^4);

%%% Total Forces for Follower 3 %%%

F_xF4 = F_1xF4 + F_2xF4 + Fx_F4L + Fx_F4F1 + Fx_F4F2 + Fx_F4F3 - K_p*xF4dot;
F_yF4 = F_1yF4 + F_2yF4 + Fy_F4L + Fy_F4F1 + Fy_F4F2 + Fy_F4F3 - K_p*yF4dot;

%% Swarm Dynamics:

dS(1) = xLdot;
dS(2) = yLdot;
dS(3) = F_xL;
dS(4) = F_yL;

18
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

dS(5) = xF1dot;
dS(6) = yF1dot;
dS(7) = F_xF1;
dS(8) = F_yF1;

dS(9) = xF2dot;
dS(10) = yF2dot;
dS(11) = F_xF2;
dS(12) = F_yF2;

dS(13) = xF3dot;
dS(14) = yF3dot;
dS(15) = F_xF3;
dS(16) = F_yF3;

dS(17) = xF4dot;
dS(18) = yF4dot;
dS(19) = F_xF4;
dS(20) = F_yF4;

dS =
[dS(1);dS(2);dS(3);dS(4);dS(5);dS(6);dS(7);dS(8);dS(9);dS(10);dS(11);dS(12);dS(13);dS(14);dS(15);
dS(16);dS(17);dS(18);dS(19);dS(20);];

end

19
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

Platoon Movie:
A movie of platoon of mobile robots is prepared for this problem. Here is a screen shot of
this movie when the leader reached the target.

Platoon of Mobile Robots Trajectories

12

10

4
y

2
Leader
Follow er 1
0 Follow er 2
Follow er 3
Follow er 4
-2
Target
Obstacles
-4
-4 -2 0 2 4 6 8 10 12
x

Also, prepared movie is attached to this homework as “PlatoonMovie.avi”.

PlatoonMovie.avi

20
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015

 MATLAB code for this problem is depicted below.

writerObj = VideoWriter('PlatoonMovie.avi');
open(writerObj);

time = length(tPR);
for j = 1:time
plot(x_L(j),y_L(j),'*','MarkerSize',14,'MarkerEdgeColor','black','LineWidth',3)
hold on
plot(x_F1(j),y_F1(j),'bo','LineWidth',3)
hold on
plot(x_F2(j),y_F2(j),'ro','LineWidth',3)
hold on
plot(x_F3(j),y_F3(j),'go','LineWidth',3)
hold on
plot(x_F4(j),y_F4(j),'co','LineWidth',3)
hold on
plot(x_g,y_g,'r-s','LineWidth',2)
hold on
plot(x_1,y_1,'k-s','LineWidth',2)
hold on
plot(x_2,y_2,'k-s','LineWidth',2)
grid on
hold off
xlim([-4 12])
ylim([-4 12])
xlabel('x')
ylabel('y')
title('\bfPlatoon of Mobile Robots Trajectories')
hh = legend('Leader','Follower 1','Follower 2','Follower 3','Follower
4','Target','Obstacles','Location','SouthEast');
set(hh,'FontSize',8);
M(j) = getframe(gcf);
writeVideo(writerObj, M(j));
end
close(writerObj);

21