Documents Downloader2

19:13 03/05/2024 Documents Downloader
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
https://documents-downloader.pages.dev/document 1/46
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ì?

TS.Tr!n Ngô Qu"c Huy
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
Off-line On-line
Evolutionary Graph-based Optimization

algorithms approaches APF5
-based control6

1
GA 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., 201 7]
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
+
= { x ∈ Rn | a k x ≤ b k },
Hk

−1 P
3 Hyperplane H1
−
Hk = { x ∈ Rn | − ak x ≤ −bk }. 2
Hyperplane H2
Hyperplane H3
−
ĐN 2: Đa diªn lÁi theo n˚a m∞t phØng: +
H1
H1
0 − +
\ H3 H3
P= +
Hk , k = 1, ..., nh 
+ 3
k −2 H2
 −2
−2 −
H2
ĐN 3 - Đa diªn lÁi theo đønh: −4
−3
−3 −2 −1 0 1 2 3 4
P = conv
 v1 , v2 , ..., vp
 Fig. 1: Polytope description.

ĐN 1 - N˚a m∞t phØng: TS.Tr!n Ngô Qu"c Huy
4
+
= { x ∈ Rn | a k x ≤ b k },
Hk

−1 P
3 Hyperplane H1
−
Hk = { x ∈ Rn | − ak x ≤ −bk }. Hyperplane H2
Hyperplane H3
2
−
ĐN 2: Đa diªn lÁi theo n˚a m∞t phØng: H1
+
H1
0 − +
\ H3 H3
P= +
Hk , k = 1, ..., nh 
+ 3
k −2 H2
 −2
−2 −
H2
−3
−3 −2 −1 0 1 2 3 4
−1 3 −2
P = conv ,
     
,
3 −2 −3 Fig. 1: Polytope description.

ĐN 1 - N˚a m∞t phØng: TS.Tr!n Ngô Qu"c Huy
4
+
= { x ∈ Rn | a k x ≤ b k },
Hk

−1 P
3 Hyperplane H1
−
Hk = { x ∈ Rn | − ak x ≤ −bk }. 2
Hyperplane H2
Hyperplane H3
−
ĐN 2: Đa diªn lÁi theo n˚a m∞t phØng: H1
+
H1
0 − +
 H3 H3
    
5 4 7
x ∈ Rn
P= −6 1 x ≤ 9 +

3
     
1 −5 13
−2

−2
H2
−
−2
H2
−3
−3 −2 −1 0 1 2 3 4
P = conv
     
−1 3 −2 Fig. 1: Polytope description.
3 −2 −3
, ,

