ĐẠ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: Nhóm 7
Chu Hải Long 20186312
Nguyễn Khánh Châu 20186310
Trịnh Minh 20181660
Trương Hải Đăng 20181381

Hà Nội, 2022
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 ngoà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 ngoà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 )  (2.1)
 t 
 ( )
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
 (2.2)
v(t ) = vL (t ) + vR (t )
 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 ) (2.3)

 ( t ) =  ( t )

Để 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  .   (2.5)
   0 1  

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 (2.6)
 0 1

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 (2.7)

   0 1  
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
 (2.8)
− x2 .sin ( +  ) + y2 .cos ( +  ) = 0
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 
 (2.9)
 y2 = y + d sin 
Từ đó suy ra:
 x2 = x −  .d sin 
 (2.10)
 y2 = y +  .d cos 
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
 (2.11)
− x.sin ( +  ) + y.cos ( +  ) +  .d cos  = 0
Từ 2.11, ta có ma trận ràng buộc A như sau:
 − sin  cos  0 0
A=
0 
(2.12)
 − sin ( +  ) cos ( +  ) d cos 
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   P m


dt  qk
 −

+

+ g k +  dk = f k −   j .a jk (2.15)
 q k q k j =1

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
 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 . (2.18)

8
 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 (2.21)

T
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  +  −1  u (2.22)
 v     −M .V  M .E 
v
 

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.

Động năng chuyển động của xe là: Wd = m ( x 2 + y 2 ) + J 2

1 1
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

= Wd − Wt = m ( x 2 + y 2 ) + J  2
1 1
(2.23)
2 2
Ta có:

 d  
 = m.x  dt  x  = mx
 x   
  d   
 = m. y     = m. y (2.24)
  y  
dt y 
 d  
 = J  
  = J
  dt   

Và


 =0
 x

 =0 (2.25)
 y

 =0
 
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 (2.26)

 J = M
Để 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
 Fx = ( R +  L ) cos 
r
 (2.27)
 F = 1 ( +  ) sin 
 x r R L

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:
L
M= ( R −  L ) (2.28)
2r

Thay 2.27, 2.28 và 2.29 vào hệ phương trình 2.26 ta thu được:
 1
 mx − 1 sin  − ( R +  L )cos  = 0
r

 1
my + 1 cos  − ( R +  L )sin  = 0 (2.29)
 r
 L
 J  − ( R −  L ) = 0
2r
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 . (2.30)

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

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

 
u =  R
 L 
 − sin  cos  0 0
A=
 − sin ( +  ) cos ( +  ) d cos  0 
 
 =  1
2 

cos  0   − sin  0
Mặt khác S = sin  0 suy ra S =   cos 
  0
   
 0 1   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;

M = S MS;
T
(2.31)

E = S E
T

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  0
v
q =  y  =  sin  0  .   = S.v
     
   0 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 toá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 ngoà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 ngoài sẽ tính toá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:
vs ( t )
 (t ) =  (t ) = sin ( ( t ) ) (3.1)
d
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:

vs ( t )
 (t ) =  (t ) = sin  K1 (ref ( t ) −  ( t ) )
d
Phương trình này được xấp xỉ thành:

K1 (ref ( t ) −  ( t ) )
V
 (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 yê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 ) )

 (3.2)
vs ( t ) = v 2 ( t ) + ( d . ( t ) )
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à luôn 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 )

(
vs ( t ) = v 2 ( t ) + ( d . ( t ) ) .sign cos ( e ( t ) )

2
)
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í yê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ỏ ( ) thì robot sẽ chuyển sang giai đoạn
2, điều khiển robot về điểm tham chiếu.

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 ) )


vs ( t ) = v 2 ( t ) + ( d . ( t ) )
2

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 toà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 toá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 ) 
(3.3)
v ( t ) = x 2 ( t ) + y 2 ( t )

 d   x ( t )  x (t ). y (t ) − x (t ). y (t )
 ( )
 t =  ()
t =  arctan   =
 dt   y ( t )   x2 (t ) + y 2 (t )

