Appendix: MATLAB Simulation Code

%====== Information ==== Obstacle==Attractor==Start==Goal========%

% Simulator for Potential Field based motion planning -- 2D
% Using Generalized Gaussian models for attractors and repulsors
%========================================================%
clear all
close all
mov = avifile('Temp.avi')
numAIn = 1; % # of attractors
numRIn = 4; % # of repulsors
%Positions of attractors and repulsors
paramA = [50 70 50]; %======(ax, ay, variance)
%=== Sparsely spaced obstacles ========%
paramR = [48 35 5 5; 50 55 5 5; 55 40 2.5 3; 56.5 60 2.5 3; 1 1 1 1; 1 1 1 1; 1 1 1
1]; %==(rx, ry, variance ,C)
%=== Different scenarios ========%
%=== Different location for repulsor ==========%
%=== Closely spaced obstacles ========%
% k = 1;
% x = 20;
% for y = 0:3:30
% paramR(k,:) = [x y 2 1];
% k = k + 1;
% end
% for x = 22:3:30
% paramR(k,:) = [x y 2 1];
% k = k + 1;
% end
% for x = 40:3:56
% paramR(k,:) = [x y 2 1];
% k = k + 1;
% end
% for y = 32:3:70
% paramR(k,:) = [x y 2 1];
% k = k + 1;
82

% end
% for x = 55:3:70
% paramR(k,:) = [x y 2 1];
% k = k + 1;
% end
% m = 32;
% for n = 0:3:16
% paramR(k,:) = [m n 2 1];
% k = k + 1;
% end
% for m = 34:3:50
% paramR(k,:) = [m n 2 1];
% k = k + 1;
% end
% for m = 60:3:70
% paramR(k,:) = [m n 2 1];
% k = k + 1;
% end
% for n = 18:3:70
% paramR(k,:) = [m n 2 1];
% k = k + 1;
% end
% x = 30;
% for y = 30:3:50
% paramR(k,:) = [x y 2 1];
% k = k + 1;
% end
% o = 40;
% for p = 30:3:50
% paramR(k,:) = [o p 2 1];
% k = k + 1;
% end
% o = 50;
% for p = 0:3:15
% paramR(k,:) = [o p 2 1];
% k = k + 1;
% end
% o = 60;
% for p = 0:3:15
% paramR(k,:) = [o p 2 1];
% k = k + 1;
% end
% numRIn = k-1;
%============================%

83

target_location = [1 1];
target_location(1) = paramA(1,1);
target_location(2) = paramA(1,2);
% Link parameters
%========================================%
linkLen = 5;
linkLen12 = 5;
linkLen13 = 5;
varHead =50;
%========================================%
flag = 0;
tp0 = 1;
flag_two = 0;
stop1 = 0;
stop2 = 0;
km= 0.3; %angular velocity coefficient
%Simulation parameters: initial conditions
%========================================%
NSTEP = 600;
stepSize = 0.2; %====== Speed (rate)
x0 = [50 20]; %====== Start point
theta0 = pi*(-105/180.0);
theta012 = pi*(-120/180.0);
theta013 = pi*(-105/180.0);
%========================================%
x1 = x0;
theta1 = theta0;
theta12 = theta012;
theta13 = theta013;
%==== Position of links with respect to head =====%
xHead = x1;
xTail = xHead + [linkLen*cos(theta1), linkLen*sin(theta1)];
xTail12 = xTail + [linkLen12*cos(theta12), linkLen12*sin(theta12)];
xTail13 = xTail12 + [linkLen13*cos(theta13), linkLen13*sin(theta13)];
xtg(1,:) = x1;
xtgT(1,:) = xTail;
xtgT12(1,:) = xTail12;
xtgT13(1,:) = xTail13;
xtgTheta(1) = theta1;
xtgTheta12(1) = theta12;

84

