Spring 18 MT 461 Path Planning for Mobile Robots Midterm Solution

Question 1a

>> syms x y

>> z=((x-2)^2/4)-((y+3)^2/9)-1

ezplot(z)

Question 1b

(x − 2)2 (y + 3)2
𝐻(𝑥, 𝑦, 𝜆) = (𝑥 − 3)2 + (𝑦 − 2)2 + 𝜆 ∗ (( )−( ) − 1)
4 9

Steps to solve above equation are:

 Find first derivatives w.r.t 𝑥, 𝑦, 𝜆

 Put first derivatives equal to zero i.e. ∇𝐻 = 0 w.r.t 𝑥, 𝑦, 𝜆
 Solve the equations from step 2 to obtain optimal points (𝑥 ∗ , 𝑦 ∗ )

1|Page
Spring 18 MT 461 Path Planning for Mobile Robots Midterm Solution

Question 1c

(x − 2)2 (y + 3)2
𝐻(𝑥, 𝑦, 𝜆) = 𝑦 − √3𝑥 + 𝜆 ∗ (( )−( ) − 1)
4 9

From the sketch, it can be observed that at 𝑥 ∗ = −0.473 𝑎𝑛𝑑 𝑦 ∗ = −0.819 an optimal solution is
obtained which satisfies the objective function as well as constraint equation.

>> x=-0.473;y=-0.819;

>> y-sqrt(3)*x = 0.00 % objective function

>> cf=(x - 2)^2/4 - (y + 3)^2/9 = 1.00 % constraint equation satisfied

Question 1d
∇f
Given f(x) objective function and g(x) as constraint, 𝜆 = − ∇g is the ratio of the change in objective
function per unit change in the constraint condition. It is also called sensitivity function. This
sensitivity function can provide information of change in cost function if trajectory parameters are
changed.

2|Page
Spring 18 MT 461 Path Planning for Mobile Robots Midterm Solution

Question 2a

Motion of the robot can be represented as a set of differential equation such that:

𝑥 ′ = 𝑣𝑐𝑜𝑠𝜃
𝑦 ′ = 𝑣𝑠𝑖𝑛𝜃
𝑣
𝜃′ = 𝜔 =
𝑟
where 𝜔 is the angular velocity of the car and r is radius of the circle with which the car is turning.

Question 2b & 2c
clear all tangent 1
clc x1 = -0.4948 7.5052
%circle plot
viscircles([1,6],2)
y1 = 7.3287 16.3287
hold on tangent 2
viscircles([9,15],2) x2 = 2.4948 10.4948
%initial information y2 = 4.6713 13.6713
a1=1;b1=6;a2=9;b2=15;r1=2;r2=2
;
a21 = a2-a1;
b21 = b2-b1;
d2 = a21^2+b21^2;
r21 = (r2-r1)/d2;
s21 = sqrt(d2-(r2-
r1)^2)/d2; % <-- If d2<(r2-
r1)^2, no solution is possible
u1 = [-a21*r21-b21*s21,-
b21*r21+a21*s21]; % Left unit
vector
u2 = [-a21*r21+b21*s21,-
b21*r21-a21*s21]; % Right unit
vector
L1 = [a1,b1]+r1*u1; L2 =
[a2,b2]+r2*u1; % Left line
tangency points
R1 = [a1,b1]+r1*u2; R2 =
[a2,b2]+r2*u2; % Right line Orientation start ~1800 − 2000
tangency points
Orientation end ~10 − 300
% tangent lines plot Distance of outer tangent = 12.0416 units
x1=[L1(1,1) L2(1,1)] Total distance = 15 units approx.
y1=[L1(1,2) L2(1,2)]
x2=[R1(1,1) R2(1,1)]
y2=[R1(1,2) R2(1,2)]
axis([-2 12 0 20])
plot(x1,y1,x2,y2)
hold off
grid on

3|Page
Spring 18 MT 461 Path Planning for Mobile Robots Midterm Solution

Question 3a

Question 3b

Selection Probability

The reproduction operator is called the selection operator. It is the first operator that is applied in
genetic algorithms, and it decides the strings that are to be selected for the next generation. The end
result of this operation is formation of ‘mating pool’ where above average strings are copied in a
probabilistic manner. The said rule can be represented as:

𝑝𝑟𝑜𝑏𝑎𝑏𝑖𝑙𝑖𝑡𝑦 𝑜𝑓 𝑠𝑒𝑙𝑒𝑐𝑡𝑖𝑜𝑛 𝑖𝑛𝑡𝑜 𝑚𝑎𝑡𝑖𝑛𝑔 𝑝𝑜𝑜𝑙 ∝ 𝑓𝑖𝑡𝑛𝑒𝑠𝑠 𝑜𝑓 𝑠𝑡𝑟𝑖𝑛𝑔

The probability of selection of ith string into mating pool is:
𝐹𝑖
𝑝𝑖 = 𝑛
∑𝑗=1 𝐹𝑗

Where 𝐹𝑖 is the fitness of the ith string, 𝐹𝑗 is the fitness of jth string, and n is the population size.

It is obvious that the string with the maximum fitness will have the most number of copies in the
mating pool.

Mutation Probability

Aim of mutation is to change the population members directly, by flipping randomly selected bits in
certain strings, to promote local searches when the optimum is nearby. Mutation is performed by
deciding mutation probability 𝑝𝑚 and selecting strings, on which mutation is to be performed at
random. Approximate number of mutations can be calculated as:

𝑎𝑝𝑝𝑟𝑜𝑥. 𝑛𝑜. 𝑜𝑓 𝑚𝑢𝑡𝑎𝑡𝑖𝑜𝑛𝑠 = 𝑛/𝑝𝑚

Question 3c

for accuracy of 2 decimal places for accuracy of 3 decimal places

4−0 4−0
= 0.01 = 0.001
2𝑛 − 1 2𝑛 − 1

𝑛 = 8.22 → 9 𝑏𝑖𝑡𝑠 𝑛 = 11.96 → 12 𝑏𝑖𝑡𝑠

4|Page
Spring 18 MT 461 Path Planning for Mobile Robots Midterm Solution

Question 4a
clear 1.0000 0.0100
2.0000 0.4800
clc 3.0000 0.5700
4.0000 0.8000
n=22; 5.0000 0.6100
6.0000 0.6800
s(1)=1; 7.0000 0.9700
rho=100; 8.0000 0.6000
g=47; 9.0000 0.2100
10.0000 0.8800
c=1; 11.0000 0.3700
12.0000 0.4000
13.0000 0.8100
for k=1:n-1 14.0000 0.0800
s(k+1)= 15.0000 0.7700
mod((g*s(k)+c),rho); 16.0000 0.2000
end 17.0000 0.4100
18.0000 0.2800
19.0000 0.1700
r=s/rho 20.0000 0
histogram(r) 21.0000 0.0100
22.0000 0.4800

title('question 4a')
xlabel('bins')
ylabel('occurence')

5|Page
Spring 18 MT 461 Path Planning for Mobile Robots Midterm Solution

Question 4b

6|Page
Spring 18 MT 461 Path Planning for Mobile Robots Midterm Solution

Question 4c

Point Distance Fitness PDF CDF

A 3.1623 0.31623 0.21055 0.21055
B 1 1 0.66581 0.87636
C 5.3852 0.1857 0.12364 0.78945

Random numbers from part a, best point occurs 6 times, and remaining points occurs twice each.

%link points
x=[1 2 1 3];
y=[3 0 4 -2];
sum_fitness=0;
%calculate distance to link points and fitness
for i=1:3
d(i)=sqrt( (x(i+1)-x(1))^2 + (y(i+1)-y(1))^2 );
f(i)=1/d(i);
sum_fitness=sum_fitness+f(i);
end
%calculate PDF, CDF
pdf=f/sum_fitness;
cdf(1)=pdf(1);
for j=2:3
cdf(j)=pdf(j)+pdf(j-1);
end
a=0;
b=0;
c=0;
%check for next point using random number
for l=1:10
rndn(l)=r(l);
if r(l)<=cdf(1)
a=a+1;
else if r(l)>cdf(1) && r(l)<=cdf(2)
b=b+1;
else
c=c+1;
end
end
end
rndn'
result=[a b c]
d=d';
f=f';
pdf=pdf';
cdf=cdf';

T=table(d,f,pdf,cdf)

7|Page