18
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 y  và mô hình trạng thái z =  x y  được mô tả như sau:
T T T
x y

19
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à
 − x + xref 
 
q = q ref − 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 ref 0   ex   −1 0 
      v fb 
 ey  =  −ref 0 vref  .  ey  +  0 0  .  
    fb
e   0
  0 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

e 
 v fb   k x 0 0   x
  =  ky k   y 
. e (3.40)
 fb   0 e 
 
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:

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

2 2

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 ) ref

2 2
(t ) gvref (t ), g 0 và lặp lại như
trên, ta được các hệ số như sau:

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

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

Bộ điều khiển thiết kế dựa trên mô hình tuyến tính. 1 mô hình tuyến tính chỉ
hợp lệ ở lân cận của những điểm như điểm zero- error và hiệu suất sẽ kém hơn mong
đợi khi mà sai lệch lớn
Kể cả tuyến tính nhưng hệ vẫn phụ thuộc thời gian, có thể thấy, nếu tất cả
điểm cực đều nằm ở nửa mặt phẳng bờ bên trái của miền s thì hệ sẽ không ổn định
Mặc dù vậy, luật điều khiển tuyến tính vẫn thường được sử dụng bởi nó đơn
giản, dễ dàng điều chỉnh trong thực tế và hiệu suất có thể chấp nhận được

23
 Phần 4: Thiết kế bộ điều khiển cho đối tượng Mobile Robot 3 bánh

Phần 4: Thiết kế bộ điều khiển cho đối tượng Mobile

robot 3 bánh
4.1. Thiết kế bộ điều khiển động học vòng ngoài
Dựa theo những nội dung đã trình bày tại phần 3, ta thiết kế bộ điều khiển
vòng ngoài cho Mobile robot dựa theo hàm ứng viên Lyapunov.

Mục đích điều khiển : ( q − qref ) → 0 khi t → 

Quỹ đạo đặt của robot là ( xref ( t ) ; yref ( t ) )

 y (t ) 
 ref ( t ) = arctan  ref
 x ( t ) 
 ref 
 xref 
 
 qref =  yref 
 
 ref 
Mặt khác ta lại có:

v = x 2 + y 2
 ref ref ref

 d  yref   xref . yref − yref .xref


 ref =  arctan    =
 dt   xref 
2
xref + yref
2

Như vậy vận tốc tham chiếu của robot khi di chuyển trên quỹ đạo tham chiếu
vref 
trên là Vref =  
ref 

Sai lệch vị trí của robot trên hệ trục tọa độ gắn với xe được xác định là:

24
 Phần 4: Thiết kế bộ điều khiển cho đối tượng Mobile Robot 3 bánh

 ex   cos  sin 0   x − xref 

   
qe = ey  = R.( q − q ref ) =  − sin  cos  0  .  y − yref 
 
e   0 0 1   −  ref 
 
 cos  ( x − xref ) + sin  ( y − yref ) 
 
= − sin  ( x − xref ) + cos  ( y − yref ) 

 
  −  ref 
 
Đạo hàm của sai lệch:
ex = − .sin  ( x − xref ) + cos  ( x − xref ) +  .cos  ( y − yref ) + sin  ( y − yref )


e y = − .cos  ( x − xref ) − sin  ( x − xref ) −  sin  ( y − yref ) + cos  ( y − yref )

e =  − ref

(
ex =  − sin  ( x − xref ) + cos  ( y − yref )

)
 + ( x cos  + y sin  ) − ( xref cos  + yref sin  )


(
 ey =  − cos  ( x − xref ) − sin  ( y − yref ) )

 + ( − x sin  + y cos  ) + ( xref sin  − yref cos  )

e =  − ref

Mặt khác:
 x = v.cos 
 y = v.sin 

 x = v .cos 
 ref ref ref

 yref = vref .sin ref



25
 Phần 4: Thiết kế bộ điều khiển cho đối tượng Mobile Robot 3 bánh

ex = .e y + v ( cos 2  + sin 2  ) − vref ( cos  ref cos  + sin  ref sin  )


 e y = −.ex + v ( − cos  sin  + sin  cos  ) + vref ( cos  ref sin  − sin  ref cos  )

