ee 5329 homework 4 mobile robot control & potential ... robotics scut/neu june...5 ee 5329 homework...

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.


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


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;


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')


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


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


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


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


8


clear all; clc;

global K_1 K_2 K_G

%% Gains

K_G = 1;

K_1 = 3;

K_2 = 5;


x_1 = 4;

y_1 = 6;


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);


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


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


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);


x_1 = 4;

y_1 = 6;


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);


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);


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;


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 − 𝐾 𝑦𝐿̇



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)


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.



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.


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;


x_1 = 4;

y_1 = 6;


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];


[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


15

x_robot = States(:,1);

y_robot = States(:,2);

%% Plot Single Robot Trajectory

figure;

plot(x_robot,y_robot,'k.')

hold on


hold on


hold on


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];


[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);







%% 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


hold on


hold on


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);


16

y = state(2);

xdot = state(3);

ydot = state(4);

%% Gains

K_p = 3;


x_1 = 4;

y_1 = 6;


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;


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);


xF2 = state(9);

yF2 = state(10);

xF2dot = state(11);

yF2dot = state(12);


xF3 = state(13);

yF3 = state(14);

xF3dot = state(15);

yF3dot = state(16);


xF4 = state(17);

yF4 = state(18);

xF4dot = state(19);

yF4dot = state(20);


x_1 = 4;

y_1 = 6;


x_2 = 6;


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);


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

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


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;



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);







Fx_F2F1 = ((2*K_F)*(xF2-xF1))/(rF2F1^4);

Fy_F2F1 = ((2*K_F)*(yF2-yF1))/(rF2F1^4);


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

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


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

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


18






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);







Fx_F3F1 = ((2*K_F)*(xF3-xF1))/(rF3F1^4);

Fy_F3F1 = ((2*K_F)*(yF3-yF1))/(rF3F1^4);


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

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


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

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






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);







Fx_F4F1 = ((2*K_F)*(xF4-xF1))/(rF4F1^4);

Fy_F4F1 = ((2*K_F)*(yF4-yF1))/(rF4F1^4);


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

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


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

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




%% Swarm Dynamics:

dS(1) = xLdot;

dS(2) = yLdot;

dS(3) = F_xL;

dS(4) = F_yL;


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


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


21


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


hold on


hold on


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);

ee 5329 homework 4 mobile robot control & potential ... robotics scut/neu june...5 ee 5329 homework...

Documents