5
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 /i iK r for each obstacle, with ri the vector to the i-th obstacle. For the target use an attractive potential of T TK r , 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
sin
coscossincos
LVVyVx
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
y
x F my F m
with (x,y) the position of the vehicle and ,x yF F 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.
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
1
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π1
+πΎ2π2
Gains are chosen as
πΎπ = 1; πΎ1 = 3; πΎ2 = 5;
3D mesh plot for total potential is depicted below.
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
2
Contour plot for total potential is also depicted below.
02
46
810
12
0
2
4
6
8
10
12
0
20
40
60
x
Potential Field on 3D Plot
y
V
x
y
Potential Field on Contour Plot
0 2 4 6 8 10 120
2
4
6
8
10
12
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
3
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')
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
4
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.
0 510 15
051015
-600
-400
-200
0
200
400
600
x
Force X-component (Fx)
y
Fx
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
5
Forces for y-component are depicted below.
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.
0 24 6
8 1012
0
5
10
15-1
-0.5
0
0.5
1
1.5
x 104
x
Force Y-component (Fy)
y
Fy
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
6
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.
0 2 4 6 8 10 120
2
4
6
8
10
12
x
y
Single Mobile Robot Trajectory
Robot
Target
Obstacles
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
7
x-y trajectories on contour plot for this robot is depicted below.
3D mesh trajectories plot for this robot is depicted below.
x
y
Single Mobile Robot Trajectory on Contour Plot
0 2 4 6 8 10 120
2
4
6
8
10
12
02
46
810
12
0
2
4
6
8
10
12
0
20
40
60
x
Single Mobile Robot Trajectory on 3D Plot
y
V
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
8
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);
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
9
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);
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
10
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;
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
11
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.
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
12
Trajectory of leader robot is depicted below.
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
0 2 4 6 8 10 120
2
4
6
8
10
12
x
y
Single Mobile Robot Trajectory (Point-Mass Dynamics)
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
13
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.
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
14
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.
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);
-2 0 2 4 6 8 10 12-2
0
2
4
6
8
10
12
x
yPlatoon of Mobile Robots Trajectories
Leader
Follow er 1
Follow er 2
Follow er 3
Follow er 4
Target
Obstacles
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
15
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);
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
16
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;
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
17
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);
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
18
%%% 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;
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
19
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
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
20
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.
Also, prepared movie is attached to this homework as βPlatoonMovie.aviβ.
PlatoonMovie.avi
-4 -2 0 2 4 6 8 10 12-4
-2
0
2
4
6
8
10
12
x
y
Platoon of Mobile Robots Trajectories
Leader
Follow er 1
Follow er 2
Follow er 3
Follow er 4
Target
Obstacles
Sukru Akif ERTURK, 1000810591, HW4 02/24/2015
21
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);
Top Related