e =  − ref
ex = e y + v − vref .cos e

 e y = ex + vref sin e

e =  − ref
Suy ra:
 ex   − cos e 0 1 e y 
   v  v
qe =  ey  =  sin e 0 .  + 0 ex  .  
ref
  ref   
e   0 1    0 1   
   
Lựa chọn bộ điều khiển:

 vc   vref cos e + v fb 
  =   +  
 c  ref fb 

v fb = k x .ex k = k = 2  2 + gv 2


Trong đó:  và 
x ref ref


 fb = k .v
y ref .e y + k sin e k y = gvref

 g = 85
Chọn 
 = 0.9

4.2. Thiết kế bộ điểu khiển động lực học vòng trong

 v  vc   0 
Mục tiêu điều khiển:    −    →   khi t → 
   c   0 

v    v
Ta có:   = M −1  E.  R  − V.   
    L    
Trong đó:

26
 Phần 4: Thiết kế bộ điều khiển cho đối tượng Mobile Robot 3 bánh

  m 0 0  cos  0 
  cos  sin  0   0 m 0  .  sin  0  =  m 0 
= T
=  0 1     0 J 
 M S .M.S .
 0 
  0 0 J   0 1 

  
  cos  cos   1 1 
  cos  sin  0  1   1
E = S .E = 
T
 . .  sin  sin   = .  L − L 
  0 0 1 r r  
 L −L  2 2 
  
  2 2 
  m 0 0   − sin  0 
  cos  sin  0   0 m 0  .   cos  0  = 0 0 
V = S .M.S = 
T

1    0 0 
.
 0 0 
  0 0 J   0 0 


v 
 v =   = M −1.E.u
 

v  vc 
Sai lệch vận tốc là: v e = v − v c =   −  
  c 
Đạo hàm sai lệch:
v e = v − v c = M −1.E.u − v c

Chọn luật điều khiển: u = E−1.M.( C.v e + v c )

Trong đó ma trận C là ma trận điều khiển 2x2 xác định dương

Khi đó:

( )
v e = M −1.E E −1.M.( −C.v e + v c ) − v c = −C.v e
 v e = e − C.t
 lim v e = 0
t →

 200 0 
Chọn C =  
 0 100 

27
 Phần 5: Mô phỏng hệ thống điều khiển trên Matlab Simulink

Phần 5: Mô phỏng hệ thống điều khiển trên Matlab

Simulink

5.1. Mô phỏng mô hình Knematic

Sử dụng khối Matlab function trong Simulink
function [x_dot, y_dot, phi_dot] = Kinematic(v, w, phi)

% q_dot = S * V = [ x_dot ;
% y_dot ;
% phi_dot ]
%

% ma tran van toc V

V = [ v ; w];

% S: ma tran cac phuong chuyen dong co ban

S = [ cos(phi) 0 ;...
sin(phi) 0 ;...
0 1 ];

% Ma tran dau ra:

q_dot = S * V;

% Dau ra:
x_dot = q_dot(1);
y_dot = q_dot(2);
phi_dot = q_dot(3);
end

28
 Phần 5: Mô phỏng hệ thống điều khiển trên Matlab Simulink

5.2. Mô phỏng mô hình Dynamic

Sử dụng khối Matlab function trong Simulink
function [v_dot, w_dot] = Dynamic(tau_r, tau_l, phi, v, w)

% Cac thong so cua xe

% m: khoi luong xe (kg)
% r: ban kinh banh xe (m)
% L: khoang cach giua 2 banh xe (m)
% J: moment quan tinh cua xe (kg.m^2)
% d_G: do lech cua khoi tam so voi diem chinh giua truc banh xe
(m)

m = 1 ;
r = 0.033 ;
L = 0.16 ;
J = 0.002 ;
d_G = 0 ;

% vector momen dau vao

u= [ tau_r ; tau_l];

% ma tran van toc V

V = [ v ; w];

% M: ma tran khoi luong va quan tinh he thong

M = [ m 0 0 ;...
0 m 0 ;...
0 0 J ] ;

