Điều khiển omni robot sử dụng thuật toán DSC và mô phỏng bằng Sfunction

Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC

LỜI NÓI ĐẦU

Cùng với sự phát triển của xã hội loài người , các ngành khoa học kỹ thuật không
ngừng đi đến những thành công mới. Nhiều công trình khoa học, những phát minh của
các nhà khoa học đã đi vào cuộc sống, phục vụ lợi ích của con người. Ngày nay Robot
được nghiên cứu và ứng dụng rộng rãi trong các ngành công nghiệp, thay thế cho các
hoạt động của con người trong các môi trường độc hại, nguy hiểm. Nổi bật trong số đó
là robot tự hành . Là robot được ứng dụng rất rộng rãi trong đời sống cũng như trong
công nghiệp.
Trong thời gian thực tập tại trường cao đẳng nghề công nghệ cao HHT, dưới sự
hướng dẫn chỉ bảo tận tình của GS.Phan Xuân Minh và TS.Ngô Mạnh Tiến chúng em
đã tìm hiểu được nguyên lý hoạt động của bánh xe Omni cũng như mô hình động học
và động lực học của robot tự hành. Từ đó đưa ra các thuật toán điều khiển theo nguyên
lý MSS hay DSC cho robot. Tuy nhiên chắc rằng báo cáo của chúng em vẫn còn nhiều
sai sót. Vì vậy chúng em mong có sự góp ý, phê bình của thầy cô để bài báo cáo được
hoàn thiện hơn. Chúng em xin chân thành cám ơn !

Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

1


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC

MỤC LỤC
LỜI NÓI ĐẦU .................................................................................................................1
MỤC LỤC .......................................................................................................................2
DANH MỤC HÌNH VẼ ..................................................................................................4
ĐẶT VẤN ĐỀ .................................................................................................................5

I.

Cấu trúc xe tự hành (Omni Robot) ..........................................................................6
1.

Xe tự hành là gì ? ...............................................................................................6

2.

Các bộ phận của xe ............................................................................................6
Bánh xe Omni .............................................................................................6
Cách di chuyển của bánh xe omni...............................................................8
Sơ đồ vector biểu diễn chiều di chuyển của robot sử dụng bánh xe omni ..9
Thân xe ......................................................................................................10

II.

Mô hình hóa xe tự hành .........................................................................................11
1.

Mô hình động học ............................................................................................11

2.

Mô hình động lực học ......................................................................................12

III.
1.

Thiết kế bộ điều khiển cho robot omni 4 bánh ....................................................13

Phương pháp Multiple Sliding Surface Control (MSS) ...................................13
Lý thuyết chung.........................................................................................13
Áp dụng cho đối tượng Omni robot ..........................................................14

2.

Phương pháp DSC ( Dynamic Surface Control ) .............................................15
Lý thuyết chung.........................................................................................15
Áp dụng cho đối tượng Omni robot ..........................................................15

3.

Mô phỏng trên Matlab ( Sử dụng công cụ S-function ) ...................................17
Theo Backstepping ....................................................................................17
Theo DSC ..................................................................................................20

4.

Code lập trình Sfunction ..................................................................................22
Khối tạo quỹ đạo tròn ................................................................................22
Khối tạo quỹ đạo thẳng .............................................................................26
Khối BackStepping ...................................................................................31
Khối DSC ..................................................................................................36
Mô hình Robot ..........................................................................................41

PHỤ LỤC ......................................................................................................................47
1.

Mô hình động lực học ......................................................................................47

Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

2


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC
2.

Mô hình động lực học ......................................................................................52

3.

So sánh mô hình động học của các bài báo nước ngoài với trong nước..........57
Ở bài báo nước ngoài ................................................................................57
Ở Việt Nam ...............................................................................................58

3.

Thiết kế bộ điều khiển......................................................................................59
Điểm ổn định .............................................................................................59
Theo dõi quỹ đạo .......................................................................................60
Dead – Reckoning .....................................................................................61

TÀI LIỆU THAM KHẢO .............................................................................................62

Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

3

Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC
DANH MỤC HÌNH VẼ
Hình 1 : Cấu trúc đơn giản của xe tự hành
Hình 2 : Một số loại bánh xe omni
Hình 3 : Khả năng di chuyển của robot omni
Hình 4 : Mô tả cách di chuyển của omni robot
Hình 5 :Hình ảnh của một bánh xe Mecanum
Hình 6 : Sơ đồ vector khi chuyển động tiến
Hình 7 : Sơ đồ vector khi chuyển động lùi
Hình 8 : Sơ đồ vector khi chuyển động sang trái
Hình 9 : Sơ đồ vector khi chuyển động sang phải
Hình 10 :Sơ đồ vector mô tả các hướng di chuyển của robot omni bốn bánh
Hình 11 : Sơ đồ mạch điện tử điều khiển động cơ xe tự hành
Hình 12 : Sơ đồ khối mô hình điều khiển xe tự hành
Hình 13 : Mô hình robot omni 4 bánh
Hình 14 : Sơ đồ Simulink khi quỹ đạo đặt là quỹ đạo thẳng theo Backstepping
Hình 15 : Robot chạy bám quỹ đạo thẳng theo Backstepping
Hình 16 : Đồ thị sai lệch vị trí khi quỹ đạo đặt là quỹ đạo thẳng theo Backstepping
Hình 17 : Sơ đồ Simulink khi quỹ đạo đặt là quỹ đạo tròn theo Backstepping
Hình 18 : Robot bám quỹ đạo tròn theo Backstepping
Hình 19 : Sai lệch quỹ đạo khi quỹ đạo đặt là quỹ đạo thẳng theo Backstepping
Hình 20 : Sơ đồ Simulink khi quỹ đạo đặt là quỹ đạo thẳng theo DSC
Hình 21 : Robot chạy bám theo quỹ đạo thẳng theo thuật toán DSC
Hình 22 : Sai lệch quỹ đạo khi quỹ đạo đặt là quỹ đạo tròn theo thuật toán DSC
Hình 23 : Sơ đồ Simulink khi quỹ đạo đặt là quỹ đạo tròn theo DSC
Hình 24 : Robot bám quỹ đạo tròn theo DSC
Hình 25 : Sai lệch quỹ đạo khi quỹ đạo đặt là quỹ đạo tròn theo DSC
Hình 26 : Mô tả hướng di chuyển và vận tốc các bánh của robot omni 4 bánh
Hình 27 : Hệ tọa độ cục bộ của robot omni 4 bánh
Hình 28 : Giản đồ di chuyển của bánh 1