xtgTheta13(1) = theta13;
flagr=0;
for k = 1:NSTEP
z = k;
% ===== for more links use loop
xHead = x1;
xTail = xHead + [linkLen*cos(theta1), linkLen*sin(theta1)];
xTail12 = xTail + [linkLen12*cos(theta12), linkLen12*sin(theta12)];
%=======for one link separation=== for more than one location or link use
loop=======%
if x1(1) < 57 && x1(1) > 53
if x1(2) <52 && x1(2) > 48
flagr = 1;
end
end
if flagr == 0
xTail13 = xTail12 + [linkLen13*cos(theta13), linkLen13*sin(theta13)];
end
%====Find Next Location and heading direction======%
if stop1 == 0
[theta1, theta12, theta13, x1] = Next_location(x1, theta1, theta12, theta13, paramA,
paramR, numRIn);
xtgTheta(k+1) = theta1;
xtgTheta12(k+1) = theta12;
xtgTheta13(k+1) = theta13;
xtg(k+1,:) = x1;
xtgT(k+1,:) = x1 + [linkLen*cos(theta1), linkLen*sin(theta1)];
xtgT12(k+1,:) = xtgT(k+1,:) + [linkLen12*cos(theta12), linkLen12*sin(theta12)];
if flagr == 0
xtgT13(k+1,:) = xtgT12(k+1,:) + [linkLen13*cos(theta13), linkLen13*sin(theta13)];
end
%====To find whether or not it is stuck.. at local minima===%
xtg3 = xtg(k+1,:);
xtg2 = xtg(k,:);
tp1 = xtg2(1);

85

tp2 = xtg3(1);
tp3 = xtg2(2);
tp4 = xtg3(2);
tp5(tp0) = tp2 - tp1;
tp6(tp0) = tp4 - tp3;
tp0 = tp0 + 1;
%=====target reached??!!====%
if abs(target_location(1) - tp1) < 0.1
if abs(target_location(2) - tp3) < 0.1
flag = 5;
end
end
if tp0 == 11
tp0 = 1;
[flag_two] = local_minima(flag_two,tp5,tp6);
end
if flag_two == 2
flag_two = 0;
[x1, theta1, theta12, theta13] = Simulated_annealing(numAIn, numRIn, xtg(k-1,:),
paramA, paramR, theta1, theta12, theta13);
end
end
figure(1)
xTrans = 0;
xmin=0;
xmax=100;
ymin=0;
ymax=100;
dx = 1;
dy = 1;
[xx,yy] = meshgrid(xmin:dx:xmax,ymin:dy:ymax);
V3 = V3Generate_field(numAIn, numRIn, xx,yy,paramA,paramR);
%========= To Increase speed of simulation =========%
if k > 250
if k < 300
hold on
contourf(xx + xTrans,yy,V3,1);%colormap
colormap cool

86

grid
end
end
hold off
%==============================%
plot(xtg(:,1)+xTrans,xtg(:,2),'r','linewidth',2)
hold on
%====show the position of robots at each step====%
if stop1 == 0
text(xtg(k,1), xtg(k,2), sprintf('%s%d','R',k),'FontSize',15)
end
hold on
plot([xTail(1),xHead(1)], [xTail(2),xHead(2)], 'bl','linewidth',5)
grid
plot([xTail12(1),xTail(1)], [xTail12(2),xTail(2)], 'bl','linewidth',5)
grid
if flagr == 0
plot([xTail13(1),xTail12(1)], [xTail13(2),xTail12(2)], 'bl', 'linewidth',5)
grid
end
if flagr == 1
plot([xTail13(1),(xTail13(1)+1)],[xTail13(2),(xTail13(2)+1)],'bl','linewidth',5)
grid
end
for variable_x = 1:numAIn
text(paramA(variable_x,1), paramA(variable_x,2), '@')
end
text(60,38,'R')
hold on
contour(xx + xTrans,yy,V3,1,'g','linewidth',2);%colormap
%colormap cool %====(to change color map, contour plots)====%
grid
hold on
plot(xtgT(:,1) + xTrans,xtgT(:,2),'g','linewidth',1)
grid
hold off
hold on
plot(xtgT12(:,1) + xTrans,xtgT12(:,2),'g','linewidth',1)
grid
hold off

87

hold on
plot(xtgT13(:,1) + xTrans,xtgT13(:,2),'g','linewidth',1)
grid
hold off
axis([20 70 0 70])
Mt = getframe;
if flag == 5
flag = 0;
stop1 = 1;
end
end
mov = close(mov)

%============== Next_location =====================%

