BÀI tập lớn học PHẦN mô HÌNH hóa mô PHỎNG hệ THỐNG điều KHIỂN đề bài mô hình hóa và mô phỏng hệ thống điều khiển đối tượng mobile robot loại 3 bánh

ĐẠI HỌC BÁCH KHOA HÀ NỘI
TRƯỜNG ĐIỆN – ĐIỆN TỬ
KHOA TỰ ĐỘNG HĨA

BÁO CÁO BÀI TẬP LỚN
HỌC PHẦN MƠ HÌNH HĨA & MƠ PHỎNG HỆ THỐNG ĐIỀU
KHIỂN
ĐỀ BÀI: Mơ hình hóa và mơ phỏng hệ thống điều khiển đối tượng Mobile Robot
loại 3 bánh

GVHD: Ts. Vũ Thị Thúy Nga
Nhóm sinh viên thực hiện:
Chu Hải Long
Nguyễn Khánh Châu
Trịnh Minh
Trương Hải Đăng

Hà Nội, 2022

Nhóm 7
20186312
20186310
20181660
20181381


Mục lục

Mục lục
Mục lục ............................................................................................................ i
Phần 1: Giới thiệu chung .................................................................................1

1.1. Wheeled Mobile Robot .........................................................................1
1.2. Một số loại Wheeled Mobile Robot .....................................................1
1.2.1. Differential Drive ..........................................................................1
1.2.2. Bicycle Drive .................................................................................1
1.3. Lựa chọn mơ hình WMR 3 bánh ..........................................................3
Phần 2: Mơ hình hóa đối tượng Mobile Robot 3 bánh ....................................4
2.1. Mơ hình động học .................................................................................4
2.2. Mơ hình động lực học ...........................................................................7
Phần 3: Các phương án điều khiển đối tượng Mobile Robot ........................13
3.1. Tổng quan về điều khiển ....................................................................13
3.2. Các hướng tiếp cận cơ bản .................................................................14
3.2.1. Điều khiển hướng và điều khiển tịnh tiến ...................................14
3.2.2. Các hướng tiếp cận cơ bản ..........................................................15
3.3. Điều khiển bám quỹ đạo .....................................................................18
3.3.1. Bám quỹ đạo sử dụng các hướng tiếp cận cơ bản .......................18
3.3.2. Phân tích thành phần feedforward và thành phần feedback ........18
3.3.3. Tuyến tính hóa phản hồi ..............................................................19
3.3.4. Phát triển mơ hình sai lệch theo dõi quỹ đạo động học...............20
3.3.5 Bộ điều khiển tuyến tính...............................................................21
Phần 4: Thiết kế bộ điều khiển cho đối tượng Mobile robot 3 bánh .............24
4.1. Thiết kế bộ điều khiển động học vịng ngồi .....................................24
4.2. Thiết kế bộ điểu khiển động lực học vịng trong ................................26
Phần 5: Mơ phỏng hệ thống điều khiển trên Matlab Simulink .....................28
5.1. Mơ phỏng mơ hình Knematic .............................................................28
5.2. Mơ phỏng mơ hình Dynamic ..............................................................29
i


Mục lục
5.3. Bộ điều khiển vịng ngồi Kynematic Controller ...............................30

5.4. Bộ điều khiển vòng trong Dynamic Controller ..................................31
5.5. Kết quả mô phỏng...............................................................................32
5.5.1. Kết quả mô phỏng với kịch bản thứ nhất ....................................32
5.5.2. Kết quả mô phỏng mới kịch bản thứ hai .....................................33
5.5.3. Kết quả mô phỏng với kịch bản thứ ba .......................................35
5.5.4. Kết quả mô phỏng với kịch bản thứ tư ........................................36
Tài liệu tham khảo .........................................................................................38

ii


Phần 1: Giới thiệu chung

Phần 1: Giới thiệu chung
1.1. Wheeled Mobile Robot
Wheeled Mobile Robot (WMR) là một loại robot có có khả năng di chuyển
bằng bánh xe trong mơi trường xung quanh. WMR có thể “tự động”, nghĩa là nó có
khả năng tự điều chỉnh hướng di chuyển trong mơi trường mà không cần đến thiết
bị dẫn hướng vật lý hay cơ điện.
Thành phần chính của một WMR bao gồm bộ điều khiển, các cảm biến, cơ
cấu chấp hanh (động cơ, bánh xe) và hệ thống điện.

