Air University

Department of Mechatronics Engineering

MT460 Path Planning in Robotics BE MTS VIII

Mid-Term Examination 8th July 2014
Time Allowed: Two Hours Max Marks: 100

SOLUTION

Question 1. [Marks: 20]

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)
b) Write the Hamiltonian to find the minimum distance of approach from the base station located
at the point P(3,2); describe how you would solve this equation. (A solution is not needed)
(Marks 6)
c) Now write an expression to extremize (minimize or maximize) the cost function 𝜑(𝑥, 𝑦) = 𝑦 −
√3𝑥 for the trajectory above taken as a constraint condition. Sketch the cost function and the
constraint on the same figure and describe the extremum condition. (Marks 6)
d) Describe the significance of the Lagrange function 𝜆 with relevance to part (c). (Marks 4)

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')
 Air University
Department of Mechatronics Engineering

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

(𝑥 − 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)
 Air University
Department of Mechatronics Engineering

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.

Part (d)

The Lagrange multiplier gives the change in the optimal value per change in the constraint condition.

Question 2 [Marks:20]
For a simple linear dynamic system given by 𝑥𝑘+1 = 𝑎𝑥𝑘 + 𝑏𝑢𝑘 , 𝑎 = 0.85, 𝑏 = 0.20 and a performance
index
1
𝐽𝑜 = 2 ∑𝑁−1 2
𝑘=0 𝑢𝑘 , it is desired to move a robot from 𝑥0 = 3 to 𝑥𝑜 = 10 in 𝑁 = 5 steps
(1 − 𝑎2 )
𝑢𝑘∗ = (𝑥 − 𝑎𝑁 𝑥𝑜 )𝑎𝑁−𝑘−1
𝑏(1 − 𝑎2𝑁 ) 𝑁
𝑘−1
1 − 𝑎2
𝑥𝑘∗ 𝑘
= 𝑎 𝑥𝑜 + (𝑥 − 𝑎𝑁 𝑥𝑜 ) ∑ 𝑎𝑘−𝑖−1 𝑎𝑁−𝑖−1
1 − 𝑎2𝑁 𝑁
𝑖=0

(a) Write the Hamiltonian (Marks: 2)

The Hamiltonian 𝐻 is written as the performance indicator (or cost function) plus the Lagrange multiplier 𝜆
multiplied by the constraint.
 Air University
Department of Mechatronics Engineering

Thus
𝐻(𝑥𝑘 , 𝑢𝑘 , 𝜆) = 𝐽𝑜 + 𝜆[𝑥𝑘+1 − 𝑎𝑥𝑘 − 𝑏𝑢𝑘 ]

(b) Guess a trajectory with controls [1,5,10,20,1], with the last control appropriately corrected,
from the beginning point to the end point, and calculate the performance index. (Marks:6)
As a guess, enter: u=[1 5 10 20 1]

, Since x(1)=5, find the trajectory:

for i=1:5
x(i+1)=0.85*x(i)+0.2*u(i);
end

This gives: x = 3.0000 2.7500 3.3375 4.8369 8.1113 10.0000

The ‘final’ control is obtained using the end-point: u(5)=(10-0.85*8.1113)/0.85
Thus, the control vector becomes: u = 1 5 10 20 15.5268
For the above, the Performance Index is: Jo = 64.91

(c) Now use the optimal control 𝒖∗𝒌 and optimal trajectory 𝒙∗𝒌 expressions given above to
calculate the optimal case. For this, calculate the optimal value of the performance index.
(Marks: 6)
The expressions given for 𝑢𝑘∗ and 𝑥𝑘∗ , give:
k 𝒖∗𝒌 𝒙∗𝒌
0 7.8179 3
1 9.1975 4.1136
2 10.8206 5.3360
3 12.7301 6.6997
4 14.9766 8.2408
5 - 10
7.8179 9.1975 10.8206 12.7301 14.9766
1
For the above “optimal” values, the performance index is: 𝐽𝑜∗ = ∑𝑁−1 ∗2
𝑘=0 𝑢𝑘 = 64.91
2
(d) Plot both trajectories (yours and optimal) on the same figure. (Marks:4)