% S: ma tran cac phuong chuyen dong co ban

S = [ cos(phi) 0 ;...
sin(phi) 0 ;...
0 1 ];

% E: Ma tran chuyen tu khong gian truyen dong sang toa do tong quat
E = 1/r *[ cos(phi) cos(phi) ;...
sin(phi) sin(phi) ;...
L/2 -L/2 ];

% S_dot: dao ham ma tran S

S_dot = [ -w*sin(phi) 0 ;...
w*cos(phi) 0 ;...
0 0 ];

% Xac dinh cac ma tran trung gian doi bien

M_ = S' * M * S ;

29
 Phần 5: Mô phỏng hệ thống điều khiển trên Matlab Simulink

V_ = S' * M * S_dot * V;
E_ = S' * E ;

% Ma tran dau ra:

V_dot = inv(M_) * (E_*u - V_);

% Dau ra:
v_dot = V_dot(1);
w_dot = V_dot(2);
end

5.3. Bộ điều khiển vòng ngoài Kynematic Controller

Đầu vào của bộ điều khiển là tư thế tham chiếu và tư thế phản hồi vê từ đối
tượng; đầu ra của bộ điều khiển là giá trị tốc độ dài và tốc độ góc điều khiển cho bộ
điều khiển vòng trong.

30
 Phần 5: Mô phỏng hệ thống điều khiển trên Matlab Simulink

function [v,w] = fcn(x, y, phi, x_ref, y_ref, phi_ref, v_ref, w_ref)

q_REF = [x_ref;y_ref;phi_ref];
% x_ref = q_REF(1);
% y_ref = q_REF(2);
% phi_ref = q_REF(3);

V_ref = [v_ref;w_ref];
% v_ref = V_REF(1);
% w_ref = V_REF(2);

% q feedback:
q_FB = [x;y;phi];
% x = q_FB(1); y = q_FB(2); phi = q_FB(3);

% Ma tran chuyen
R = [ cos(phi) sin(phi) 0;...
-sin(phi) cos(phi) 0;...
0 0 1];
%
% Sai lech vi tri
e = R * (q_REF-q_FB);
eX = e(1); eY = e(2); ePHI = e(3);

% he so bo dieu khien
zeta = 0.9;
g=85;

Kx = 2 * zeta * sqrt(w_ref^2 + g * v_ref^2);

Kphi = Kx;
Ky = g * v_ref;

% bo dieu khien:
v = v_ref * cos(ePHI)+ Kx * eX;
w = w_ref + Ky * v_ref * eY + Kphi * sin(ePHI);
end

5.4. Bộ điều khiển vòng trong Dynamic Controller

31
 Phần 5: Mô phỏng hệ thống điều khiển trên Matlab Simulink

Đầu vào của bộ điều khiển là tín hiệu vận tốc điều khiển từ bộ điều khiển vòng
ngoài và tín hiệu tốc độ phản hồi từ đối tượng; đầu ra là giá trị moment đặt vào mô
hình Dynamic cảu đối tượng.
function [tau_R, tau_L] = dynamic_ctr(v_REF, w_REF, v_REF_dot,
w_REF_dot, v_FB, w_FB)

V_REF = [v_REF;w_REF];
V_REF_dot = [v_REF_dot; w_REF_dot];
V_FB = [v_FB; w_FB];
% Cac thong so cua xe
% m: khoi luong xe (kg)
% r: ban kinh banh xe (m)
% L: khoang cach giua 2 banh xe (m)
% J: moment quan tinh cua xe (kg.m^2)
% d_G: do lech cua khoi tam so voi diem chinh giua truc banh xe
(m)

m = 1 ;
r = 0.033;
L = 0.16 ;
J = 0.002 ;

M_ = [ m 0;...
0 J];
E_ = 1/r * [ 1 1 ;...
L/2 -L/2 ];

% V_dot = inv(M_) * E_ * u
err = - V_REF + V_FB;

K = [ 200 0; 0 100];

u = (inv(E_)*M_) * (-K * err + V_REF_dot);