1.2. Một số loại Wheeled Mobile Robot
1.2.1. Differential Drive
Differrential Dive là một loại WMR có
2 bánh với 2 bánh được bố trí đồng trục và cả
2 đều là bánh chủ động có thể điều khiển được,
tức là chúng được gắn với 2 cơ cấu chấp hanh
(động cơ) riêng biệt để điều khiển riêng từng
bánh.

Ở loại xe này thơng thường nó rất khó
để có thể đứng được ở trạng thái cân bằng tự
nhiên nên khá ít được sử dụng trong thực tế
1.2.2. Bicycle Drive
Khác với Differrential Dive, Bicycle
Drive mặc dù cũng có 2 bánh, nhưng 2 bánh
của nó được bố trí trên 1 đường thẳng, và
thơng thường chỉ có một bánh chủ động và
một bánh có thể điều khiển được góc lái, cấu
tạo giống như một chiếc xe đạp.
Loại robot này rất hiếm gặp do tính ứng
dụng trong thực tiễn khơng cao.
1.2.3. Tricycle Drive
Tricycle Drive là sự kết hợp giữa 2 loại WMR đã nêu ở trên, nó có 3 bánh xe
và trong đó 2 bánh sau được bố trí đồng trục, và một bánh trước làm bánh lái; 2 trong

1


Phần 1: Giới thiệu chung
3 bánh xe đó sẽ được gắn với cơ cấu chấp hanh để điều khiển
và bánh cịn lại để tự do hoặc cũng có thể sử dụng cơ cấu chấp
hành để điều khiển chỉ một bánh lái phía trước với tốc độ và
góc lái mong muốn.

1.2.4. Car Drive

WMR loại này có cấu tạo tương tự như một chiếc ơ tơ với 2 bánh trước là
bánh có thể thay đổi được góc lái.
1.2.5. Omni Robot

Là một loại robot được thế kế đặc biệt với trục của các bánh đồng quy tại 1
điểm, đặc điểm này sẽ giúp cho robot có thể di chuyển theo mọi phương và tới được
mọi điểm.
Ngoài những loại WMR vừa nêu ở trên, còn rất nhiều loại WMR khác nữa,
trên đây chỉ là một số ví dụ điển hình về WMR.
2


Phần 1: Giới thiệu chung

1.3. Lựa chọn mơ hình WMR 3 bánh
Như đã giới thiệu ở trên, WMR loại 3 bánh có 2 loại chính là Tricycle Drive
(hay Nonholonomic) và Omni Robot, trong đó Tricyle Drive có thể được chia tiếp
thành các loại nhỏ hơn tùy thuộc vào việc bánh nào được gắn cơ cấu chấp hành.

Hai bánh sau truyền
động
Robot nonholonomic
Bánh trước truyền
động

Omnirobot

Ba bánh độc lập

Đối với đề tài này, nhơm lựa chọn mơ hình robot Tricycle Drive với 2 bánh
sau là bánh chủ động được gắn với cơ cấu chấp hanh để điều khiển và bánh trước là
bánh tự do.

3


Phần 2: Mơ hình hóa đối tượng Mobile Robot 3 bánh

Phần 2: Mơ hình hóa đối tượng Mobile Robot 3
bánh
2.1. Mơ hình động học
Mơ hình xe được mơ tả như hình H2.1:

Trong đó:
ICR: là tâm quay tức thời của xe
R(t): bán kính tức thời của quỹ đạo chuyển động của xe

 : Tốc độ góc của xe quanh tâm ICR
v : vận tốc dài theo phương dọc xe

r : bán kính bánh xe

 : góc bánh lái so với trục Om X m
Mơ hình xe được đặt trong một hệ trục tọa độ tổng quát ( X g , Yg ) , và hệ trục
tọa độ chuyển động gắn với xe ( X m , Ym ) . Vector trạng thái của xe trong hệ tọa độ
tổng quát là:

4


Phần 2: Mơ hình hóa đối tượng Mobile Robot 3 bánh
 x (t ) 



q (t ) =  y (t ) 
 t 
 ( )

(2.1)

