TP1 - Report - AC555

LAJMI Mohamed - Khánh Ðinh Công

November 2022

Contents
1 Basics on set operations, invariant set description and optimization 2
1.1 Example 1: Polyhedron and polytope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2 Example 2: Tutorial of a constrained optimization problem . . . . . . . . . . . . . . . . . . . 4
1.3 Matlab Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2 MPC stabilization for the double integrator dynamics 6

2.1 Matlab Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.1.1 Main function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.1.2 Plot script . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.2 Graphical results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3 MPC target tracking for the double integrator dynamics 15

3.0.1 Main function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
3.1 Graphical results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.2 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

4 MPC trajectory tracking for the double integrator dynamics 22

4.1 Reference tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
4.2 Main function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
4.3 Graphical results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

5 Target tracking with avoidance of one obstacle 25

5.1 Design of the system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
5.2 Using CPLEX . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
5.3 Main function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
5.4 Graphical results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
5.5 Target tracking with avoidance of many obstacles . . . . . . . . . . . . . . . . . . . . . . . . 34
5.6 Main function for many obstacles and results . . . . . . . . . . . . . . . . . . . . . . . . . . 34

6 Trajectory generation with differential flatness 36

6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
6.2 Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
6.3 Matlab script . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
6.4 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

1
 1 Basics on set operations, invariant set description and optimization
Optimization-based control in general terms refers to the control design using an optimization criterion and
the respective resolution techniques in order to obtain the parameters of the control law, the optimality
being generally equivalent to a certain desired property as for example, stability, reactivity or robustness. A
widely used optimization-based control technique in this class is Model Predictive Control (MPC) also called,
receding horizon control
In this manipulation, our objective is to solve the different type of optimization problems in different situations

1.1 Example 1: Polyhedron and polytope

- Polyhedron : A convex polyhedron is the intersection of a finite set of halfspaces of Rn
- Polytope : A convex polytope is a bounded convex polyhedron (hyperplane representation: Ax ≤ B).
1 opt = { ’ Color ’ , ’b ’ , ’ Alpha ’ , 0.4}
2
3 % % Define the polytope from the half - space r eprese ntatio n
4 F = [ eye (2) - eye (2) ] ’;
5 g = [1 1 1 1] ’;
6 Feq = ones (1 ,2) ;
7 geq = 0;
8
9 Ph = Polyhedron (F , g ) ;
10 Pheq = Polyhedron ( ’A ’ ,F , ’b ’ ,g , ’ Ae ’ ,Feq , ’ be ’ , geq ) ; % inequalities and equalities
11 plot ( Ph , opt {:})
12 title ( ’ Polytope given in H - repres entati on ’)
13 hold on
14 plot ( Pheq , opt {:})
15 hold off
16
17 % % Define the polytope from the vertex and rays repr esenta tion
18 figure
19 V = [ 0.2 -0.2; 0.2 0.4 ; -0.3 0.4; -0.3 -0.5];
20 Pv = Polyhedron ( V )
21 plot ( Pv , opt {:})
22 title ( ’ Polytope given in generator repre sentat ion ’)
23 hold on ;
24
25 R =[1 1;1 3];
26 Pvr = Polyhedron ( ’V ’ , V , ’R ’ , R ) % unbounded polyhedron with vertices and rays
27 plot ( Pvr , opt {:})
28
29 Pr = Polyhedron ( ’R ’ ,R )
30 % plot ( Pr , opt {:})
31
32 % % Construct array of polytope
33 figure
34 Ph2 = Ph + [ 5 ; 2 ]
35 Pall =[ Ph Ph2 ]
36 plot ( Pall , opt {:})
37 title ( ’ Plot union of polytopes ’)
38
39 % % Retrieve information from the Polyhedron object
40 % Get the vertices
41 vert = Pvr . V
42 % Get the constraint matrices in the H - r eprese ntatio n
43 Pvr . A
44 Pvr . b
45
46 % % Generate Polyhedron from random distribution vertices
47 Points = 2* rand (30 ,2) -1
48 Prandom = Polyhedron ( Points )
49
50 figure
51 plot ( Prandom , opt {:})
52 title ( ’ Plot random vertices polytope ’)

2
53 % % Polytope operation
54 % Create 2 polytopes for implementing differents operations
55 P1 = Polyhedron ( rand (5 ,2) -0.5) ;
56 P2 = Polyhedron ( rand (4 ,2) -0.5) -[1;1];
57
58 % Inclusion test :
59 P1 <= P2
60 % Equality test (2 - way inclusion ) :
61 P1 == P2
62 P1 <= P2
63
64 figure
65 subplot (2 ,2 ,1)
66 Ptotal =[ P1 P2 ]
67 plot ( Ptotal , opt {:})
68 axis ([ -2 2 -2 2])
69 title ( ’ Plot union of polytopes ’)
70 % Intersect :
71 subplot (2 ,2 ,2)
72 plot ( P1 . intersect ( P2 ) , opt {:})
73 axis ([ -2 2 -2 2])
74 title ( ’ Plot intersection of polytopes ’)
75 % Minkowski addition :
76 subplot (2 ,2 ,3)
77 plot ( P1 + P2 , opt {:})
78 axis ([ -2 2 -2 2])
79 title ( ’ Minkowski addition of polytopes ’)
80 % Pontryagin difference :
81 subplot (2 ,2 ,4)
82 plot ( P1 - P2 , opt {:})
83 axis ([ -2 2 -2 2])
84 title ( ’ Pontryagin difference of polytopes ’)
85
86 % Hausdorff distance :
87 dist = P1 . distance ( P2 )
88 % Plot 2 polytopes and the segment representing Hausdorff distance
89 figure
90 plot ([ P1 P2 ] , opt {:})
91 hold on
92 scatter ([ dist . x (1) dist . y (1) ] , [ dist . x (2) dist . y (2) ] , ’ filled ’ , ’r ’)
93 plot ([ dist . x (1) dist . y (1) ] , [ dist . x (2) dist . y (2) ] , ’r ’ , ’ LineWidth ’ ,2)
94 title ( ’ Hausdorff distance ’)

Figure 1: Operation on the polytopes

3
 Figure 2: Hausdorff distance of the polytopes

In this code, we want to learn how to manipulate the Multiparametric Toolbox mpt3 in order to create the
polytopes and use some geometrical operations on this kind of sets. As shown in the figure above, we had
created 2 randomly polytopes and showing on the plot the segment representing the Hausdorff distance of
the 2 polyhedron.

1.2 Example 2: Tutorial of a constrained optimization problem

Matlab Code :

x(k + 1) = Ax(k) + Bu(k)
y(k) = Cx(k)
We will compute the control input :
NX
p−1
⊺ ⊺ ⊺
U ∗ = arg min y(k) Qy y(k) + u(k) Ru(k) + y(N p) Py y(N p)
U
k=0

1.3 Matlab Code

1 clear all
2 clc
3 close all
4
5 A = [1 1;0 1]
6 B = [1; 0.3]
7 C = [1 0]
8 D = 0
9
10 % % Constraints
11 umin = -1 * 0.25;
12 umax = +1 * 0.25;
13 delta_umin = -1 * 0.1;
14 delta_umax = +1 * 0.1;
15 ymin = -10 ,
16 ymax = +10 ,
17