tau_R = u(1);
tau_L = u(2);
end

5.5. Kết quả mô phỏng

5.5.1. Kết quả mô phỏng với kịch bản thứ nhất
Với kịch bản mô phỏng thứ nhất này, ta lựa các bộ điều khiển như Phần 4 đã
nêu.

32
 Phần 5: Mô phỏng hệ thống điều khiển trên Matlab Simulink

Lấy trạng thái ban đầu của robot là  x ( 0 ) ; y ( 0 ) ; ( 0 )  = 1,2;1,4;0 Quỹ

T T

đạo tham chiếu sử dụng:

  2 
 xref = 1.1 + 0.7sin  30 t 
  

 y = 0,8 + 0,7sin  4 t 
 ref  
 30 
Kết quả bám quỹ đạo của robot:

Song, để kiểm tra khả năng tác động tới mô hình của các bộ điều khiển ta thử
thêm một số kịch bản điều khiển khác như ở các phần dưới đây.
5.5.2. Kết quả mô phỏng mới kịch bản thứ hai
Vị trí ban đầu cũng như quỹ đạo tham chiếu ta vẫn sử dụng tương tự như ở
phần 5.5.1, song với kịch bản này ta sẽ thử kiểm tra tính đáp ứng của hệ thống khi
quỹ đạo tham chiếu thay đổi với tốc độ nhanh hơn so với trên với tần số nhanh hơn
4 lần:

33
Phần 5: Mô phỏng hệ thống điều khiển trên Matlab Simulink

  8 
x
 ref = 1.1 + 0.7sin  t
  30 

 y = 0,8 + 0,7sin  16 t 
 ref  
 30 
Kết quả bám quỹ đạo của robot:

So sánh với kết quả ở 5.5.1:

34
 Phần 5: Mô phỏng hệ thống điều khiển trên Matlab Simulink

5.5.3. Kết quả mô phỏng với kịch bản thứ ba

Ở phần này ta sẽ kiểm tra độ tác động của bộ điều khiển vòng trong tới mô
hình bằng các thay đổi ma trận xác định dương trong bộ điều khiển động lực học
2 0
thành: C =  
0 1
Kết quả mô phỏng bám quỹ đạo của robot:

So sánh với quỹ đạo trong kịch bản thứ nhất:

35
 Phần 5: Mô phỏng hệ thống điều khiển trên Matlab Simulink

Nhận xét: Như vậy khả năng bám quỹ đạo của robot khi thay đổi ma trận xác
định dương trong bộ điều khiển vòng trong không thay đổi nhiều trong cả 2 trường
hợp. Điều này là do đáp ứng của bộ điêu khiển vòng trong là rất nhanh so với vòng
ngoài do đó dù có thay đổi thì ảnh hưởng của nó tới hệ thống là rất nhỏ.
5.5.4. Kết quả mô phỏng với kịch bản thứ tư
Đối với kịch bản này, ta sẽ xét đến khả năng tác động của bộ điều khiển vòng
ngoài đến hệ thống bằng cách thay đổi kệ số tắt dần  = 0.75 và hệ số g = 200

Khi đó kết quả bám quỹ đạo thu được như sau:

So với trong kịch bản thứ nhất :

36
 Phần 5: Mô phỏng hệ thống điều khiển trên Matlab Simulink

Nhận xét: Như vậy ta có thể thấy rằng, so với khi thay đổi bộ điều khiển vòng
trong, việc thay đổi bộ điều khiển vòng ngoài tác động rất nhiều tới hệ thống.

37
Tài liệu tham khảo

Tài liệu tham khảo

[1] WHEELED MOBILE ROBOTICS, Gregor Klancar, Andrej Zdešar,
Sašo Blažic, Igor Škrjanc
[2] Mobile robot, 2016, https://en.wikipedia.org/wiki/Mobile_robot
(accessed 18.07.16)
[3] “Design and implementation of an adaptive sliding-mode dynamic
controller for wheeled mobile robots”, Chih-Yang Chen, Tzuu-Hseng
S. Li *, Ying-Chieh Yeh, Cha-Cheng Chang

38