xguess = 3.0000 2.7500 3.3375 4.8369 8.1113 10.0000

xopt = 3.0000 4.1136 5.3360 6.6997 8.2408 10.0000

 Air University
Department of Mechatronics Engineering

10

2
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5

Figure 3: Optimal (solid line) and guess trajectories (dashed). (Matlab program is given in Annex B).

(e) Comment on the difference between your guessed trajectory and the optimal
trajectory.(Marks: 2)
As seen in the Fig. above, the optimal goes gradually to the target position while the guessed control had an
overshoot. Generally, a guessed solution will have an associated PI value larger than that for the corresponding
optimal control strategy. In this case, the PI for the guessed solution was 76.7081 compared with the optimal value
of 64.9151. Theory says there is no better value for the PI than the optimum value.

Question 3 [Marks:10, each part has 02 marks]

a) What is the difference between a continuous and a discrete system?

b) How is a continuous model discretized?
c) Write down Newton’s 2nd law of motion for a robot in state space form.
d) What is a “Dubin’s car model” of a mobile robot?
e) What does the term LQR stand for? Describe briefly.
 Air University
Department of Mechatronics Engineering

Annex A
Matlab Program for Fig. 2

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

% [X,Y] = meshgrid(-6:.2:6,-20:.2:20);
% Z = 0.25*X.^2 - X + X.*Y - (1/9)*Y.^2 - (6/9)*Y ;
% [C,h] = contour(X,Y,Z);
% set(h,'ShowText','on','TextStep',get(h,'LevelStep')*2)
% colormap cool

[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
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)
 Air University
Department of Mechatronics Engineering

Annex B
Matlab Program for Fig. 3
clear all

% COURSE: PATH PLANNING IN ROBOTICS

% DEPT OF MECHATRONICS ENGG, AIR UNIVERSITY
% 18 OCTOBER 2013

fid=fopen('out','w');

tic

a=0.85;b=0.20;NSTEPS=5;INpos=3;FINpos=10;

uguess=[1;5;10;20;1]; % 5 controls NSTEPS-1

xguess(1)=INpos;
for k=1:1:NSTEPS
k1=k+1;
if (k==(NSTEPS))
uguess(k)=(FINpos-a*xguess(k))/b;
end
xguess(k1)=a*xguess(k)+b*uguess(k);
end
xguess

% performance index for guessed u

sumG=0.0;
for ig=1:NSTEPS
sumG=sumG+0.5*uguess(ig)*uguess(ig);
end
PIguess=sumG/NSTEPS

t1=(1-a^2)/(b*(1-a^(2*NSTEPS)));

pi1=0;
for k=0:1:NSTEPS-1
kk=k+1;
uopt(kk)=t1*( FINpos- (a)^NSTEPS *INpos )*a^(NSTEPS-1-k);
pi1=pi1+0.5*uopt(kk)*uopt(kk); % Performance index
end
uopt
PIndex=pi1/NSTEPS;
PIndex

pp1=(1-a^2)/(1-a^(2*NSTEPS));
for k=1:NSTEPS

t(k)=k;
sum=0;
for i = 0:(k-1)
sum = sum + (a^(k-i-1)) * ( a^(NSTEPS-i-1));
end
 Air University
Department of Mechatronics Engineering

sum;
xopt(k) = (a^k)*INpos + pp1*(FINpos-INpos*(a^NSTEPS))*sum;
end

xoptP(1)=INpos;
tt(1)=0;
for i=1:NSTEPS
i1=i+1;
xoptP(i1)=xopt(i);
tt(i1)=t(i);
end

plot(tt,xoptP)
hold on
plot (tt,xguess,':')
grid on

toc
fclose(fid)