Air University

Department of Mechatronics Engineering

MT460 Path Planning in Robotics BE MTS VII

SURFACES AND CONTOURS

[X,Y] = meshgrid(-8:.5:8);
R = sqrt(X.^2 + Y.^2);
Z = sin (R)./R;
mesh(X,Y,Z,'EdgeColor','black')

surf(X,Y,Z)
colormap hsv
colorbar

0.8
1

0.6
0.5

0.4

0.2

-0.5
10
0
5 10
0 5
0
-5 -5 -0.2
-10 -10

A mobile robot is moving on a 2D trajectory, in x-y space, on the curve

(𝑥 − 2)2 (𝑦 + 3)2
− =1
4 9
a) Sketch the trajectory (Marks 4)
 Air University
Department of Mechatronics Engineering

Part (a)
x = 4:.02:8;
y1 = sqrt(9*( 0.25*(x - 2).^2 - 1 )) - 3.0;
y2 = -sqrt(9*( 0.25*(x - 2).^2 - 1 )) - 3.0;

plot (x,y1)
hold on
plot (x,y2)
grid on
xlabel('x')
ylabel('y')

-2
y

-4

-6

-8

-10

-12
4 4.5 5 5.5 6 6.5 7 7.5 8
x
Figure 1. Trajectory of the mobile robot

Part (b)
The Hamiltonian is
(𝑥 − 2)2 (𝑦 + 3)2
𝐻(𝑥, 𝑦, 𝜆) = (𝑥 − 3)2 + (𝑦 − 2)2 + 𝜆 [ − − 1]
4 9
put ∇𝐻 = 0, for all three variables 𝑥, 𝑦, 𝜆 and solve for 𝑥, 𝑦. The required minimum distance is then
𝑑 = √(𝑥 − 3)2 + (𝑦 − 2)2

Part (c)
 Air University
Department of Mechatronics Engineering

(𝑥 − 2)2 (𝑦 + 3)2
𝐻(𝑥, 𝑦, 𝜆) = (𝑦 − √3𝑥) + 𝜆 [ − − 1]
4 9
Solution is 𝑥 = 6, 𝑦 = 3√3 − 3 which gives the maximum value of 𝜑(𝑥, 𝑦) ≡ 𝑦 − √3𝑥 = −8.1961. The
contours are shown below (Matlab program in Annex A)

8 0
4

6 -4
0

4 -4
0

2 -4 -8

0 -8
y

-4

-2 -8 -1 2

-4 -8 -1 2

-6
-1 2 -1 6

-8
2 2.5 3 3.5 4 4.5 5 5.5 6
x

Figure 2. Contours of the cost function 𝜑(𝑥, 𝑦) ≡ 𝑦 − √3𝑥 and the trajectory of the mobile robot.

Matlab Program for Fig. 2

clear all
fid=fopen('outEx221','w')
tic

[X,Y] = meshgrid(2:.2:6,-8:.2:8);
Z = Y - sqrt(3) *X ;
[C,h] = contour(X,Y,Z);
set(h,'ShowText','on','TextStep',get(h,'LevelStep')*2)
colormap cool
 Air University
Department of Mechatronics Engineering

hold on

xx= 0:0.01:8
yy=sqrt(3) *xx- 8.1961;
plot (xx,yy)
hold on

x = 4:.02:8;
y1 = sqrt(9*( 0.25*(x - 2).^2 - 1 )) - 3.0;
y2 = -sqrt(9*( 0.25*(x - 2).^2 - 1 )) - 3.0;

plot (x,y1)
hold on
plot (x,y2)
grid on
xlabel('x')
ylabel('y')
toc
fclose(fid)