Do sự chuyển động của xe là nhờ sự truyền động của 2 bánh sau từ đó điều
chỉnh hướng xe di chuyển, nên vận tốc dài của xe theo phương dọc xe được xác định
bởi:

vR (t ) − vL (t )
 (t ) =
L

v(t ) = vL (t ) + vR (t )

2

(2.2)

Phương trình động học ngoài của xe trên hệ tọa độ tổng quát được xác định
như sau:
 x ( t ) = cos ( ( t ) ) .v ( t )

 y ( t ) = cos ( ( t ) ) .v ( t )

 ( t ) =  ( t )

(2.3)

Để ngắn gọn trong cách trình bày, ta tạm bỏ qua sự phụ thuộc của các đại
lượng vào thời gian, hệ phương trình trên được viết lại như sau:
 x = cos  .v

 y = sin  .v
 = 


(2.4)

Hay ta có thể viết dưới dạng ma trận như sau:
 x  cos 0
 v
  
y
=
sin

0
 .  
  
   0
1  

(2.5)

Như vậy với đầu vào điều khiển là vector vận tốc v =  v   , ta có ma trận S
T

với các cột của S là các trường vector thể hiện các hướng di chuyển khả dĩ của xe:

5


Phần 2: Mơ hình hóa đối tượng Mobile Robot 3 bánh
cos 0
S =  sin  0
 0
1

(2.6)

cos  
0
Trong đó các phương chuyển động khả dĩ là: s1 = sin   ; s 2 = 0


 
0

1 

Phương trình 2.5 được viết lại như sau:
 x  cos 0
v
q =  y  =  sin  0 .   = S.v

   0
1  

(2.7)

Tuy vậy, như đã nói xe chỉ có thể chuyển động dọc theo bánh xe, và không
thể chuyển động trượt sang của bánh. Do đó chuyển động của xe chịu các ràng buộc
sau:

− x.sin  + y.cos  = 0

− x2 .sin ( +  ) + y2 .cos ( +  ) = 0

(2.8)

Trong đó:

( x2 , y2 ) : là tọa độ của bánh trước.
Mối quan hệ của ( x2 , y2 ) với ( x, y ) như sau:

 x2 = x + d cos 

 y2 = y + d sin 

(2.9)

 x2 = x −  .d sin 

 y2 = y +  .d cos 

(2.10)

Từ đó suy ra:

Thay 2.10 vào 2.8 ta thu được:

6


Phần 2: Mơ hình hóa đối tượng Mobile Robot 3 bánh

− x.sin  + y.cos  = 0

− x.sin ( +  ) + y.cos ( +  ) +  .d cos  = 0

(2.11)

Từ 2.11, ta có ma trận ràng buộc A như sau:

cos 
0
 − sin 
A=
 − sin ( +  ) cos ( +  ) d cos 

0
0 

(2.12)

Như vậy ta có:
A.q = 0

(2.13)

Từ 2.7 và 2.13 suy ra: A.S.v = 0
Nói cách khác:

A.S = 0

(2.14)

2.2. Mơ hình động lực học
Mơ hình xe 3 bánh đang xét là mơ hình có 2 bánh sau là bánh chủ động được
điều khiển bằng 2 động cơ, bánh trước là bánh tự do. Hai cơ cấu chấp hành này lần
lượt sẽ sinh ra các momen  R và  L .
Mơ hình động lực học chuyển động của xe được mô tả bởi công thức Lagrange
như sau:

d

dt  qk

m
 
P
−
+
+
g
+


=
f
−
 j .a jk


k
dk
k

q

q
j =1
k
k


Trong đó:
: là hiệu giữa động năng và thế năng của hệ thống
P : là công suất tổn hao do ma sát

k : là chỉ số của thành phần tọa độ tổng quát qk
g k : trọng lực phân tích theo phương của qk

 d : thành phần nhiễu theo phương của qk
k

7

(2.15)


Phần 2: Mơ hình hóa đối tượng Mobile Robot 3 bánh

k : nhân tử Lagrange kết hợp với mối quan hệ ràng buộc thứ j
theo qk

a jk : hệ số ràng buộc theo qk
Mơ hình động lực học 2.13 có thể được viết lại dưới dạng ma trận như sau:
M ( q ) q + V ( q, q ) + F ( q ) + G ( q ) +  d = E ( q ) .u − AT ( q ) .

