2 LTV MPC

A.
8emorud Model Predlctlve Control z

Llneur 1lmevurylng Model Predlctlve Control
E
k
(t, x(t))x
k
+F
k
(t, x(t))u
k
h
k
(t, x(t)) k = 0, 1, . . . , N 1
min
N
k=0
k
(y
k
, u
k
, r(t +k), t, x(t))
k
= quadratic function of y
k
, u
k
min
U
1
2
U
H(t)U +F(t)
U +(t)
s.t. G(t)U W(t)
x
k+1
= A
k
(t, x(t))x
k
+B
k
(t, x(t))u
k
+f
k
(t, x(t))
y
k
= C
k
(t, x(t))x
k
+D
k
(t, x(t))u
k
+g
k
(t, x(t))
A. 8emorud Model Predlctlve Control z
Llneur tlmevurylng MPC
MPC cun euslly hundle !"#$%& (")$*+%&,"#- ./012 roblems
z
L1vMPC stlll leuds to u
34%5&%("6 7&8-&%) .372 !
Allcutlons: tlmevurylng systems (e.g.: uerosuce), NL systems
Lxumle: erlodlc double lntegrutor
min 10(x
2,k
r(t + k))
2
+ 0.001u
2
k
s.t. (no constraints)
!mpcsoft_doubleint demo
time-varying double-
integrator model
LTV-MPC block
A=[1 0;Ts 1]*(1+.5*sin(t+k*Ts));
B=[Ts;0];
f=[0;0];
C=[0 1];
no preview
on reference
signal
Lxumle: double lntegrutor
preview
enabled
dCA
dt
=
F
V
(CAf CA) CAk0e
E
RT
dT
dt
=
F
V
(Tf T) +
UA
CpV
(Tj T)
H
Cp
CAk0e
E
RT
T : temperature inside the reactor [K] (state)
CA : concentration of the reagent in the reactor [kgmol/m
3
] (state)
Tj : jacket temperature [K] (input)
Tf : feedstream temperature [K] (measured disturbance)
CAf : feedstream concentration [kgmol/m
3
] (measured disturbance)
Lxumle: L1vMPC of u nonllneur CS1R system
!"#$ &'()*'+ ', - ./-0-)/& &'()/(1'12 2)/**3. )-(4 *3-&)'* 5$6789
!:0;3&)/<3= >-(/?1+-)3 Tj )' 433? CA '( )@3 .32/*3. 23)A?'/()

!#*'&322 >'.3+ /2 *-)@3* ('(+/(3-*=
Model ls nonllneur und contlnuous ln tlme. ve uroxlmute lt to u
dlscretetlme L1v model by onllne llneurlzutlon:
dx
dt
= f(x(t), u(t), t)
xk+1 = (I + Ts
f
x
(x(t), u(t), t))xk + Ts
f
u
(x(t), u(t), t)uk + fk
fk = Ts
f(x(t), u(t), t)
f
x
(x(t), u(t), t)x(t)
f
u
(x(t), u(t), t)u(t)
+
f
t
(x(t), u(t), t)kTs

CS1R model
6
dx
dt
(t + |t) f(x(t), u(t), t) +
f
x
(x(t), u(t), t)(x(t + ) x(t))
+
f
u
(x(t), u(t), t)(u(t + ) u(t))
+
f
t
(x(t), u(t), t)
generic NL model
linearized model
discrete-time
LTV model
A(t) B(t) f(t)
model
matrices
depend on
current
time t
J(t + 1) = J(t) + (CA(t) r(t))
2
+ 10
4
T
2
j
(t)
Slmulutlon results
,
!$+'23.A+''? )*-;3&)'*/32= &'(&3()*-)/'( CA -(. .32/*3. 23) ?'/() r
$1>1+-)3. &'2) J
Numerlcul derlvutlves of cumuluted cost J
ure u good "udvlsor' to fne tune the L1vMPC deslgn
Wx =
0 1
, W
u
= 10
2
Wx =
10
4
2
W
u
= 0.008
J
Wx
=
4.8 12.0
,
J
W
u
= 1233
Sensltlvltles und controller retunlng
8
!$'>?1)3 (1>3*/&-+ .3*/<-)/<32 ', J(Tfinal) B/)@ *32?3&) )' B3/C@)2 '(
2)-)32 -(. /(?1) /(&*3>3()2=
Negative sensitivities suggest
to increase state weights
Adjust welghts uccordlng
to sensltlvlty
Crlglnul tunlng:
Positive sensitivity suggests
to decrease input-rate weight
Lxumle: CS1R demo Performunce tunlng
Crlglnul tunlng: J=.cc

New tunlng: J=.c
p(t) =

2
0
s
2
+ 20s +
2
0
pc(t)
Lxumle: L1vMPC for UAv nuvlgutlon
Coul: nuvlgute two lunur vehlcles umong obstucles
to u turget osltlon
1he dynumlcul model of euch vehlcles ls descrlbed by
c
p= [x,y] vehlcle osltlon
pc= communded osltlon
!D3@/&+3 .E(->/&2 &'(<3*)3. )' ./2&*3)3A)/>3
0E 3F-&) 2->?+/(C
!G/<3 2H1-*3 '02)-&+32 ?+-&3. /( B'*42?-&3
98%!: Use !"#$%& (")$*+%&,"#- ./012 :7; to generute deslred osltlons to
stublllzlng controller ln !"#$%&'(" to uvold 8<=(%6!$= und other UAvs
7&8<!$): feuslble suce ls nonconvex !
:%"# "5$%: get u convex lnner uroxlmutlon
of the feuslble suce thut ls
- )*$+,"-!#$ =llneur construlnts on
redlcted stutes ln L1vMPC roblem
- ."!+ 0'(1$" to comute onllne
- Cun hundle olytolc obstucles
of #!2'&!#!+ 0,#1" und -'("30'*3
L1vMPC for obstucle uvoldunce
5I3>?'*-.J 8'&&@/J KGL$ MNOO9