Hình 29 : Mô tả vận tốc các bánh, góc lệch và vận tốc góc của robot
Hình 30 : Chiếu thành phần vecto vận tốc tiếp tuyến với bánh xe lên mỗi trục tọa độ x
và y
Hình 31 : Mô tả vận tốc dài và góc lệch giữa các bánh xe so với khung tọa độ robot
Hình 32 : Mô tả lực kéo cùng chiều vận tốc và góc lệch giữa các bánh xe so với khung
tọa độ robot
Hình 33 : Momen và lực tác động lên robot
Hình 34 : Mô hình robot tự hành và chiều dương vận tốc góc,momen lực tác động lên
robot
Hình 35 : Mô tả robot Omni bốn bánh theo các tài liệu nước ngoài
Hình 36 : Mô tả vận tốc dài của bốn bánh xe - vận tốc dài và pháp tuyến của robot

Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

4


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC

ĐẶT VẤN ĐỀ

Robot tự hành (Mobile Robot) là một thành phần có vai trò quan trọng trong
ngành Robot học. Cũng với sự phát triển mạnh mẽ của các hệ thống Cơ - Điện tử,
robot tự hành ngày một được hoàn thiện và càng cho thấy lợi ích của nó trong công
nghiệp và sinh hoạt. Một vấn đề rất được quan tâm khi nghiên cứu về robot tự hành là
làm thế nào để robot có thể di chuyển tới một vị trí xác định mà có thể tựđộng tránh
được các chướng ngại vật trên đường đi. Theo lý thuyết, môi trường hoạt động của
robot tự hành có thể là bất kỳ địa hình nào . Robot được nghiên cứu ở đây là robot di
chuyển bằng bánh (wheeled).
Bài toán đặt ra ở đây là điều khiển vị trí robot bám theo quỹ đạo đặt trước, ta

thường chọn vector biến khớp là q=[x y θ]T trong đó x,y là tọa độ của robot trong hệ
tọa độ toàn cục, θ là hướng của robot .
Phụ thuộc vào hệ phương trình động lực học của robot là hệ có ràng buộc
(nonholonomic) hay không ràng buộc (holonomic) ta cũng chia robot tự hành thành
robot có ràng buộc và robot không ràng buộc.
Ví dụ về robot không ràng buộc chính là Omni Robot. Các biến khớp của robot
lúc này sẽ được điều khiển tự do, robot sẽ di chuyển hoàn toàn linh hoạt.
Trong Mobile Robot thông thường, các bánh xe được đặt dọc theo trục của robot
nhưng trong Omni robot các bánh xe được đặt theo chu vi của robot .

Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

5


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC
I. Cấu trúc xe tự hành (Omni Robot)

Hình 1 : Cấu trúc đơn giản của xe tự hành
1. Xe tự hành là gì ?
Robot tự hành được định nghĩa là một loại xe robot có khả năng tự dịch chuyển ,
tự vận động ( có thể lập trình được ) dưới sự điều khiển tự động để thực hiện thành
công công việc được giao.
2. Các bộ phận của xe
Bánh xe Omni
Bánh xe Omni là loại bánh có các bánh vệ tinh nhỏ đặt xung quanh chu vi bánh, 2
vòng bánh con sole với nhau và vuông góc với trục bánh . Nhờ đó bánh xe có thể di
chuyển dọc (do động cơ truyền động) và trượt theo chiều ngang (phụ thuộc vào
chuyển động của các bánh còn lại). Kết hợp hai chuyển động này (cộng vector ) thì
bánh Omni có thể di chuyển theo mọi hướng trong khi vẫn giữ được hướng của đầu

robot không đổi trong quá trình di chuyển ( do Robot sử dụng bánh omni có thể di
chuyển theo bất kỳ hướng nào,ở bất kỳ góc nào mà không cần quay trước khi di
chuyển ) . Đó là sự khác biết cơ bản nhất so với các loại bánh xe truyền thống hay
bánh xích dùng trong xe Tăng, khi đổi hướng thì phải quay đầu rồi đi theo hướng đó.

Hình 2 : Một số loại bánh xe omni
Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

6


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC
Vì có khả năng như vậy nên robot omni có khả năng tiết kiệm năng lượng so với robot
sử dụng bánh xe thông thường.
Để thực hiện khả năng di chuyển đó,robot omni phải có 3 bậc tự do trong mặt
phẳng di chuyển (mp xoy),gồm: di chuyển độc lập theo các trục x,y và xoay theo trục
z. Do đó cần tối thiểu 3 bánh omni với các động cơ điều khiển độc lập,vì mỗi động cơ
cho khả năng điều khiển một bậc tự do.