(2.16)

Trong đó:

q : Vector tọa độ tổng quát
M ( q ) : Ma trận xác định dương của khối lượng và quán tính
V ( q, q ) : Vector lực Coriolis và lực ly tâm
F ( q ) : Vector lực ma sát
G ( q ) : Vector trọng lực

 d : Vector nhiễu
E ( q ) : Ma trận chuyển từ không gian truyền động sang không
gian tọa độ
u : Vector moment đầu vào
AT ( q ) : Ma trận hệ số ràng buộc chuyển động

 : Vector lực ràng buộc
Để đơn giản hóa 2.16, ta tạm bỏ qua sự phụ thuộc vào q . Khi đó 2.16 được

viết lại thành:

Mq + V + F + G +  d = E.u − AT .

(2.17)

Mặt khác, đạo hàm 2 vế của 2.7 ta có: q = S.v + S.v . Suy ra 2.17 trở thành:

M.S.v + M.S.v + V + F + G +  d = E.u − AT .
8

(2.18)


Phần 2: Mơ hình hóa đối tượng Mobile Robot 3 bánh
Nhân cả 2 vế 2.18 với ST thu được:
ST MS.v + ST M.S.v + ST ( V + F + G +  d ) = ST E.u

(2.19)

ST MSv + ST ( V + F + G +  d ) = V;

Đặt  ST M.S = M;
 T
S E = E

Phương trình 2.19 trở thành:
M.v + V = E.u

(2.20)

Suy ra
−1

(

v = M . Eu − V

)

Từ 2.7 và 2.21, ta có thể biết diễn khơng gian trạng thái x = qT vT 
x
 y
q     S.v   032 
x =   =   = 
 +  −1  u
−1
 v     −M .V  M .E 
v
 

(2.21)
T

(2.22)

Xét cụ thể đối với mơ hình động lực học của xe 3 bánh. Với m, J lần lượt là
khối lượng của xe, moment quán tính của xe quanh tâm ICR đồng thời bỏ qua thành
phần nhiễu  dk trong công thức 2.15.
Đồng thời ta xét bài toán đơn giản rằng xe chỉ chạy trên mặt phẳng, nghĩa là

thế năng của nó là khơng đổi, hay ta có thể coi mặt đất là mốc thế năng và do đó thế
năng Wt của xe là 0 và thành phần g k cũng biến mất khỏi công thức 2.15.
1
1
Động năng chuyển động của xe là: Wd = m ( x 2 + y 2 ) + J 2
2
2

Hàm Lagrangian tính bởi:

9


Phần 2: Mơ hình hóa đối tượng Mobile Robot 3 bánh
1
1
= Wd − Wt = m ( x 2 + y 2 ) + J  2
2
2

(2.23)

d  

= m.x  dt  x  = mx


 
 x
 d   


=
m
.
y


 
 = m. y

y
dt

y


 

d  
= J

 
 = J



 dt   

(2.24)


=0


x


=0

 y

=0

 

(2.25)

Ta có:

Và

Trong điều kiện lý tưởng, bỏ qua ma sát gây cản trở chuyển động của xe, tức
là coi công suất tổn hao P=0.
Do đó phương trình 2.15 trở thành hệ sau:

mx − 1 sin  = Fx

my + 1 cos  = Fy

 J = M

(2.26)

Để 2 bánh sau của xe có thể chạy được với tốc độ dài ( vR ; vL ) thì cần các
moment của bánh quanh trục quay và trục lái lần lượt là  R , L .

1
Lực tổng hợp dùng để kéo toàn bộ xe di chuyển là F = ( R +  L ) ; xét lực
r
này theo 2 phương chuyển động thẳng x và y ta có

10


Phần 2: Mơ hình hóa đối tượng Mobile Robot 3 bánh
1

F
=
( R +  L ) cos 
x

r

 F = 1 ( +  ) sin 
 x r R L

(2.27)

Xét trên hệ trục tọa độ di động gắn trên xe, lực kéo do 2 bánh tác động lên xe

có xu hướng làm xe quay quanh trục của nó với moment M xác định bởi:

M=

L
( R −  L )
2r

(2.28)

