C3-MAS Control Update SV

LäP Kò HOÑCH DI CHUYöN
CHO PH◊ÃNG TIõN T‹ HÀNH

TS. Tr¶n Ngô QuËc Huy
tnqhuy@ute.udn.vn
The University of Technology and Education, Da Nang 550000, Vietnam
Tr¶n Ngô QuËc Huy (UTE) L™p k∏ ho§ch di chuy∫n Ngày 9 tháng 4 n´m 2023 1 / 31
Contents
1 GiÓi thiªu
2 Ràng buÎc lÁi a ønh

TS.Trần Ngô Quốc Huy
3 L™p k∏ ho§ch di chuy∫n d¸a trên MPC
GiÓi thiªu
Contents
1 GiÓi thiªu TS.Trần Ngô Quốc Huy

GiÓi thiªu
L™p k∏ ho§ch di chuy∫n là gì?

Thách th˘c:
Ëi phó vÓi các vùng lõm, là

vùng làm viªc cıa các tác nhân,
Không gian làm viªc luôn bi∏n

Íi,
Tránh va ch§m,
Duy trì s¸ k∏t nËi gi˙a các tác

nhân,...
[Roberge et al., 2003], [Johansen et al., 2004], [Scattolini, 2009], [Molinari et al., 2017], [Breivik et al., 2017], [Baek et
al., 2018], [Baillard et al., 2018], [Hou et al., 2018], [Wang et al., 2019a],...
GiÓi thiªu
Các ph˜Ïng pháp l™p k∏ ho§ch di chuy∫n

Motion planning
for MAS
O↵-line On-line
Evolutionary Graph-based Optimization

algorithms approaches APF5 -based control6

GA1 PSO2 PRM3 RRT4
1
Genetic Algorithms [Davis, 1991], [Karami and Hasanzadeh, 2015]
2
Particle Swarm Optimization [Eberhart and Kennedy, 1995], [Mandava et al., 2019]
3
Probabilistic Road Map [Kavraki et al., 1994], [Baek et al., 2018]
4
Rapidly-exploring Random Trees [LaValle, 1998], [Dalpe and Thein, 2017]
5
Artificial Potential Field [Khatib, 1986], [Rasekhipour et al., 2017]
6
Optimization-based control [Bellman, 1957], [Rawlings and Mayne, 2009]
GiÓi thiªu
Mˆc tiêu, thách th˘c và gi£i pháp

Mˆc tiêu
Tìm con ˜Ìng di chuy∫n không va
ch§m cho MAS.
Thách th˘c
Ràng buÎc cho vùng không lÁi (lõm)
Gi£i pháp
Mixed-Integer Programming (MIP)
[Prodan et al., 2015]
Artificial Potential Field (APF) [Egerstedt

et al., 2002]
Polytopic constraints
Contents
1 GiÓi thiªu TS.Trần Ngô Quốc Huy

‡nh nghæa vùng lÁi a ønh

N 1 - N˚a m∞t phØng: TS.Trần Ngô Quốc Huy
4 
Hk+ = {x 2 Rn | ak x  bk }, 1 P
3 Hyperplane H1
Hk = {x 2 Rn | ak x  bk }. Hyperplane H2
2 Hyperplane H3
H1
N 2: a diªn lÁi theo n˚a m∞t phØng: H1+
0 H3+
\ H3
P= Hk+ , k = 1, ..., nh 
3
k 2 H2+
 2
2
H2
N3- a diªn lÁi theo ønh: 3
4
3 2 1 0 1 2 3 4
P = conv v1 , v2 , ..., vp
Fig. 1: Polytope description.

4 
Hk+ = {x 2 Rn | ak x  bk }, 1 P
3 Hyperplane H1
2 Hyperplane H3
H1
0 H3+
\ H3
P= Hk+ , k = 1, ..., nh 
3
k 2 H2+
 2
2
H2
⇢   4
3 2 1 0 1 2 3 4
1 3 2
P = conv , , Fig. 1: Polytope description.
3 2 3

4 
Hk+ = {x 2 Rn | ak x  bk }, 1 P
3 Hyperplane H1
2 Hyperplane H3
H1
0 H3+
8 2 3 2 39 H3
< 5 4 7 =

P= x 2R n 4 6 1 5x  4 9 5 3
: ; 2 H2+
1 5 13  2
2
3 H2
3 2 1 0 1 2 3 4
⇢  
1 3 2 Fig. 1: Polytope description.
P = conv , ,
3 2 3

N 1 - N˚a m∞t phØng:
⇤ Nh≠c l§i: tìm ˜Ìng thØng i
Hk+= {x 2 R | n
ak x  bk }, qua 2 i∫m.
Hk = {x 2 Rn | ak x  bk }. Cho i∫m A(a1 , a2 ) và B(b1 , b2 )
N 2: a diªn lÁi theo n˚a m∞t phØng:

8 2 3 2 39
< 5 4 7 =
P= x 2R n 4 6 1 5x  4 9 5
: ;
1 5 13
N3-
a diªn lÁi theo ønh:
⇢  
1 3 2
P = conv , ,
3 2 3