Hình 3 : Khả năng di chuyển của robot omni
Bánh xe Omni có thể sử dụng 3 hoặc 4 bánh tùy ý. Ngoài ra bánh Omni cho tốc
độ di chuyển đều các hướng cũng như cho kết quả tốt khi di chuyển theo hướng chéo
45o .
Bánh xe Mecanum về cơ bản cũng giống như bánh xe omni nhưng các bánh xe
vệ tinh được đặt chếch đi 45 độ. Có 2 loại bánh (trái, phải) khác nhau về hướng của
trục bánh xe vệ tinh. Robot cần cả 2 loại trái, phải để có thể di chuyển linh hoạt được.
Khi lắp lên đế robot các bánh sẽ được đặt thẳng như bánh bình thường (cần chú ý trục
các bánh vệ tinh hướng về tâm đế) .
Ưu điểm là khi di chuyển động cơ sẽ quay nhiều vòng hơn bánh xe... (giống
nguyên lý xích xe đạp), do trục động cơ truyền gián tiếp qua dây sin và buli nên khi

chạy bánh xe sẽ không tác động lực trực tiếp vào động cơ => Ít hại FET hơn so với
chuyển động đồng trục.Nhược điểm: Khi thiết kế khoảng cách giữa buli trung gian và
trục động cơ, trục bánh xe có thể bị chênh lệch, ko chính xác tuyệt đội giữa các bánh
xe, khi chạy các bánh xe sẽ ko đều nhau và giá thành đắt.

Hình 4 :Hình ảnh của một bánh xe Mecanum
Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

7


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC

Cách di chuyển của bánh xe omni

Hình 5 : Mô tả cách di chuyển của omni robot
Ta quy định động cơ và bánh xe (trong hình dùng kỹ thuật đồng trục, động cơ và
bánh xe quay trên cùng 1 trục) quay theo chiều mũi tên là chiều thuận kim đồng hồ.
+ Đi thẳng về trước: Bánh 1 và 4 quay theo chiều mũi tên, bánh 2 và 3 quay ngược
chiều mũi tên
+ Đi thẳng về sau: Bánh 2 và 3 quay theo chiều mũi tên, bánh 1 và 4 quay ngược chiều
mũi tên
+ Sàng qua bên trái: Bánh 2 và 4 quay theo chiều mũi tên, bánh 1 và 3 quay ngược
chiều mũi tên
+ Sàng qua bên phải: Bánh 1 và 3 quay theo chiều mũi tên, bánh 2 và 4 quay ngược
chiều mũi tên
+ Sàng chéo tới theo trục động cơ bánh số 1: Bánh 1 và 2 ghì (dừng), bánh 4 theo
chiều mùi tên, bánh 4 ngược chiều mũi tên
+ Sàng chéo tới theo trục động cơ bánh số 2: Bánh 1 và 2 ghì (dừng), bánh 3 theo
chiều mùi tên, bánh 4 ngược chiều mũi tên

+ Sàng chéo tới theo trục động cơ bánh số 3: Bánh 3 và 4 ghì (dừng), bánh 1 theo
chiều mùi tên, bánh 2 ngược chiều mũi tên
+ Sàng chéo tới theo trục động cơ bánh số 4: Bánh 3 và 4 ghì (dừng), bánh 2 theo
chiều mùi tên, bánh 1 ngược chiều mũi tên
+ Xoay vòng tròn tại chỗ: qua phải (bánh 1, 2, 3, 4 quay theo chiều mũi tên), qua trái
(bánh 1, 2, 3, 4 quay ngược chiều mũi tên).
Khớp động (hình): giúp cho khi di chuyển gặp phải bề mặt gồ gề (mặt sân lồi
lõm) thì 4 bánh xe vẫn có thể tiếp xúc được với mặt sân.
Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

8


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC
Sơ đồ vector biểu diễn chiều di chuyển của robot sử dụng bánh xe omni
Khi chuyển động tiến (forward) : Vector của hai bánh ở phía trước hay phía sau
được biểu diễn như sau:

Hình 6 : Sơ đồ vector khi chuyển động tiến
Khi chuyển động lùi ( back ) : Vector của hai bánh ở phía trước hay phía sau
được biểu diễn như sau:

Hình 7 : Sơ đồ vector khi chuyển động lùi
Khi chuyển động sang trái (left) : Vector của hai bánh ở phía trước hay phía sau
được biễu diển như sau:

Hình 8 : Sơ đồ vector khi chuyển động sang trái
Khi chuyển động sang phải ( right ):Vector của hai bánh ở phía trước hay phía
sau được biễu diễn như sau:

Hình 9 : Sơ đồ vector khi chuyển động sang phải
Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

9


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC
Khi chuyển động theo đường chéo xe thì hai bánh xe nằm trên đường chéo của
thân xe chuyển động cùng chiều hay ngược chiều.
Như vậy thiết kế xe tự hành sử dụng bánh xe Omni có thể chuyển động được 8
hướng khác nhau .

Hình 10 :Sơ đồ vector mô tả các hướng di chuyển của robot omni bốn bánh
Đặc biệt khi 4 bánh xe chuyển động cùng một hướng thì xe sẽ chuyển động tròn
theo hướng đó.
Thân xe
Thân xe ở đây là một giá đỡ kim loại được thiết kế để tối ưu không gian cho các
mạch điện tử và động cơ được gắn lên trên nó để điều khiển xe di chuyển. Sau đây là
một số chi tiết về các thành phần trên.
2.4.1. Cơ cấu chấp hành
Ở đây ta sử dụng động cơ Planet
- Tích hợp sẵn encoder.
- Điện áp hoạt động 24VDC.
- Có bộ giảm tốc để tăng momen tác dụng lên bánh xe.
2.4.2. Mạch điện tử
Bao gồm mạch công suất , cảm biến, khối nguồn và mạch điều khiển .
- Mạch công suất ở đây sử dụng mạch cầu MOSFET
- Khối nguồn sử dụng các acquy 24V
- Cảm biến sử dụng ở đây là cảm biến hướng và encoder tích hợp sẵn trong động
cơ

- Mạch điều khiển sử dụng chip Atmega 128L

Hình 11 : Sơ đồ mạch điện tử điều khiển động cơ xe tự hành
Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

10


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC
II. Mô hình hóa xe tự hành