Thay 2.27, 2.28 và 2.29 vào hệ phương trình 2.26 ta thu được:
1

mx
−

sin

−
( R +  L )cos  = 0
1

r

1

my + 1 cos  − ( R +  L )sin  = 0
r

L


J

−
( R −  L ) = 0

2r

(2.29)

Mơ hình động học 2.30 được viết về dưới dạng ma trận như sau:
M ( q ) .q = Eu − AT .

Trong đó:
m 0 0 
M =  0 m 0


 0 0 J 

cos 
1
E =  sin 
r
 L

 2


cos  


sin  
−L 

2 

11

(2.30)


Phần 2: Mơ hình hóa đối tượng Mobile Robot 3 bánh
 
u =  R
 L 
cos 
0
 − sin 
A=
 − sin ( +  ) cos ( +  ) d cos 

0
0 

 
 =  1
2 

 − sin 
cos  0 



Mặt khác S = sin  0 suy ra S =   cos 



 0
 0
1 

0
0

0 

Từ đó, khơng gian trạng thái ở 2.22 được xác định bởi các ma trận:
V = ST MSv;

T
M = S MS;

T
E = S E

(2.31)

Song do tính phi tuyến của hệ thống nên để thuận tiện cho việc thiết kế điều
khiển ta thường tách mơ hình trạng thái của hệ thống ra thành 2 thành phần. Gồm:
Thành phần mơ hình động học:
 x  cos 

q =  y  =  sin 
  
   0

0
v
0  .   = S.v
  
1 

Thành phần mơ hình động lực học:
−1

(

v = M . Eu − V

)

Trong đó các ma trận M, E, V được xác định bởi 2.32

12


Phần 3: Các phương án điều khiển đối tượng Mobile Robot

Phần 3: Các phương án điều khiển đối tượng Mobile
Robot
3.1. Tổng quan về điều khiển
Như đã trình bày ở trên, để thuận tiện cho việc thiết kế điều khiển ta thường

chia mơ hình trạng thái của xe thành 2 mơ hình thành phần là động học và động lực
học. Do đó việc xây dựng các phương án điều khiển cũng dựa trên 2 mơ hình này.
Thơng thường để thiết kế điều khiển cho đối tượng mobile robot dạng này, ta
sẽ sử dụng tới phương án điều khiển kinh điển hay gặp trong các bài tốn điều khiển,
đó là sử dụng cấu trúc 2 vòng điều khiển, đây là 1 dạng sách lược điều khiển tầng.
Trong đó vịng ngồi là vịng điều khiển tốc độ (hay chính là vịng điều khiển động
học) và vòng trong là vòng điều khiển moment (vòng điều khiển động lực học). Bộ
điều khiển động học ở vịng ngồi sẽ tính tốn xác định giá trị tốc độ là trở thành
giá trị đặt cho bộ điều khiển động lực học ở vòng trong. Sơ đồi khối của hệ thống
được miêu tả như sau:

Ngoài ra, hệ thống điều khiển trên cịn có thể biểu diễn theo một cách khác
theo phương pháp EMR như sau:

13


Phần 3: Các phương án điều khiển đối tượng Mobile Robot

3.2. Các hướng tiếp cận cơ bản
3.2.1. Điều khiển hướng và điều khiển tịnh tiến
Gọi góc hướng của robot ở thời điểm t là φ(t) và góc hướng mong muốn là
φref(t), sai lệch góc hướng điều khiển là:
𝑒𝜑 (𝑡) = 𝜑𝑟𝑒𝑓 (𝑡) − 𝜑(𝑡)
Trong trường hợp này, biến cần điều khiển là φ(t) và để điều khiển φ(t) đạt
được tới giá trị mong muốn ta cần điều khiển sai lệch 𝑒𝜑 (𝑡) về 0.
Góc hướng của robot được thể hiện qua hệ phương trình vi phân của góc
hướng như sau:

 (t ) =  (t ) =

vs ( t )
sin ( ( t ) )
d

(3.1)

Trong trường hợp này, biến điều khiển được chọn là  ( t ) , nó được xác định
tương ứng với sai lệch góc hướng 𝑒𝜑 (𝑡) thông qua bộ điều khiển tỷ lệ như sau:

 ( t ) = 𝐾1 (𝜑𝑟𝑒𝑓 (𝑡) − 𝜑(𝑡))