Hk+= {x 2 R | n
!
Vector AB ˜Òc xác ‡nh:
N 2: a diªn lÁi theo n˚a m∞t phØng: !
8 2 3 39 2 AB = (b1 a 1 , b2 a2 )
< 5 4 7 =
P= x 2R n 4 6 1 5x  4 9 5
: ;
1 5 13
N3-
a diªn lÁi theo ønh:
⇢  
1 3 2
P = conv , ,
3 2 3

Hk+= {x 2 R | n
!
Vector AB ˜Òc xác ‡nh:
N 2: a diªn lÁi theo n˚a m∞t phØng: !
8 2 3 39 2 AB = (b1 a 1 , b2 a2 )
< 5 4 7 =
P= x 2R n 4 6 1 5x  4 9 5 ˜Ìng thØng i qua 2 i∫m
: ;
1 5 13 A, B:
N3-
a diªn lÁi theo ønh: x a
=
x 1 1 2 a2
⇢   ! !
1 3 2 AB(1) AB(2)
P = conv , ,
3 2 3
‡nh nghæa vùng không lÁi (lõm)

vÓi Hk = {x 2 Rn | ak x  bk }.
20 • Vùng làm viªc cıa các tác nhân vËn dæ là
vÙng lõm:
Hk ) ã các ràng buÎc vào viªc gi£i
10
P quy∏t vßn ∑ b¨ng i∑u khi∫n tËi ˜u là
x2
bßt kh£ thi.

0
Hyperplane Hk
Half-space Hk
Polytope P
10

12 10 8 6 4 2
x1
Fig. 1: Polytope P and its complement C(P).

[
C(P) = Hk , k = 1, ..., nh (1)
k

vÙng lõm:
10
x2
bßt kh£ thi.

0 ) c¶n có ph˜Ïng pháp/thu™t toán ∫
Hyperplane Hk có th∫ Ëi phó vÓi vùng lõm (môi
Half-space Hk
Polytope P tr˜Ìng làm viªc cıa các tác nhân).
10

12 10 8 6 4 2
x1
Fig. 1: Polytope P and its complement C(P).

[
C(P) = Hk , k = 1, ..., nh (1)
k

vÙng lõm:
10
x2
bßt kh£ thi.

0 ) c¶n có ph˜Ïng pháp/thu™t toán ∫
Hyperplane Hk có th∫ Ëi phó vÓi vùng lõm (môi
Half-space Hk
Polytope P tr˜Ìng làm viªc cıa các tác nhân).
10
TS.Trần Ngô Quốc ËiHuy

12 10 8 6 4 2
x1
phó vÓi vùng lõm
Fig. 1: Polytope P and its complement C(P). Quy ho§ch sË nguyên hÈn hÒp
[
C(P) = Hk , k = 1, ..., nh (1) Tr˜Ìng th∏ nńg nhân t§o
k
Quy ho§ch sË nguyên hÈn hÒp

Quy ho§ch sË nguyên hÈn hÒp (Mixed-Integer Programming):
là tr˜Ìng hÒp ∞c biªt cıa quy ho§ch tuy∏n tính (Linear Program) mà
trong ó có mÎt sË bi∏n quy∏t ‡nh b‡ ràng buÎc chø nh™n các giá tr‡
nguyên.

nguyên. TS.Trần Ngô Quốc Huy
⇤ Cˆ th∫ là ã thêm vào các ràng buÎc lõm mÎt sË M có giá tr‡ rßt lÓn,
và bi∏n nh‡ phân bÍ trÒ, z 2 {0, 1}

nguyên. TS.Trần Ngô Quốc Huy
• VÓi vùng lÁi ˜Òc ‡nh nghæa t¯ (1):
[
W = C(P) = Hk , k = 1, ..., nh , Hk = {x 2 Rn | ak x  bk }
k

[
k

[
k
Ràng buÎc lõm theo MIP tÍng quát ˜Òc bi∫u diπn nh˜ sau:
ak x  bk + Mzk , k = 1, ..., nh , (2)
nh
X
z k  nh 1 (3)
k=1

Ví dˆ: Ràng buÎc lõm theo MIP ˜Òc bi∫u diπn vÓi nh = 3:
a1 x  b1 + Mz1
a2 x  b2 + Mz2
a3 x  b3 + Mz3
TS.TrầnXNgô
z 2
Quốc Huy3
k
k=1

Ví dˆ: Ràng buÎc lõm theo MIP
˜Òc bi∫u diπn vÓi nh = 3:
a1 x  b1 + Mz1
a2 x  b2 + Mz2
a3 x  b3 + Mz3
3
X TS.Trần
z 2
Ngô Quốc Huy
k
k=1

Ví dˆ: Ràng buÎc lõm theo MIP Chú ˛ 1: M là h¨ng sË có giá tr‡
˜Òc bi∫u diπn vÓi nh = 3: rßt lÓn ˜Òc chÂn tùy ˛, óng vai trò
nÓi l‰ng các ràng buÎc
a1 x  b1 + Mz1 Chú ˛ 2: z là bi∏n nh‡ phân, óng
a2 x  b2 + Mz2 vai trò kích ho§t ho§t s¸ nÓi l‰ng
a3 x  b3 + Mz3 Chú ˛ 3: các bi∏n nh‡ phân z không
3
X th∫ Áng thÌi Áng thÌi b¨ng 1
k=1
TS.Trần
z 2k
Ngô Quốc Huy

Ví dˆ: Ràng buÎc lõm theo MIP Chú ˛ 1: M là h¨ng sË có giá tr‡
˜Òc bi∫u diπn vÓi nh = 3: rßt lÓn ˜Òc chÂn tùy ˛, óng vai trò
nÓi l‰ng các ràng buÎc
a1 x  b1 + Mz1 Chú ˛ 2: z là bi∏n nh‡ phân, óng
a2 x  b2 + Mz2 vai trò kích ho§t ho§t s¸ nÓi l‰ng
a3 x  b3 + Mz3 Chú ˛ 3: các bi∏n nh‡ phân z không
3
X th∫ Áng thÌi Áng thÌi b¨ng 1
zk  2 • N∏u z1 = z2 = 1 thì z3 = 0,
k=1 • N∏u z2 = z3 = 1 thì z1 = 0,
TS.Trần Ngô Quốc Huy • N∏u z1 = z3 = 1 thì z2 = 0,

H3
16 P
a1 x  b1 + Mz1 12
H4
a2 x  b2 + Mz2 8
x2
H2
a3 x  b3 + Mz3 4
H1
0
a4 x  b4 + Mz4
4
X TS.Trần
z 3 k
Ngô Quốc Huy 4
8 6 4
x1
2 0 2
Fig. 2: Relaxing H by Big-M value.

k=1

H3
16 P
a1 x  b1 + Mz1 12 z3 = 1
H4
a2 x  b2 + Mz2 8
x2
z2 = 1
H2
a3 x  b3 + Mz3 4
H1
0
a4 x  b4 + Mz4
4
X TS.Trần
z 3
Ngô Quốc Huy 4
8 6 4
x1
2 0 2
k Fig. 2: Relaxing H by Big-M value.
k=1
Tr˜Ìng th∏ nńg nhân t§o (APF [Khatib, 1986])

Khái niªm:
• ây là mÎt hàm th∏ nńg vô h˜Óng ˜Òc k∏t hÒp bi hàm ©y và hàm hßp d®n (thu
hút).
• Hàm ©y là hàm bi∫u diπn cho ch˜Óng ng§i v™t, hàm hßp d®n §i diªn cho ích ∏n
mong muËn.
Khatib, O. (1986). Real-time obstacle avoidance for manipulators and mobile robots. In Autonomous robot vehicles, pages 396–404. Springer. 5, 21.

Khái niªm:
• ây là mÎt hàm th∏ nńg vô h˜Óng ˜Òc k∏t hÒp bi hàm ©y và hàm hßp d®n (thu
hút).
• Hàm ©y là hàm bi∫u diπn cho ch˜Óng ng§i v™t, hàm hßp d®n §i diªn cho ích ∏n
mong muËn.
fi t˜ng cıa [Khatib, 1986]
t§o ra mÎt tr˜Ìng vector l¸c gÁm các l¸c ©y xung quanh các v™t c£n nh¨m ©y các tác
nhân ra xa, và các l¸c hút t™p trung t§i ích ∏n ∫ các tác nhân có th∫ §t ∏n v‡ trí
mong muËn.

• GÂi W ✓ Rn là vùng làm viªc cıa các tác nhân (vùng lõm), bao gÁm t™p hÒp các
vùng lÁi §i diªn cho các v™t c£n:
Nobs
[
W O, O = O` ,
`=1

Nobs
[
W O, O = O` ,

`=1
trong ó O` = {xi 2 Rn | ak` xi  bk` , k = 1, . . . , nh` }, with ak` 2 R1⇥n , bk` 2 R, nh` là sË
l˜Òng n˚a m∞t phØng mô t£ O` , ` = 1, . . . , Nobs , and Nobs là sË l˜Òng các vùng cßm
bi∫u diπn cho các v™t c£n.

Nobs
[
W O, O = O` ,
`=1
• GÂi S(·) : Rn ! R 0 là tr˜Ìng th∏ nńg, tÍng cıa hàm ©y và thu hút:
Nobs
X
S(x) = ` (x) + ⌅(x) (4)

`=1
vÓi ⌅(x) : Rn ! R 0 là hàm hßp d®n and ` (x) làm hàm ©y cıa v™t c£n th˘ `th ,
` = 1, . . . , Nobs .

Nobs
[
W O, O = O` ,

`=1
• GÂi S(·) : Rn ! R 0 là tr˜Ìng th∏ nńg, tÍng cıa hàm ©y và thu hút:
Nobs
X
S(x) = ` (x) + ⌅(x) (4)
`=1
) vßn ∑ là ph£i xây d¸ng ˜Òc hàm ©y và hàm hßp d®n


Hàm tÍng (Sum function): là tÍng cıa các hàm tuy∏n tính ∫ t§o nên
vùng lÁi.

vùng lÁi.
\
Nh≠c l§i: P = Hk+ , Hk+ = {x 2 Rn |ak x  bk }, k = 1, ..., nh
k

vùng lÁi.
\
Nh≠c l§i: P = Hk+ , Hk+ = {x 2 Rn |ak x  bk }, k = 1, ..., nh
k
TS.Trần
XNgô Quốc Huy nh
(x) = (ak x bk + |ak x bk |) (5)
k=1

vùng lÁi.
nh
X
(x) = (ak x bk + |ak x bk |) (5)
k=1
Phân tích pt. (5) TS.Trần Ngô Quốc Huy

• N∏u 9x 2 Rn ; ak x bk < 0, 8k = 1, ..., nh thì . . .
• N∏u 9x 2 Rn ; ak x bk 0, 8k = 1, ..., nh thì . . .

• N∏u 9x 2 Rn ; ak x bk < 0, 8k = 1, ..., nh • N∏u 9x 2 Rn ; ak x bk 0, 8k = 1, ..., nh
thì thì
P
nh
P
9k
i (x) = (ak x bk (ak x bk )) = 0 (x) = (ak x bk + (ak x bk )) =
k=1 8k
P
9k
2 (ak x bk )
8k


thì thì
P
nh
P
9k
k=1 8k
) hàm tÍng có gíá tr‡ b¨ng 0 bên trong P
9k
vùng lÁi P 2 (ak x bk )

8k
) hàm tÍng có giá tr‡ khác 0 bên ngoài
vùng lÁi P, cˆ th∫ là giá tr‡ s≥ tńg d¶n n∏u
kho£ng cách càng xa vùng lÁi P.


thì thì
P
nh
P
9k
k=1 8k
P
9k
2 (ak x bk )
TS.Trần Ngô Quốc Huy 8k
) hàm tÍng có giá tr‡ khác 0 bên ngoài
vùng lÁi P, cˆ th∫ là giá tr‡ s≥ tńg d¶n n∏u
kho£ng cách càng xa vùng lÁi P.
) hàm tÍng chø kh£ vi trên t¯ng ph¶n trên
các mi∑n con cıa nó (toàn mi∑n k th∫ kh£
vi t§i các i∫m d¯ng).

thì thì
P
nh
P
9k
k=1 8k
P
9k
2 (ak x bk )
8k
9k
X
r (x) = 2 (ak> )
TS.Trần Ngô Quốc Huy 8k
) r (x) có n giá tr‡ ()do kích th˜Óc cıa

nó phˆ thuÎc vào x).