4
18 % Dimension of system
19 [ dx , du ] = size ( B )
20 dy = size (C ,1)
21
22 % Weighting parameters
23 Q = eye ( dx ) * 5
24 R = eye ( du ) * 5
25 Qy = eye ( dy )
26 P = eye ( dx ) * 5
27
28
29 % NBR of simulations and predictions
30 Npred = 2
31 Npred1 = [5 2]
32 Nsim = 100
33 coef = [1 5];
34 figure ( ’ Name ’ ," Motion y ( x ) " , ’ NumberTitle ’ , ’ off ’)
35 nbr_it = [0 0];
36 flag = [0 0];
37 for j = 1:2
38
39 % % Define the variables , constraints and cost : Npred + 1 predictions
40 % Define variables : states x = [ x (1) ’, x (2) ’, ... , x ( Npred +1) ]
41 % and inputs U = [ u (0) ’, u (1) ’ ,... , u ( Npred ) ’] ’
42 u = sdpvar ( repmat ( du ,1 , Npred * coef ( j ) ) , ones (1 , Npred * coef ( j ) ) ) ;
43 x = sdpvar ( repmat ( dx ,1 , Npred * coef ( j ) +1) , ones (1 , Npred * coef ( j ) +1) ) ;
44
45 % Define initial inputs for every instant t : u (0)
46 u_init = sdpvar ( du ,1)
47 % Define state variable u
48 utmp = sdpvar ( du ,1)
49
50 % Initialize constraints and objective
51 constraints = []
52 objective = 0;
53
54 % % Write the constraints and objectives over the prediction horizon
55 for k = 1: Npred * coef ( j )
56 if ( k == 1)
57 utmp = u_init ;
58 else
59 utmp = u {k -1};
60 end
61
62 % constraints = [ constraints ,...
63 % x { k +1}== A * x { k } + B * u { k }];
64 constraints = [ constraints ,...
65 x { k +1}== A * x { k } + B * u { k } ,...
66 umin <= u { k } <= umax ,...
67 ymin <= C * x { k } + D * u { k } <= ymax ,...
68 delta_umin <= u { k } - utmp <= delta_umax ];
69
70 objective = objective + x { k } ’ * Q * x{k} + u{k}’ * R * u { k };
71
72 end
73
74 objective = objective + x { k } ’ * P * x{k} ;
75
76 % % Options
77 options = [];
78
79 % % Parameters and outputs :
80 parameters = { x {1} , u_init };
81 outputs = { u {1}};
82
83 % % Design MPC controller :
84 % Controller = optimizer ( constraints , objective , options , parameters , outputs )
85

5
86 Controller = optimizer ( constraints , objective , options , parameters , outputs )
87
88
89 % % Simultaion over Nsim steps :
90 usim = zeros ( du , Nsim ) ;
91 xsim = zeros ( dx , Nsim + 1) ;
92 ysim = zeros ( dy , Nsim ) ;
93
94 % Initial condition : x (0) or x (1) in matlab
95 xsim (: ,1) = ones ( dx ,1) * 1;
96 usim_init (: ,1) = zeros ( du ,1) ;
97
98
99 final_error = 1e -2;
100
101 tic
102 % Simulation loop
103 for i = 1: Nsim
104 u = Controller {{ xsim (: , i ) , usim_init }};
105 usim_init = u ;
106 xsim (: , i +1) = A * xsim (: , i ) + B * u ;
107 usim (: , i ) = u ;
108 ysim (: , i ) = C * xsim (: , i ) + D * usim (: , i ) ;
109 if ( flag ( j ) ==0) && all (( abs ( xsim (: , i +1) ) < final_error ) )
110 flag ( j ) = 1;
111 nbr_it ( j ) = i ;
112 end
113 end
114
115 toc
116
117 scatter ( xsim (1 ,:) , xsim (2 ,:) )
118
119 hold on
120 % title ( ’ Number of iteration : ’, num2str ( nbr_it ) )
121 end
122 legend ( strcat ( ’ Npred = ’ , num2str ( Npred * coef (1) ) , char (9) , ’ Number of iteration : ’ ,
num2str ( nbr_it (1) ) ) , strcat ( ’ Npred = ’ , num2str ( Npred * coef (2) ) , char (9) , ’ Number of
iteration : ’ , num2str ( nbr_it (2) ) ) )
123 figure ( ’ Name ’ ," Output ysim " , ’ NumberTitle ’ , ’ off ’)
124 stem ( ysim )
125
126 figure ( ’ Name ’ ," Input ysim " , ’ NumberTitle ’ , ’ off ’)
127 stem ( usim )

2 MPC stabilization for the double integrator dynamics

In this situation the model is described by :

x(k + 1) = Ax(k) + Bu(k)
y(k) = Cx(k)

We will compute the control input :

NX
p−1
⊺ ⊺ ⊺
U ∗ = arg min y(k) Qy y(k) + u(k) Ru(k) + y(N p) Py y(N p)
U
k=0

2.1 Matlab Code

2.1.1 Main function

1 clear all
2 clc

6
 3 close all
4
5 A = [1 0 0.5 0;0 1 0 0.5; 0 0 1 0; 0 0 0 1]
6 B = [0 0; 0 0; 0.5 0; 0 0.5];
7 C = [1 0 0 0; 0 1 0 0];
8 D = [0 0; 0 0];
9
10 % % Constraints
11 umin = -10 * [1; 1];
12 umax = +10 * [1; 1];
13 % delta_umin = -1 * 0.1;
14 % delta_umax = +1 * 0.1;
15
16
17 % Dimension of system
18 [ dx , du ] = size ( B )
19 dy = size (C ,1)
20
21 % Weighting parameters
22 Q = eye ( dx ) * 5
23 R = eye ( du ) * 5
24 Qy = eye ( dy ) * 5
25
26 P = eye ( dx ) * 5
27 Py = eye ( dy ) * 5
28 Q1 = C ’* Qy * C ;
29 P1 = C ’* Py * C ;
30 % NBR of simulations and predictions
31 Npred = 5
32 Nsim = 100
33
34 % % Define the variables , constraints and cost : Npred + 1 predictions
35 % Define variables : states x = [ x (1) ’, x (2) ’, ... , x ( Npred +1) ]
36 % and inputs U = [ u (0) ’, u (1) ’ ,... , u ( Npred ) ’] ’
37 u = sdpvar ( repmat ( du ,1 , Npred ) , ones (1 , Npred ) ) ;
38 x = sdpvar ( repmat ( dx ,1 , Npred +1) , ones (1 , Npred +1) ) ;
39
40 % Define initial inputs for every instant t : u (0)
41 u_init = sdpvar ( du ,1)
42 % Define state variable u
43 utmp = sdpvar ( du ,1)
44
45 % Initialize constraints and objective
46 constraints = []
47 objective = 0;
48
49 % % Write the constraints and objectives over the prediction horizon
50 for k = 1: Npred
51 if ( k == 1)
52 utmp = u_init ;
53 else
54 utmp = u {k -1};
55 end
56
57 % constraints = [ constraints ,...
58 % x { k +1}== A * x { k } + B * u { k }];
59 constraints = [ constraints ,...
60 x { k +1}== A * x { k } + B * u { k } ,...
61 umin <= u { k } <= umax ];
62
63 % constraints = [ constraints ,...
64 % x { k +1}== A * x { k } + B * u { k } ,...
65 % umin <= u { k } <= umax ,...
66 % ymin <= C * x { k } + D * u { k } <= ymax ,...
67 % delta_umin <= u { k } - utmp <= delta_umax ];
68
69 objective = objective + x { k } ’ * Q1 * x { k } + u { k } ’ * R * u { k };
70