ĐN 1 - N˚a m∞t phØng:
∗ Nh≠c l§i: tìm đưÌng thØng đi
Hk
+ n
= { x ∈ R | a k x ≤ b k }, qua 2 đi∫m.
Hk
−
= { x ∈ 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:


     5 4 7
n
P= x ∈R −6 1 x ≤ 9
     1 −5 13
ĐN 3 - Đa diªn lÁi theo đønh:
     
P = conv
−1 3 −2
3 −2 −3
, ,

Hk
+ n
= { x ∈ R | a k x ≤ b k }, qua 2 đi∫m.
Hk
−
−→
Vector AB đưÒc xác đ‡nh:
ĐN 2: Đa diªn lÁi theo n˚a m∞t phØng: −→
 AB = (b1 − a1 , b2 − a2 )
    5 4 7
P= x ∈ Rn −6 1 x ≤ 9
      1 −5 13
ĐN 3 - Đa diªn lÁi theo đønh:
P = conv
     
−1 3 −2
3 −2 −3
, ,

Hk
+ n
= { x ∈ R | a k x ≤ b k }, qua 2 đi∫m.
Hk
−
−→
Vector AB đưÒc xác đ‡nh:
ĐN 2: Đa diªn lÁi theo n˚a m∞t phØng: −→
 AB = (b1 − a1 , b2 − a2 )
     5 4 7
P=
 x ∈R
     ĐưÌ: ng thØng đi qua 2 đi∫m
n
−6 1
1 −5
x ≤ 9
13 A, B

ĐN 3 - Đa diªn lÁi theo đønh: x1 − a1 x2 − a 2
−→ = −→
1 2
P = conv
     
−1 3 −2 AB ( ) AB ( )
3 −2 −3
, ,
Đ‡nh nghĩa vùng không lÁi (lõm)

vÓi Hk− = {x ∈ 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:
−
H
k ⇒ đưa các ràng buÎc vào viªc gi£i
10
2
x
P quy∏t vßn đ∑ b¨ng đi∑u khi∫n tËi ưu là
bßt kh£ thi.
0
Hyperplane H k
Half-space H k−
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Ói Hk− = {x ∈ Rn | − ak x ≤ −bk }.
vũng lõm:
−
H
10
2
x
bßt kh£ thi.
0 ⇒ c¶n có phương pháp/thu™t toán đ∫
Hyperplane H k có th∫ đËi phó vÓi vùng lõm (môi
Half-space H k−
Polytope P trưÌng làm viªc cıa các tác nhân).
−10
−12 −10 −8 −6 −4 −2
x1

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

vÓi Hk− = {x ∈ Rn | − ak x ≤ −bk }.
vũng lõm:
−
H
10
2
x
bßt kh£ thi.
0 ⇒ c¶n có phương pháp/thu™t toán đ∫
Hyperplane H k có th∫ đËi phó vÓi vùng lõm (môi
Half-space H k−
Polytope P trưÌng làm viªc cıa các tác nhân).
−10
−12 −10 −8 −6 −4 −2
TS.Tr n Ngô Qu cĐËHuy x1
i phó vÓi vùng lõm
[ Quy ho§ch sË nguyên hÈn hÒp
C (P ) = Hk
−
, k = 1, ..., nh (1) TrưÌng th∏ n ăng 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à đưa 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 ∈ {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 ∈ Rn | − ak x ≤ −bk }
k

W = C (P ) =
[ −
Hk , k = 1, ..., nh , Hk
−
= { x ∈ Rn | − ak x ≤ −bk }
k

W = C (P ) =
[ Hk
−
, k = 1, ..., nh , Hk
−
= { x ∈ Rn | − ak x ≤ −bk }
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 zk ≤ 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ô
≤2
Qu"c Hu 3
zk
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
XTS.Tr!n Ngô Qu"c Hu
3
zk ≤2
k =1

Ví dˆ: Ràng buÎc lõm theo MIP Chú ý 1: là h¨ng sË có giá tr‡
M
đưÒ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 th∫ đ󰃁ng thÌi đ󰃁ng thÌi b¨ng 1
!n Ngô Qu"c Hu
X ≤2
k =1
TS.Tr
zk

Ví dˆ: Ràng buÎc lõm theo MIP Chú ý 1: là h¨ng sË có giá tr‡
M
đưÒ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
X 3 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 Hu • N"∏u z1 = z3 = 1 thì z2 = 0,

đưÒc bi∫u diπn vÓi nh = 4: −
H
3
16 P
−a1 x ≤ −b1 + Mz1 12 −
H4
−a2 x ≤ −b2 + Mz2 x
2
8
−
H2
−a3 x ≤ −b3 + Mz3 4
−
H
1
0
−a4 x ≤ −b4 + Mz4
4
zk ≤3
−4
−8 −6 −4
x1
−2
Fig. 2: Relaxing H − by Big-M value.

0 2
k =1

đưÒc bi∫u diπn vÓi nh = 4: −
H
3
16 P
−a1 x ≤ −b1 + Mz1 12 − z3 = 1
H4
−a2 x ≤ −b2 + Mz2 x
2
8
z2 = 1 −
H2
−a3 x ≤ −b3 + Mz3 4
−
H
1
0
−a4 x ≤ −b4 + Mz4
4
zk ≤3
−4
−8 −6 −4
x1
Fig. 2: Relaxing H − by Big-M value.
−2 0 2
k =1
TrưÌng th∏ năng nhân t§o (APF [Khatib, 1986])

Khái niªm:
• Đây là mÎt hàm th∏ n ăng 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 ăng 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.
Ý 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 ∈ Rn | ak` xi ≤ bk` , k = 1, . . . , nh` }, with ak` ∈ R1×n , bk` ∈ 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ăng, tÍng cıa hàm đ©y và thu hút:
Nobs
S (x ) =
X Φ` (x ) + Ξ(x ) (4)
TS.Tr!n Ngô Qu"c Hu

`=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ăng, tÍng cıa hàm đ©y và thu hút:
Nobs
S (x ) =
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 ∈ Rn |ak x ≤ bk }, k = 1, ..., nh

k

vùng l󰃁i.
Nh≠c l§i: P =
\ Hk
+
, Hk+ = { x ∈ Rn |ak x ≤ bk }, k = 1, ..., nh
k
TS.Tr!n Ngô Qu"c Huy X nh

γ (x ) = (ak x − bk + |ak x − bk |) (5)
k =1

vùng l󰃁i.
X nh
γ (x ) = (ak x − bk + |ak x − bk |) (5)
k =1
Phân tích pt. (5) TS.Tr!n Ngô Qu"c Hu

• N∏u ∃x ∈ Rn ; ak x − bk < 0, ∀k = 1, ..., nh thì . . .
• N∏u ∃x ∈ Rn ; ak x − bk ≥ 0, ∀k = 1, ..., nh thì . . .

• N ∏u ∃x ∈ Rn ; ak x − bk < 0, ∀k = 1, ..., nh • N ∏u ∃x ∈ Rn ; ak x − bk ≥ 0, ∀k = 1, ..., nh
thì nh
thì
∃k
(ak x − bk − (ak x − bk )) = 0
γi (x ) =
P γ (x ) =
P (ak x − bk + (ak x − bk )) =
k =1 ∀k
∃k
2
P (ak x − bk )
∀k

thì nh
thì
∃k
(ak x − bk − (ak x − bk )) = 0
γi (x ) =
P γ (x ) =
P (ak x − bk + (ak x − bk )) =
k =1 ∀k
⇒ hàm tÍng có gíá tr‡ b¨ng 0 bên trong ∃k
2
vùng l󰃁i P
P (ak x − bk )
∀k
⇒ 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 ăng d¶n n∏u
kho£ng cách càng xa vùng l󰃁i P .

thì nh
thì
∃k
(ak x − bk − (ak x − bk )) = 0
γi (x ) =
P γ (x ) =
P (ak x − bk + (ak x − bk )) =
k =1 ∀k
∃k
2
P (ak x − bk )
TS.Tr!n Ngô⇒ hàm
Qu tÍng"cóc
giá Hu
tr‡ khác 0 bên ngoài
∀k
vùng l󰃁i P , cˆ th ∫ là giá tr‡ s ≥ t ăng 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ì nh
thì
∃k
(ak x − bk − (ak x − bk )) = 0
γi (x ) =
P γ (x ) =
P (ak x − bk + (ak x − bk )) =
k =1 ∀k
∃k
2
P (ak x − bk )
∀k
∃k
X
∇γ (x ) = 2 (ak>)
TS.Tr!n Ngô Qu"c Hu ∀k
⇒ ∇γ (x ) có giá tr‡ ()do kích thưÓc cıa

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

thì nh
thì
∃k
(ak x − bk − (ak x − bk )) = 0
γi (x ) =
P γ (x ) =
P (ak x − bk + (ak x − bk )) =
k =1 ∀k
∃k
2
4 − 
1 P
Hyperplane
P (ak x − bk )
3
Hyperplane
H1 ∀k
H2
2 Hyperplane H3
−
H1 10
  !n Ngô Qu"c Hu
+
TS.Tr
H1 7.5
0 − +
H3 H γ 5
3
2.5
3 0
−2 +
H2
−  −2
2 − 5
H2
−3 2.5
5
−4 0 2.5
−3 −2 −1 0 1 2 3 4 0
x2 −2.5
−5 −2.5 x1
−5

thì nh
thì
∃k
(ak x − bk − (ak x − bk )) = 0
γi (x ) =
P γ (x ) =
P (ak x − bk + (ak x − bk )) =
k =1 ∀k
∃k
2
4 − 
1 P
Hyperplane
P (ak x − bk )
3
Hyperplane
H1 ∀k
H2
2 Hyperplane H3
−
H1 10
TS.Tr !
 n Ngô Qu"c Huy
+
H1 7.5
0 − +
H3 H3 γ 5
2.5
3 0
−2 +
H2
−  −2
2 − 5
H2
−3 2.5
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 ∈ Rn |ak x ≤ bk }, k = 1, ..., nh
TS.Tr!n Ngô
 Qu" c Huy
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
−
  !n Ngô Qu"c Hu
H1 7 .5
TS.Tr
+
H1 γ 5
0 − +
H3 H3 2 .5
3
−2 +
H2
−  −2 5
5
2 −
2 .5
H2 0 2.5
−3 0
− 2.5
−4 x2
− 2 .5
−3 −2 −1 0 1 2 3 4 −5 x1
−5

∗ D§ng phân sË: TS.Tr!n Ngô Qu"c Huy
∗ D§ng hàm mũ:
c1` 2
f
Φ` (γ` (x ) = , (6)
e
Φ` (γ` (x ) = c3` e −(γ (x )−c4 ) ,
` `
(7)
(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 hàm mũ:
∗ D§ng phân sË:
−(γ (x )−c4 )2
e
Φ` (γ` (x ) = c3` e , ` `
(7)
c1`
f
Φ` (γ` (x ) = , (6)
(c2` + γ` (x ))2 Repulsive potential field
)
Repulsive potential field x
(
` 20
γ
(
e `
60 Φ
)
)
x 0
(
` 40
γ
(
f ` 20 8
Φ
0
4
8
0
6
4 x2 4
−4 2
0 0
6 −2 x1
x2
4 −8 −4
−4 2
−6
0
−2 x
1
−8 −4
−6
Fig. 4: RP constructed by exponential function.
Fig. 3: RP constructed by fractional function.

∗ D§ng hàm mũ:
−(γ (x )−c4 )2
e
Φ` (γ` (x ) = c3` e , ` `
(7)
c1`
f
Φ` (γ` (x ) = , (6)
(c2` + γ` (x ))2 Repulsive potential field
60

)
Repulsive potential field x 40

(
`
γ
(
e `
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.

∗ D§ng hàm mũ:
−(γ (x )−c4 )2
e
Φ` (γ` (x ) = c3` e , ` `
(7)
c1`
f
Φ` (γ` (x ) = , (6)
(c2` + γ` (x ))2
• Hai hàm đ©y đưÒc mô t£ bi pt (6) và (7) s≥ chuy∫n mi∑n [0, ∞] cıa pt (5) sang
 c1`
 h
, 0 và c3` e (c4` )2
,0
i
(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ăng lên (i.e. gi£m v∑ 0 khi γ` (x ) → ∞)

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
2
(8)
2D projection
Ξ(x ) = kx − xref kQ ,
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

5
5
2.5 2.5
0 0
x2 −2.5 −2.5 x1
−5

• [Khatib, 1986]: lßy gradient âm cıa hàm th∏ năng như bi ∫u diπn  (4):
XNobs
S (x ) = Φ` (x ) + Ξ(x )
`=1

• [Khatib, 1986]: lßy gradient âm cıa hàm th∏ năng như bi ∫u diπn  (4):
XNobs
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 ) = −∇S (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 ) = −∇S (x )

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

Nobs
Xh iX Nobs
c1`
F (x ) = −∇ (c2` +γ` (x ))3 ∇ γ` (x ) − 2Q(x − xref )∇x ,
`=1 `=1

• N∏u hàm đ©y có d§ng hàm mũ:
Nobs
−(γ` (x )−c4` )2
S (x ) =
X c3` e + Ξ(x )
`=1

Nobs Nobs
F (x ) = −∇
X c3` e −( ` (x )−c4` )
γ 2  X
∇2 γ` (x ) − 2Q(x − xref )∇x
`=1 `=1

XNobs
S (x ) = Φ` (x ) + Ξ(x )

`=1
F (x ) = −∇S (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ăng 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ăng 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
     
5 , 0 , 0
.
10 6 2

−4 −8 −5 10 5 12
O1 = Conv
      , , , O2 = Conv
     
5 , 0 , 0
.
10 6 2
Gi£i: TS.Tr n Ngô Qu c Huy

−4 −8 −5 10 5 12
O1 = Conv
      , , , O2 = Conv
     
5 , 0 , 0
.
10 6 2
Gi£i: TS.Tr!n Ngô Qu"c Huy
• 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
     
5 , 0 , 0
.
10 6 2
Gi£i:
 !n Ngô Qu"c Huy
TS.Tr
  −1 1
   14
O1 = x ∈R 2
 −4 −3 x
   ≤ 14 ,
 8 1 −42

−4 −8 −5 10 5 12
O1 = Conv
     
, , , O2 = Conv
     
5 , 0 , 0
.
10 6 2
Gi£i: TS.Tr n Ngô Qu c Huy
−1 1 14 0 −1 0
           
O1 = x ∈R 2
−4 −3   x ≤ 14 , O2 = x ∈R 2
−1 1 x
   ≤ −5
 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:
−1 1 14 −1 1 14
       
γ1 (x ) = −4 −3
     x − 14 + −4 −3  x − 14  
8 1 −42 8 1 −42
0 −1 0 0 −1 0
       
γ2 (x ) = −1 1
     x − −5 + −1 1  x − −5  
5 2 60 5 2 60


Gi£i:
−1 1 14 −1 1 14
       
γ1 (x ) = −4 −3
        x − 14 + −4 −3 x − 14
8 1 −42 8 1 −42
 !n Ngô
TS.Tr  "c Huy
  Qu
0 −1
  0 0 −1 0
γ2 (x ) = −1 1
        x − −5 + −1 1 x − −5
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ăng 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ăng qua công th˘c:
Nobs
S (x ) =
X Φ` (x ) + Ξ(x )
`=1
TS.Tr!n Ngô Qu"c Huy )
x 75
(
S
100
50
25
12
x2
0 16
12
8
−4 4
0
−8 −4
−8
x1
−12

• Ví dˆ 2: Gi£ s˚ có đËi tưÒng ho§t đÎng trong trưÌng th∏ năng 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ăng 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 ) = −∇S (x ) s ≥ thu đưÒc quˇ đ§o mong muËn.

Gi£i:
Obstacle 1
Obstacle 2
Potential field
20 Force vector field
Desired target
2
x 0
−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]
"P Np Np −1
#
min xt> Qxt ut> Rut (2)
u=[ ]
+
P
.
t =1 t =0
phˆ thuÎc: TS.Tr n Ngô Qu c Huy

= Axt + But , t = 0, 1, ..., Np − 1, (prediction model)
xt +1
xt = x (k ), (measured state)
xt ∈ X , ut ∈ U , (box-type constraints)
−ak xi (τ ) ≤ −bk + Mzk , k = 1, . . . , nh , zk ∈ {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ăng nhân t§o
APF-based MPC approach [Tran et al., 2017]

tk +Tp
min xref k2P S (x̄ (τ )) + kū (τ )k2R d τ
kx̄ (tk + Tp ) − +
R 
ū (·) tk
phˆ thuÎc:
x̄˙ (τ )= f (x̄ (τ ), ū (τ )), (prediction model)
x̄ (tk ) = x (tk ), (measured state)
x̄ (τ ) ∈ X ; ū (τ ) ∈ U , ∀τ ∈ [tk , tk + T p ], (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 +Tp RÌi r§c mô hình danh nghĩa vÓi các đi∑u
kx̄ (tk + T p ) − xref k2P + S (x̄ (τ )) + kū (τ )k2R d τ
kiªn ban đ¶u đ∫ có th∫ đËi phó vÓi
R  
min
ū (·) tk
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̄˙ (τ ) = f (x̄ (τ ) ū (τ )) (prediction model)
, , tiêu, ràng buÎc b¨ng các bi∏n rÌi r§c;
x̄ (tk ) = x (tk ) (measured state)
,
Vßn đ∑ tËi ưu (kích thưÓc h˙u h§n) s≥
x̄ (τ ) ∈ X ; ū (τ ) ∈ U, ∀τ ∈ [ 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 +Tp
direct multiple shooting [Bock and Plitt.
kx̄ (tk + T p ) − xref k2P + S (x̄ (τ )) + kū (τ )k2R d τ 1984] do gi£i quy∏t các ràng buÎc cıa hàm
R  
min
ū (·) tk
mˆc tiêu đơn gi£n hơn các phương pháp khác;
phˆ thuÎc:

, ,

,
, .
∗ 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 +Tp Chia vùng thÌi gian h˙u h§n [tk , tk + Tp ]
thành Np kho£ng con [tk , tk +1 ], và tk +1
R  
min
ū (·) tk
đưÒc xác đ‡nh bi tk +1 = tk + T e ;

phˆ thuÎc: Te = Tp /Np (time step),
, ,
k = 0, 1, .., Np − 1;
,
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 +Tp 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
R  
min
ū (·) tk
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≥ đưa vào hª
, , thËng thông qua đi∑u kiªn đ¶u
,
(x̄ (tk ) = x (tk )) t §i tk , t˘c là mô hình hª
, .
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 +Tp K∏t qu£ c ıa vßn đ∑ t Ëi ưu là mÎt chuÈi
tín hiªn đi∑u khi∫n ū ∗ (·; x (tk )).
R  
min
ū (·) tk
Chø có tín hiªu đ¶u tiên trong chuÈi đưÒc

phˆ thuÎc:
TS.Tr!n Ngô Qu"c Huy lßy cho chu kỳ lßy m®u ti∏p theo tk +1
, ,

, u (τ ; x (tk )) = ū ∗ (τ ; x (tk )), τ ∈ [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 +Tp
min kx̄ (tk + T p ) − xref k2P +
R 
S (x̄ (τ )) + kū (τ )k2R d τ

NMPC scheme for obstacle avoidance
ū (·) tk For each controller and at each sampling time,
phˆ thuÎc:
TS.Tr!n Ngô Qu"c Huy 1: measure the current states x (tk );
2: solve OCP and obtain predicted states;
x̄ (τ ) based on a predicted control sequence,
, ,
x̄ (t ) = x (t ) (measured state)
ū (.);
k k ,
x̄ (τ ) ∈ X ; ū (τ ) ∈ U ∀τ ∈ [ t t + T ] (box constraints)
,
k, k p .
3: apply the first element to the nominal

khi∫n tËi ư u đ c gi£i quy∏t t§i các thÌi đ i∫m lßy m®u t d¸a
model;
k
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 1 đËi tưÒng đưÒc bi∫u diπn  d§ng liên tˆc như sau:
 
0 0 1 0 0 0
 
0 0 0
 1 0 0   
A= , B= .
0 0 −0.05 0  60
1
0 
0 0 0 −0.05 0 601
hiªu đi∑u!khi
Ràng buÎc cıa tínTS.Tr n∫nNgô
u ∈ [−15Qu, 15]"
. c Huy
Ràng buÎc cıa các tr§ng thái x ∈ [−50, 50].
−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

] ]
m m
[
Y 6 [
Y 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
]
m
]
m
[
Y 6 [
Y
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 1 đËi tưÒng đưÒc bi∫u diπn  d§ng liên tˆc như sau:
 
0 0 1 0 0 0
 
0 0 0
 1 0 0   
A=
0 0 −0.05 TS.Tr
 0 n Ngô601 Qu
, B =
0
. c Huy

0 0 0 −0.05 0 601
Ràng buÎc cıa tín hiªu đi∑u khi∫n u ∈ [−15, 15].
Ràng buÎc cıa các tr§ng thái x ∈ [−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”.

Documents Downloader2

Uploaded by

Copyright:

Available Formats

You might also like

Documents Downloader2

Uploaded by

Document Information

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

Documents Downloader2

Uploaded by

Copyright:

Available Formats

19:13 03/05/2024 Documents Downloader

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

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

TS.Tr!n Ngô Qu"c Huy

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

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

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

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

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

−a1 x ≤ −b1 + Mz1

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 Hu

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

TS.Tr!n Ngô Qu"c Hu

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 Hu

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

TS.Tr!n Ngô Qu"c Huy X nh

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