Hình 12 : Sơ đồ khối mô hình điều khiển xe tự hành
Nhiệm vụ của bài toán điều khiển ở đây là xác định được momen đặt vào các
bánh xe omni theo thời gian để đưa ra tín hiệu điều khiển phù hợp cho bánh xe di
chuyển đúng theo quỹ đạo mong muốn của người điều khiển.
Đối với omni robot thì quỹ đạo của nó sẽ bao gồm tọa độ (x,y) theo mặt phẳng
của 4 bánh và góc θ là góc nhìn của robot.
1. Mô hình động học

Hình 13 : Mô hình robot omni 4 bánh
Bốn bánh xe được đặt lệch nhau 90o , khoảng cách từ bánh đến tâm xe là D Oxy
là hệ toàn cục , V là vận tốc thẳng của xe , Vn là vận tốc theo pháp tuyến của xe và 𝜔
là vận tốc góc của xe .
q= [𝑥 𝑦 𝜃]𝑇 là vector tọa độ và hướng tâm của xe trong hệ tọa độ toàn cục .
Chiếu các vector V,Vn lên hệ trục tọa độ toàn cục Oxy ta có
𝑥̇
𝑐𝑜𝑠𝜃 −𝑠𝑖𝑛𝜃 0 𝑉
𝑦̇
[ ]= [ 𝑠𝑖𝑛𝜃 𝑐𝑜𝑠𝜃 0] [𝑉𝑛]
0

0
1 𝜔
𝜃̇
𝑇
Mặt khác [𝑉 𝑉𝑛 𝜔] được tính theo vận tốc các bánh như sau :
Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

11


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC

𝑉
[𝑉𝑛] =
𝜔

𝑘

−𝑘

−𝑘

𝑘

2
𝑘

2
𝑘

2
−𝑘

2
−𝑘

2
1

2
1

2
1

4𝐷

4𝐷

[4𝐷

𝑉1
𝑘
𝑉2
−𝑘
[ ]=[
𝑉3
−𝑘
𝑉4
𝑘

Với k = cos45o =

√2
2

𝑘
𝑘
−𝑘
−𝑘

𝑉1
𝑉2
.[ ]
2
𝑉3
1
] 𝑉4

4𝐷

𝐷
𝑉
𝐷
] [𝑉𝑛]
𝐷
𝜔
𝐷

.

2. Mô hình động lực học
Phương trình động lực học của Omni robot dựa trên công thức Euler – Lagrange :
M(q) . 𝑞̈ + C(q,𝑞̇ ). 𝑞̇ + G(q) + τd = B(q) .M
Trong đó
+ q là vector biến khớp . q= [x y θ] T
+ τd là nhiễu bất định ( ≈ 0 )
+ M : momen đặt vào mỗi bánh xe .

𝑀1
𝑀2
M=[ ]
𝑀3
𝑀4

+ M(q) là ma trận hệ số quán tính và momen quán tính .
+ C(q,𝑞̇ ) là ma trận hệ số nhớt và hướng tâm.
+ G(q) là ma trận trọng lực .
+ B(q) là ma trận chuyển đổi đầu vào.
- Động năng của robot

1
1
1
K = mẋ2 + mẏ2 + J.𝜃̇2
2

2

2

+ Với m là khối lượng của xe ; J là momen quán tính của xe.
+ Thế năng của robot : P = 0
- Theo công thức Euler – Lagrange ta có L = K – P
1
1
1
 L = K = mẋ2 + mẏ2 + J.𝜃̇2
2

2

2

- Mặt khác Momen/lực đặt lên biến khớp q là



𝑑

(

𝜕𝐿

𝑑𝑡 𝜕 𝑞̇
𝑑 𝜕𝐿

)−

𝜕𝐿

𝜕𝑞
𝜕𝐿 𝑇

M(q) . 𝑞̈ + C(q,𝑞̇ ). 𝑞̇ + G(q) = ( ( ) − )
𝑑𝑡 𝜕 𝑞̇
𝜕𝑞
𝑚 0 0
M(q) = [ 0 𝑚 0] , C(q,𝑞̇ ) = G(q) = 0;
0 0 𝐽

Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

12


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC
𝐹𝑥
- Ta cũng có B(q) . M = [ 𝐹𝑦 ]
𝑀𝜃
Với Fx , Fy là tổng các lực theo phương Ox , Oy. 𝑀𝜃 là tổng momen quay của
robot .
- Gọi r là bán kính bánh xe ta sẽ có
𝜋

B(q) =

1
𝑟

3𝜋

3𝜋

𝜋

𝑐𝑜𝑠( 𝜃 + ) 𝑐𝑜𝑠(𝜃 + ) 𝑐𝑜𝑠(𝜃 − ) 𝑐𝑜𝑠( 𝜃 − )
4
4
4
4
[ 𝑠𝑖𝑛(𝜃 + 𝜋) 𝑠𝑖𝑛( 𝜃 + 3𝜋) 𝑠𝑖𝑛( 𝜃 − 3𝜋) 𝑠𝑖𝑛(𝜃 − 𝜋) ]
4
4
4
4
𝑅
𝑅
𝑅
𝑅

III. Thiết kế bộ điều khiển cho robot omni 4 bánh
1. Phương pháp Multiple Sliding Surface Control (MSS)
Lý thuyết chung
MSS là phương pháp điều khiển với các bước làm theo phương pháp thiết kế
cuốn chiếu(Backstepping).
- Xét hệ phi tuyến:
𝑥̇ 1 = 𝑥2 + 𝑓 (𝑥1 )
𝑥̇ 2 = 𝑢
(1.1)
𝜕𝑓

- Với f và
là hàm liên tục .
𝜕𝑥1

- Đầu tiên ta xây dựng mặt trượt thứ nhất : 𝑆1 = 𝑥1 − 𝑥1𝑑 .Đạo hàm 𝑆1 dựa vào hệ (1.1)
𝑆1̇ = 𝑥2 + 𝑓 − 𝑥̇ 1𝑑