thì thì
P
nh
P
9k
k=1 8k
P
9k
4 
1 P
2 (ak x bk )
3 Hyperplane H1 8k
Hyperplane H2
2 Hyperplane H3
H1 10
H1+

7.5
0 H3+
H3 5
2.5

3 0
2 H2+
 2
2 5
3 H2 5
2.5
4 0 2.5
3 2 1 0 1 2 3 4 0
x2 2.5
5 2.5 x1
5

thì thì
P
nh
P
9k
k=1 8k
P
9k
4 
1 P
2 (ak x bk )
3 Hyperplane H1 8k
Hyperplane H2
2 Hyperplane H3
H1 10

H1+ 7.5
0 H3+
H3 5
2.5

3 0
2 H2+
 2
2 5
3 H2 5
2.5
4 0 2.5
3 2 1 0 1 2 3 4 0
x2 2.5
5 2.5 x1
5

Hàm ©y (Repulsive potential function): viªc t§o ra hàm ©y là rßt thách th˘c vì ph£i
£m b£o 2 i∑u kiªn chính:
xßp xø úng các v™t c£n (theo quan i∫m hình hÂc);
£m b£o tránh va ch§m cho các tác nhân.

V™t c£n ˜Òc t§o ra bi
\
P= Hk+ , Hk+ = {x 2 Rn |ak x  bk }, k = 1, ..., nh