%========== Function : Next Location =================%
function [theta1,theta12,theta13,x1] = Next_location(x1, theta1In, theta12In, theta13In,
paramAIn, paramRIn, numRIn1)
delta =1e-12;
theta1 = theta1In;
theta12 = theta12In;
theta13 = theta13In;
paramR = paramRIn;
stepSize = 0.2;
linkLen = 5;
linkLen12 = 5;
linkLen13 = 5;
varHead =50;
xHead = x1;
xTail = xHead + [linkLen*cos(theta1), linkLen*sin(theta1)];
xTail12 = xTail + [linkLen12*cos(theta12), linkLen12*sin(theta12)];
xTail13 = xTail12 + [linkLen13*cos(theta13), linkLen13*sin(theta13)];
numAIn = 1; % # of attractors
numRIn = numRIn1; % # of repulsors
paramA = paramAIn;

88

%===== f: function, g: gradient, H: hessian ========%

[f,g,H] = fnPF(numAIn, numRIn, x1,paramA,paramR);
%=====Newtons Method : Hessian Matrix ======%
epslon = max( 0, delta - 0.5*( H(1,1)+H(2,2) - sqrt((H(1,1)-H(2,2))^2+4*H(1,2)^2) ) );
dtm = (epslon+H(1,1))*(epslon+H(2,2)) - H(1,2)^2;
B(1,1) = (epslon+H(2,2))/dtm;
B(2,2) = (epslon+H(1,1))/dtm;
B(1,2) = -H(1,2)/dtm;
B(2,1) = B(1,2);
%===== for control law ========%
dk = - (B*g')';
paramATail = [xHead(1),xHead(2),varHead];
paramATail12 = [xTail(1),xTail(2),varHead];
paramATail13 = [xTail12(1),xTail12(2),varHead];
numAInTail = 1;
numAInTail12 = 1;
numAInTail13 = 1;
%==== for tails ===========%
[fTail,gTail,HTail] = fnPF(numAInTail, numRIn, xTail,paramATail,paramR);
epslon = max( 0, delta - 0.5*( HTail(1,1)+HTail(2,2) - sqrt((HTail(1,1)HTail(2,2))^2+4*HTail(1,2)^2) ) );
dtm = (epslon+HTail(1,1))*(epslon+HTail(2,2)) - HTail(1,2)^2;
BT(1,1) = (epslon+HTail(2,2))/dtm;
BT(2,2) = (epslon+HTail(1,1))/dtm;
BT(1,2) = -HTail(1,2)/dtm;
BT(2,1) = BT(1,2);

[fTail12,gTail12,HTail12] = fnPF(numAInTail12, numRIn, xTail12, paramATail12,

paramR);
epslon = max( 0, delta - 0.5*( HTail12(1,1)+HTail12(2,2) - sqrt((HTail12(1,1)HTail12(2,2))^2+4*HTail12(1,2)^2) ) );
dtm = (epslon+HTail12(1,1))*(epslon+HTail12(2,2)) - HTail12(1,2)^2;
BT2(1,1) = (epslon+HTail12(2,2))/dtm;
BT2(2,2) = (epslon+HTail12(1,1))/dtm;
BT2(1,2) = -HTail12(1,2)/dtm;

89

BT2(2,1) = BT2(1,2);
[fTail13,gTail13,HTail13] = fnPF(numAInTail13, numRIn, xTail13, paramATail13,
paramR);
epslon = max( 0, delta - 0.5*( HTail13(1,1)+HTail13(2,2) - sqrt((HTail13(1,1)HTail13(2,2))^2+4*HTail13(1,2)^2) ) );
dtm = (epslon+HTail13(1,1))*(epslon+HTail13(2,2)) - HTail13(1,2)^2;
BT3(1,1) = (epslon+HTail13(2,2))/dtm;
BT3(2,2) = (epslon+HTail13(1,1))/dtm;
BT3(1,2) = -HTail13(1,2)/dtm;
BT3(2,1) = BT3(1,2);
% Orientation as the reference direction
refDirFlag = 1;
if refDirFlag == 1
refVect = [cos(theta1), sin(theta1)];
coordAngle = pi/2;
end
% Negative gradient of head as the reference direction
if refDirFlag ~= 1
%refVect = -g;
refVect = - (BT*gTail')';
coordAngle = -pi/2;
end
refRot = [cos(coordAngle), -sin(coordAngle); sin(coordAngle), cos(coordAngle)];
refVect2 = refRot*refVect';
inProd = -gTail(1)*refVect2(1) - gTail(2)*refVect2(2);
if inProd >= 0
omegaDir = 1;
else
omegaDir = -1;
end
%dk= -g; %Gradient

% Orientation as the reference direction link 2

refDirFlag12 = 1;
if refDirFlag12 == 1
refVect12 = [cos(theta12), sin(theta12)];
coordAngle12 = pi/2;
end

90

% Negative gradient of head as the reference direction

if refDirFlag12 ~= 1
%refVect12 = -g;
refVect12 = - (BT2*gTail12')';
coordAngle12 = -pi/2;
end
refRot12 = [cos(coordAngle12), -sin(coordAngle12); sin(coordAngle12),
cos(coordAngle12)];
refVect122 = refRot12*refVect12';
inProd12 = -gTail12(1)*refVect122(1) - gTail12(2)*refVect122(2);
if inProd12 >= 0
omegaDir12 = 1;
else
omegaDir12 = -1;
end
%dk= -g; %Gradient
% Orientation as the reference direction link 3
refDirFlag13 = 1;
if refDirFlag13 == 1
refVect13 = [cos(theta13), sin(theta13)];
coordAngle13 = pi/2;
end
% Negative gradient of head as the reference direction
if refDirFlag13 ~= 1
%refVect13 = -g;
refVect13 = - (BT3*gTail13')';
coordAngle13 = -pi/2;
end
refRot13 = [cos(coordAngle13), -sin(coordAngle13); sin(coordAngle13),
cos(coordAngle13)];
refVect123 = refRot13*refVect13';
inProd13 = -gTail13(1)*refVect123(1) - gTail13(2)*refVect123(2);
if inProd13 >= 0
omegaDir13 = 1;
else
omegaDir13 = -1;
end
% dk= -g; %Gradient

91

x2(1) = x1(1) + stepSize*dk(1)/sqrt(dk(1)^2+dk(2)^2);

x2(2) = x1(2) + stepSize*dk(2)/sqrt(dk(1)^2+dk(2)^2);
theta2 = theta1 + 0.05*omegaDir;
theta122 = theta12 + 0.05*omegaDir12;
theta123 = theta13 + 0.05*omegaDir13;
x10 = x1;
x1 = x2;
theta1 = theta2;
theta12 = theta122;
theta13 = theta123;
return

%===== fnPF : find gradient and hessian ==============%

% Calculate function/gradient/Hessian info at position xin
% xin = [x,y]
% paramAin = [ax1,ay1,var1; ax2,ay2,var2; ...]
% paramRin = [rx1,ry1,var1,C1; rx2,ry2,var2,C2; ...]
% [f,g,H] = [function, gradient, Hessian]
%
% Attractor: function [f,g,H]=fnAttractor(xin,paramA)
% Repulsor: function [f,g,H]=fnRepulsor(xin,paramR)
function [f,g,H]=fnPF(numAIn, numRIn, xin, paramAIn, paramRIn)
numA = numAIn;
numR = numRIn;
f = 0;
g = [0 0];
H = [0 0; 0 0];
% ========Attractors========%
for k = 1:numA
paramA = paramAIn(k,:);
[fa,ga,Ha]=fnAttractor(xin,paramA);
f = f + fa;
if nargout > 1 % Gradient evaluated at x,y
g = g + ga;
end

92

if nargout > 2 % Hessian evaluated at x,y

H = H + Ha;
end
end
% =========Repulsors==========%
for k = 1:numR
paramR = paramRIn(k,:);
[fr,gr,Hr]=fnRepulsor(xin,paramR);
f = f + fr;
if nargout > 1 % Gradient evaluated at x,y
g = g + gr;
end
if nargout > 2 % Hessian evaluated at x,y
H = H + Hr;
end
end
g = g*1000;
return

%==== function : Attractor =====%

%================================================%
% Calculate gradient information at position xin
% xin = [x,y]
% param = [ax,ay,var]
% [f,g,H] = [function, gradient, Hessian]
%================================================%
function [f,g,H]=fnAttractor(xin,param)
x = xin(1);
y = xin(2);
ax = param(1);
ay = param(2);
var = param(3);
Amp = 300;
% Compute the objective function value at x,y
f = 1 - exp(-((x-ax)^2 + (y-ay)^2)/(2*var^2));
f1 = f - 1 ;
% Compute the gradient at x,y

93

if nargout > 1 % fun called with two output arguments

% Gradient of the function evaluated at x,y
g(1) = f1*(ax-x)/(var^2);
g(2) = f1*(ay-y)/(var^2);
end
% Compute the Hessian at x,y
if nargout > 2
% Hessian evaluated at x,y
H(1,1) = f1*((ax-x)^2 - var^2)/(var^4);
H(2,2) = f1*((ay-y)^2 - var^2)/(var^4);
H(1,2) = f1*(ax-x)*(ay-y)/(var^4);
H(2,1) = H(1,2);
end
return

%==== function : Repulsor =====%

%=============================================%
% Calculate gradient information at position xin
% xin = [x,y]
% param = [rx,ry,var,C]
% [f,g,H] = [function, gradient, Hessian]
%=============================================%
function [f,g,H]=fnRepulsor(xin,param)
x = xin(1);
y = xin(2);
rx = param(1);
ry = param(2);
var = param(3);
C = param(4);
% Compute the objective function value at x,y
Axy = ((x-rx)^2 + (y-ry)^2)/(var^2);
f = exp(-0.5*Axy^C);

% Compute the gradient at x,y

94

if nargout > 1 % fun called with two output arguments

% Gradient of the function evaluated at x,y
g(1) = f*Axy^(C-1)*C*(rx-x)/(var^2);
g(2) = f*Axy^(C-1)*C*(ry-y)/(var^2);
end
% Compute the Hessian at x,y
if nargout > 2
% Hessian evaluated at x,y
H(1,1) = f*( Axy^(2*C-2)*(C*(rx-x)/(var^2))^2 - Axy^(C-1)*C/(var^2) - Axy^(C2)*2*C*(C-1)*(rx-x)^2/(var^4) );
H(2,2) = f*( Axy^(2*C-2)*(C*(ry-y)/(var^2))^2 - Axy^(C-1)*C/(var^2) - Axy^(C2)*2*C*(C-1)*(ry-y)^2/(var^4) );
H(1,2) = f*C*(rx-x)*(ry-y)/(var^4)*( C*Axy^(2*C-2) - 2*(C-1)*Axy^(C-2) );
H(2,1) = H(1,2);
end
return

%====== Local Minima ======%

%====== Whether or not its stuck condition =====%
function [flag_two] = local_minima(flag_twoIn,tp5In,tp6In)
tp5 = tp5In;
tp6 = tp6In;
flag_two = flag_twoIn;
tp55 = 0;
tp66 = 0;
for m = 1:10
tp55 = tp5(m) + tp55;
tp66 = tp6(m) + tp66;
end
tp55 = tp55 / 10;
tp66 = tp66 / 10;
tp55=abs(tp55);
tp66=abs(tp66);
if tp55 < 0.05
if tp66 < 0.05
flag_two = flag_two + 1;
end

95

end
return

%======= Simulated Annealing ============%

%======= Local Minima Recovery ===============%

function [x1,theta1,theta12,theta13] = Simulated_annealing(numAIn, numRIn, xtg,

paramA, paramR, theta1, theta12, theta13)
no_solution = 0;
no_solution1 = 0;
no_solution2 = 0;
%=== find navigation function for neighbor ======%
[f_minima] = fnPF_nbr(numAIn, numRIn, xtg,paramA,paramR);
temp_th = 0.1430;
temp = 20;
x1=xtg;
t=1;
while temp > temp_th
temp = temp * 0.9;
%=== find bunch of neighbors ====%
[x_random] = neighbours(x1);
%========= Avoid back tracking =======%
x20(t,:)=x1;
t = t+1;
f_best = 1;
for S = 1:9
[f_random(S,:)] = fnPF_nbr(numAIn, numRIn, x_random(S,:), paramA, paramR);
flag_three = 0;
for t1 = 1:(t-1)
if x20(t1,:) == x_random(S,:)
flag_three = 1;
end
end

96

if (flag_three == 1)
continue
else
if f_random(S,:) <= f_best
f_best = f_random(S,:);
R = S;
end
end
end
%====== finding the delta E , to check 0.2 <= e^(-deltaE / T) ======%
delta_f = f_best - f_minima;
f_temp = delta_f / temp;
x_random(R,:)
if delta_f < 0
x21 = x1;
x1 = x_random(R,:);
hold off
%=== Move towards the local target ===%
[theta1,theta12,theta13] = local_goal(x1, x21, theta1, theta12, theta13, paramR);
no_solution = 1;
break
elseif (0 <= delta_f) && (delta_f <= 0.99)
if (f_temp <= 1.60) && (f_best < 1)
x21 = x1;
x1 = x_random(R,:);
hold off
%=== Move towards the local target ===%
[theta1,theta12,theta13] = local_goal(x1, x21, theta1, theta12, theta13, paramR);
else
no_solution1 = 1;
break
end
else
no_solution2 = 1;
break
end
end

97

return

%========= Function for Neighbor =======%

%======= Find value of navigation function for neighbors ==========%

function [f_nbr]=fnPF_nbr(numAIn, numRIn, xin,paramAIn,paramRIn)

numA = numAIn;
numR = numRIn;
f_nbr = 0;
%==== Attractors ======%
for k = 1:numA
paramA = paramAIn(k,:);
[fa_nbr]=fnAttractor_nbr(xin,paramA);
f_nbr = f_nbr + fa_nbr;
end
%==== Repulsors ======%
for k = 1:numR
paramR = paramRIn(k,:);
[fr_nbr]=fnRepulsor_nbr(xin,paramR);
f_nbr = f_nbr + fr_nbr;
end
return

%====== Function Attractor ======%

%====== Attractor Function for neighbor =======%
function [f]=fnAttractor_nbr(xin,param)
x = xin(1);
y = xin(2);
ax = param(1);
ay = param(2);
var = param(3);
%=== Compute the objective function value at x,y ===%
f = 1 - exp(-((x-ax)^2 + (y-ay)^2)/(2*var^2));
return

98

%====== Function Repulsor ======%

%====== Repulsor Function for neighbor =======%
function [f]=fnRepulsor_nbr(xin,param)
x = xin(1);
y = xin(2);
rx = param(1);
ry = param(2);
var = param(3);
C = param(4);
%=== Compute the objective function value at x,y ====%
Axy = ((x-rx)^2 + (y-ry)^2)/(var^2);
f = exp(-0.5*Axy^C);
return

%==== Neighbor ======%

%==== Find Neighbors of current location ======%
function [x_random] = neighbours (x1)
x_1 = x1(:,1);
y_1 = x1(:,2);
o = 1;
for m = (x_1 - 0.5): 0.5 : (x_1 + 0.5)
for n = (y_1 - 0.5): 0.5 : (y_1 + 0.5)
x_random(o,:) = [m,n];
o = o + 1;
end
end
%==== To find randomize bunch of neighbor, use matlab function randi() with the loop
=====%
return

99

%========== Local Goal : SA ==========%

%======== Move towards local goal set by annealing ========%

function [theta1,theta12,theta13] = local_goal(local_goal, local_start, local_theta,

local_theta12, local_theta13, paramRIn)
numAIn = 1; % # of attractors
numRIn = 4; % # of repulsors
%Positions of attractors and repulsors : attractor is local goal
paramA = [local_goal(1) local_goal(2) 50; 50 100 100]; %(ax,ay,var)
paramR = paramRIn;
paramA2 = [50 70 50];
% Link parameters
linkLen = 5;
linkLen12 = 5;
linkLen13 = 5;
varHead =50;
flag = 0;
flag_two = 0;
km= 0.3; %angular velocity coefficient
%Simulation parameters: initial conditions
NSTEP2 = 8;
stepSize = 0.1; %Speed (rate)
x0 = local_start; %Start point
x1 = x0;
theta1 = local_theta;
theta12 = local_theta12;
theta13 = local_theta13;
xHead = x1;
xTail = xHead + [linkLen*cos(theta1), linkLen*sin(theta1)];
xTail12 = xTail + [linkLen12*cos(theta12), linkLen12*sin(theta12)];
xTail13 = xTail12 + [linkLen13*cos(theta13), linkLen13*sin(theta13)];
xtg(1,:) = x1;
xtgT(1,:) = xTail;
xtgT12(1,:) = xTail12;
xtgT13(1,:) = xTail13;

100

xtgTheta(1) = theta1;
xtgTheta12(1)=theta12;
xtgTheta13(1)=theta13;
for k2 = 1:NSTEP2
xHead = x1;
xTail = xHead + [linkLen*cos(theta1), linkLen*sin(theta1)];
xTail12 = xTail + [linkLen12*cos(theta12), linkLen12*sin(theta12)];
xTail13 = xTail12 + [linkLen13*cos(theta13), linkLen13*sin(theta13)];
[f,g,H] = fnPF(numAIn, numRIn, x1,paramA,paramR);
paramATail = [xHead(1),xHead(2),varHead];
numAInTail = 1;
[fTail,gTail,HTail] = fnPF(numAInTail, numRIn, xTail, paramATail, paramR);
paramATail12 = [xTail(1),xTail(2),varHead];
numAInTail12 = 1;
[fTail12,gTail12,HTail12] = fnPF(numAInTail12, numRIn, xTail12, paramATail12,
paramR);
paramATail13 = [xTail12(1),xTail12(2),varHead];
numAInTail13 = 1;
[fTail13,gTail13,HTail13] = fnPF(numAInTail13, numRIn, xTail13, paramATail13,
paramR);
% Orientation as the reference direction
refDirFlag = 1;
if refDirFlag == 1
refVect = [cos(theta1), sin(theta1)];
coordAngle = pi/2;
end
% Negative gradient of head as the reference direction
if refDirFlag ~= 1
refVect = -g;
coordAngle = -pi/2;
end
refRot = [cos(coordAngle), -sin(coordAngle); sin(coordAngle), cos(coordAngle)];
refVect2 = refRot*refVect';
inProd = -gTail(1)*refVect2(1) - gTail(2)*refVect2(2);
if inProd >= 0
omegaDir = 1;

101

else
omegaDir = -1;
end
dk= -g; %Gradient
% Orientation as the reference direction Link 2
refDirFlag12 = 1;
if refDirFlag12 == 1
refVect12 = [cos(theta12), sin(theta12)];
coordAngle12 = pi/2;
end
% Negative gradient of head as the reference direction
if refDirFlag12 ~= 1
refVect12 = -g;
coordAngle12 = -pi/2;
end
refRot12 = [cos(coordAngle12), -sin(coordAngle12); sin(coordAngle12),
cos(coordAngle12)];
refVect122 = refRot12*refVect12';
inProd12 = -gTail12(1)*refVect122(1) - gTail12(2)*refVect122(2);
if inProd12 >= 0
omegaDir12 = 1;
else
omegaDir12 = -1;
end
dk= -g; %Gradient
% Orientation as the reference direction Link 3
refDirFlag13 = 1;
if refDirFlag13 == 1
refVect13 = [cos(theta13), sin(theta13)];
coordAngle13 = pi/2;
end
% Negative gradient of head as the reference direction
if refDirFlag13 ~= 1
refVect13 = -g;
coordAngle13 = -pi/2;
end
refRot13 = [cos(coordAngle13), -sin(coordAngle13); sin(coordAngle13),
cos(coordAngle13)];
refVect123 = refRot13*refVect13';

102

inProd13 = -gTail13(1)*refVect123(1) - gTail13(2)*refVect123(2);

if inProd13 >= 0
omegaDir13 = 1;
else
omegaDir13 = -1;
end
dk= -g; %Gradient

x2(1) = x1(1) + stepSize*dk(1)/sqrt(dk(1)^2+dk(2)^2);

x2(2) = x1(2) + stepSize*dk(2)/sqrt(dk(1)^2+dk(2)^2);
theta2 = theta1 + 0.05*omegaDir;
theta122 = theta12 + 0.05*omegaDir12;
theta123 = theta13 + 0.05*omegaDir13;
x10 = x1;
x1 = x2;
theta1 = theta2;
theta12 = theta122;
theta13 = theta123;
xtgTheta(k2+1) = theta1;
xtgTheta12(k2+1) = theta12;
xtgTheta13(k2+1) = theta13;

xtg(k2+1,:) = x1;
xtgT(k2+1,:) = x1 + [linkLen*cos(theta1), linkLen*sin(theta1)];
xtgT12(k2+1,:) = xtgT(k2+1,:) + [linkLen12*cos(theta12), linkLen12*sin(theta12)];
xtgT13(k2+1,:) = xtgT12(k2+1,:) + [linkLen13*cos(theta13), linkLen13*sin(theta13)];
figure(1)
xTrans = 0;
xmin=0;
xmax=100;
ymin=0;
ymax=100;
dx = 1;
dy = 1;
[xx,yy] = meshgrid(xmin:dx:xmax,ymin:dy:ymax);
V3 = V3Generate_field(numAIn, numRIn, xx,yy,paramA2,paramR);

103

hold on
contourf(xx + xTrans,yy,V3,1);%colormap
colormap cool
grid
%hold off
plot(xtg(:,1)+xTrans,xtg(:,2),'r','linewidth',2)
hold on
%show the position of robots at each step
text(xtg(k2,1), xtg(k2,2), sprintf('%s%d','R',k2),'FontSize',15)
hold on
plot([xTail(1),xHead(1)], [xTail(2),xHead(2)], 'bl','linewidth',5)
grid
plot([xTail12(1),xTail(1)], [xTail12(2),xTail(2)], 'bl','linewidth',5)
grid
plot([xTail13(1),xTail12(1)], [xTail13(2),xTail12(2)], 'bl','linewidth',5)
grid
for variable_x = 1:numAIn
text(paramA2(variable_x,1), paramA2(variable_x,2), '@')
end
hold on
plot(xtgT(:,1) + xTrans,xtgT(:,2),'g','linewidth',1)
grid
hold off
hold on
plot(xtgT12(:,1) + xTrans,xtgT12(:,2),'g','linewidth',1)
grid
hold off
hold on
plot(xtgT13(:,1) + xTrans,xtgT13(:,2),'g','linewidth',1)
grid
hold off
axis([20 70 0 70])
Mt = getframe;
end
return

104

%=========== V3Generate_field =========%

% Generate 3D data at grids (xx,yy)
% xx,yy: matrices of grid points
% paramAIn = [ax1,ay1,var1; ax2,ay2,var2; ...]
% paramRIn = [rx1,ry1,var1,C1; rx2,ry2,var2,C2; ...]
% Attractor: function V3=fnAContour(xin,paramA)
% Repulsor: function V3=fnRContour(xin,paramR)
%===========================================%
function V3 = V3Generate_2(numAIn,numRIn, xx,yy,paramAIn,paramRIn)
numA = numAIn;
numR = numRIn;
V3 = 0;
for k = 1:numA
paramA = paramAIn(k,:);
fa = fnAContour(xx,yy,paramA);
V3 = V3 + fa;
end
for k = 1:numR
paramR = paramRIn(k,:);
fr = fnRContour(xx,yy,paramR);
V3 = V3 + fr;
end
return

%====== function : Attractor : Contour =======%

%========== Create contours plots for attractive field =======%
function V3=fnAContour(xxIn,yyIn,paramA)
xx = xxIn;
yy = yyIn;
ax = paramA(1);
ay = paramA(2);
var = paramA(3);
% Compute the objective function value at x,y
V3 = 1 - exp(-((xx-ax).^2 + (yy-ay).^2)/(2*var^2));
return

105

%====== function : Repulsor : Contour =======%

%========== Create contours plots for repulsive field =======%
function V3=fnRContour(xxIn,yyIn,paramR)
xx = xxIn;
yy = yyIn;
rx = paramR(1);
ry = paramR(2);
var = paramR(3);
C = paramR(4);
% Compute the objective function values at grids (xx,yy)
Axy = ((xx-rx).^2 + (yy-ry).^2)/(var^2);
V3 = exp(-0.5*Axy.^C);
return

106