(1.2)

- Tiếp theo ta xét mặt trượt thứ hai : 𝑆2 = 𝑥2 − 𝑥2𝑑
- Trong đó 𝑥2𝑑 gọi là đầu vào ảo được thiết kế để lái 𝑆1 → 0 .Đạo hàm 𝑆2 :
𝑆2̇ = 𝑢 − 𝑥̇ 2𝑑

(1.3)

- u sẽ được thiết kế để 𝑆2 → 0 ,tức là 𝑥2 → 𝑥2𝑑 , 𝑥2𝑑 sẽ được chọn như sau:
𝑥2𝑑 = 𝑥̇ 1𝑑 − 𝑓 − 𝐾1 𝑆1

(1.4)

- Hằng số dương 𝐾1 sẽ được xác định sau, u được chọn là :
𝑢 = 𝑥̇ 2𝑑 − 𝐾2 𝑆2
- Từ (1.4) và (1.5) ta có hệ sau:

- Chọn hàm Lyapunov 𝑉

=

𝑆1̇ = 𝑆2 − 𝐾1 𝑆1
𝑆2̇ = −𝐾2 𝑆2

𝑆12 +𝑆22
2

(1.5)

(1.6)

.Từ (1.6) ta tính đạo hàm của V :

Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

13


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC
𝑉̇ = 𝑆1 𝑆1̇ + 𝑆2 𝑆2̇ = −𝐾1 𝑆12 − 𝐾2 𝑆22 + 𝑆1 𝑆2
𝑆21 +𝑆22
- Sử dụng bất đẳng thức 𝑆1 𝑆2 ≤
ta có:
2

𝑉̇ ≤=
1

−𝐾1 𝑆12

−

𝐾2 𝑆22

𝑆21 + 𝑆22
+
2

- Chọn 𝐾1 = 𝐾2 = 𝐾 + với K >0 ta thu được:
2
𝑉̇ ≤ −2𝐾𝑉
Theo định lý Luapunov thì hệ ổn định .
Một nhược điểm của phương pháp này là sự xuất hiện của “nhiễu bất thường”
trong tính toán đầu vào ảo 𝑥2𝑑 dẫn tới tính toán đầu vào thực u đưa ra kết quả không
phù hợp.
Áp dụng cho đối tượng Omni robot
Xét hệ Omni Robot (*):
𝑐𝑜𝑠𝜃 −𝑠𝑖𝑛𝜃 0
𝑥̇ 1 = [ 𝑠𝑖𝑛𝜃 𝑐𝑜𝑠𝜃 0] 𝑥2 = 𝐵−1 𝑥2
0
0
1
𝜔𝑣𝑛
𝑥̇ 2 = 𝑢 + [−𝜔𝑣 ] = 𝑢 + 𝑓
(1.7)
0
𝑥
𝑣
- Với 𝑥1 = [𝑦 ] và 𝑥2 = [𝑣𝑛]
𝜔
𝜃
𝑆11
𝑐𝑜𝑠𝜃 𝑠𝑖𝑛𝜃 0 𝑥 − 𝑥𝑑

- Xét mặt trượt 𝑆1 = [𝑆12 ] = [−𝑠𝑖𝑛𝜃 𝑐𝑜𝑠𝜃 0] [𝑦 − 𝑦𝑑 ] = 𝐵(𝑥1 − 𝑥1𝑑 )
𝑆13
0
0
1 𝜃 − 𝜃𝑑
- Đạo hàm 𝑆1 kết hợp với hệ phương trình (1.7) ta có:
𝑆1̇ = 𝐵̇(𝑥1 − 𝑥1𝑑 ) + 𝐵𝐵−1 𝑥2 − 𝐵𝑥̇ 1𝑑
𝑣
−𝑠𝑖𝑛𝜃𝜔 𝑐𝑜𝑠𝜃𝜔 0 𝑥 − 𝑥𝑑
= [−𝑐𝑜𝑠𝜃𝜔 −𝑠𝑖𝑛𝜃𝜔 0] [𝑦 − 𝑦𝑑 ] + [𝑣𝑛]
𝜔
0
0
0 𝜃 − 𝜃𝑑
𝑐𝑜𝑠𝜃 𝑠𝑖𝑛𝜃 0 𝑥̇ 𝑑
− [−𝑠𝑖𝑛𝜃 𝑐𝑜𝑠𝜃 0] [𝑦̇ 𝑑 ]
0
0
1 𝜃̇𝑑
- Bỏ qua thành phần 𝐵̇(𝑥1 − 𝑥1𝑑 ) .
𝑐𝑜𝑠𝜃𝑥̇ 𝑑 + 𝑠𝑖𝑛𝜃𝑦̇ 𝑑 − 𝑘11 𝑆11
𝑐𝑜𝑠𝜃 𝑠𝑖𝑛𝜃 0 𝑥̇ 𝑑
- Chọn 𝑥2𝑑 = [−𝑠𝑖𝑛𝜃 𝑐𝑜𝑠𝜃 0] [𝑦̇ 𝑑 ] − 𝐾1 𝑆1 = [−𝑠𝑖𝑛𝜃𝑥̇ 𝑑 + 𝑐𝑜𝑠𝜃𝑦̇ 𝑑 − 𝑘12 𝑆12 ]
0
0
1 𝜃̇𝑑
𝜃̇𝑑 − 𝑘13 𝑆13
𝑆21
- Xét mặt trượt 𝑆2 = [𝑆22 ] = 𝑥2 − 𝑥2𝑑
𝑆23

- Đạo hàm 𝑆2 kết hợp với hệ phương trình (1.7) ta có:
𝑆2̇ = 𝑥̇ 2 − 𝑥̇ 2𝑑 = 𝑢 + 𝑓 − 𝑥̇ 2𝑑
Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