Alternutlve: nonconvex feuslble suce modeled by mlxedlnteger construlnts,
guldunce roblem solved by (tlmelnvurlunt) >,<&"5 :7;
initial position
target position
p
0
R
d
= current vehicle position
q
1
, q
2
, . . . , q
M
R
d
= obstacle positions
p
0
= q
i
, i = 1, . . . , M P =
_
_
p R
d
:
_
_
(q
1
p
0
)
.
.
.
(q
M
p
0
)
_p
_
_
(q
1
p
0
)
q
1
.
.
.
(q
M
p
0
)
q
M
_
_
_
_
lust greedy uroxlmutlon ulgorlthm (j)
z
78!,>$5&%! %??&8@")%("8#:
Assume (for the moment) thut vehlcle und obstucles ure olnts ln suce
P contulns p0 und does not contuln uny of the obstucles q1, q2, ..., qM ln lts lnterlor
q1
qM
q2
p0
5I3>?'*-.J 8'&&@/J $P$ MNOO9
lf the vertlces of WM ure uvulluble, slmly
Assume now ?8!,>$5&%! 8<=(%6!$= wlth shues W1, W2, ... WM
g
j
= min
wR
d (q
j
p
0
)
w
s.t. w W
j
g
j
= min
h=1,...,sj
A
j
c
w
jh
P =
_
_
p R
d
:
_
_
(q
1
p
0
)
.
.
.
(q
M
p
0
)
_p
_
_
(q
1
p
0
)
q
1
+ g
1
.
.
.
(q
M
p
0
)
q
M
+ g
M
_
_
_
_
lust greedy uroxlmutlon ulgorlthm (zj)
lf P ls nonemty, then P contulns p0 und does not contuln uny of the obstucles
ln lts lnterlor
q1
qM
q2
p0
W2
W1
WM
lor euch obstucle j defne the sculur g
j
B
j
= {q
j
} W
j
(wjh = vertex of Wj)
78!,>$5&%! %??&8@")%("8#:
Assume olyhedrul obstucles und ?8!,(8?"6 AB1
V
P =
_
_
p R
d
:
_
_
(q
1
p
0
)
.
.
.
(q
M
p
0
)
_p
_
_
(q
1
p
0
)
(q
1
d
i
) + g
1
.
.
.
(q
M
p
0
)
(q
M
d
i
) + g
M
_
_, i = 1, . . . , r
_
_
lust greedy uroxlmutlon ulgorlthm (j)
q1
qM
q2
p0
W2
W1
WM
V conv(p
0
+ d
1
, . . . , p
0
+ d
r
)
lf P ls nonemty, then P contulns V und does not contuln uny of the obstucles
ln lts lnterlor B
j
= {q
j
} W
j
78!,>$5&%! %??&8@")%("8#:
p
i
(t + 1) = A
i
p
i
(t) + B
i
p
ci
(t) p
i
(t) =
x
i
(t)
y
i
(t)
z
i
(t)
V(t) = {p(t)} + conv(d

1
(t), . . . , d
r
(t))
B
j
(t) = {q
j
(t)} W
j
(t)
L1vMPC guldunce ulgorlthm: redlctlon model
Llneur uroxlmutlon of stublllzed dynumlcs of UAv #i

?'2/)/'(
', <3@/&+3 Qi
-) )/>3 t
Polytolc shue of UAv #i ut tlme t
p(t)
V(t)
Polyhedrul shue of obstucle #j ut tlme t
qj(t)
Bj(t)
Note: other vehlcles ln formutlon ure treuted us olytolc obstucles (Bj = V )
p
ci
(t) =
x
ci
(t)
y
ci
(t)
z
ci
(t)
?'2/)/'(
&'>>-(.3.
-) )/>3 t
Ac =
(q1 p0)
.
.
.
(qM p0)
, bc =
(q1 p0)
q1
.
.
.
(qM p0)
qM
min
2
+
N1
k=0
W
y
(pk pd(t))
2
+W
u
(pc,k pc,k1)
2
s.t. pk+1 = Apk + Bpc,k, k = 0, . . . , N 1
min pc,k pc,k1 max, k = 0, . . . , Nu 1
Ac,kpk bc,k + gk Ac,kdh,k + 1I
k = 0, . . . , N 1, h = 1, . . . , ri
pc,k = pc,Nu1, k = Nu, . . . , N
L1vMPC guldunce ulgorlthm: otlmlzutlon model
6
At tlme t, select the deslred osltlon pc(t) for the UAv by solvlng the otlmul
control roblem (=C4%5&%("6 ?&8-&%))"#-)
prediction horizon
bounds on
input
increments
desired position
UAV dynamics
blocked moves
(soft)
obstacle
avoidance
constraints
Lxumle: nuvlgutlon demo
,
lnltlul osltlon # = (c,c)
1urget osltlon # = (,c)
lnltlul osltlon #z = (,)
1urget osltlon #z = (c,zc)
ROBMPC project
Robust Model Predictive
Control (MPC) for Space
Constrained Systems
!"#$%&'()* ,-.-/,-..0
"#$6',7 7''+0'F
,'* "L7RLI
Lxumle: nuvlgutlon demo (cooerutlve MPC)
1wo vehlcles uvoldlng euch other und obstucles towurds thelr turgets
8
vehicle
dynamics #1
vehicle
dynamics #2
MPC #1
MPC #2
previous optimal sequences are exchanged
Lecentrullzed L1vMPC for formutlon fylng
Decentralized Linear Time-Varying Model Predictive Control of a

Formation of Unmanned Aerial Vehicles
Alberto Bemporad and Claudio Rocchi
AbstractThis paper proposes a hierarchical MPC approach
to stabilization and autonomous navigation of a formation of
unmanned aerial vehicles (UAVs), under constraints on motor
thrusts, angles and positions, and under collision avoidance
constraints. Each vehicle is of quadcopter type and is stabilized
by a local linear time-invariant (LTI) MPC controller at the
lower level of the control hierarchy around commanded desired
set-points. These are generated at the higher level and at a
slower sampling rate by a linear time-varying (LTV) MPC
controller per vehicle, based on an a simplied dynamical
model of the stabilized UAV and a novel algorithm for convex
under-approximation of the feasible space. Formation ying is
obtained by running the above decentralized scheme in accor-
dance with a leader-follower approach. The performance of the
hierarchical control scheme is assessed through simulations, and
compared to previous work in which a hybrid MPC scheme is
used for planning paths on-line.
I. INTRODUCTION
The last few years have been characterized by an increas-
ing interest in stabilizing and maneuvering a formation of
multiple aerial vehicles. Research areas include both military
and civilian applications (such as intelligence, reconnais-
sance, surveillance, exploration of dangerous environments)
where Unmanned Aerial Vehicles (UAVs) can replace hu-
mans. VTOL (Vertical Take-Off and Landing) UAVs pose
control challenges because of their highly nonlinear and cou-
pled dynamics, and of limitations on actuators and pitch/roll
angles. In particular, quadcopters are a class of VTOL
vehicles for whose stabilization several approaches were
proposed in literature, such as classical PID [1], nonlinear
control [2], H
control [3], and recently linear MPC (Model

Predictive Control) [4].
MPC is particularly suitable for control of multivariable
systems governed by costrained dynamics, as it allows one to
operate closer to the boundaries imposed by hard constraints.
In the context of UAVs, MPC techniques have been already
applied for control of formation ight in [5][9] and for
spacecraft rendezvous [10][12].
In the context of path planning for obstacle avoidance,
several other solutions have been proposed in the litera-
ture, such as potential elds [13], [14], A
with visibility
graphs [1], nonlinear trajectory generation (see e.g. the
NTG software package developed at Caltech [5]), vertex-
graph (VGRAPH) algorithms [15], and mixed-integer linear
programming (MILP) [16], [17].
A. Bemporad is with the IMT (Institutions, Market,
Technologies) Institute for Advanced Studies Lucca, Italy,
alberto.bemporad@imtlucca.it. C. Rocchi is with the Department
of Mechanical and Structural Engineering, University of Trento, Italy,
claudio.rocchi@ing.unitn.it. This work was partially supported
by the European Space Agency through project ROBMPC Robust
Model Predictive Control for Space Constrained Systems.
linear
MPC
linear
MPC
linear
MPC
LTV
MPC
Ts=71 ms
Tsn=1.5 s
continuous
time
measurements
voltages
measurements
desired
positions
measure-
ments
LTV
MPC
LTV
MPC
measure-
ments
navigation
stabilization
dynamics
Fig. 1. Hierarchical control structure for UAV navigation
This paper adopts the two-layer MPC approach depicted
in Figure 1 to stabilization and on-line trajectory gener-
ation for autonomous navigation with obstacle avoidance
of a formation of quadcopters. At the lower level, linear
constrained MPC controllers with integral action take care
of stabilizing the quadcopters with offset-free tracking of
desired set-points. At the higher hierarchical level and at
a lower sampling rate, linear time-varying (LTV) MPC con-
trollers generate on line the paths to follow for the formation
to reach a given target position and shape while avoiding
obstacles. The performance of the strategy adopted in this
paper is assessed through simulations and compared to an
alternative technique based on decentralized hybrid MPC
proposed in a previous work [18]. The lower-level linear
MPC algorithms control motor speeds directly via pulse posi-
tion modulation, under admissible thrust and angle/position
constraints. Based on a leader-follower approach in which
the leader points to the target and the followers track a
given relative position from the leader, the higher-level LTV-
MPCs (one per vehicle) maintain the vehicles in formation
towards a desired target in a decentralized way. Obstacles
and vehicle-to-vehicle collisions are constraining predicted
vehicle positions within a convex polyhedron contained in
the feasible space, generated on-line via a novel approach
proposed in this paper. We assume that target and obstacle
positions may be time-varying and only known at run time,
a situation for which off-line (optimal) planning cannot be
easily accomplished.
The paper is organized as follows. After introducing
the UAV dynamics in Section II, a linear MPC design is
proposed in Section III-A for stabilization under constraints
and trajectory tracking. Section III-B proposes a convex
polyhedral approximation approach for obstacle avoidance,
which is used in Section III-C to formulate the higher-
level LTV-MPC controller for safe path planning. Section IV
/"#$%&*(")$ +%&,"#- ./012 :7; for UAvs fylng ln formutlon
Elerurchlcul & decentrullzed structure (leuderjfollower urouch)
;8#+$@ 4#5$&*%??&8@")%("8# of the obstuclefree suce
V
Mi
, i = 1, . . . , 4
x
f1
f2
f3
f4
z
y
mg
l
1
2
3
4
m x = f sin x
m y = f cos sin y
m z = f cos cos mg z
=

Ixx
=

Iyy
=
l
Izz
(f1 + f2 f3 + f4)
f = f
1
+ f
2
+ f
3
+ f
4
= (f
2
f
4
)l
= (f
3
f
1
)l
fi =
9.81(22.5VMi 9.7)
1000
, i = 1, . . . , 4
Nonllneur dynumlcul model of u quudcoter
commund lnuts: motor voltuges
zc
z stutes , , , x, y, z,

,

,

, x, y, z
Elghly nonllneur und couled dynumlcs
- Sumllng tlme
- Predlctlon horlzon
- Control horlzon
Llneur MPC setu:
- Construlnts:
- Motor voltuge suturutlon
- Altltude
- Pltch und roll ungles
(soft construlnts)
z 0
0 V
Mi
11.1V
6
,

6
NL = 20
NLu = 3
Llneur MPC for stublllzutlon
Llneur MPC control deslgn bused on MA1LA8 MPC 1oolbox
Llneurlzutlon uround the hoverlng condltlon und tlmedlscretlzutlon
z
Ts =
1
14
s
Lxcellent stublllzutlon roertles, deslte NL dynumlcs und construlnts.
Muny other technlques uvulluble for stublllzutlon uroses
5$-2)/++'J PS1+J R'2-('J MNNT9
At tlme t, euch UAv #i solves lts own L1vMPC roblem
vehlcle # conslders urrlvul olnt pd! us deslred turget, pd!(t)!pd!, !t!0
vehlcle #j conslders leuder's osltlon us deslred turget, pdj(t)=pc!(t), !j>
Prevlous otlmul sequences
ure exchunged to redlct future osltlons of other UAvs #j
pcj(t|t 1), pcj(t + 1|t 1), . . . , pcj(t + N 2|t 1)
L1vMPC guldunce ulgorlthm: formutlon fylng
zz
pd!
pc!(t)
pc"(t)
pcM(t)
pd!
tetruhedrul obstucles,
UAvs modeled us smull urulleleleds
! "#$%&'( (&&*+&, used for L1vMPC deslgn,
slmulutlon, und code generutlon
! QP roblem bullder und solver lmlemented
ln Lmbedded MA1LA8
CPU tlme = ~DE )= (er tlmeste)
5"L7RLI 8MNNU0J "-&0''4 L/*9
Lxumle: L1vMPC for formutlon fylng
z
W = conv(
1/3
1/3
1/2
2/3
1/3
1/2
1/3
2/3
1/2
0
0
1/2
)
Comurlson wlth other guldunce methods
z
.3&3()*-+/S3. >,<&"5 "#$
.3&3()*-+/S3. /01 "#$
?8($#("%! F$!5=
5>3)@'. ', #-1+ -. /*0J MNNV9
5I3>?'*-.J 8'&&@/J KGL$ MNOO9
5)@/2 )-+49
CPU tlme = ~DE )=
CPU tlme = ~G )=
CPU tlme = ~HE )=
(l8M CPLL)
J
tt
=
3080
k=350
p
L
(k) pt
2
2
J
fpt
=
3080
k=350
p
L
(k) p
F1
(k) p
d1
2
2
+p
L
(k) p
F2
(k) p
d2
2
2
Ju =
3080
k=350
u(k) u(k 1)
1
Performunce comurlson
z
- 'u
5/(./&32 ('*>-+/S3. B/)@ *32?3&) )' )@3 &3()*-+/S3. @E0*/. "#$ ?3*,'*>-(&3)
./12-. .1/34562 lntegrul Squure Lrror (lSL)
'&17/.5&6 8/..-16 .1/34562 lSL
/+9&*:.- ;-15</.5<- &' 568:. 9526/*9 (lALU)
Jtt Jfpt Ju
centralized hybrid MPC 1 1 1
decentralized hybrid MPC -0.09% -0.51% -0.03%
decentralized LTV MPC -2.46% -15.47% +9.20%
potential elds +210.32% +294.30% +212.75%

2 LTV MPC

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

2 LTV MPC

Uploaded by

Copyright:

Available Formats

A.

8emorud Model Predlctlve Control z

!:0;3&)/<3= >-(/?1+-)3 Tj )' 433? CA '( )@3 .32/*3. 23)A?'/()

A. 8emorud Model Predlctlve Control z

Crlglnul tunlng: J=.cc

5I3>?'*-.J 8'&&@/J KGL$ MNOO9

V(t) = {p(t)} + conv(d

Llneur uroxlmutlon of stublllzed dynumlcs of UAv #i

Decentralized Linear Time-Varying Model Predictive Control of a

control [3], and recently linear MPC (Model

You might also like