Khi đó phương trình (3.1) trở thành:

 (t ) =  (t ) =

vs ( t )
sin  K1 (ref ( t ) −  ( t ) )
d

Phương trình này được xấp xỉ thành:

 (t ) =  (t ) =

V
K1 (ref ( t ) −  ( t ) )
d

Song để xe bám được theo 1 quỹ đạo cho trước hoặc 1 trạng thái cho trước, ta
cần điều khiển tịnh tiến, tức là điều khiển tốc độ dài của xe. Ý tưởng thiết kế điều
khiển hợp lý nhất là điều khiển tỷ lệ với sai lệch khoảng cách từ vị trí hiện tại tới vị

trí tham chiếu:
2

2

𝑣(𝑡) = 𝐾2 √(𝑥𝑟𝑒𝑓 (𝑡) − 𝑥(𝑡)) + (𝑦𝑟𝑒𝑓 (𝑡) − 𝑦(𝑡))

14


Phần 3: Các phương án điều khiển đối tượng Mobile Robot
3.2.2. Các hướng tiếp cận cơ bản
Ở phần này, bằng cách kết hợp việc điều khiển góc hướng và điều khiển tịnh
tiến đã nói đến ở phần trước, ta sẽ có một số hướng tiếp cận cơ bản trong thực tế để
điều khiển robot tới một trạng thái tham chiếu.
a. Điều khiển đến một vị trí đặt
Trong trường hợp này, robot được yêu cầu đi đến một vị trí tham chiếu, nơi
mà khơng u cầu góc hướng cuối cùng. Góc hướng của robot được điều khiển liên
tục theo hướng của điểm tham chiếu, tức là hướng từ vị trí hiện tại của robot tới điểm
tham chiếu, gọi là góc r , được xác định bởi:

Vận tốc dài và góc lái của bánh trước được điều khiển như sau:
 ( t ) = K1 (ref ( t ) −  ( t ) )


2
vs ( t ) = v 2 ( t ) + ( d . ( t ) )

(3.2)

Trong đó: v(t) và  ( t ) được xác định như phần trên.
Tuy vậy luật điều khiển như trên sẽ gặp phải một số vấn đề. Cụ thể, giá trị tốc
độ như trên là ln dương, do đó robot có thể vơ tình đi qua điểm tham chiếu, và
điều này sẽ khiến cho tốc độ lại tiếp tục tăng lên (do khoảng cách tới điểm tham
15


Phần 3: Các phương án điều khiển đối tượng Mobile Robot
chiếu lại tiếp tục tăng). Mặt khác khi đi qua điểm tham chiếu , vơ tình góc hướng
tham chiếu sẽ đột ngột quay ngược lại. Giải pháp được đề ra bằng cách thay đổi cơng
thức tính vận tốc và góc lái như sau:

e ( t ) = ref ( t ) −  ( t )

 ( t ) = K1e ( t )

2
vs ( t ) = v 2 ( t ) + ( d . ( t ) ) .sign cos ( e ( t ) )


(

)

b. Điều khiển tới một trạng thái tham chiếu (reference pose) thông qua điểm
trung gian
Khác với phần trước, trường hợp này ngoai việc đi tới vị trí u cầu, robot cịn
cần đạt tới một góc hướng cho trước. Về cơ bản việc này khá dễ thực hiện do có thể
áp dụng lại các luật điều khiển ở phần trên.
Ý tưởng của phương pháp này là sử một điểm trung gian ( xt , yt ) được đặt cách

điểm tham chiếu một khoảng r sao cho hướng từ điểm trung gian về phía điểm tham
chiếu trùng với hướng tham chiếu:

Thuật toán điều khiển robot sẽ được chia làm 2 giai đoạn: Ở giai đoạn 1, robot
được điều khiển đi tới điểm trung gian. Khi khoảng cách từ vị trí của robot tới điểm
trung gian đủ nhỏ (
2, điều khiển robot về điểm tham chiếu.

) thì robot sẽ chuyển sang giai đoạn

16


Phần 3: Các phương án điều khiển đối tượng Mobile Robot