14


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC
- Chọn 𝑢 = 𝑥̇ 2𝑑 − 𝑓 − 𝐾2 𝑆2
2. Phương pháp DSC ( Dynamic Surface Control )
Lý thuyết chung
Để khắc phục nhược điểm đã nêu của phương pháp MSS, người ta sử dụng thêm
một bộ lọc thông thấp cho các đầu vào ảo.Theo như đã nêu ở phần trước cách làm này
chính là thuật toán điều khiển DSC.
Ta sẽ đặt 𝑥̅2 = 𝑥̇ 1𝑑 − 𝑓 − 𝐾1 𝑆1 và 𝑥2𝑑 được tính qua bộ lọc là khâu quán tính bậc nhất:
𝜏𝑥̇ 2𝑑 + 𝑥2𝑑 = 𝑥̅2
- Đặt 𝜉 = 𝑥2𝑑 − 𝑥̅2 ta có hệ phương trình sau:
𝑆1̇ = 𝑆2 + 𝜉 − 𝐾1 𝑆1
𝑆2̇ = −𝐾2 𝑆2
−𝜉
𝜉̇ =
+𝜂
𝜏
- Lúc này hàm Lyapunov được chọn là: 𝑉 =

(2.1)

𝑆12 +𝑆22 +𝜉 2
2

−𝜉
𝑉̇ = 𝑆1 𝑆1̇ + 𝑆2 𝑆2̇ + 𝜉𝜉̇ = 𝑆1 (𝑆2 + 𝜉 − 𝐾1 𝑆1 ) − 𝐾2 𝑆22 + 𝜉( + 𝜂) (2.2)
𝜏
- Sử dụng các bất đẳng thức :
𝑆 2 +𝑆 2

𝑆1 𝑆2 ≤ 1 2 ; 𝑆1 𝜉 ≤
2
- Từ (2.2) ta có:

𝑆12 +𝜉 2
2

; 𝜉𝜂 ≤

𝜂2 𝜉 2 +1
2

𝜉 2 2𝑆12 + 𝑆22 + 𝜉 2 𝜂 2 𝜉 2 + 1
𝑉̇ ≤
−
− +
+
𝜏
2
2
1
−1
1
𝑀2 2