7
71 end
72
73 objective = objective + x { k } ’ * P1 * x { k } ;
74
75 % % Options
76 options = [];
77
78 % % Parameters and outputs :
79 parameters = { x {1} , u_init };
80 outputs = { u {1}};
81
82 % % Design MPC controller :
83 % Controller = optimizer ( constraints , objective , options , parameters , outputs )
84
85 Controller = optimizer ( constraints , objective , options , parameters , outputs )
86
87 % % Simultaion over Nsim steps :
88 usim = zeros ( du , Nsim ) ;
89 xsim = zeros ( dx , Nsim + 1) ;
90 ysim = zeros ( dy , Nsim ) ;
91
92 % Initial condition : x (0) or x (1) in matlab
93 xsim (: ,1) = [20 -20 0 -10] ’;
94 usim_init (: ,1) = zeros ( du ,1) ;
95
96 nbr_it = 0;
97 flag = 0;
98 final_error = 1e -1;
99 tic
100 % Simulation loop
101 for i = 1: Nsim
102 u = Controller {{ xsim (: , i ) , usim_init }};
103 usim_init = u ;
104 xsim (: , i +1) = A * xsim (: , i ) + B * u ;
105 usim (: , i ) = u ;
106 ysim (: , i ) = C * xsim (: , i ) + D * usim (: , i ) ;
107 if ( flag ==0) && all (( abs ( xsim (: , i +1) ) < final_error ) )
108 flag = 1;
109 nbr_it = i ;
110 end
111 end
112
113 toc
114
115 figure ( ’ Name ’ ," Output ysim " , ’ NumberTitle ’ , ’ off ’)
116 hold on
117 % stem ( ysim )
118 subplot (2 ,1 ,1)
119 stem ( ysim (1 ,:) )
120 title ( " Position x " )
121 subplot (2 ,1 ,2)
122 stem ( ysim (2 ,:) )
123 title ( " Position y " )
124
125 figure ( ’ Name ’ ," Motion y ( x ) " , ’ NumberTitle ’ , ’ off ’)
126 hold on
127 scatter ( xsim (1 ,:) , xsim (2 ,:) )
128
129
130 % Velocity
131 figure ( ’ Name ’ ," Velocity " , ’ NumberTitle ’ , ’ off ’)
132 hold on
133 subplot (2 ,1 ,1)
134 stem ( xsim (1 ,:) )
135 title ( " Velocity vx " )
136 subplot (2 ,1 ,2)
137 stem ( xsim (2 ,:) )
138 title ( " Velocity vy " )

8
139
140 figure ( ’ Name ’ ," Input usim " , ’ NumberTitle ’ , ’ off ’)
141 hold on
142 % stem ( usim )
143 subplot (2 ,1 ,1)
144 stem ( usim (1 ,:) )
145 title ( " Input ux " )
146 subplot (2 ,1 ,2)
147 stem ( usim (2 ,:) )
148 title ( " Input ux " )
149
150 % data1 = [ usim ; xsim ; ysim ; nbr_it ; Npred ; Q ; R ; P ];
151 save data7 usim xsim ysim nbr_it Npred Qy R Py
152 % save data1 data1

2.1.2 Plot script

As we can see from the code above, we want to save the data into .mat files so that we can make the plot for
comparing the different curves. Here is the script used for plotting the data
1 clear all
2 clc
3 close all
4
5 %% Tuning parameters
6 nQ = [0 0];
7 nR = [0 0];
8 nP = [0 0];
9 nN = [0 0];
10
11 % % Load data to workspace and plot on the same figures
12 load data1
13 nQ (1) = Qy (1 ,1) ;
14 nR (1) = R (1 ,1) ;
15 nP (1) = Py (1 ,1) ;
16 nN (1) = Npred ;
17 figure (1)
18
19 % stem ( ysim )
20 subplot (2 ,1 ,1)
21 hold on
22 stem ( ysim (1 ,:) )
23 title ( " Position x " )
24 subplot (2 ,1 ,2)
25 hold on
26 stem ( ysim (2 ,:) )
27 title ( " Position y " )
28
29 figure (2)
30 scatter ( xsim (1 ,:) , xsim (2 ,:) )
31 hold on
32
33 % Velocity
34 figure (3)
35
36 subplot (2 ,1 ,1)
37 hold on
38 stem ( xsim (1 ,:) )
39 title ( " Velocity vx " )
40 subplot (2 ,1 ,2)
41 hold on
42 stem ( xsim (2 ,:) )
43 title ( " Velocity vy " )
44
45 figure (4)
46
47 % stem ( usim )