c. Từ trạng thái tham chiếu tới quỹ đạo tham chiếu
Hướng tiếp cận vấn đề này khá đơn giản, nó hoàn toàn dựa trên tinh thần của
điều khiển tới 1 trạng thái tham chiếu. Ở đây quỹ đạo sẽ được chia thành nhiều đoạn
thẳng nhỏ và đầu mút của các đoạn thẳng sẽ là các trạng thái tham chiếu. Nói cách
khác một quỹ đạo tham chiếu sẽ gồm một chuỗi trạng tham chiếu Ti với i = 1, 2,3...n
Robot sẽ phải di chuyển lần lượt từ T1 tới T2 ,… cho tới Tn .

17


Phần 3: Các phương án điều khiển đối tượng Mobile Robot

3.3. Điều khiển bám quỹ đạo
3.3.1. Bám quỹ đạo sử dụng các hướng tiếp cận cơ bản
Tương tự như phần 3.2.2.c, ta có thể hình dung quỹ đạo tham chiếu như một

vị trí chuyển động tham chiếu. Mỗi thời điểm trích mẫu, điểm tham chiếu lại được
xác định bằng điểm hiện tại của quỹ đạo tham chiếu theo thời gian ( xref ( t ) , yref ( t ) )
Việc điều khiển tới điểm tham chiếu này được áp dụng luật điều khiển như công
thức (3.2).
 ( t ) = K1 (ref ( t ) −  ( t ) )


2
vs ( t ) = v 2 ( t ) + ( d . ( t ) )

3.3.2. Phân tích thành phần feedforward và thành phần feedback
Cách tiếp cận nêu ở phần 3.3.1 rất dễ thực hiện nhưng nó lại có nhược điểm
là rất dễ bị nhiễu trong các vịng điều khiển, do đó cần có thành phần bù nhiễu
feedforward.
Trước tiên, ta xét tới một hệ thống vi phân phẳng. Một hệ thống được gọi là vi
phân phẳng nếu có một tập các đầu ra phẳng và tất cả các trạng thái và đầu vào hệ
thống có thể được viết lại như các hàm của các đầu ra phẳng và một số hữu hạn đạo
hàm theo thời gian của nó. Hệ quả là, nếu một hệ thống là vi phân phẳng thì mọi
biến hệ thống đều có thể tính tồn được từ các đầu ra phẳng mà khơng cần tích phân.
Nghĩa là từ quỹ đạo tham chiếu ta có thể tính tốn ra các đầu vào điều khiển.
Ta có thể dễ dàng nhận thấy đối tượng mobile robot 3 bánh đang xét là một
hệ thống vi phân phẳng. Thật vậy:
v ( t ) = x 2 ( t ) + y 2 ( t )


 x (t ) 
 ( t ) = arctan 


 y (t ) 

v ( t ) = x 2 ( t ) + y 2 ( t )


 x ( t )  x (t ). y (t ) − x (t ). y (t )
d 

t
=

t
=
arctan
()

 ( )

 =
dt
y
t
x2 (t ) + y 2 (t )
(
)



 

18

(3.3)


Phần 3: Các phương án điều khiển đối tượng Mobile Robot
Như vậy cơng thức (3.3) chính là cơng thức tính toán các đầu vào tham chiếu
ứng với một quỹ đạo tham chiếu. Đây cũng chính là một bộ điều khiển vịng hở cho
mơ hình động học nhằm đảm bảo cho robot bám theo quỹ đạo đặt.
3.3.3. Tuyến tính hóa phản hồi
Ý tưởng của phương pháp này là thực hiện các biến đổi nhằm tuyến tính hóa
đầu vào hệ thống làm cho hệ thống giữa đầu vào mới và đầu ra tuyến tính. Từ đó các
việc thiết kế các bộ điều khiển tuyến tính cho hệ thống trở nên khả thi. Quy trình
thiết kế tuyến tính hóa phản hồi như sau:
− Chọn các đầu ra phẳng thích hợp, số lượng đầu ra phải bằng số lượng
đầu vào
− Lấy vi phân các đầu ra và kiểm tra sự xuất hiện của các đầu vào, lặp lại
đến khi tất cả đầu vào đều xuất hiện.
− Hệ phương trình được giải cho đạo hàm bậc cao nhất của từng đầu vào.
Trong trường hợp của mobile robot 3 bánh, như đã chứng minh từ phần trước,
nó là một hệ thống vi phân phẳng. Đầu ra phẳng của hệ thống là x(t) và y(t). Đạo
hàm bậc nhất của các đầu vào phẳng là:
 x = v.cos ( )

 y = v.sin ( )