2
2
)𝜉
= (−𝐾1 + 1)𝑆1 + (−𝐾2 + ) 𝑆2 + (
+ +
2
𝜏
2
2
𝜂2 𝜉 2 + 1
𝑀2 𝜉 2
+
−
2
2
1
−1 1 𝑀2 2 1
)𝜉 +
= (−𝐾1 + 1)𝑆12 + (−𝐾2 + ) 𝑆22 + (
+ +
2
𝜏
2
2
2
2
2 2
(𝜂 − 𝑀 )𝜉
+
2

1
1
1
𝑀2
- Chọn 𝐾1 = 1 + 𝐾; 𝐾2 = + 𝐾; = 𝐾 + +
2
𝜏
2
2
𝑆1
- Xét tập B chứa [𝑆2 ] thỏa mãn 𝑆12 + 𝑆22 + 𝜉 2 ≤ 2𝑝 với p>0 là tập kín và lồi.Như vậy
𝜉
luôn tìm được hằng số dương M để η ≤ M.
1
- Như vậy : 𝑉̇ ≤ −2𝐾𝑉 +
2
- Theo định lý Lyapunov thì hệ ổn định.
−𝐾1 𝑆12

𝐾2 𝑆22

Áp dụng cho đối tượng Omni robot
Tương tự với phương pháp MSS thì ta chỉ thay:
Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

15


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC

𝑐𝑜𝑠𝜃
[
𝑥̅2 = −𝑠𝑖𝑛𝜃
0

𝑠𝑖𝑛𝜃
𝑐𝑜𝑠𝜃
0

𝑐𝑜𝑠𝜃𝑥̇ 𝑑 + 𝑠𝑖𝑛𝜃𝑦̇ 𝑑 − 𝑘11 𝑆11
0 𝑥̇ 𝑑
0] [𝑦̇ 𝑑 ] − 𝐾1 𝑆1 = [−𝑠𝑖𝑛𝜃𝑥̇ 𝑑 + 𝑐𝑜𝑠𝜃𝑦̇ 𝑑 − 𝑘12 𝑆12 ]
1 𝜃̇𝑑
𝜃̇𝑑 − 𝑘13 𝑆13

- Và 𝑥̇ 2𝑑 được tính thông qua phương trình sau:
𝑥̇ 2𝑑 =

𝑥̅2 −𝑥2𝑑
𝜏

(2.3)

- Từ (2.3) ta tính toán u bởi công thức : 𝑢 = 𝑥̇ 2𝑑 − 𝑓 − 𝐾2 𝑆2
- Chú thích (*):
Từ phương trình động học :
𝑥̇
𝑐𝑜𝑠𝜃 −𝑠𝑖𝑛𝜃 0 𝑣
[𝑦̇ ] = [ 𝑠𝑖𝑛𝜃 𝑐𝑜𝑠𝜃 0] [𝑣𝑛]
0

0
1 𝜔
𝜃̇
Ta có:
𝑥̈ = 𝑐𝑜𝑠𝜃𝑣̇ − 𝑠𝑖𝑛𝜃𝑣𝑛̇ − 𝑠𝑖𝑛𝜃𝜃̇𝑣 − 𝑐𝑜𝑠𝜃𝜃̇ 𝑣𝑛
𝑦̈ = 𝑠𝑖𝑛𝜃𝑣̇ + 𝑐𝑜𝑠𝜃𝑣𝑛̇ + 𝑐𝑜𝑠𝜃𝜃̇𝑣 − 𝑠𝑖𝑛𝜃𝜃̇ 𝑣𝑛
𝜃̈ = 𝜔̇

Suy ra:
𝑐𝑜𝑠𝜃𝑥̈ + 𝑠𝑖𝑛𝜃𝑦̈ = 𝑣̇ − 𝜔𝑣𝑛
−𝑠𝑖𝑛𝜃𝑥̈ + 𝑐𝑜𝑠𝜃𝑦̈ = 𝑣𝑛̇ − 𝜔𝑣
- Từ phương trình động lực học:
𝑚 0 0 𝑥̈
[ 0 𝑚 0] [𝑦̈ ]
0 0 𝐽 𝜃̈
𝜏1
𝑜
𝑜
𝑜
𝑜
cos
(𝜃
−
135
)
cos
(𝜃
−
45
)

cos
(𝜃
+
45
)
cos
(𝜃
+
135
)
1
𝜏
= [ sin(𝜃 + 45𝑜 ) sin(𝜃 + 135𝑜 ) s in(𝜃 − 135𝑜 ) sin(𝜃 − 45𝑜 ) ] [𝜏2 ]
3
𝑟
𝐷
𝐷
𝐷 𝐷
𝜏4
- Ta cũng tính được:
𝑘
𝑐𝑜𝑠𝜃𝑥̈ + 𝑠𝑖𝑛𝜃𝑦̈ =
(𝜏 − 𝜏2 − 𝜏3 + 𝜏4 )
𝑚𝑟 1
𝑘
−𝑠𝑖𝑛𝜃𝑥̈ + 𝑐𝑜𝑠𝜃𝑦̈ =
(𝜏 + 𝜏2 − 𝜏3 − 𝜏4 )
𝑚𝑟 1
𝐷
𝜃̈ = (𝜏1 + 𝜏2 + 𝜏3 + 𝜏4 ) (**)

𝐽𝑟

- Đặt 𝜏 = 𝜏1 − 𝜏2 − 𝜏3 + 𝜏4 ; 𝜏𝑛 = 𝜏1 + 𝜏2 − 𝜏3 − 𝜏4 ; 𝜏𝜔 = 𝜏1 + 𝜏2 + 𝜏3 + 𝜏4
𝜏1
1
1 1 𝜏
𝜏2
1 −1
1 1] [ 𝜏 𝑛 ]
- Suy ra: [𝜏 ] = [
4
3
1 −1 1 𝜏
𝜏4
−1 −1 1 𝜔
- Hệ phương trình (**) trở thành:

Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

16


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC
𝑘
𝜏
𝑚𝑟
𝑘
𝑣𝑛̇ − 𝜔𝑣 =
𝜏
𝑚𝑟 𝑛

𝐷
𝜔̇ = 𝜏𝜔
𝐽𝑟
𝑣̇ − 𝜔𝑣𝑛 =

𝑘

𝑥
𝑣
- Ta xác đinh các biến trạng thái như sau: 𝑥1 = [𝑦] , 𝑥2 = [𝑣𝑛] và u=
𝜔
𝜃
- Từ đó ta có mô hình trạng thái của đối tượng Omni Robot như sau:
𝑐𝑜𝑠𝜃 −𝑠𝑖𝑛𝜃
𝑥̇ 1 = [ 𝑠𝑖𝑛𝜃 𝑐𝑜𝑠𝜃
0
0
𝜔𝑣𝑛
𝑥̇ 2 = 𝑢 + [−𝜔𝑣 ]
0

1
1
−1
−1

1 𝜏
1
1
1] [ 𝜏𝑛 ] = [−1

1 𝜏
4 1
𝜔
1
−1

𝜏

𝜏
𝑚𝑟 𝑛
𝐷
[ 𝐽𝑟 𝜏𝜔 ]

0
0] 𝑥2
1

Và:
𝜏1
1
1 −1
𝜏2
[𝜏 ] = [
3
4 1
𝜏4
−1

𝑚𝑟
𝑘

1
1
−1
−1

𝑚𝑟
1 𝑘
1] 0
1
1
[0

0
𝑚𝑟
𝑘
0

0
0 𝑢
𝐽𝑟
𝐷]

3. Mô phỏng trên Matlab ( Sử dụng công cụ S-function )
Theo Backstepping
3.1.1. Quỹ đạo đặt là quỹ đạo thẳng
[ x y θ ] T = [ 5t 3+5t atan(5/3) ] T

Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

17


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC

Hình 14 : Sơ đồ Simulink khi quỹ đạo đặt là quỹ đạo thẳng theo Backstepping

Hình 15 : Robot chạy bám quỹ đạo thẳng theo Backstepping

Hình 16 : Đồ thị sai lệch vị trí khi quỹ đạo đặt là quỹ đạo thẳng theo Backstepping
Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

18


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC

3.1.2. Quỹ đạo đặt là quỹ đạo tròn
𝜋

𝜋

𝜋

15

15

15

[x y θ] T = [10cos( 𝑡) 10sin( 𝑡)

𝜋

𝑡 + ]T
2

Hình 17 : Sơ đồ Simulink khi quỹ đạo đặt là quỹ đạo tròn theo Backstepping

Hình 18 : Robot bám quỹ đạo tròn theo Backstepping

Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

19


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC

Hình 19 : Sai lệch quỹ đạo khi quỹ đạo đặt là quỹ đạo thẳng theo Backstepping
Theo DSC
3.2.1. Quỹ đạo đặt là quỹ đạo thẳng
[ x y θ ]T = [ 5t 3+5t atan(5/3) ]T

Hình 20 : Sơ đồ Simulink khi quỹ đạo đặt là quỹ đạo thẳng theo DSC

Hình 21 : Robot chạy bám theo quỹ đạo thẳng theo thuật toán DSC
Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

20

Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC

Hình 22 : Sai lệch quỹ đạo khi quỹ đạo đặt là quỹ đạo tròn theo thuật toán DSC
3.2.2. Quỹ đạo đặt là quỹ đạo tròn

Hình 23 : Sơ đồ Simulink khi quỹ đạo đặt là quỹ đạo tròn theo DSC

Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

21


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC

Hình 24 : Robot bám quỹ đạo tròn theo DSC

Hình 25 : Sai lệch quỹ đạo khi quỹ đạo đặt là quỹ đạo tròn theo DSC
4. Code lập trình Sfunction
Khối tạo quỹ đạo tròn
function quy_dao_tron(block)
%MSFUNTMPL_BASIC A Template for a Level-2 MATLAB S-Function
% The MATLAB S-function is written as a MATLAB function with the
% same name as the S-function. Replace 'msfuntmpl_basic' with the
% name of your S-function.
%
% It should be noted that the MATLAB S-function is very similar
% to Level-2 C-Mex S-functions. You should be able to get more
% information for each of the block methods by referring to the
% documentation for C-Mex S-functions.

%
% Copyright 2003-2010 The MathWorks, Inc.
%%
%% The setup method is used to set up the basic attributes of the

Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

22


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC
%% S-function such as ports, parameters, etc. Do not add any other
%% calls to the main body of the function.
%%
setup(block);
%endfunction
%% Function: setup ===================================================
%% Abstract:
%% Set up the basic characteristics of the S-function block such as:
%% - Input ports
%% - Output ports
%% - Dialog parameters
%% - Options
%%
%% Required
: Yes
%% C-Mex counterpart: mdlInitializeSizes
%%
function setup(block)
% Register number of ports

block.NumInputPorts = 1;
block.NumOutputPorts = 2;
% Setup port properties to be inherited or dynamic
block.SetPreCompInpPortInfoToDynamic;
block.SetPreCompOutPortInfoToDynamic;
% Override input port properties
block.InputPort(1).Dimensions
= 1;
block.InputPort(1).DatatypeID = 0; % double
block.InputPort(1).Complexity = 'Real';
block.InputPort(1).DirectFeedthrough = true;
% Override output port properties
block.OutputPort(1).Dimensions
= 3;
block.OutputPort(1).DatatypeID = 0; % double
block.OutputPort(1).Complexity = 'Real';
block.OutputPort(2).Dimensions
= 3;
block.OutputPort(2).DatatypeID = 0; % double
block.OutputPort(2).Complexity = 'Real';

% Register parameters
block.NumDialogPrms

= 0;

% Register sample times
% [0 offset]
: Continuous sample time
% [positive_num offset] : Discrete sample time

Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

23


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC
%
% [-1, 0]
: Inherited sample time
% [-2, 0]
: Variable sample time
block.SampleTimes = [0.001 0];
% Specify the block simStateCompliance. The allowed values are:
% 'UnknownSimState', < The default setting; warn and assume DefaultSimState
% 'DefaultSimState', < Same sim state as a built-in block
% 'HasNoSimState', < No sim state
% 'CustomSimState', < Has GetSimState and SetSimState methods
% 'DisallowSimState' < Error out when saving or restoring the model sim state
block.SimStateCompliance = 'DefaultSimState';
%% ----------------------------------------------------------------%% The MATLAB S-function uses an internal registry for all
%% block methods. You should register all relevant methods
%% (optional and required) as illustrated below. You may choose
%% any suitable name for the methods and implement these methods
%% as local functions within the same file. See comments
%% provided for each function for more information.
%% ----------------------------------------------------------------%block.RegBlockMethod('SetinputPortSamplingMode', @SetinputPortSamplingMode);
block.RegBlockMethod('PostPropagationSetup', @DoPostPropSetup);
block.RegBlockMethod('InitializeConditions', @InitializeConditions);
block.RegBlockMethod('Start', @Start);

block.RegBlockMethod('Outputs', @Outputs); % Required
block.RegBlockMethod('Update', @Update);
block.RegBlockMethod('Derivatives', @Derivatives);
block.RegBlockMethod('Terminate', @Terminate); % Required
block.RegBlockMethod('SetInputPortSamplingMode', @SetInpPortFrameData);
%end setup
%%
%% PostPropagationSetup:
%% Functionality : Setup work areas and state variables. Can
%%
also register run-time methods here
%% Required
: No
%% C-Mex counterpart: mdlSetWorkWidths
%%
function SetInpPortFrameData(block, idx, fd)
block.InputPort(idx).SamplingMode = fd;
block.OutputPort(1).SamplingMode = fd;
block.OutputPort(2).SamplingMode = fd;
%endfunction
%function SetInpPortFrameData(block)

Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

24


Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC
%block.InputPort(1).SamplingMode = 'Sample';
%block.InputPort(1).SamplingMode = 'Sample';

%block.OutputPort(1).SamplingMode = 'Sample';
%block.OutputPort(2).SamplingMode = 'Sample';
%end SetInput
function DoPostPropSetup(block)
block.NumDworks = 0;

%%
%% InitializeConditions:
%% Functionality : Called at the start of simulation and if it is
%%
present in an enabled subsystem configured to reset
%%
states, it will be called when the enabled subsystem
%%
restarts execution to reset the states.
%% Required
: No
%% C-MEX counterpart: mdlInitializeConditions
%%
function InitializeConditions(block)
%end InitializeConditions

%%
%% Start:
%% Functionality : Called once at start of model execution. If you
%%
have states that should be initialized once, this
%%
is the place to do it.
%% Required

: No
%% C-MEX counterpart: mdlStart
%%
function Start(block)

%endfunction
%%
%% Outputs:
%% Functionality : Called to generate block outputs in
%%
simulation step
%% Required
: Yes
%% C-MEX counterpart: mdlOutputs
%%
function Outputs(block)
t= block.InputPort(1).Data;
x=10*cos(pi*t/15);
y=10*sin(pi*t/15);
tt = pi/2 + pi*t/15;
Ad = [x;y;tt];

Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56

25