k
ho∞c
P = conv v1 , v2 , ..., vp
và chúng ph£i có giá tr‡ cao  bên trong ∫ t§o ra Î dËc sao cho các tác nhân có th∫
” Í v∑” i∫m ích mong muËn.

VÓi ˛ t˜Ïng nh˜ v™y thì hàm ©y có th∫ ˜Òc ‡nh nghæa t¯ hàm tÍng

4 
1 P
3 Hyperplane H1
Hyperplane H2
2 Hyperplane H3
10
H1

7.5
H1+ 5
0 H3+
H3 2.5
0

3
2 H2+
 2 5
5
2 2.5
3 H2 0 2.5
0
x2 2.5
4 2.5 x1
3 2 1 0 1 2 3 4 5
5

⇤ D§ng phân sË: TS.Trần Ngô ⇤Quốc
D§ng hàm Huy
mÙ:
c1` e ( ` (x) c4` )2
` ( ` (x) = c3` e , (7)
f
` ( ` (x) = , (6)
(c2` + ` (x))2
trong ó ` (x) hàm tÍng; c1` , c3` bi∫u diπn cho Î lÓn cıa hàm; c2` , c4` bi∫u diπn cho
vùng £nh h˜ng cıa hàm

Hàm ©y (Repulsive potential function):
⇤ D§ng phân sË: ⇤ D§ng hàm mÙ:
e ( ` (x) c4` )2
` ( ` (x) = c3` e , (7)
f c1`
` ( ` (x) = , (6)
(c2` + ` (x))2 Repulsive potential field
Repulsive potential field
e( (x)
20
` `
60
` ( `(x))
0
40
20 8
f
0
4
8
0 6
4 x2 4
4 2
0 0
6 2 x1
x2 4 8 4
4 2 6
0
2 x1
8 4
6
Fig. 4: RP constructed by exponential function.
Fig. 3: RP constructed by fractional function.

e ( ` (x) c4` )2
` ( ` (x) = c3` e , (7)
f c1`
` ( ` (x) = , (6)
(c2` + ` (x))2 Repulsive potential field
60
e( (x)
Repulsive potential field 40
` `
6 20
` ( `(x))
4
0
f
2
8
0
4
8
0
4 6
x2
4
4 2
0 0
x2 6 8 2 x1
4 4 4
2 6
0
8 2 x1
4
6
Fig. 4: RP constructed by exponential function.
Fig. 3: RP constructed by fractional function.

e ( ` (x) c4` )2
` ( ` (x) = c3` e , (7)
f c1`
` ( ` (x) = , (6)
(c2` + ` (x))2
• Hai hàm ©y ˜Òc mô t£ bi pt (6) và (7) s≥ chuy∫n mi∑n [0, 1] cıa pt (5) sang
c1` h i
(c4` )2
, 0 và c 3` e , 0
(c2` )2
• KhØng ‡nh: hàm ©y có giá tr‡ cao bên trong vùng lÁi, Ît ngÎt gi£m nhanh v∑ 0 khi
kho£ng cách tính t¯ vùng lÁi tńg lên (i.e. gi£m v∑ 0 khi ` (x) ! 1)

Hàm hßp d®n (Attractive potential function): ˜Òc t§o ra vÓi mˆc dích
thu hút các Ëi t˜Òng Í v∑. Ki∫u toàn ph˜Ïng (quadractic form) ˜Òc
s˚ dˆng phÍ bi∏n nhßt:
Attractive potential field
2D projection
⌅(x) = kx xref k2Q , (8)

25
vÓi xref bi∫u diπn cho i∫m ∏n 20
15
mong muËn cıa tác nhân và Q là
⌅(x)
10
ma tr™n trÂng sË. 5
TS.Trần Ngô Quốc Huy 5

2.5
x2
0
2.5
5
2.5
0
x1
2.5
5

• [Khatib, 1986]: lßy gradient âm cıa hàm th∏ nńg nh˜ bi∫u diπn  (4):
Nobs
X
S(x) = ` (x) + ⌅(x)
`=1

• [Khatib, 1986]: lßy gradient âm cıa hàm th∏ nńg nh˜ bi∫u diπn  (4):
Nobs
X
S(x) = ` (x) + ⌅(x)
`=1
Mˆc ích: t§o ra mÎt tr˜Ìng vector l¸c ∫ ©y các tác nhân ra xa các
v™t c£n và thu hút các tác nhân di chuy∫n ∏n v‡ trí mong muËn.
F (x) = rS(x) (9)

• N∏u hàm ©y có d§ng phân sË:
Nobs
X c1`
S(x) = + ⌅(x)
(c2` + ` (x))2
`=1

F (x) = rS(x)

• N∏u hàm ©y có d§ng phân sË:
Nobs
X c1`
S(x) = + ⌅(x)
(c2` + ` (x))2
`=1
TS.Trần
Xh
Ngô
i X
Quốc Huy
Nobs Nobs
c1`
F (x) = r (c2` + ` (x))3 r ` (x) 2Q(x xref )rx,
`=1 `=1

• N∏u hàm ©y có d§ng hàm mÙ:
Nobs
X
( ` (x) c4` )2
S(x) = c3` e + ⌅(x)
`=1
TS.Trần
X Nobs
Ngô Quốc
X
Huy Nobs
⇥ ( ` (x) c4` )2
⇤
F (x) = r c3` e r2 ` (x) 2Q(x xref )rx
`=1 `=1

Nobs
X
S(x) = ` (x) + ⌅(x)

`=1
F (x) = rS(x)
L˜u ˛ 1: N∏u s¸ k∏t hÒp cıa các n˚a m∞t phØng (t§o thành vùng lÁi)
không Íi, thì hàm tÍng có giá tr‡ không Íi ) gradient cıa tr˜Ìng th∏
nńg cÙng không Íi.
L˜u ˛ 2: N∏u ban ¶u các tác nhân n¨m bên trong v™t c£n thì s≥ không
có các l¸c ∫ ©y chúng ra kh‰i vùng lÁi.

• Ví dˆ 1: Thi∏t l™p tr˜Ìng th∏ nńg mô t£ không gian làm viªc cıa tác nhân vÓi 2 v™t
c£n ˜Òc mô t£ b¨ng vùng lÁi nh˜ sau:
⇢   ⇢  
4 8 5 10 5 12
O1 = Conv , , , O2 = Conv , , .
10 6 2 5 0 0

⇢   ⇢  
4 8 5 10 5 12
O1 = Conv , , , O2 = Conv , , .
10 6 2 5 0 0
Gi£i: TS.Trần Ngô Quốc Huy

⇢   ⇢  
4 8 5 10 5 12
O1 = Conv , , , O2 = Conv , , .
10 6 2 5 0 0

• Chuy∫n Íi cách bi∫u diπn v™t c£n t¯ d§ng convex hull (t™p hÒp cıa các ønh) sang
d§ng n˚a m∞t phØng:

⇢   ⇢  
4 8 5 10 5 12
O1 = Conv , , , O2 = Conv , , .
10 6 2 5 0 0
Gi£i: TS.Trần
8 2
Ngô3 Quốc
2 39
Huy
< 1 1 14 =
O1 = x 2 R2 4 4 35 x  4 14 5 ,
: ;
8 1 42

⇢   ⇢  
4 8 5 10 5 12
O1 = Conv , , , O2 = Conv , , .
10 6 2 5 0 0

8 2 3 2 39 8 2 3 2 39
< 1 1 14 = < 0 1 0 =
O1 = x 2 R 2 4 4 35 x  4 14 5 , O2 = x 2 R2 4 1 1 5 x  4 55
: ; : ;
8 1 42 5 2 60

Gi£i:
• Vi∏t các hàm tÍng cho t¯ng v™t c£n nh˜ (5)

Gi£i:
02 3 2 3 2 3 2 31
1 1 14 1 1 14
1 (x) =
@4 4 35 x 4 14 5 + 4 4 35 x 4 14 5 A
8 1 42 8 1 42
02 3 2 3 2 3 2 31
0 1 0 0 1 0
2 (x) = @ 4 1 1 5 x 4 55 + 4 1 1 5x 4 55 A
5 2 60 5 2 60


Gi£i:
02 3 2 3 2 3 2 31
1 1 14 1 1 14
1 (x) =
@4 4 35 x 4 14 5 + 4 4 35 x 4 14 5 A
8 1 42 8 1 42
02
0 1
3 TS.Trần Ngô Quốc Huy
2 3
0
2
0 1
3 2 31
0
2 (x) = @ 4 1 1 5 x 4 55 + 4 1 1 5x 4 55 A
5 2 60 5 2 60
• Thi∏t l™p hàm ©y qua hàm tÍng

Gi£i:
• Thi∏t l™p hàm thu hút

Gi£i:
• Thi∏t l™p hàm thu hút
• Bi∫u diπn tr˜Ìng th∏ nńg qua công th˘c:
TS.Trần Ngô
X
Quốc Huy Nobs
S(x) = ` (x) + ⌅(x)
`=1

• Bi∫u diπn tr˜Ìng th∏ nńg qua công th˘c:
Nobs
X
S(x) = ` (x) + ⌅(x)
`=1
TS.Trần Ngô Quốc Huy 100
S(x)
75
50
25
0
12
8
4
x2 0 16
12
8
4 4
0
8 4 x1
8
12

• Ví dˆ 2: Gi£ s˚ có Ëi t˜Òng ho§t Îng  trong tr˜Ìng th∏ nńg v¯a thi∏t l™p, có không
0.5755 0.0614 1 0
gian tr§ng thái bi∫u diπn nh˜ sau: A = , B= .
0.6578 1.0245 0 1
Hãy trình bày lu™t i∑u khi∫n sao cho Ëi t˜Òng hÎi tˆ v∑ i∫m xref = [0; 0].

• Ví dˆ 2: Gi£ s˚ có Ëi t˜Òng ho§t Îng  trong tr˜Ìng th∏ nńg v¯a thi∏t l™p, có không
0.5755 0.0614 1 0
gian tr§ng thái bi∫u diπn nh˜ sau: A = , B= .
0.6578 1.0245 0 1
Hãy trình bày lu™t i∑u khi∫n sao cho Ëi t˜Òng hÎi tˆ v∑ i∫m xref = [0; 0].
Gi£i:
• Áp dˆng gradient âm: F (x) = rS(x) s≥ thu ˜Òc quˇ §o mong muËn.

Gi£i:
Obstacle 1
Obstacle 2
Potential field
20 Force vector field
Desired target
x2
20

30 20 10 0
x1
10 20 30
Fig. 3: Motion planning of agent with different initial points.

Gi£i:

Gi£i:
Motion planning - based MPC
Contents
1 GiÓi thiªu
i∑u khi∫n mô hình d¸ báo vÓi sË nguyên hÈn hÒp

MIP-based MPC approach [Ionela et al, 2013]
" #
N
P >
p NP >
p 1
min xt Qxt + ut Rut (2)
u=[.] t=1 t=0
phˆ thuÎc: TS.Trần Ngô Quốc Huy

xt+1 = Axt + But , t = 0, 1, ..., Np 1, (prediction model)
xt = x(k), (measured state)
xt 2 X , ut 2 U, (box-type constraints)
ak xi (⌧ )  bk + Mzk , k = 1, . . . , nh , zk 2 {0, 1}nh , (MIP constraints)
Prodan Ionela, Stoican Florin, Olaru Sorin, Stoica Cristina and Niculescu Silviu-Iulian: Mixed-integer programming techniques in distributed MPC problems,
Distributed Model Predictive Control Made Easy, 2014, pp.275–291, Springer.

Mô ph‰ng
S˚ dˆng môi tr˜Ìng làm viªc cıa Matlab và các toolbox ∫ x˚ l˛ các t™p
hÒp và gi£i quy∏t vßn ∑ tËi ˜u.
MPT3 và Yalmip toolbox

Chú ˛ chø c¶n cài ∞t MPT3 là ã bao gÁm Yalmip
Link download MPT3: https://www.mpt3.org
T£i và cài ∞t CPLEX Optimization Studio ∫ gi£i quy∏t bài toán
MIP: https://www.ibm.com/products/ilog-cplex-optimization-studio
i∑u khi∫n mô hình d¸ báo vÓi tr˜Ìng th∏ nńg nhân t§o
APF-based MPC approach [Tran et al., 2017]
tk R
+Tp ⇥ ⇤
min kx̄(tk + Tp ) xref k2P + S(x̄(⌧ )) + kū(⌧ )k2R d⌧
ū(·) tk
phˆ thuÎc:
˙ ) = f (x̄(⌧ ), ū(⌧ )), (prediction model)
x̄(⌧
x̄(tk ) = x(tk ), (measured state)
x̄(⌧ ) 2 X ; ū(⌧ ) 2 U, 8⌧ 2 [tk , tk + Tp ], (box constraints)
NQH Tran, Prodan Ionela, Laurent Lefevre: Nonlinear optimization for multi-agent motion planning in a multi-obstacle environment, 21st International
Conference on System Theory, Control and Computing (ICSTCC), 2017, pp.488–493, IEEE . vsvvbbebebebebfbbebenenenenen zgzgzhzhz zgzgzg s
APF-based (N)MPC approach [Tran et al., 2017]
⇤ C¶n l˜u ˛ mÎt sË vßn ∑:
tk R
+Tp ⇥ ⇤ RÌi r§c mô hình danh nghæa vÓi các i∑u
ū(·) tk kiªn ban ¶u ∫ có th∫ Ëi phó vÓi
ph˜Ïng pháp tích phân sË 1 ;
phˆ thuÎc: TS.Trần Ngô Quốc Huy Thay th∏ các bi∏n liên tˆc cıa hàm mˆc
x̄(⌧ tiêu, ràng buÎc b¨ng các bi∏n rÌi r§c;
Vßn ∑ tËi ˜u (kích th˜Óc h˙u h§n) s≥
x̄(⌧ ) 2 X ; ū(⌧ ) 2 U , 8⌧ 2 [tk , tk + Tp ].(box constraints)
˜Òc ‡nh nghæa bi sË bi∏n quy∏t ‡nh có
th∫ ˜Òc gi£i quy∏t b¨ng ph˜Ïng pháp sË.
1 Xßp xø tích phân b¨ng s˚ dˆng ph˜Ïng pháp sË
⇤ RÌi r§c mô hình: S˚ dˆng ph˜Ïng pháp
tk R
+Tp ⇥
direct multiple shooting [Bock and Plitt.
⇤
min
ū(·)
kx̄(tk + Tp ) xref k2P +
tk
S(x̄(⌧ )) + kū(⌧ )k2R d⌧ 1984] do gi£i quy∏t các ràng buÎc cıa hàm
mˆc tiêu Ïn gi£n hÏn các ph˜Ïng pháp khác;
phˆ thuÎc:
x̄(⌧
⇤ L˜u ˛: Trong d˙ liªu lßy m®u cıa NMPC, vßn ∑ i∑u

khi∫n tËi ˜u c gi£i quy∏t t§i các thÌi i∫m lßy m®u tk d¸a
trên thông tin tr§ng thái hiªn t§i.
A multiple shooting algorithm for direct solution of optimal control problems. IFAC Proceedings, Volumes, 17(2):1603–1608
tk R
+Tp ⇥ ⇤ Chia vùng thÌi gian h˙u h§n [tk , tk + Tp ]
ū(·) tk thành Np kho£ng con [tk , tk+1 ], và tk+1
˜Òc xác ‡nh bi tk+1 = tk + Te ;
phˆ thuÎc: Te = Tp /Np (time step),
x̄(⌧ k = 0, 1, .., Np 1;
x̄(tk ) = x(tk ), (measured state) Gi£i quy∏t vßn ∑ i∑u khi∫n tËi ˜u t§i các
thÌi i∫m lßy m®u tk d¸a trên các thông
⇤ L˜u ˛: Trong d˙ liªu lßy m®u cıa NMPC, vßn ∑ i∑u tin tr§ng thái hiªn t§i o §c ˜Òc x(tk ).
tk R
+Tp ⇥ ⇤ Gi£i quy∏t vßn ∑ i∑u khi∫n tËi ˜u t§i các
ū(·) tk thÌi i∫m lßy m®u tk d¸a trên các thông
tin tr§ng thái hiªn t§i o §c ˜Òc x(tk ).
phˆ thuÎc:
TS.Trần Ngô Quốc Huy Tr§ng thái o §c ˜Òc s≥ ã vào hª
x̄(⌧ thËng thông qua i∑u kiªn ¶u
x̄(tk ) = x(tk ), (measured state) (x̄(tk ) = x(tk )) t§i tk , t˘c là mô hình hª
x̄(⌧ ) 2 X ; ū(⌧ ) 2 U , 8⌧ 2 [tk , tk + Tp ].(box constraints) thËng ˜Òc s˚ dˆng ∫ d¸ oán hành vi
cıa hª thËng trong t˜Ïng lai ( ˜Òc khi
khi∫n tËi ˜u c gi£i quy∏t t§i các thÌi i∫m lßy m®u tk d¸a t§o bi tr§ng thái cıa hª thËng th¸c).
tk R
+Tp ⇥ ⇤ K∏t qu£ cıa vßn ∑ tËi ˜u là mÎt chuÈi
ū(·) tk tín hiªn i∑u khi∫n ū ⇤ (·; x(tk )).
Chø có tín hiªu ¶u tiên trong chuÈi ˜Òc
phˆ thuÎc:
x̄(⌧
lßy cho chu k˝ lßy m®u ti∏p theo tk+1
x̄(tk ) = x(tk ), (measured state) u(⌧ ; x(tk )) = ū ⇤ (⌧ ; x(tk )), ⌧ 2 [tk , tk+1 ];
và u(⌧ ; x(tk )) ˜Òc xem nh˜ mÎt ph£n
hÁi vì i∑u này ˜Òc l™p l§i sau mÈi chu
trên thông tin tr§ng thái hiªn t§i. k˝ lßy m®u s˚ dˆng các thông tin mÓi.
tk R
+Tp ⇥ ⇤
min kx̄(tk + Tp ) xref k2P + S(x̄(⌧ )) + kū(⌧ )k2R d⌧ NMPC scheme for obstacle avoidance
ū(·) tk For each controller and at each sampling time,
phˆ thuÎc:
1: measure the current states x(tk );
2: solve OCP and obtain predicted states;
x̄(⌧
x̄(⌧ ) based on a predicted control sequence,
ū(.);
3: apply the first element to the nominal
⇤ L˜u ˛: Trong d˙ liªu lßy m®u cıa NMPC, vßn ∑ i∑u model;
trên thông tin tr§ng thái hiªn t§i. 4: go to the next sampling instance.
Mô ph‰ng
S˚ dˆng môi tr˜Ìng làm viªc cıa Matlab và các toolbox ∫ x˚ l˛ các t™p
hÒp và gi£i quy∏t vßn ∑ tËi ˜u.
T£i và cài ∞t Casadi, link: https://github.com/casadi/optistack
T£i và cài ∞t IPOPT solver ∫ gi£i quy∏t vßn ∑ phi tuy∏n:
https://www.ibm.com/products/ilog-cplex-optimization-studio
K∏t qu£ mô ph‰ng - k‡ch b£n 1
Fig. 3: Motion planning of agent with PF-based MPC and MIP-based MPC.

Xét 12 Ëi t˜Òng ˜Òc bi∫u diπn
3  d§ng liên tˆc nh˜ sau:
2 3
0 0 1 0 0 0
6 7 6 0 0 7
60 0 0 1 7
A=6 7, B = 6 4 1 0 5.
7
4 0 0 0.05 0 5 60
1
0 0 0 0.05 0 60
Ràng buÎc cıa tín hiªu i∑u khi∫n u 2 [ 15, 15].
⇢ tr§ngthái x 2 [ 50, 50]. ⇢
Ràng buÎc cıa các  
4 8 5 10 5 12
2 v™t c£n, conv , , , conv , ,
10 6 2 5 0 0

APF-based MPC MIP-based MPC
Obstacle 1 Obstacle 1
18 Obstacle 2 18 Obstacle 2
Initial points Initial points
Desired point Desired point
14 14
10 TS.Trần Ngô Quốc Huy 10

Y [m]
Y [m]
6 6
2 2
2 2
6 6
16 12 8 4 0 4 8 12 16 20 16 12 8 4 0 4 8 12 16 20
X[m] X[m]
Fig. 4: 50 trajectories, Te = 1[s]. Fig. 5: 50 trajectories, Te = 1[s].

APF-based MPC MIP-based MPC
22
Obstacle 1 Obstacle 1
18 Obstacle 2 Obstacle 2
Initial points Initial points
Desired point 18 Desired point
14
14
10 TS.Trần Ngô Quốc Huy 10

Y [m]
Y [m]
6
6
2
2
2 2
6 6
16 12 8 4 0 4 8 12 16 20 16 12 8 4 0 4 8 12 16 20
X[m] X[m]
Fig. 4: 50 trajectories, Te = 1[s]. Fig. 5: 50 trajectories, Te = 0.1[s].

Bài t™p lÓn
Xét 12 Ëi t˜Òng ˜Òc bi∫u diπn
3  d§ng liên tˆc nh˜ sau:
2 3
0 0 1 0 0 0
6 7 6 0 0 7
60 0 0 1 7
A=6
4 0 0 0.05 0 5 TS.Trần Ngô Quốc Huy
7, B = 64 1
60 0
7.
5
1
0 0 0 0.05 0 60
Ràng buÎc cıa tín hiªu i∑u khi∫n u 2 [ 15, 15].
Ràng buÎc cıa các tr§ng thái x 2 [ 50, 50].
“We cannot teach people anything. We can only help them discover it within themselves.”
WIKIPEDIA info: Galileo Galilei (who died at age 77), was an Italian astronomer, physicist,
engineer, philosopher, and mathematician who played a major role in the scientific revolution
during the Renaissance. Galileo has been called the “father of modern observational
astronomy”, and the “father of modern physics" and “the father of modern science”.

C3-MAS Control Update SV

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

C3-MAS Control Update SV

Uploaded by

Copyright:

Available Formats

LäP Kò HOÑCH DI CHUYöN

CHO PH◊ÃNG TIõN T‹ HÀNH

The University of Technology and Education, Da Nang 550000, Vietnam

2 Ràng buÎc lÁi a ønh

1 GiÓi thiªu TS.Trần Ngô Quốc Huy

3 L™p k∏ ho§ch di chuy∫n d¸a trên MPC

L™p k∏ ho§ch di chuy∫n là gì?

Ëi phó vÓi các vùng lõm, là

Không gian làm viªc luôn bi∏n

Duy trì s¸ k∏t nËi gi˙a các tác

Các ph˜Ïng pháp l™p k∏ ho§ch di chuy∫n

Evolutionary Graph-based Optimization

TS.Trần Ngô Quốc Huy

Mˆc tiêu, thách th˘c và gi£i pháp

Artificial Potential Field (APF) [Egerstedt

1 GiÓi thiªu TS.Trần Ngô Quốc Huy

3 L™p k∏ ho§ch di chuy∫n d¸a trên MPC

‡nh nghæa vùng lÁi a ønh

‡nh nghæa vùng lÁi a ønh

‡nh nghæa vùng lÁi a ønh

‡nh nghæa vùng lÁi a ønh

N 2: a diªn lÁi theo n˚a m∞t phØng:

‡nh nghæa vùng lÁi a ønh

‡nh nghæa vùng lÁi a ønh

‡nh nghæa vùng không lÁi (lõm)

bßt kh£ thi.

TS.Trần Ngô Quốc Huy

Fig. 1: Polytope P and its complement C(P).

‡nh nghæa vùng không lÁi (lõm)

bßt kh£ thi.

TS.Trần Ngô Quốc Huy

Fig. 1: Polytope P and its complement C(P).

‡nh nghæa vùng không lÁi (lõm)

bßt kh£ thi.

TS.Trần Ngô Quốc ËiHuy

Quy ho§ch sË nguyên hÈn hÒp

Quy ho§ch sË nguyên hÈn hÒp

Quy ho§ch sË nguyên hÈn hÒp

Quy ho§ch sË nguyên hÈn hÒp

TS.Trần Ngô Quốc Huy

Quy ho§ch sË nguyên hÈn hÒp

Quy ho§ch sË nguyên hÈn hÒp

Quy ho§ch sË nguyên hÈn hÒp

Quy ho§ch sË nguyên hÈn hÒp

Quy ho§ch sË nguyên hÈn hÒp

Quy ho§ch sË nguyên hÈn hÒp

Fig. 2: Relaxing H by Big-M value.

Quy ho§ch sË nguyên hÈn hÒp

Tr˜Ìng th∏ n´ng nhân t§o (APF [Khatib, 1986])

TS.Trần Ngô Quốc Huy

Tr˜Ìng th∏ n´ng nhân t§o (APF [Khatib, 1986])

Tr˜Ìng th∏ n´ng nhân t§o (APF [Khatib, 1986])

TS.Trần Ngô Quốc Huy

Tr˜Ìng th∏ n´ng nhân t§o (APF [Khatib, 1986])

TS.Trần Ngô Quốc Huy

Tr˜Ìng th∏ n´ng nhân t§o (APF [Khatib, 1986])

TS.Trần Ngô Quốc Huy

Tr˜Ìng th∏ n´ng nhân t§o (APF [Khatib, 1986])

TS.Trần Ngô Quốc Huy

) vßn ∑ là ph£i xây d¸ng ˜Òc hàm ©y và hàm hßp d®n

Tr˜Ìng th∏ n´ng nhân t§o (APF [Khatib, 1986])

Tr˜Ìng th∏ n´ng nhân t§o (APF [Khatib, 1986])

Tr˜Ìng th∏ n´ng nhân t§o (APF [Khatib, 1986])