Từ hệ trên ta nhận thấy mới chỉ có sự xuất hiện của đầu vào vs , do đó ta tiếp
tục lấy đạo hàm bậc 2:

Đến đây hệ phương trình đã xuất hiện đủ cả 2 đầu và là vs và  =  . Hệ trên
được viết lại thành

Như vậy với phép biến đổi này đầu vào của hệ thống đã trở thành

u1

u2  =  x
T

y  và mơ hình trạng thái z =  x
T

19

x

y

y  được mô tả như sau:
T


Phần 3: Các phương án điều khiển đối tượng Mobile Robot

Hay: z = Az + Bu
Tiếp theo ta cần thiết kế bộ điều khiển cho hệ thống tuyến tính mới này. Quỹ
đạo tham chiếu được cho bởi ( xref ( t ) , yref ( t ) ) , từ đó dễ dàng tính được trạng thái
tham chiếu của hệ thống như sau: z ref =  xref

xref

yref

yref  . Ta có:

z ref = Az ref + Bu ref
Sai lệch giữa trạng thái hệ thống thực tế với trạng thái tham chiếu là
z = z − z ref . Suy ra:
Sử dụng phương pháp đặt điểm cực ta xác định bộ điều khiển K:
Trong đó:

3.3.4. Phát triển mơ hình sai lệch theo dõi quỹ đạo động học
Sai lệch vị trí của robot trong hệ tọa độ toàn cục (hệ tọa độ cố định) với vị trí
tham chiếu là

q = q ref

 − x + xref 


− q =  − y + yref 
 − + ref 



Sai lệch vị trí của robot trong hệ tọa độ gắn với xe là

20


Phần 3: Các phương án điều khiển đối tượng Mobile Robot

Đạo hàm của sai lệch:

Trong đó vref và wref là vận tốc tham chiếu tuyến tính

vfb và wfb là các tín hiệu được xác định sau tùy thuộc vào luật điều khiển mà
ta chọn . Phương trình sai lệch được viết lại

3.3.5 Bộ điều khiển tuyến tính
Nhận thấy điểm zero-error ( ex = e y = e =0) là 1 điểm cân bằng khi cả 2 thành
phần phản hồi v fb =  fb =0. Từ mơ hình sai lệch phi tuyến ở 3.3.4, ta tuyến tính hóa
quanh điểm zero-error:

 ex   0
  
 ey  =  −ref
e   0
 

ref
0
0

0   ex   −1 0 
 v fb 
 
vref  .  ey  +  0 0  .  


  fb



0  e   0 −1  

Nhờ có cấu trúc đặc biệt ở hệ (3.39), ta có thể sử dụng 1 ma trận phản hồi
trạng thái tĩnh đơn giản, nhờ đó sai lệch trong hướng lái và góc lái sẽ đc điều chỉnh
lại bằng v fb và  fb :
21


Phần 3: Các phương án điều khiển đối tượng Mobile Robot

 v fb   k x
  = 
 fb   0

0
ky

e 
0   x
. e
k   y 
e 
 

(3.40)

Các hệ số điều khiển k x , k y , k được chọn sao cho các điểm cực của hệ thống
nằm ở vị trí thích hợp trong miền s. Hệ thống có 3 điểm cực, trong đó 1 điểm cực
thực và 2 điểm cực phức liên hợp. Các điểm cực được đặt ở vị trí cố định

2n , n

n 1  2 với n

0, 0  1 . Dựa vào đa thức đặc trưng của hệ kín:

2
2
Và đa thức mong muốn: ( s 2n )( s 2n s n )

Ta được các hệ số điều khiển

Trên thực tế các hệ số ở 3.41 vẫn khá khó áp dụng, bởi k y (t ) trở nên khá lớn,
trong khi vận tốc vref thì lại nhỏ.
Để giải quyết vấn đề này ta chọn n (t )
trên, ta được các hệ số như sau:

Ta rút ra 2 nhận xét quan trọng:

22

2
2
ref
(t ) gvref
(t ), g 0 và lặp lại như