9
48 subplot (2 ,1 ,1)
49 hold on
50 stem ( usim (1 ,:) )
51 title ( " Input ux " )
52 subplot (2 ,1 ,2)
53 hold on
54 stem ( usim (2 ,:) )
55 title ( " Input ux " )
56 hold on
57
58
59 %%
60 load data7
61 nQ (2) = Qy (1 ,1) ;
62 nR (2) = R (1 ,1) ;
63 nP (2) = Py (1 ,1) ;
64 nN (2) = Npred ;
65 text1 = strcat ( " Q = " , num2str ( nQ (1) ) , " R = " , num2str ( nR (1) ) , " P = " , num2str ( nP (1) ) , "
Npred = " , num2str ( nN (1) ) )
66 text2 = strcat ( " Q = " , num2str ( nQ (2) ) , " R = " , num2str ( nR (2) ) , " P = " , num2str ( nP (2) ) , "
Npred = " , num2str ( nN (2) ) )
67
68 figure (1)
69
70 % stem ( ysim )
71 subplot (2 ,1 ,1)
72 hold on
73 stem ( ysim (1 ,:) )
74 title ( " Position x " )
75 legend ( text1 , text2 )
76
77 subplot (2 ,1 ,2)
78 hold on
79 stem ( ysim (2 ,:) )
80 title ( " Position y " )
81
82
83 legend ( text1 , text2 )
84
85
86 figure (2)
87 scatter ( xsim (1 ,:) , xsim (2 ,:) )
88 title ( " Trajectory y ( x ) " )
89 legend ( text1 , text2 )
90
91 % Velocity
92 figure (3)
93
94 subplot (2 ,1 ,1)
95 hold on
96 stem ( xsim (1 ,:) )
97 title ( " Velocity vx " )
98 legend ( text1 , text2 )
99
100 subplot (2 ,1 ,2)
101 hold on
102 stem ( xsim (2 ,:) )
103 title ( " Velocity vy " )
104
105 legend ( text1 , text2 )
106
107 figure (4)
108
109 % stem ( usim )
110 subplot (2 ,1 ,1)
111 hold on
112 stem ( usim (1 ,:) )
113 title ( " Input ux " )

10
114 legend ( text1 , text2 )
115
116 subplot (2 ,1 ,2)
117 hold on
118 stem ( usim (2 ,:) )
119 title ( " Input ux " )
120 legend ( text1 , text2 )

2.2 Graphical results

To observe the behaviour resulting from this optimisation, we repeated the simulation for different parameter
(Q, R, P and Npred) values as shown in the following table :

Q R P Npred Computing time number of simulation

5 5 5 5 0.487 42
100 5 5 5 0.498 45
5 100 5 5 0.4646 43
5 5 100 5 0.4781 36
5 5 5 100 1.4146 35
5 5 5 2 0.4664 75
100 100 100 2 0.449 75
100 100 100 10 0.5633 35

The purpose of varying the parameters is to know on the one hand the time and number of simulations
needed to reach the end point trajectory and on the other hand which parameters influence more on the
results
As results we obtained the following paces :
- For Q = R = P = Npred =5 :

11
Figure 3: Input/ output and motion path of the object

12
- For Q = R = P = 5 and Npred = 100:

Figure 4: Motion path of the object

As results we obtained the following paces :

- For P=5 and P = 50:

Figure 5: Motion of the outputs

As we can see from the motion in 2 cases, when we increase the final cost, the number of steps required to
reach the zero point become smaller. This means that the terminal cost helps reach the target point faster
and the object will stay at this point with more stability.

13
- For Npred =2 and Npred = 10:

Figure 6: Motion of the outputs

It is seen from the trajectories when we compute the input with longer prediction horizon, we could have a
faster response to reach the target point. However, the computational time will drastically rise and depass 1
second. This cause a dilemma in reality when we want to control a real robot
- For Q =5 and Q = 50

Figure 7: Motion of the outputs

14
In this example, we want to consider the influence of the energy cost Q. This parameter helps minimize the
energy better, but cause the drop in the performance of the system as can be seen in the figure the difference
between 2 cases.
- For R =5 and R = 50

Figure 8: Motion of the outputs

We have a better performance with the performance cost R increasing.

3 MPC target tracking for the double integrator dynamics

In this case, we have a double integrator dynamics in 4D. We have two vectors representing the input and
state of the model respectively as following:
 
  x
ux  y 
u(k) = , x(k) =  
uy  vx 
vy
with the coordinates vx and vy are the velocities of the agent
The model is described by :

x(k + 1) = Ax(k) + Bu(k)
y(k) = Cx(k) + Du(k)
   
1 0 0.5 0 0 0    
 0 1 0 0.5   0 0  1 0 0 0 0 0
With : A =  0 0 1
 B =   C = D=
0   0.5 0  0 1 0 0 0 0
0 0 0 1 0 0.5
We will minimize the following function :
Np
X ⊺ ⊺ ⊺
U ∗ = arg min (y(k) − ytg ) Qy (y(k) − ytg ) + u(k) Ru(k) + (y(N p + 1) − ytg ) Py (y(Np + 1) − ytg )
U
k=1

15
 Such that constraints:

     
−10 ux 10
≤ ≤
−10 uy 10

3.0.1 Main function

1 clear all
2 clc
3 close all
4
5 A = [1 0 0.5 0;0 1 0 0.5; 0 0 1 0; 0 0 0 1]
6 B = [0 0; 0 0; 0.5 0; 0 0.5];
7 C = [1 0 0 0; 0 1 0 0];
8 D = [0 0; 0 0];
9
10 % % Constraints
11 umin = -10 * [1; 1];
12 umax = +10 * [1; 1];
13 % delta_umin = -1 * 0.1;
14 % delta_umax = +1 * 0.1;
15
16
17 % Dimension of system
18 [ dx , du ] = size ( B )
19 dy = size (C ,1)
20
21 % Weighting parameters
22 Q = eye ( dx ) * 5
23 R = eye ( du ) * 5
24 Qy = eye ( dy ) * 5
25
26 P = eye ( dx ) * 5
27 Py = eye ( dy ) * 5
28 Q1 = C ’* Qy * C ;
29 P1 = C ’* Py * C ;
30 % NBR of simulations and predictions
31 Npred = 5
32 Nsim = 100
33 ytarget = [ linspace (0 ,100 , Nsim ) ; - linspace (0 ,100 , Nsim ) ];
34
35 % % Define the variables , constraints and cost : Npred + 1 predictions
36 % Define variables : states x = [ x (1) ’, x (2) ’, ... , x ( Npred +1) ]
37 % and inputs U = [ u (0) ’, u (1) ’ ,... , u ( Npred ) ’] ’
38 u = sdpvar ( repmat ( du ,1 , Npred ) , ones (1 , Npred ) ) ;
39 x = sdpvar ( repmat ( dx ,1 , Npred +1) , ones (1 , Npred +1) ) ;
40
41 % Define initial inputs for every instant t : u (0)
42 u_init = sdpvar ( du ,1)
43 % Define state variable u
44 utmp = sdpvar ( du ,1)
45
46 % Initialize constraints and objective
47 constraints = []
48 objective = 0;
49
50 % % Write the constraints and objectives over the prediction horizon
51 for k = 1: Npred
52 if ( k == 1)
53 utmp = u_init ;
54 else
55 utmp = u {k -1};
56 end
57

16
58 % constraints = [ constraints ,...
59 % x { k +1}== A * x { k } + B * u { k }];
60 constraints = [ constraints ,...
61 x { k +1}== A * x { k } + B * u { k } ,...
62 umin <= u { k } <= umax ];
63
64 % constraints = [ constraints ,...
65 % x { k +1}== A * x { k } + B * u { k } ,...
66 % umin <= u { k } <= umax ,...
67 % ymin <= C * x { k } + D * u { k } <= ymax ,...
68 % delta_umin <= u { k } - utmp <= delta_umax ];
69
70 objective = objective + ( C * x { k } - ytarget (: , k ) ) ’ * Qy * ( C * x { k } - ytarget (: , k ) ) + u {
k } ’ * R * u { k };
71
72 end
73
74 objective = objective + ( C * x { k } - ytarget (: , k ) ) ’ * Py * ( C * x { k } - ytarget (: , k ) ) ;
75
76 % % Options
77 options = [];
78
79 % % Parameters and outputs :
80 parameters = { x {1} , u_init };
81 outputs = { u {1}};
82
83 % % Design MPC controller :
84 % Controller = optimizer ( constraints , objective , options , parameters , outputs )
85
86 Controller = optimizer ( constraints , objective , options , parameters , outputs )
87
88 % % Simultaion over Nsim steps :
89 usim = zeros ( du , Nsim ) ;
90 xsim = zeros ( dx , Nsim + 1) ;
91 ysim = zeros ( dy , Nsim ) ;
92
93 % Initial condition : x (0) or x (1) in matlab
94 xsim (: ,1) = [20 -20 0 -10] ’;
95 usim_init (: ,1) = zeros ( du ,1) ;
96
97 nbr_it = 0;
98 flag = 0;
99 final_error = 1e -1;
100 tic
101 % Simulation loop
102 for i = 1: Nsim
103 u = Controller {{ xsim (: , i ) , usim_init }};
104 usim_init = u ;
105 xsim (: , i +1) = A * xsim (: , i ) + B * u ;
106 usim (: , i ) = u ;
107 ysim (: , i ) = C * xsim (: , i ) + D * usim (: , i ) ;
108 if ( flag ==0) && all (( abs ( xsim (: , i +1) ) < final_error ) )
109 flag = 1;
110 nbr_it = i ;
111 end
112 end
113
114 toc
115
116 figure ( ’ Name ’ ," Output ysim " , ’ NumberTitle ’ , ’ off ’)
117 hold on
118 % stem ( ysim )
119 subplot (2 ,1 ,1)
120 stem ( ysim (1 ,:) )
121 title ( " Position x " )
122 subplot (2 ,1 ,2)
123 stem ( ysim (2 ,:) )
124 title ( " Position y " )

17
125
126 figure ( ’ Name ’ ," Motion y ( x ) " , ’ NumberTitle ’ , ’ off ’)
127 hold on
128 scatter ( xsim (1 ,:) , xsim (2 ,:) )
129
130
131 % Velocity
132 figure ( ’ Name ’ ," Velocity " , ’ NumberTitle ’ , ’ off ’)
133 hold on
134 subplot (2 ,1 ,1)
135 stem ( xsim (1 ,:) )
136 title ( " Velocity vx " )
137 subplot (2 ,1 ,2)
138 stem ( xsim (2 ,:) )
139 title ( " Velocity vy " )
140
141 figure ( ’ Name ’ ," Input usim " , ’ NumberTitle ’ , ’ off ’)
142 hold on
143 % stem ( usim )
144 subplot (2 ,1 ,1)
145 stem ( usim (1 ,:) )
146 title ( " Input ux " )
147 subplot (2 ,1 ,2)
148 stem ( usim (2 ,:) )
149 title ( " Input ux " )
150
151 % data1 = [ usim ; xsim ; ysim ; nbr_it ; Npred ; Q ; R ; P ];
152 save data1r usim xsim ysim nbr_it Npred Qy R Py
153 % save data1 data1

3.1 Graphical results

To observe the behaviour resulting from this optimisation, we repeated the simulation for different parameters
(Q, R, P and Npred) values as shown in the following table :
Q R P Npred Computing time number of simulation
5 5 5 5 0.680744 22
100 5 5 5 0.729314 13
5 100 5 5 0.690529 43
5 5 100 5 0.4781 >100
5 5 5 100 0.699632 16
5 5 5 2 1.458235 19
100 100 100 2 0.685801 91
100 100 100 10 0.757629 19
The purpose of varying the parameters is to know on the one hand the time and number of simulations
needed to reach the end point trajectory and on the other hand which parameters influence more on the
results
In this case we have taken the first row of the previous table as our reference and we have made comparisons
for the remaining values against this reference.

18
- Refrence : Q = R = P = Npred =5 :

Figure 9: Reference

As results of the comparison we obtained the following paces :

19
- For Q = R = Npred = 5 and P = 100:

Figure 10: Motion of the output and Velocity

20
- For Q = R = P = 5 and Npred = 3:

Figure 11: Motion of the output and Velocity

- When we increase the cost of the output (Q=100), the input signal will be higher.
- When we increase the cost of the input (R=100), we need more number of steps to track the target because
of the slow dynamics also we have a big number of simulations ( Nsim > 100).
- When we increase the terminal cost (P=100) we go faster to the target and the stability is better.
- When we increase Npred : To many predictions step cost delay on computing
- For Q = R = P = 5 and Npred = 3 :with small number of predictions and small values of the input, output
and the terminal cost ==> the number of simulations is 91 so we go to the final target slowly.

21
 3.2 Conclusion
Each of the diagonal elements in Q reflects how much we care about a particular error signal as we want it to
go to zero. On the other hand, each element in R reflects how much we care about performing a particular
action. In this practice, Q and R are manually choose depending on how we want the systems to perform.
The terminal cost P helps tracking error to be stable at the equilibrium point.
Otherwise, the MPC controller has the significant advantage that it can take into account the additional
constraints. However, this method can only show good performance when it can predict many steps which
would cause the computational time in a real system. So this method may not be feasible in some systems
which have fast response, but in the better situation, for example, with a faster computer, this method could
be a very efficient solution.

4 MPC trajectory tracking for the double integrator dynamics

4.1 Reference tracking
We want to minimize the following function :

Np
X ⊺ ⊺ ⊺
U ∗ = arg min ∆y(k) Qy ∆y(k) + ∆u(k) R∆u(k) + ∆y(Np + 1) Py ∆y(Np + 1)
U
k=1

with ∆y(k) = y(k) − yref (k) and ∆u(k) = u(k) − uref (k)

4.2 Main function

1 clear all
2 clc
3 close all
4
5 A = [1 0 0.5 0;0 1 0 0.5; 0 0 1 0; 0 0 0 1]
6 B = [0 0; 0 0; 0.5 0; 0 0.5];
7 C = [1 0 0 0; 0 1 0 0];
8 D = [0 0; 0 0];
9
10 % % Constraints
11 umin = -10 * [1; 1];
12 umax = +10 * [1; 1];
13 % delta_umin = -1 * 0.1;
14 % delta_umax = +1 * 0.1;
15
16
17 % Dimension of system
18 [ dx , du ] = size ( B )
19 dy = size (C ,1)
20
21 % Weighting parameters
22 Q = eye ( dx ) * 5
23 R = eye ( du ) * 5
24 Qy = eye ( dy ) * 5
25
26 P = eye ( dx ) * 5
27 Py = eye ( dy ) * 5
28 Q1 = C ’* Qy * C ;
29 P1 = C ’* Py * C ;
30 % NBR of simulations and predictions
31 Npred = 5

22
32 Nsim = 100
33 ytarget = [ linspace (0 ,100 , Nsim ) ; - linspace (0 ,100 , Nsim ) ];
34
35 % % Define the variables , constraints and cost : Npred + 1 predictions
36 % Define variables : states x = [ x (1) ’, x (2) ’, ... , x ( Npred +1) ]
37 % and inputs U = [ u (0) ’, u (1) ’ ,... , u ( Npred ) ’] ’
38 u = sdpvar ( repmat ( du ,1 , Npred ) , ones (1 , Npred ) ) ;
39 x = sdpvar ( repmat ( dx ,1 , Npred +1) , ones (1 , Npred +1) ) ;
40
41 % Define initial inputs for every instant t : u (0)
42 u_init = sdpvar ( du ,1)
43 % Define state variable u
44 utmp = sdpvar ( du ,1)
45
46 % Initialize constraints and objective
47 constraints = []
48 objective = 0;
49
50 % % Write the constraints and objectives over the prediction horizon
51 for k = 1: Npred
52 if ( k == 1)
53 utmp = u_init ;
54 else
55 utmp = u {k -1};
56 end
57
58 % constraints = [ constraints ,...
59 % x { k +1}== A * x { k } + B * u { k }];
60 constraints = [ constraints ,...
61 x { k +1}== A * x { k } + B * u { k } ,...
62 umin <= u { k } <= umax ];
63
64 % constraints = [ constraints ,...
65 % x { k +1}== A * x { k } + B * u { k } ,...
66 % umin <= u { k } <= umax ,...
67 % ymin <= C * x { k } + D * u { k } <= ymax ,...
68 % delta_umin <= u { k } - utmp <= delta_umax ];
69
70 objective = objective + ( C * x { k } - ytarget (: , k ) ) ’ * Qy * ( C * x { k } - ytarget (: , k ) ) + u {
k } ’ * R * u { k };
71
72 end
73
74 objective = objective + ( C * x { k } - ytarget (: , k ) ) ’ * Py * ( C * x { k } - ytarget (: , k ) ) ;
75
76 % % Options
77 options = [];
78
79 % % Parameters and outputs :
80 parameters = { x {1} , u_init };
81 outputs = { u {1}};
82
83 % % Design MPC controller :
84 % Controller = optimizer ( constraints , objective , options , parameters , outputs )
85
86 Controller = optimizer ( constraints , objective , options , parameters , outputs )
87
88 % % Simultaion over Nsim steps :
89 usim = zeros ( du , Nsim ) ;
90 xsim = zeros ( dx , Nsim + 1) ;
91 ysim = zeros ( dy , Nsim ) ;
92
93 % Initial condition : x (0) or x (1) in matlab
94 xsim (: ,1) = [20 -20 0 -10] ’;
95 usim_init (: ,1) = zeros ( du ,1) ;
96
97 nbr_it = 0;
98 flag = 0;

23
99 final_error = 1e -1;
100 tic
101 % Simulation loop
102 for i = 1: Nsim
103 u = Controller {{ xsim (: , i ) , usim_init }};
104 usim_init = u ;
105 xsim (: , i +1) = A * xsim (: , i ) + B * u ;
106 usim (: , i ) = u ;
107 ysim (: , i ) = C * xsim (: , i ) + D * usim (: , i ) ;
108 if ( flag ==0) && all (( abs ( xsim (: , i +1) ) < final_error ) )
109 flag = 1;
110 nbr_it = i ;
111 end
112 end
113
114 toc
115
116 figure ( ’ Name ’ ," Output ysim " , ’ NumberTitle ’ , ’ off ’)
117 hold on
118 % stem ( ysim )
119 subplot (2 ,1 ,1)
120 stem ( ysim (1 ,:) )
121 title ( " Position x " )
122 subplot (2 ,1 ,2)
123 stem ( ysim (2 ,:) )
124 title ( " Position y " )
125
126 figure ( ’ Name ’ ," Motion y ( x ) " , ’ NumberTitle ’ , ’ off ’)
127 hold on
128 scatter ( xsim (1 ,:) , xsim (2 ,:) )
129
130
131 % Velocity
132 figure ( ’ Name ’ ," Velocity " , ’ NumberTitle ’ , ’ off ’)
133 hold on
134 subplot (2 ,1 ,1)
135 stem ( xsim (1 ,:) )
136 title ( " Velocity vx " )
137 subplot (2 ,1 ,2)
138 stem ( xsim (2 ,:) )
139 title ( " Velocity vy " )
140
141 figure ( ’ Name ’ ," Input usim " , ’ NumberTitle ’ , ’ off ’)
142 hold on
143 % stem ( usim )
144 subplot (2 ,1 ,1)
145 stem ( usim (1 ,:) )
146 title ( " Input ux " )
147 subplot (2 ,1 ,2)
148 stem ( usim (2 ,:) )
149 title ( " Input ux " )
150
151 % data1 = [ usim ; xsim ; ysim ; nbr_it ; Npred ; Q ; R ; P ];
152 save data1r usim xsim ysim nbr_it Npred Qy R Py
153 % save data1 data1

24
4.3 Graphical results

Figure 12: Reference tracking using MPC controller

In this simulation, we set the initial position at the origin. The reference chosen for tracking is a straight
line going from the origin to the point [10,-10]. At the instant time k, the MPC controller will compute the
tracking error for Npred number of prediction horizon in the future between the position and the target point
at the time k+1 that we want to track. We can see that the controller can track very well the reference given.
However, the problem with the trajectory of the agent is that it move away the reference trajectory and then
return at a very long distance. This can be understood because the reference that we choose is not satisfied
the dynamics of the system. Later, we can fix this issue by generating a flat trajectory which will take into
account the conditions of the system.

5 Target tracking with avoidance of one obstacle

To avoid the obstacle, the distance between the object and the obstacle must be taken into account, because
if this distance is very small, it will be difficult to avoid the obstacle. This also depends on the speed of the
movement (so we have to take into account the predictions). Finally, our goal is to minimize the distance
between the current state and the final position while taking into account the avoidance of the obstacle. Note
that the solver will choose the acceleration and the way to arrive at the destination.

25
5.1 Design of the system
In this TP, the obstacle is defined as a polytope. To avoid this obstacle, the position of the agent has to stay
in the complement of these bounded polyhedron. This means that we have to solve an optimization problem
in a non-convex region. By using the Mixed-Integer Programming, we can turn this problem into solving an
additional integer variable α as defined by the following:

Figure 13: Design of the system

The system is represented by :

Np
X ⊺ ⊺ ⊺
U ∗ = arg min y(k) Qy y(k) + u(k) Ru(k) + y(N p + 1) Py y(N p + 1)
U
k=1

with :
 
  x
ux  y 
u(k) = , x(k) =  
uy  vx 
vy
 
x
−A ≤ −b + M α
y
where: A and b are deduced from the H-representation of the polyhedron.

5.2 Using CPLEX

The CPLEX Interactive Optimizer is an executable program that can read a problem interactively or from
files in certain standard formats, solve the problem, and deliver the solution interactively or into text files.
The program consists of the file cplex.exe on Windows platforms or cplex on UNIX platforms. The CPLEX
for MATLAB Toolbox provides functions for solving a variety of mathematical programming problems.
The toolbox functions are designed to take a model description as input and produce a solution as output.
Get the power needed to solve large, real-world optimization problems, and the speed required for today’s

26
 interactive decision optimization applications. CPLEX Optimizer has solved optimization models with
millions of constraints and variables.

5.3 Main function

1 %%
2 clear all
3 clc
4 close all
5
6 A = [1 0 0.5 0;0 1 0 0.5; 0 0 1 0; 0 0 0 1]
7 B = [0 0; 0 0; 0.5 0; 0 0.5];
8 C = [1 0 0 0; 0 1 0 0];
9 D = [0 0; 0 0];
10 opt ={ ’ Color ’ , ’b ’ , ’ Alpha ’ ,0.4};
11
12 % % Constraints
13 umin = -1 * [1; 1];
14 umax = +1 * [1; 1];
15 % delta_umin = -1 * 0.1;
16 % delta_umax = +1 * 0.1;
17
18
19 % Dimension of system
20 [ dx , du ] = size ( B )
21 dy = size (C ,1)
22
23 % Weighting parameters
24 Q = eye ( dx ) * 5
25 R = eye ( du ) * 5
26 Qy = eye ( dy ) * 5
27
28 P = eye ( dx ) * 5
29 Py = eye ( dy ) * 5
30 Q1 = C ’* Qy * C ;
31 P1 = C ’* Py * C ;
32 % NBR of simulations and predictions
33 Npred = 5
34 Nsim = 100
35
36 figure (1)
37 axis ([ -20 20 -20 20])
38 grid on
39 [ xi , yi ] = getpts ( figure (1) )
40 [ xf , yf ] = getpts ( figure (1) )
41 [ xo , yo ] = getpts ( figure (1) )
42 sob = [7 , 7];
43 Vo = [ xo + sob (1) /2 , yo + sob (2) /2; xo - sob (1) /2 , yo + sob (2) /2;...
44 xo + sob (1) /2 , yo - sob (2) /2; xo - sob (1) /2 , yo - sob (2) /2]
45 % V = [ 0.2 -0.2; 0.2 0.4 ; -0.3 0.4; -0.3 -0.5];
46 Pv = Polyhedron ( Vo )
47 size_alpha = size ( Pv .A ,1)
48 ytarget = [ xf yf ] ’;
49 % % Define the variables , constraints and cost : Npred + 1 predictions
50 % Define variables : states x = [ x (1) ’, x (2) ’, ... , x ( Npred +1) ]
51 % and inputs U = [ u (0) ’, u (1) ’ ,... , u ( Npred ) ’] ’
52 u = sdpvar ( repmat ( du ,1 , Npred ) , ones (1 , Npred ) ) ;
53 x = sdpvar ( repmat ( dx ,1 , Npred +1) , ones (1 , Npred +1) ) ;
54 alpha = binvar ( repmat ( size_alpha ,1 , Npred +1) , ones (1 , Npred +1) )
55
56 % Define initial inputs for every instant t : u (0)
57 u_init = sdpvar ( du ,1)
58 alpha_init = sdpvar ( size_alpha ,1)
59 % Define state variable u
60 utmp = sdpvar ( du ,1)
61
62 % Initialize constraints and objective

27
63 constraints = []
64 objective = 0;
65
66 % % Write the constraints and objectives over the prediction horizon
67 for k = 1: Npred
68 if ( k == 1)
69 utmp = u_init ;
70 else
71 utmp = u {k -1};
72 end
73
74 % constraints = [ constraints ,...
75 % x { k +1}== A * x { k } + B * u { k }];
76 constraints = [ constraints ,...
77 x { k +1}== A * x { k } + B * u { k } ,...
78 umin <= u { k } <= umax ,...
79 - Pv . A * x { k +1}(1:2 ,:) <= - Pv . b + 1000 * alpha { k } ,...
80 sum ( alpha { k +1}) <= size_alpha -1 ,...
81 % sum ( alpha { k +1}| alpha { k }) <= size_alpha -1 % Additional constraint to avoid the
corner of the obstacle
82 sum ( abs ( alpha { k +1} - alpha { k }) ) <= 1
83 ];
84
85 % constraints = [ constraints ,...
86 % x { k +1}== A * x { k } + B * u { k } ,...
87 % umin <= u { k } <= umax ,...
88 % ymin <= C * x { k } + D * u { k } <= ymax ,...
89 % delta_umin <= u { k } - utmp <= delta_umax ];
90
91 objective = objective + ( C * x { k } - ytarget ) ’ * Qy * ( C * x { k } - ytarget ) + u { k } ’ * R *
u { k };
92
93 end
94
95 objective = objective + ( C * x { k } - ytarget ) ’ * Py * ( C * x { k } - ytarget ) ;
96
97 % % Options
98 options = [];
99
100 % % Parameters and outputs :
101 parameters = { x {1} , u_init };
102 outputs = { u {1} , alpha {1}};
103
104 % % Design MPC controller :
105 % Controller = optimizer ( constraints , objective , options , parameters , outputs )
106
107 Controller = optimizer ( constraints , objective , options , parameters , outputs )
108
109 % % Simultaion over Nsim steps :
110 usim = zeros ( du , Nsim ) ;
111 xsim = zeros ( dx , Nsim + 1) ;
112 ysim = zeros ( dy , Nsim ) ;
113 alphasim = zeros (4 , Nsim ) ;
114 % Initial condition : x (0) or x (1) in matlab
115 xsim (: ,1) = [ xi yi 0 -2] ’;
116 usim_init (: ,1) = zeros ( du ,1) ;
117 alphasim_init (: ,1) = zeros ( size_alpha ,1) ;
118
119 nbr_it = 0;
120 flag = 0;
121 final_error = 1e -1;
122 tic
123 % Simulation loop
124 for i = 1: Nsim
125 u = Controller {{ xsim (: , i ) , usim_init }};
126 usim_init = u {1};
127 alphasim_init = u {2};
128 xsim (: , i +1) = A * xsim (: , i ) + B * u {1};

28
129 usim (: , i ) = u {1};
130 alphasim (: , i ) = u {2};
131 ysim (: , i ) = C * xsim (: , i ) + D * usim (: , i ) ;
132 if ( flag ==0) && all (( abs ( xsim (: , i +1) ) < final_error ) )
133 flag = 1;
134 nbr_it = i ;
135 end
136 end
137
138 toc
139
140 figure ( ’ Name ’ ," Output ysim " , ’ NumberTitle ’ , ’ off ’)
141 hold on
142 % stem ( ysim )
143 subplot (2 ,1 ,1)
144 stem ( ysim (1 ,:) )
145 title ( " Position x " )
146 subplot (2 ,1 ,2)
147 stem ( ysim (2 ,:) )
148 title ( " Position y " )
149
150 figure ( ’ Name ’ ," Motion y ( x ) " , ’ NumberTitle ’ , ’ off ’)
151 hold on
152 scatter ( xsim (1 ,:) , xsim (2 ,:) )
153 plot ( Pv )
154
155 % Velocity
156 figure ( ’ Name ’ ," Velocity " , ’ NumberTitle ’ , ’ off ’)
157 hold on
158 subplot (2 ,1 ,1)
159 stem ( xsim (1 ,:) )
160 title ( " Velocity vx " )
161 subplot (2 ,1 ,2)
162 stem ( xsim (2 ,:) )
163 title ( " Velocity vy " )
164
165 figure ( ’ Name ’ ," Input usim " , ’ NumberTitle ’ , ’ off ’)
166 hold on
167 % stem ( usim )
168 subplot (2 ,1 ,1)
169 stem ( usim (1 ,:) )
170 title ( " Input ux " )
171 subplot (2 ,1 ,2)
172 stem ( usim (2 ,:) )
173 title ( " Input ux " )
174
175 % data1 = [ usim ; xsim ; ysim ; nbr_it ; Npred ; Q ; R ; P ];
176 save data1o usim xsim ysim nbr_it Npred Qy R Py
177 % save data1 data1

29
5.4 Graphical results
As a first step we chose a random value for the velocity, therefore we obtained the following graph:

Figure 14: First result

We have concluded that in order to avoid cutting the corner, the speed must be minimised but the most
important thing is to add an additional constraint in order to optimise the path and avoid cutting the corner,
which is described by the following graph :

Figure 15: The objectif

30
In the first time we decreased the value of the velocity but we placed the objectacle a little far away (just to
better observe the deviation) :

Figure 16: Optimization with low value of velocity

In order to improve the collision in the corner of the obstacle as we solve the problem in discrete-time, we
have to introduce another constraint when the trajectory changes the half-space. The constraint here: as we
want our moving object to transit between the 2 half-space, the trajectory has to pass in the space between
them, this means that the binary variable alpha should represent this space. In our TP for instance, at a
moment the object change from the half-space (1,1,1,0) to (1,1,0,1) we need the alpha pass through (1,1,0,0)
prior to going to other space.

In our case, for example, the (1,1,1,0) OR (1,1,0,1) gives (1,1,1,1), so all the variable is turn ON. (1,1,1,0)
OR (1,1,0,0) =(1,1,1,0) so only 3 of them can be turn ON. We can create a condition so that the transition
state should appear in the solution by using this condition: current alpha OR next alpha gives N-1 condition
ON.

31
 Figure 17: The additional Constraint

As result we obtained the following trajectory :

Figure 18: Optimization with low value of velocity and additional Constraint

32
 Figure 19: Optimization with High value of velocity and additional Constraint

For a system with only one obstacle we managed to optimise the trajectory in order to wait for the final
destination and without cutting the corners of the obstacle, but this optimisation is valid for a small number
of obstacles and it will be more complex when the number will be increased.

33
 Figure 20: What is the solution ?

5.5 Target tracking with avoidance of many obstacles

In this part we tried to test the validity of our code and our conditions to avoid corner cutting on several
obstacles.

5.6 Main function for many obstacles and results

1 figure (1)
2 axis ([ -20 20 -20 20])
3 grid on
4 [ xi , yi ] = getpts ( figure (1) )
5 [ xf , yf ] = getpts ( figure (1) )
6 [ xo , yo ] = getpts ( figure (1) )
7 sob = [5 , 5];
8 nob = length ( xo )
9 Vo = zeros (4 ,2 , nob )
10 for i = 1: nob
11 Vo (: ,: , i ) = [ xo ( i ) + sob (1) /2 , yo ( i ) + sob (2) /2; xo ( i ) - sob (1) /2 , yo ( i ) + sob (2) /2;...
12 xo ( i ) + sob (1) /2 , yo ( i ) - sob (2) /2; xo ( i ) - sob (1) /2 , yo ( i ) - sob (2) /2]
13 end
14 % Vo = [ xo + sob (1) /2 , yo + sob (2) /2; xo - sob (1) /2 , yo + sob (2) /2;...
15 % xo + sob (1) /2 , yo - sob (2) /2; xo - sob (1) /2 , yo - sob (2) /2]
16 % V = [ 0.2 -0.2; 0.2 0.4 ; -0.3 0.4; -0.3 -0.5];
17
18 Pv = [];
19 for i = 1: nob
20 Pv = [ Pv Polyhedron ( Vo (: ,: , i ) ) ]
21 end

34
 Figure 21: Avoidance of 3 Obstacles

Figure 22: Avoidance of 4 Obstacles

With our optimization condition to avoid the cutting of corners we have, as a result, managed to avoid 4
obstacles at the same time.

35
6 Trajectory generation with differential flatness
6.1 Introduction
In this section, we will investigate the trajectory generation part in motion planning base on the properties
of differential flatness.

6.2 Implementation
Implement the trajectory generation for the model using the flat output representation with a polynomial
parameterization. We consider the model of the following system:

 ẋ(t) = Va (t) cos ψ(t) + Wx
ẏ(t) = Va (t) sin ψ(t) + Wy
g tan ϕ(t)
ψ̇(t) =

Va (t)

described generally by :
ξ˙ (t) = f (ξ (t) , u (t))
where:  ⊺  ⊺
The state ξ (t) = x (t) y (t) ξ (t) and the input u (t) = Va (t) ϕ (t)
 ⊺
Here we assume the wind velocities W (t) = Wx (t) Wy (t) are negligible.
 ⊺
We choose the flat output z(t) as the position components of the state: z (t) = z1 (t) z2 (t) =
 ⊺
x (t) y (t)
Thus we obtain :
tan−1 zz˙˙12 (t)
(t)

 ψ(t) =

 q
z˙1 2 (t) + z˙2 2 (t)

Va (t) =
  
 ϕ(t) = tan−1 g1 z¨2 (t) √z˙1 (t)− z˙2 (t)z¨1 (t)


2 2
z˙1 (t)+z˙2 (t)

Flat trajectory generation :

ξ ref (t) =

η0 (z (t) , ż (t))
uref (t) = η1 (z (t) , ż (t) , z̈ (t))
The flat output in this case is parametrized using classical polynomials such that:
 PN i
zj (t) = i=1 αji Λ (t)
i i−1
Λ (t) = t
With N is the number of boundary conditions or the unknown variables in the system. The coefficient alpha
is found from boundary condition:

36
Or we can represent the matrix of coefficients in one column:

37
 6.3 Matlab script
1 clear ;
2 close all ;
3 % initialize the system parameters
4 param . g =9.8;
5 param . h =0.05;
6 param . dblAlt =150;
7 % initialize the time stamp , the state and input
8 reference :
9 t = [] ;
10 xiref = [] ;
11 uref = [] ;
12 % define the initial and final conditions :
13 xi = [50 50 pi /4];
14 ui = [19 0];
15 xf = [200 200 - pi /4];
16 uf = [19 0];
17 T = 15;
18 % construct the matrix M \ alpha =\ bar { z } as in eq . (6) :
19 M =[ 1 0 0 0 0 0 0 0 0 0 0 0;
20 0 1 0 0 0 0 0 0 0 0 0 0;
21 0 0 2 0 0 0 0 0 0 0 0 0;
22 0 0 0 0 0 0 1 0 0 0 0 0;
23 0 0 0 0 0 0 0 1 0 0 0 0;
24 0 0 0 0 0 0 0 0 2 0 0 0;
25 1 T T ^2 T ^3 T ^4 T ^5 0 0 0 0 0 0;
26 0 1 2* T 3* T ^2 4* T ^3 5* T ^4 0 0 0 0 0 0;
27 0 0 2 6* T 12* T ^2 20* T ^3 0 0 0 0 0 0;
28 0 0 0 0 0 0 1 T T ^2 T ^3 T ^4 T ^5;
29 0 0 0 0 0 0 0 1 2* T 3* T ^2 4* T ^3 5* T ^4;
30 0 0 0 0 0 0 0 0 2 6* T 12* T ^2 20* T ^3];
31 b =[ xi (1) ui (1) * cos ( xi (3) ) - sin ( xi (3) ) * param . g * tan ( ui (2) ) ...
32 xi (2) ui (1) * sin ( xi (3) ) cos ( xi (3) ) * param . g * tan ( ui (2) ) ...
33 xf (1) uf (1) * cos ( xf (3) ) - sin ( xf (3) ) * param . g * tan ( uf (2) ) ...
34 xf (2) uf (1) * sin ( xf (3) ) cos ( xf (3) ) * param . g * tan ( uf (2) ) ] ’;
35 a = M\b;
36 % define the flat output and its derivatives
37 z1 = @ ( t ) ( a (1) + a (2) * t + a (3) * t .^2+ a (4) * t .^3+ a (5) * t .^4+ a (6) * t .^5) ;
38 z2 = @ ( t )
39 ( a (7) + a (8) * t + a (9) * t .^2+ a (10) * t .^3+ a (11) * t .^4+ a (12) * t .^5) ;
40 zd1 = @ ( t ) ( a (2) +2* a (3) * t +3* a (4) * t .^2+4* a (5) * t .^3+5* a (6) * t .^4) ;
41 zd2 = @ ( t ) ( a (8) +2* a (9) * t +3* a (10) * t .^2+4* a (11) * t .^3+5* a (12) * t .^4) ;
42 zdd1 = @ ( t ) (2* a (3) +6* a (4) * t +12* a (5) * t .^2+20* a (6) * t .^3) ;
43 zdd2 = @ ( t ) (2* a (9) +6* a (10) * t +12* a (11) * t .^2+20* a (12) * t .^3) ;
44 X = @ ( t ) ([ feval ( z1 , t ) ; feval ( z2 , t ) ;
45 atan2 ( feval ( zd2 , t ) , feval ( zd1 , t ) ) ]) ;
46 U = @ ( t ) ([ sqrt (( feval ( zd1 , t ) .^2) +( feval ( zd2 , t ) .^2) ) ;
47 atan2 (( feval ( zdd2 , t ) .* feval ( zd1 , t ) -
48 feval ( zd2 , t ) .* feval ( zdd1 , t ) ) ,...
49 ( param . g .*( sqrt (( feval ( zd1 , t ) .^2) +( feval ( zd2 , t ) .^2) ) ) ) ) ]) ;
50
51 % compute the state and input reference :
52 tt = param . h : param . h : T ;
53 xiref =[ xiref feval (X , tt ) ];
54 uref =[ uref feval (U , tt ) ];
55 figure ;
56 subplot (2 ,2 ,1)
57 plot ( tt , xiref (1 ,:) ) ;
58 title ( " Position X ( t ) " ) ;
59 xlabel ( " t " ) ;
60 ylabel ( " X ( t ) " ) ;
61 subplot (2 ,2 ,2)
62 plot ( tt , xiref (2 ,:) ) ;
63 title ( " Position Y ( t ) " ) ;
64 xlabel ( " t " ) ;
65 ylabel ( " Y ( t ) " ) ;
66 subplot (2 ,2 ,3)

38
67 plot ( xiref (1 ,:) , xiref (2 ,:) ) ;
68 title ( " Trajectory " ) ;
69 xlabel ( " X " ) ;
70 ylabel ( " Y " ) ;
71 subplot (2 ,2 ,4)
72 plot ( tt , uref (1 ,:) ) ;
73 title ( " Velocity " ) ;
74 xlabel ( " t " ) ;
75 ylabel ( " Va ( t ) " ) ;

6.4 Results

Figure 23: Plot results

In this case, we have implemented the trajectory generation using classical polynomials by change of coordinate
to the flat output space. Working with the flat output, we can reduce the dimension of output and take into
account the dynamics of the system which helps increase the performance of the controller. However the
additional constraints for the velocity will not be hold. Otherwise, from this point of view, we can combine
the flat trajectory generation in our previously implemented optimization controller to handle the others
constraints of the system.

39