Đều khiển phi tuyến hệ agv

BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC CÔNG NGHỆ TP.HCM
PHẠM QUỐC THIỆN
ĐIỀU KHIỂN PHI TUYẾN HỆ AGV
LUẬN VĂN THẠC SỸ
Chuyên ngành : Kỹ thuật Cơ – điện tử
Mã số ngành : 60520114
TP. HỒ CHÍ MINH, tháng 02 năm 2014

BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC CÔNG NGHỆ TP.HCM
PHẠM QUỐC THIỆN
ĐIỀU KHIỂN PHI TUYẾN HỆ AGV
LUẬN VĂN THẠC SỸ
Chuyên ngành : Kỹ thuật Cơ – điện tử
Mã số ngành : 60520114
HƯỚNG DẪN KHOA HỌC: TS. NGUYỄN THANH PHƯƠNG
TP. HỒ CHÍ MINH, tháng 02 năm 2014

CÔNG TRÌNH ĐƯỢC HOÀN THÀNH TẠI
TRƯỜNG ĐẠI HỌC CÔNG NGHỆ TP. HCM
Cán bộ hướng dẫn khoa học : TS. NGUYỄN THANH PHƯƠNG
Luận văn Thạc sĩ được bảo vệ tại Trường Đại học Kỹ thuật Công nghệ
TP. HCM ngày 25 tháng 01 năm 2014
Thành phần Hội đồng đánh giá Luận văn Thạc sĩ gồm:
(Ghi rõ họ, tên, học hàm, học vị của Hội đồng chấm bảo vệ Luận văn Thạc sĩ)
TT Họ và tên Chức danh hội đồng
1 PGS.TS. Nguyễn Tấn Tiến Chủ tịch
2 TS. Nguyễn Quốc Hưng Phản biện 1
3 TS. Nguyễn Hùng Phản biện 2
4 TS. Võ Hoàng Duy Ủy viên
5 TS. Ngô Cao Cường Ủy viên, thư ký
Xác nhận của Chủ tịch Hội đồng đánh giá Luận sau khi Luận văn đã được
sửa chữa (nếu có).
Chủ tịch Hội đồng đánh giá LV

TRƯỜNG ĐH CÔNG NGHỆ TP. HCM
PHÒNG QLKH – ĐTSĐH
CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM
Độc lập - Tự do - Hạnh phúc
TP. HCM , ngày..… tháng ….. năm …….
NHIỆM VỤ LUẬN VĂN THẠC SĨ
Họ tên học viên: PHẠM QUỐC THIỆN Giới tính: Nam
Ngày, tháng, năm sinh: 22/2/1988 Nơi sinh: ĐỒNG NAI
Chuyên ngành: Kỹ thuật Cơ điện tử MSHV: 1241840017
I- TÊN ĐỀ TÀI:
ĐIỀU KHIỂN PHI TUYẾN HỆ AGV.
II- NHIỆM VỤ VÀ NỘI DUNG:
Nghiên cứu hệ thống AGV, mô hình hóa hệ thống AGV, thiết lập bộ điều khiển phi
tuyến trên cơ sở ổn định của Lyapunov.
Chương 1: Mở đầu.
Chương 2: Tổng quan về AGV.
Chương 3: Mô hình toán học AGV.
Chương 4: Thiết kế bộ điều khiển và hệ thống đo lường.
Chương 5: Thiết kế, thi công AGV và mô phỏng hệ thống trên Matlab.
III- NGÀY GIAO NHIỆM VỤ:
IV- NGÀY HOÀN THÀNH NHIỆM VỤ:
V- CÁN BỘ HƯỚNG DẪN: TS. NGUYỄN THANH PHƯƠNG.
CÁN BỘ HƯỚNG DẪN KHOA QUẢN LÝ CHUYÊN NGÀNH
(Họ tên và chữ ký) (Họ tên và chữ ký)

i
LỜI CAM ĐOAN
Tôi xin cam đoan đây là công trình nghiên cứu của riêng tôi. Các số liệu, kết
quả nêu trong Luận văn là trung thực và chưa từng được ai công bố trong bất kỳ
công trình nào khác.
Tôi xin cam đoan rằng mọi sự giúp đỡ cho việc thực hiện Luận văn này đã
được cảm ơn và các thông tin trích dẫn trong Luận văn đã được chỉ rõ nguồn gốc.
Học viên thực hiện Luận văn
(Ký và ghi rõ họ tên)
Phạm Quốc Thiện

ii
LỜI CÁM ƠN
Sau thời gian học tập và nghiên cứu tại trường, nay tôi đã hoàn thành đề tài
tốt nghiệp cao học của mình. Để có được thành quả này, tôi đã nhận được rất nhiều
sự hỗ trợ và giúp đỡ tận tình từ thầy cô, gia đình và bạn bè.
Tôi xin chân thành cảm ơn quý Thầy cô khoa Quản Lý Khoa Học - Đào Tạo
Sau Đại Học, quý Thầy cô khoa Cơ - Điện - Điện Tử Trường Đại Học Kỹ Thuật
Công Nghệ TP.HCM đã tận tình giúp đỡ, hỗ trợ tôi trong suốt quá trình thực hiện
luận văn.
Với lòng tri ân sâu sắc, tôi muốn nói lời cám ơn đến Thầy TS. Nguyễn Thanh
Phương, người đã nhiệt tình hướng dẫn và chỉ bảo cho tôi trong suốt thời gian thực
hiện nghiên cứu này.
Tp. Hồ Chí Minh, tháng 02 năm 2014
Người thực hiện luận văn
Phạm Quốc Thiện

iii
TÓM TẮT LUẬN VĂN
Trong bài báo này, một bộ điều khiển phi tuyến dựa trên phương pháp Lyapunov
được đề xuất và áp dụng cho xe tự hành. Đầu tiên quỹ đạo − (G3
) với đa
thức bậc 7 được thiết kế. Thứ hai, dựa vào vị trí được ước lượng của xe tự hành, bộ
điều khiển này làm cho xe tự hành bám theo quỹ đạo G3
và di chuyển với vận vận
tốc không đổi. Sự ổn định của hệ thống được chứng minh bằng phương pháp
Lyapunov. Mô phỏng và kết quả thực nghiệm được trình bày để chứng minh tính
hiệu quả của bộ điều khiển được đề xuất.

iv
ABSTRACT
In this paper, a nonlinear controller base on Lyapunov method is proposed and
applied for wheel mobile robot (WMR). First, trajectory − (G3
) with 7th
order polinom is designed for WMR. Then, based on the estimated position of the
WMR, this controller makes WMR follow trajectory G3
which is moving with
desired constant velocity. The stability of system is proved by the Lyapunov
stability theory. The simulations and experimental results are shown to prove the
effectiveness of the proposed controller.

v
MỤC LỤC
Tên đề mục Trang
Lời cam đoan ...........................................................................................................i
Lời cảm ơn ..............................................................................................................ii
Tóm tắt luận văn .....................................................................................................iii
Abstract ...................................................................................................................iv
Mục lục ...................................................................................................................v
Danh mục các từ viết tắt ........................................................................................vii
Danh mục các bảng biểu ........................................................................................viii
Danh mục các sơ đồ, hình ảnh................................................................................ix
Chương 1: Mở đầu ................................................................................................1
1.1 Đặt vấn đề ....................................................................................................1
1.2 Tính cấp thiết của đề tài ..............................................................................1
1.3 Mục tiêu đề tài .............................................................................................2
1.4 Nội dung nghiên cứu ...................................................................................2
1.5 Phương pháp luận ........................................................................................2
1.6 Phương pháp nghiên cứu .............................................................................3
1.7 Nội dung luận văn ........................................................................................3
Chương 2: Tổng quan về AGV ............................................................................4
2.1 Sơ lược quá trình phát triển của AGV ...................................................4
2.2 Phân loại AGV.......................................................................................5
2.2.1 Tàu không người lái ........................................................................5
2.2.2 Xe nâng pallet .................................................................................6
2.2.3 Xe chở hàng ...................................................................................7
2.3 Tóm tắt các công trình nghiên cứu ........................................................8
2.4 Nhận xét và hướng tiếp cận ..................................................................32
Chương 3: Mô hình toán học của AGV .............................................................33
3.1 Cấu trúc AGV .............................................................................................33

vi
3.2 Xây dựng phương trình động học ...............................................................34
3.3 Xây dựng quỹ đạo đường đi cho AGV ......................................................36
Chương 4: Thiết kế bộ điều khiển và hệ thống đo lường .................................39
4.1 Cơ sở lý thuyết ............................................................................................39
4.1.1 Giới thiệu phương pháp Lyapunov ................................................39
4.1.2 Điểm cân bằng của hệ phi tuyến ....................................................39
4.1.3 Ổn định tại điểm cân bằng .............................................................41
4.1.3.1 Định nghĩa ............................................................................41
4.1.3.2 Ổn định và ổn định tiệm cận Lyapunov .................................41
4.1.4 Phương pháp tuyến tính hóa Lyapunov...........................................42
4.1.5 Phương pháp trực tiếp Lyapunov ...................................................44
4.1.5.1 Định lý ổn định Lyapunov .....................................................44
4.1.5.2 Định lý không ổn định Lyapunov ..........................................45
4.2 Thiết kế bộ điều khiển ................................................................................47
4.3 Hệ thống đo lường ......................................................................................48
Chương 5: Thiết kế thi công AGV và mô phỏng ..............................................51
5.1 Thiết kế mô hình .........................................................................................51
5.2 Thiết kế mạch điện .....................................................................................53
5.2.1 Phương án thiết kế ...............................................................................53
5.2.2 Sơ đồ nguyên lý ...................................................................................54
5.2.3 Thiết bị sử dụng ...................................................................................57
5.3 Kết quả mô phỏng và thực nghiệm .............................................................60
5.3.1 Kết quả mô phỏng trường hợp 1...........................................................61
5.3.2 Kết quả mô phỏng trường hợp 2 ..........................................................63
5.4 Kết luận và hướng phát triển ......................................................................65
TÀI LIỆU THAM KHẢO
PHỤ LỤC

vii
DANH MỤC CÁC TỪ VIẾT TẮT
AGV Automated Guided Vehicles
GPS Global Positioning System
WMR Wheel Mobile Robot
RF Radio Frequency
FL Feedback Linearization
SMC Sliding Mode Control
FSMC Fuzzy Sliding Mode Control
WP Way Point
cm centimeter
s second
DC Direct Current
RPM Revolutions Per Minute

viii
DANH MỤC CÁC BẢNG BIỂU
Trang
Bảng 2.1 Luật điều khiển mờ...........................................................................19
Bảng 2.2 Giá trị thông số của AGV .................................................................27
Bảng 2.3 Giá trị khởi tạo ban đầu ...................................................................27
Bảng 5.1 Thông số mô phỏng ................................................................................60

ix
DANH MỤC CÁC LƯU ĐỒ, HÌNH ẢNH
Trang
Hình 2.1 AGV dạng tàu không người lái 6
Hình 2.2 AGV dạng xe nâng pallet 7
Hình 2.3 AGV dạng xe chở hàng 8
Hình 2.4 Phương pháp tính toán vị trí 9
Hình 2.5 Môi trường thực nghiệm 10
Hình 2.6 Kết quả khi sử dụng bộ lọc Kalman 10
Hình 2.7 Dữ liệu vị trí 11
Hình 2.8 Sơ đồ khối của vòng điều khiển robot di động
bám theo quỹ đạo G3
12
Hình 2.9 Quỹ đạo mong muốn và quỹ đạo thực của robot 12
Hình 2.10 Robot di động hai bánh 13
Hình 2.11 Vị trí thật và mục tiêu của robot 13
Hình 2.12 Đáp ứng vận tốc của bánh trái và phải 14
Hình 2.13 Sai số quỹ đạo , , 14
Hình 2.14 Sơ đồ khối vòng điều khiển mobile robot 15
Hình 2.15 Điều khiển vòng kín với = 0.6 , = 2
để mô phỏng robot. Quỹ đạo robot tín hiệu
ngõ ra với nhiễu và tín hiệu ngõ vào 16
Hình 2.16 Điều khiển vòng kín với = 0.6 , = 2
để mô phỏng robot. Quỹ đạo robot tín hiệu
ngõ ra với nhiễu và tín hiệu ngõ vào 17
Hình 2.17 Sơ đồ khối SMC 18
Hình 2.18 Sơ đồ khối FSMC 19
Hình 2.19 Hàm membership của ngõ vào-ngõ ra , ̇, 20
Hình 2.20 WMR bám theo quỹ đạo tham chiếu 20

x
Hình 2.21 Sai số ( − ) 21
Hình 2.22 Sai số ( − ) 21
Hình 2.23 Tín hiệu điều khiển torque cho bánh phải 22
Hình 2.24 Sai số góc định hướng − 22
Hình 2.25 WMR bám theo quỹ đạo tham chiếu: (a) FL;
(b) đường chấm chấm-SMC, đường liền-FSMC 23
Hình 2.26 Sai số ( − ): (a) FL;
Hình 2.27 Sai số ( − ): (a) FL;
Hình 2.28 Sai số góc định hướng − : (a) FL;
Hình 2.29 Tín hiệu điều khiển torque cho bánh phải: (a) FL;
Hình 2.30 Lưu đồ giải thuật điều khiển tuyến tính hồi tiếp 26
Hình 2.31 Quỹ đạo mong muốn của AGV
có dạng đường thẳng ( y = x ) 26
Hình 2.32 Quỹ đạo của AGV ở thời gian ban đầu 28
Hình 2.33 Sai lệch vị trí trong toàn thời gian 28
Hình 2.34 Vận tốc tuyến tính của AGV trong toàn thời gian 29
Hình 2.35 Vận tốc góc của bánh phải và bánh trái
đối với quỹ đạo là đường thẳng 29
Hình 2.36 Véc tơ điều khiển đầu vào 30
Hình 2.37 Véc tơ điều khiển đầu vào u 30
Hình 2.38 Véc tơ điều khiển đầu vào mới 31
Hình 3.1 Mô hình động học của WMR 33
Hình 3.2 Khái niệm về AGV bám theo quỹ đạo tham chiếu 36
Hình 3.3 Quỹ đạo G3 đi qua hai điểm A và B 38
Hình 4.1 Sơ đồ khối của vòng điều khiển AGV bám quỹ đạo G3 49

xi
Hình 5.1 Bánh xe dùng cho động cơ có bộ giảm tốc đường kính 100 mm 51
Hình 5.2 Động cơ DC Tsukasa TG-35F-AG-18-A27 51
Hình 5.3 Bánh xe omni nhôm 52
Hình 5.4 Mô hình AGV mô phỏng 52
Hình 5.5 Cấu hình hệ thống điều khiển 53
Hình 5.6 Mạch nguồn 5 VDC 54
Hình 5.7 Mạch vi điều khiển Pic 18f4550 54
Hình 5.8 Khối giao tiếp I2C và RS232 55
Hình 5.9 Khối hiển thị 55
Hình 5.10 Mạch vi điều khiển Pic 18f4431 56
Hình 5.11 Mạch kết nối encoder 56
Hình 5.12 Pic 18F4550 và 18F4431 57
Hình 5.13 HM-TR/TTL/433 57
Hình 5.14 Mạch cầu H đôi 58
Hình 5.15 Pin 18650 58
Hình 5.16 AGV được thi công 59
Hình 5.17 AGV bám theo quỹ đạo G3
trường hợp 1 61
Hình 5.18 Sai số tiếp tuyến và sai số định hướng trường hợp 1 61
Hình 5.19 Vận tốc góc bánh phải và bánh trái trường hợp 1 62
Hình 5.20 Kết quả đo giá trị ước lượng của AGV trường hợp 1 62
Hình 5.21 e1 được ước lượng trường hợp 1 63
trường hợp 2 65
Hình 5.25 Vec-tơ sai số trường hợp 2 65
Hình 5.26 Vận tốc góc bánh phải và bánh trái trường hợp 2 66
Hình 5.27 Kết quả đo giá trị ước lượng của AGV trường hợp 2 66

xii

1
Chương 1
MỞ ĐẦU
1.1 Đặt vấn đề
Một hệ thống được gọi là thông minh và tự động khi nó có thể cảm nhận và
tương tác với môi trường bên ngoài hoặc có thể tích hợp những ứng dụng phục vụ nhu
cầu cuộc sống của con người hằng ngày, những yếu tố đó đã thúc đẩy sự phát triển
trong các lĩnh vực về trí thông minh nhân tạo, lý thuyết điều khiển, xe tự hành, robot
v.v. Việc nghiên cứu này rất phức tạp, các hệ thống tự động đang dần dần trở thành
một phần cuộc sống của chúng ta. Trong thời gian gần đây có những cải tiến của hệ
thống robot về ứng dụng khác nhau như trong nhà và ngoài trời, từ các hệ thống giao
vận tải, giao thông công cộng, hệ thống an ninh để ứng dụng cho quân sự v.v.
Đất nước ta đang trong giai đoạn công nghiệp hóa và hiện đại hóa, mở cửa hợp
tác với các nhà đầu tư nước ngoài để xây dựng như khu công nghiệp phát triển như
hiện nay, trong đó vấn đề về tự động hóa dây chuyền sản xuất có vai trò quan trọng.
Đối với lĩnh vực sản xuất kinh doanh, các nhà đầu tư luôn phải giải các bài toán về
giảm chi phí đầu vào, tăng năng suất, tăng lợi nhuận, chất lượng sản phẩm tốt, giảm
sức lao động con người.
Việc vận chuyển hàng hóa vào kho mà bất cứ dây chuyền sản xuất nào cũng phải
có thì sự xuất hiện của xe forklift không người lái (Automated Guided Vehicle) hay
còn gọi là AGV vào những năm 50 đã giúp các nhà sản xuất tiết kiệm thời gian, tiền
bạc, và sức lao động. Ngày nay nền công nghiệp sản xuất AGV phát triển mạnh mẽ,
các nhà sản xuất có thể lựa chọn những loại AGV phù hợp với công ty của họ như
AGV vận chuyển đồ uống, giấy, lon, lắp ráp ô tô, thực phẩm v.v.
1.2 Tính cấp thiết của đề tài
AGV là một thay thế hiệu quả cho forklift vì nó hoạt động liên tục, tăng độ tin
cậy. Theo ước tính ở các nước Tây Âu và các nước có nền kinh tế phát triển, số tiền
phải trả cho người điều khiển forklift là 40.000 USD/năm cộng với chi phí bảo trì xe

2
thì con số thật không nhỏ, AGV sẽ là hướng giải quyết số chi phí này. Một điểm đáng
lưu ý của các sản xuất ngoài hiệu quả công việc đó là an toàn, AGV sẽ thay thế xe
folklift thông thường, nó giúp người lao động tránh được tai nạn trong vận chuyển
hàng hóa và trong môi trường độc hại.
Vì vậy được sự hướng dẫn của thầy TS.Nguyễn Thanh Phương, luận văn này
nghiên cứu đề tài: “Điều khiển phi tuyến hệ AGV“ dựa trên mô hình mobile robot, ứng
dụng matlab để kiểm chứng và xây dựng mô hình thực nghiệm.
1.3 Mục tiêu của đề tài
- Mục tiêu trước mắt: chủ yếu nghiên cứu để thiết kế AGV dựa trên mô hình mobile
robot, bám theo quỹ đạo đường đi được định trước để tiếp cận mục tiêu.
- Mục tiêu lâu dài: hoàn thiện AGV có thể làm việc cùng lúc trong không gian lớn
với nhiều AGV khác, chúng sẽ được lập trình để nhường đường theo mức độ ưu tiên
để tránh tình trạng va chạm vào nhau. Tất cả những xe AGV này sẽ được quản lý qua
máy tính.
1.4 Nội dung nghiên cứu
- Thiết kế bộ điều khiển phi tuyến trên cơ sở ổn định của Lyapunov.
- Xây dựng mô hình thực nghiệm.
- Sử dụng phương pháp ước lượng để xác định vị trí của AGV.
- Thiết kế bộ điều khiển vật lý để điều khiển AGV theo quỹ đạo.
1.5 Phương pháp luận
- Luận văn là tài liệu tham khảo có giá trị cho những ai quan tâm, nghiên cứu về
AGV.
- Tìm hiểu phương pháp ước lượng để xác định vị trí.
- Tìm hiểu thiết kế quỹ đạo − với đa thức bậc 7.
- Tìm hiểu thiết kế bộ điều khiển phi tuyến trên cơ sở ổn định của Lyapunov.
- Luận văn này cung cấp một mô hình điều khiển AGV có thể ứng dụng trong thực tế.

3
1.6 Phương pháp nghiên cứu
- Thu thập và đọc hiểu các tài liệu liên quan từ cán bộ hướng dẫn, sách, các bài báo
và internet v.v.
- Nghiên cứu mô hình động học của WMR.
- Nghiên cứu lý thuyết ổn định Lyapunov.
- Nghiên cứu phần mềm Matlab.
- Lập trình mô phỏng trên Matlab, lập trình phần mềm vi điều khiển cho hệ thống
AGV.
1.7 Nội dung luận văn
Nội dung luận văn gồm 5 chương:
Chương 1: Mở đầu.
Chương 2: Tổng quan về AGV.
Chương 3: Mô hình toán học AGV.
Chương 4: Thiết kế bộ điều khiển và hệ thống đo lường.
Chương 5: Thiết kế thi công AGV và mô phỏng hệ thống trên Matlab.

4
Chương 2
TỔNG QUAN VỀ AGV
2.1 Sơ lược quá trình phát triển của AGV
AGV (viết tắt của Automated Guided Vehicles) là xe tự hành di chuyển theo những
con đường được xác định, dựa vào tín hiệu từ dây được bố trí dưới sàn nhà hoặc từ
cảm biến thị giác kết hợp với các thuật toán chuyển động để xử lý. Trong công nghiệp,
AGV là xe tải không người lái, thường được trang bị động cơ điện và ắc quy.
AGV đầu tiên được đưa ra thị trường vào những năm 1950 bởi Barrett, và vào
thời điểm đó nó chỉ đơn giản là một chiếc xe tải kéo một toa hàng hóa trong nhà kho,
sử dụng một dây trên cao để định hướng di chuyển.
Công nghệ AGV bắt đầu trở lại vào 1962 khi Goran Netzler và Anders Dahlgren
sáng lập NDC Sweden, chủ yếu tập trung vào các công nghệ mới về thiết kế và tiêu
chuẩn hóa các giải pháp mô-đun điện công nghiệp và bộ phận điều khiển để đáp ứng
nhu cầu bảo trì hiệu quả và dễ dàng trong việc giao tiếp giữa các người vận hành và hệ
thống. Sự ra đời của công nghệ AGV có thể được công nhận cho những nỗ lực tiên
phong của Goran Netzler và Anders Dahlgren đã mang đến cho ngành công nghiệp
vận chyển hàng hóa.
Sự phát triển lớn đầu tiên cho ngành công nghiệp AGV là sự ra đời của chiếc xe
nâng vào 1970, chúng được chấp nhận rộng rãi vì có khả năng liên kết trong việc kiểm
soát hệ thống thông tin. Cũng cùng năm đó công nghệ định hướng ra đời, một dây
được đặt dưới sàn nhà phát ra tần số và một bộ biến tần cho dây để định hướng cho
AGV theo tuyến đường dự kiến. Một ăng-ten trên AGV sẽ tìm ra các tần số và định
hướng chiếc xe dựa trên tín hiệu phát ra của dây. Công nghệ này cần nhiều dây trong
sàn để xử lý các nút giao thông hoặc điểm quyết định khác. Hệ thống cung cấp năng
lượng cho dây sẽ tương ứng với hướng đi mong muốn.

5
Vào cuối những 1980, định hướng không dây cho các hệ thống AGV đã được
giới thiệu. Sử dụng laser cho phép tăng tính linh hoạt và chính xác của hệ thống trong
việc di chuyển theo đường đi mong muốn.
Như với tất cả các sản phẩm công nghệ cao dựa trên phần mềm máy tính và điện
tử. Các máy tính được sử dụng trong các hệ thống AGV có thể lưu trữ thông tin, đưa
ra quyết định và thực hiện quy trình. Trong thực tế, AGV có khả năng thực hiện gần
như tất cả các quyết định và các chức năng điều khiển. Chúng có thể sắp xếp thời gian,
giữ hàng tồn kho, quản lý chi tiết hệ thống và kiểm soát nhiều loại hệ thống máy khi
hoạt động chung. Ngày nay AGV được sử dụng rộng rãi ở hầy hết các ngành công
nghiệp lớn như hàng không, may mặc, sách và hệ thống thư viện, y tế, nhà kho, các cơ
sở lưu trữ hàng hóa v.v.
Nhu cầu thị trường cho AGV có thể được đánh giá thông qua số lượng các nhà
sản xuất AGV. Vào cuối những năm 1970 có 6 nhà cung cấp AGV ở Hoa Kỳ và chỉ có
3 loại xe khác nhau. Năm 1990 đã có hơn 40 nhà cung cấp trên toàn thế giới và hơn 15
loại xe với trọng tâm là tiêu chuẩn thiết kế. Công nghệ phát triển sẽ thúc đẩy sự gia
tăng trong việc sử dụng AGV do đó sẽ thúc đẩy việc nghiên cứu và phát triển AGV.
2.2 Phân loại AGV
Có ba dạng AGV chính:
- Tàu không người lái.
- Xe nâng pallet.
- Xe chở hàng.
2.2.1 Tàu không người lái
- Gồm một đầu máy kéo, kéo một hoặc nhiều toa xe phía sau tạo thành một đoàn xe
giống như một con tàu.
- Loại xe này được dùng trong việc chuyên chở những hàng hóa có tải trọng nặng
trong không gian nhà ko hoặc nhà máy lớn.
- Có 5-10 toa xe phía sau và là hệ thống vận chuyển hiệu quả.
- Đầu máy kéo có thể kéo được 60,000 pound.

6
Hình 2.1 AGV dạng tàu không người lái.
Source: http://www.jbtc-agv.com/en/Solutions/Products/Tugger-Automatic-Guided-
Vehicles-AGVs.
2.2.2 Xe nâng pallet
- Dùng để vận chuyển các pallet hàng hóa theo tuyến đường xác định trước.
- Nâng được hàng ngàn kilogam, một số loại AGV có thể nâng hai pallet cùng lúc.
- Loại xe này được dùng trong chuyển động thẳng đứng thường là sắp xếp pallet hàng
hóa lên kệ hàng.

7
Hình 2.2 AGV dạng xe nâng pallet.
Source: http://www.egemin-automation.com/en/automation/material-handling-
automation_ha-solutions_agv-systems_agv-types/forklift-agv
2.2.3 Xe chở hàng
- Dùng để vận chuyển hàng hóa từ trạm này đến trạm khác bằng cách đặt hàng hóa
lên xe.
- Có thể vận chuyển được tối đa 250 kg.
- Loại xe này được thiết kế đặc biệt để vận chuyển hàng hóa với tải trọng nhỏ.

8
Hình 2.3 AGV dạng xe chở hàng.
Source: http://www.jbtc-agv.com/en/Solutions/Products/Unit-Load-Automatic-
Guided-Vehicles-AGVs.
2.3 Tóm tắt các công trình nghiên cứu
[1] Trong bài báo này nhóm tác giả đã giới thiệu một hệ thống GPS indoor đơn giản,
sử dụng cảm biến siêu âm. Thông thường hệ thống siêu âm cần ít nhất ba bộ phát,
trong đó thời gian cập nhật thông tin vị trí là 5 cycles/s để cho sóng siêu âm từ ba bộ
phát không trùng nhau. Hệ thống này bao gồm một bộ phát và hai bộ thu, trong đó bộ
phát dùng để phát sóng siêu âm và RF, bộ thu dùng tính khoảng cách tương ứng với
tham chiếu đến các tín hiệu RF. Hai giá trị được dùng để xác định vị trí của bộ phát
bằng các công thức lượng giác.

9
Hình 2.4 Phương pháp tính toán vị trí.
, là khoảng cách đo được. Ta có thể tính , :
= − (2.1)
= − (2.2)
= + (2.3)
= ( − ) + (2.4)
Từ (2.3) và (2.4) ta hoàn toàn xác định được ( Xr , Yr ).
Do đặc điểm của cảm biến siêu âm, ảnh hưởng của nhiệt độ hoặc chướng ngại
vật xung quanh sẽ sinh ra nhiễu trên cảm biến. Vì thế sai số vị trí được giảm thiểu
bằng cách sử dụng bộ lọc Kalman tuyến tính. Để chứng minh tính hiệu quả của hệ
thống này, các thí nghiệm đã được tiến hành trong một không gian kích thước tương
bằng 3.5 m * 2.2 m, trong đó sai số vị trí tối đa 2 cm.

10
Hình 2.5 Môi trường thực nghiệm.
Hình 2.6 Kết quả khi sử dụng bộ lọc Kalman.

11
Hình 2.7 Dữ liệu vị trí.
Tuy nhiên nghiên cứu này cần có những cải thiện như sau:
- Cảm biến nên phát triển để nhận biết vị trí ở những không gian lớn hơn vì lúc đó ta sẽ
xét thêm góc của chùm tia song siêu âm.
- Khi đặt bộ nhận tại nhiều nơi, một mạng lưới cảm biến là cần thiết để tính toán
khoảng cách giữa các cảm biến.
[2] Bài báo này đề xuất giải pháp thiết kế quỹ đạo và điều khiển tối ưu xe tự hành cho
bài toán vận chuyển kệ hàng (pallets) trong kho. Giải pháp bao gồm thuật toán thiết kế
quỹ đạo − với đa thức bậc 7 kết hợp với thuật điều khiển tối ưu theo chuẩn
toàn phương để xác định tốc độ góc cần thiết cho robot bám quỹ đạo. Ngoài ra việc định
vị xe tự hành từ các tín hiệu đo can nhiễu được xác định bằng phương pháp lọc kalman
mở rộng. Các thử nghiệm trên robot tự hành Pioneer – 3DX trong phòng thí nghiệm cho
kết quả khả quan.

12
Hình 2.8 Sơ đồ khối của vòng điều khiển robot di động bám theo quỹ đạo G3
.
Hình 2.9 Quỹ đạo mong muốn và quỹ đạo thực của robot.
Những kết quả trên nhóm tác giả chỉ dùng thiết bị đo duy nhất là encoder ( đo
tương đối ) để tính vận tốc tức thời và dự đoán tọa độ, hướng robot mà chưa sử dụng
cảm biến ngoài như camera để đo vị trí tuyệt đối của robot trong không gian hoạt động
nên bộ lọc Kalman mở rộng chưa thể phản ánh chính xác vị trí thực của robot. Do đó,
trên thực tế khi gắp thả kệ hàng, tọa độ của rô bốt thực bị sai lệch từ 1 đến 3 cm so với
mong muốn, hướng thực bị sai lệch vài độ so với mong muốn.
[3] Báo cáo trình bày bộ điều khiển tích hợp động học và moment của mô hình động
lực học robot di động noholonomic. Trong báo cáo này sẽ đề xuất mở rộng điều khiển
đáp ứng của bộ điều khiển. Nếu bộ điều khiển có thể điều khiển đáp ứng quỹ đạo cho

13
động học robot với những tham số không biết trước, thì đáp ứng quỹ đạo cho động lực
học robot có thể thiết kế được bằng cách dùng lý thuyết Lyapounov. Kết quả được
kiểm nghiệm bằng mô hình robot di dộng hai bánh. Trong báo cáo này, một bộ điều
khiển đáp ứng động học của robot được đề xuất sau đó đáp ứng về mô-men sẽ được
đưa ra từ bộ điều khiển trên.
Hình 2.10 Robot di động hai bánh.
Hình 2.11 Vị trí thật và mục tiêu của robot.

14
Hình 2.12 Đáp ứng vận tốc của bánh trái và phải.
Hình 2.13 Sai số quỹ đạo , , .
Tác giả đã đưa ra được phương pháp điều khiển đáp ứng quỹ đạo cho robot di
động với những tham số không biết trước. Bộ điều khiển động lực học cho robot di
động trên cơ sở bộ điều khiển động học đã được thiết kế dùng đáp ứng backstepping.
Tiến hành mô phỏng trên máy tính bằng phần mềm Matlab nghiên cứu trên robot di
động hai bánh đã cho kết quả tốt. Cơ sở cho chúng ta tiến hành áp dụng thực tế cho

15
robot di động khác. Hướng phát triển tiếp theo sẽ làm mô hình robot thật và đưa
phương pháp này vào áp dụng cho robot di động.
[4] Trong bài báo này trình bày thiết kế bộ điều khiển mobile robot nonholonomic.
Trên cơ sở phương trình động học của robot, một robot điều khiển được thiết kế sao
cho bám theo quỹ đạo tham chiếu tùy ý với vận tốc được xác định trước. Các thuật
toán điều khiển được thiết kế chứng minh ổn định cho WMR. Luật điều khiển được
chứng minh trên một ví dụ quỹ đạo đơn giản, tuy nhiên đối với một ứng dụng tổng
quát hơn một thuật toán lập kế hoạch chuyển động theo thời gian tối ưu với ràng buộc
gia tốc được trình bày là tốt.
Hình 2.14 Sơ đồ khối vòng điều khiển mobile robot.
Hình 2.15 cho thấy kết quả mô phỏng vòng kín trong đó robot có trạng thái sai số
ban đầu (không bắt đầu với góc định hướng và vị trí đúng), các tín hiệu ngõ vào và
ngõ ra có nhiễu tác động. Bộ điều khiển triệt tiêu các trạng thái sai số ban đầu và robot
đã bám theo quỹ đạo tham chiếu với độ chính xác đạt yêu cầu.

17
Hình 2.15 Điều khiển vòng kín với = 0.6 , = 2 để mô phỏng robot (reference --,
robot path -). Quỹ đạo robot tín hiệu ngõ ra với nhiễu và tín hiệu ngõ vào
(feedforward: - -, feedforward and closed-loop: -).
Kết quả điều khiển vòng kín của robot thực nghiệm được thể hiện trong hình
2.16. Tín hiệu ngõ vào của robot là vận tốc tuyến tính và vận tốc góc . Robot
được điều khiển bởi hai động cơ DC cho mỗi bánh xe, từ , và các tín hiệu từ
encoder tác giả sử dụng bộ điều khiển PID để đảm bảo robot đạt được vận tốc mong
muốn , .

18
Hình 2.16 Điều khiển vòng kín với = 0.6 , = 2 để mô phỏng robot thực
nghiệm (reference - - , robot path -). Quỹ đạo robot tín hiệu ngõ ra với nhiễu và tín
hiệu ngõ vào (feedforward: - - , feedforward and closed-loop: -).
Nghiên cứu được trình bày cũng như ứng dụng thực tế chứng minh rằng mobile
robot có thể bám theo quỹ đạo tham chiếu mong muốn với vận tốc quy định và độ
chính xác đạt yêu cầu. Một thuật toán tối ưu hóa thiết lập chuyển động cho robot được
trình bày. Tối ưu hóa là công việc mất nhiều thời gian và trong tương lai một giải pháp
với những quỹ đạo tối ưu được xác định trước cho một mạng lưới các vị trí bắt đầu sẽ
được dùng để thay thế.
[5] Nghiên cứu này trình bày một quỹ đạo hoàn hảo cho WMR được phát triển.
Phương pháp hồi tiếp tuyến tính hóa (FL) có thể được sử dụng để WMR đạt được vị trí
mong muốn và góc định hướng quỹ đạo. Do sự thể hiện không ổn định từ các tín hiệu
bên ngoài nên bộ điều khiển FL được thay thế bằng bộ điều khiển trượt (SMC). SMC
không thể loại bỏ được chattering do đó bộ điều khiển trượt mờ (FSMC) được đề xuất
để giảm chattering. Tính ưu việt của FSMC được trình bày thông qua kết quả mô
phỏng.
Hình 2.17 Sơ đồ khối SMC.

19
Hình 2.18 Sơ đồ khối FSMC.
Bảng 2.1 Luật điều khiển mờ.

20
Hình 2.19 Hàm membership của ngõ vào-ngõ ra , ̇, .
Hình 2.20 WMR bám theo quỹ đạo tham chiếu.

21
Hình 2.21 Sai số ( − ).
Hình 2.22 Sai số ( − ).

22
Hình 2.23 Tín hiệu điều khiển torque cho bánh phải.
Hình 2.24 Sai số góc định hướng − .

23
Hình 2.25 WMR bám theo quỹ đạo tham chiếu: (a) FL; (b) đường chấm chấm-SMC,
đường liền-FSMC.
Hình 2.26 Sai số ( − ): (a) FL; (b) đường chấm chấm-SMC, đường liền-FSMC.

24
Hình 2.27 Sai số ( − ): (a) FL; (b) đường chấm chấm-SMC, đường liền-FSMC.
Hình 2.28 Sai số góc định hướng − : (a) FL; (b) đường chấm chấm-SMC, đường
liền-FSMC.

25
Hình 2.29 Tín hiệu điều khiển torque cho bánh phải: (a) FL; (b) đường chấm chấm-
SMC, đường liền-FSMC.
Từ hình 2.20–2.24 là kết quả mô phỏng bộ điều khiển được thiết kế chưa chưa có
nhiễu tác động từ bên ngoài. Trong hình 2.20 với tất cả các bộ điều khiển WMR bám
theo quỹ đạo tham chiếu với độ chính xác đáng kể. Có thể thấy được từ hình 2.21–2.24
bộ điều khiển FL cho hiệu suất tốt hơn cho vị trí và góc định hướng so với SMC và
FSMC. Tuy nhiên, trong hình 2.23 độ lớn moment xoắn khi sử dụng FL là lớn, FSMC
có kết quả sai số nhỏ hơn so với SMC. Trong hình 2.25 bộ điều khiển FL rất nhạy với
nhiễu đo được trong khi đó SMC thể hiện tương đối tốt với nhiễu tác động. Từ hình
2.26–2.28 sai số vị trí và góc định hướng của SMC và FSMC tốt hơn so với FL. Hơn
nữa FSMC có hiệu suất đáng tin cậy hơn so với SMC. Một sự vượt trội khác liên quan
đến FL , SMC có tín hiệu điều khiển moment xoắn nhỏ điều này được thể hiện trong
hình 2.29.
Khi có các tác động bên ngoài FL có hiệu suất không tốt với sai số ở tần số cao.
Bằng cách sử dụng SMC kết hợp với một mặt trượt PID hiệu suất của WMR được cải
thiện. FSMC được đề xuất tiếp tục nâng cao hiệu suất của WMR loại bỏ được nhiễu vì
vậy nó sẽ phù hợp hơn trong hệ thống WMR thực.

26
[6] Trong bài báo này, trình bày phương pháp thiết kế bộ điều khiển cho xe tự hành
sử dụng kỹ thuật tuyến tính hồi tiếp. Các mô hình động lực học của hệ thống với nhiễu
bên ngoài được trình bày. Véc tơ điều khiển đầu vào được thiết kế dùng cho kỹ thuật
tuyến tính hồi tiếp. Véc tơ điều khiển đầu vào chuyển đổi toàn bộ hệ thống thành hai
hệ thống con tuyến tính của hệ thống bao gồm sai số vị trí và sai số vận tốc. Dựa trên
hai hệ thống con tuyến tính, véc tơ đầu vào mới của xe tự hành được thiết kế. Véc tơ
điều khiển đầu vào mới đảm bảo rằng các véc tơ sai số hội tụ theo hàm mũ về không.
Hình 2.30 Lưu đồ giải thuật điều khiển tuyến tính hồi tiếp.
Hình 2.31 Quỹ đạo mong muốn của AGV có dạng đường thẳng (y = x).

27
Bảng 2.2 Giá trị thông số của AGV.
Bảng 2.3 Giá trị khởi tạo ban đầu.

28
Hình 2.32 Quỹ đạo của AGV ở thời gian ban đầu.
Từ hình 2.32 cho thấy: quỹ đạo mong muốn của AGV và quỹ đạo tham chiếu
trùng với nhau sau khi AGV di chuyển một đoạn đường rất ngắn. Điều này cho thấy
AGV đã bám theo quỹ đạo mong muốn rất chính xác.
Hình 2.33 Sai lệch vị trí trong toàn thời gian.
Hình 2.33 biểu diễn sự sai lệch vị trí giữa điểm bám P và điểm tham chiếu R
trong toàn thời gian đi hết quỹ đạo tham chiếu của AGV. Từ hình 2.33 cho thấy các sai

29
lệch vị trí hội tụ về 0 nhanh trước 8 giây khi AGV bám theo quỹ đạo tham chiếu là
đường thẳng và ổn định ở 0 trong toàn thời gian.
Hình 2.34 Vận tốc tuyến tính của AGV trong toàn thời gian.
Hình 2.34 biểu diễn vận tốc tuyến tính của AGV sau thời gian rất ngắn khoảng 3
giây rồi tiến đến bằng với vận tốc đặt trước = 0.05 [ / ] và ổn định ở giá trị này
trong suốt thời gian chuyển động của AGV.
Hình 2.35 Vận tốc góc của bánh phải và bánh trái đối với quỹ đạo là đường thẳng.

30
Hình 2.35 biểu diễn vận tốc góc bánh phải (wrw) và bánh trái (wlw) của AGV
đối với quỹ đạo tham chiếu là đường thẳng. Nó cho thấy rằng vận tốc góc của bánh xe
trái và phải thay đổi một cách nhanh chóng vào thời điểm bắt đầu và có cùng một giá
trị 0.32 [rad / s] theo đường thẳng sau khoảng 7 giây.
Hình 2.36 Véc tơ điều khiển đầu vào .
Hình 2.36 biểu diễn véc tơ điều khiển đầu vào bánh phải (trw) và bánh trái (tlw)
của AGV đối với quỹ đạo tham chiếu là đường thẳng. Nó cho thấy rằng véc tơ điều
khiển bánh xe trái và phải thay đổi một cách nhanh chóng vào thời điểm bắt đầu và có
cùng một giá trị [−5.2; 5.2] / sau khoảng thời gian 4 giây.
Hình 2.37 Véc tơ điều khiển đầu vào u.

31
Hình 1.33 biểu diễn véc tơ điều khiển đầu vào và của AGV đối với quỹ đạo
tham chiếu là đường thẳng. Nó cho thấy rằng véc tơ điều khiển đầu vào thay đổi một
cách nhanh chóng vào thời điểm bắt đầu và có cùng một giá trị [−7.6; 7.6] sau
khoảng thời gian 4 giây.
Hình 2.38 Véc tơ điều khiển đầu vào mới .
Hình 2.38 biểu diễn véc tơ điều khiển đầu vào mới và của AGV đối với
quỹ đạo tham chiếu là đường thẳng. Nó cho thấy rằng véc tơ điều khiển đầu vào mới
thay đổi một cách nhanh chóng vào thời điểm bắt đầu và có giá [−0.1; 0.1] /
Và véc tơ điều khiển đầu vào mới giảm dần từ thời điểm khi bắt đầu tiến về 0 trong
toàn thời gian quỹ đạo của AGV.
Trong bài báo này, một bộ điều khiển tuyến tính hồi tiếp được đề xuất dựa trên
mô hình động lực học của AGV với những nhiễu từ bên ngoài. Sử dụng kỹ thuật tuyến
tínhhồi tiếp, véc tơ điều khiển đầu vào chuyển đổi toàn bộ hệ thống thành hai hệ thống
con tuyến tính của hệ thống bao gồm sai số vị trí và sai số vận tốc. Dựa trên hai hệ
thống con tuyến tính, véc tơ đầu vào mới của xe tự hành được thiết kế. Véc tơ điều
khiển đầu vào mới đảm bảo rằng các véc tơ sai số hội tụ theo hàm mũ về không. Các
kết quả mô phỏng được trình bày để minh họa hiệu quả cho bộ điều khiển bám cho
AGV sử dụng thuật toán tuyến tính hồi tiếp.

32
2.4 Nhận xét và hướng tiếp cận
Nhận xét: qua các công trình nghiên cứu của các tác giả đã nêu trên, ta nhận thấy rằng
có rất nhiều phương pháp để giải quyết vấn đề đó là định vị và điều khiển bám cho
mobile robot theo quỹ đạo mong muốn trên một mặt phẳng hai chiều. Tuy nhiên, mỗi
phương pháp đều có ưu, khuyết điểm được thể hiện qua kết quả mô phỏng mà các bài
báo cũng đã chỉ ra.
Hướng tiếp cận: xây dựng thuật toán điều khiển dựa trên phương pháp Lyapunov cho
WMR nhằm điều khiển WMR bám theo quỹ đạo đã được định trước, sau đó tiếp cận
vị trí mong muốn. Cuối cùng, áp dụng thuật toán điều khiển này trên mô hình WMR
bao gồm :
- Khung xe.
- Hai bánh chủ động hay còn gọi là bánh lái điều khiển nằm ở phía sau xe. Hai bánh
chủ động này được điều khiển bằng hai động cơ điện một chiều để đạt được các chuyển
động và định hướng.
- Một bánh thụ động ở phía trước xe bánh này giúp cho xe cân bằng.

33
Chương 3
MÔ HÌNH TOÁN HỌC CỦA AGV
3.1 Cấu trúc AGV
AGV trong đề tài này dựa trên mô hình của WMR, được thể hiện như trên hình
3.1. Hai bánh xe được cố định ở mỗi bên khung của AGV, được điều khiển độc lập
bằng hai động cơ DC có trang bị encoder và một bánh xe đa hướng ở phía trước cần
thiết cho việc ổn định hướng. AGV có thể di chuyển qua lại và thay đổi góc định
hướng thông qua việc điều khiển vận tốc của hai bánh lái. Cả hai bánh xe có bán kính
như nhau là r và khoảng cách nhau với khoảng cách là 2b.
Hình 3.1 Mô hình động học của WMR.

34
Do AGV là một hệ thống phi tuyến nên việc lập kế hoạch đường đi và điều khiển
bám theo quỹ đạo trở nên rất khó khăn để tối ưu quá trình điều khiển và ổn định hệ
thống. Để tránh bánh xe bi trượt và những tác động ảnh hưởng của cơ khí thì việc
chuyển động trơn tru và ổn định là rất quan trọng.
3.2 Xây dựng phương trình động học
Trong hình 3.1 thể hiện AGV trong hệ trục tọa độ Oxy cũng chính là không gian
nhà kho của AGV , CXY là hệ tọa độ cục bộ gắn trên AGV. Vị trí của AGV tại điểm C
trong hệ tọa độ toàn cục hoàn toàn được xác định bởi tọa độ tổng suy rộng =
[ Θ ] , trong đó , là tọa độ điểm C, Θ là góc định hướng của AGV.
Giả sử bánh xe không trượt và robot phải chuyển động với ràng buộc về tốc độ
như sau:
̇ Θ − ̇ Θ = 0 (3.1)
Ràng buộc có thể được viết lại như sau:
( ) ̇ = 0
( ) = [− Θ Θ 0] = [ ] = 1,2,3
̇ = ( ) ⇔ ̈ = ( ) ̇ + ̇( )
( ) =
Θ 0
Θ 0
0 1
, =
Trong đó ( ) là ma trận Jacobi phải thỏa ( ) ( ) = 0, là vector vận tốc,
, là vận tốc tuyến tính và vận tốc góc tương ứng của AGV. Với điều kiện ràng
buộc này chuyển động của AGV được mô tả như sau:
̇ = Θ
̇ = Θ
Θ̇ =
(3.2)

35
Mối quan hệ giữa , và vận tốc góc của hai bánh xe:
=
⎣
⎢
⎢
⎢
⎡
1
1 −
⎦
⎥
⎥
⎥
⎤
(3.3)
Trong đó , lần lượt là vận tốc góc bánh phải và bánh trái của AGV.
Phương trình động học tham chiếu của AGV:
̇ = Θ
̇ = Θ
Θ̇ =
(3.4)
Trong đó , ,Θ là vị trí mong muốn của AGV. , lần lượt là vận tốc tuyến
tính và vận tốc góc mong muốn.
Muốn AGV bám theo quỹ đạo mong muốn ta phải tìm một luật điều khiển sao
cho lim → ( ) = .
Vector sai số được thể hiện tương đối so với tọa độ cục bộ được cố định trên
robot như sau:
=
Θ Θ 0
− Θ Θ 0
0 0 1
−
−
Θ − Θ
(3.5)
Đạo hàm bậc 1 sai số theo thời gian:
̇ = − +
̇ = − +
̇ = −
(3.6)

36
Hình 3.2 Khái niệm về AGV bám theo quỹ đạo tham chiếu.
Đối với điều khiển AGV việc khắc phục sai số tích lũy là rất quan trọng, thêm
vào đó cần phải có giới hạn về động lực học.
| | ≤
| | ≤
3.3 Xây dựng quỹ đạo đường đi cho AGV
Xây dựng quỹ đạo nhằm mục đích tìm ra một quỹ đạo trơn nhằm đảm bảo điều
kiện ràng buộc về (3.1) cho hệ thống robot (3.2). Từ kết quả [2] ta có thể tạo một
đường cong nội suy của hàm đa thức bậc 7 − đi qua hai điểm A( , )và
B( , ) được tính như sau:
P(s , η) = ( ), ( ) (3.7)
( ) = + + + + + + + (3.8)
( ) = + + + + + + + (3.9)
[0 1] tham số quãng đường đã được chuẩn hóa.
Tại thời điểm ban đầu.

37
= 0 , ( ) = , ( ) =
Tại thời điểm kết thúc.
= 1 , ( ) = , ( ) =
Các hệ số của ( ) được tính như sau:
̅ = ‖ − , − ‖ = ( − ) + ( − )
=
= ̅ , = ̅
= 0 , = 0
= 0 , = 0
= 35( − ) − 20 − 10 − 4 − 15 + 5 −
= −84( − ) + 45 + 20 + 6 + 39 − 14 + 3
= 70( − ) − 36 − 15 − 4 − 34 + 13 − 3
= −20( − ) + 10 + 4 + + 10 − 4 +
Các hệ số của đa thức ( ) được tính như sau:
=
= ̅ , = ̅
= 0 , = 0
= 0 , = 0
= 35( − ) − 20 − 10 − 4 − 15 + 5 −
= −84( − ) + 45 + 20 + 6 + 39 − 14 + 3
= 70( − ) − 36 − 15 − 4 − 34 + 13 − 3
= −20( − ) + 10 + 4 + + 10 − 4 +

38
Các điểm trên quỹ đạo G3
gọi là way point (WP), AGV sẽ đi qua các điểm này để
đến điểm đích đã được xác định trước. Số điểm WP có thể tùy chọn và nằm trong dải
giới hạn, quá trình chuyển động sử dụng − phù hợp để AGV tiếp cận pallet.
Hình 3.3 Quỹ đạo G3 đi qua hai điểm A và B.

39
Chương 4
THIẾT KẾ BỘ ĐIỀU KHIỂN VÀ HỆ THỐNG ĐO LƯỜNG
4.1. Cơ sở lý thuyết
4.1.1 Giới thiệu phương pháp Lyapunov
- Phương pháp Lyapunov cung cấp đủ điều kiện đủ để đánh giá tính ổn định của hệ
phi tuyến.
- Có thể áp dụng cho hệ phi tuyến bậc cao bất kỳ.
- Có thể dùng phương pháp Lyapunov để thiết kế các bộ điều khiển phi tuyến.
- Hiện nay phương pháp Lyapunov là phương pháp được sử dụng rộng rãi nhất để
phân tích và thiết kế hệ phi tuyến.
4.1.2 Điểm cân bằng của hệ phi tuyến
Xét hệ phi tuyến bất biến theo thời gian mô tả bởi phương trình trạng thái sau:
̇ = ( , ) (4.1)
Một điểm ∈ được gọi là điểm cân bằng của hệ thống nếu như hệ đang ở
trạng thái và không có tác động bên ngoài thì hệ sẽ nằm nguyên tại đó.
Dễ thấy điểm cân bằng phải là nghiệm của phương trình:
( , )| , = 0 (4.2)
Hệ phi tuyến có thể có nhiều điểm cân bằng hoặc không có điểm cân bằng nào.
Điều này hoàn toàn khác so với hệ tuyến tính, hệ tuyến tính luôn luôn có một điểm cân
bằng là = 0.

40
Ví dụ:
Xét hệ con lắc mô tả bởi PTVP:
̈( ) + ̇( ) + = ( )
Xác định điểm cân bằng (nếu có).
Giải quyết:
Thành lập PTTT. Đặt
( ) = ( )
( ) = ̇( )
PTTT mô tả hệ con lắc: ̇( ) = ( ( ), ( ))
Trong đó: ( , ) =
( )
− ( ) − ( ) + ( )
Điểm cân bằng phải là nghiệm của phương trình:

̇ = ( , )| , = 0 ⇒
= 0
− − = 0
⇒
=
= 0

41
Kết luận: Hệ con lắc có vô số điểm cân bằng.
4.1.3 Ổn định tại điểm cân bằng
4.1.3.1 Định nghĩa
Một hệ thống được gọi là ổn định tại điểm cân bằng nếu như có một tác động
tức thời đánh bật hệ ra khỏi và đưa đến điểm thuộc lân cận nào đó của thì sau
đó hệ có khả năng tự quay được về điểm cân bằng ban đầu.
Chú ý: Tính ổn định của hệ phi tuyến chỉ có nghĩa khi đi cùng với điểm cân
bằng. Có thể hệ ổn định tại điểm cân bằng này nhưng không ổn định tại điểm cân bằng
khác.
Ví dụ: Con lắc đơn.
4.1.3.2 Ổn định và ổn định tiệm cận Lyapunov
Cho hệ phi tuyến không kích thích mô tả bởi PTTT:
̇ = ( , )| (4.3)
Giả sử hệ thống có điểm cân bằng = 0.
- Hệ thống được gọi là ổn định Lyapunov tại điểm cân bằng = 0 nếu với > 0 bất
kỳ bao giờ cũng tồn tại phụ thuộc sao cho nghiệm ( ) của phương trình (4.3) với
điều kiện đầu (0) thỏa mãn:
‖ (0)‖ < ⇒ ( ) < , ∀ ≥ 0

42
- Hệ thống được gọi là ổn định tiệm cận Lyapunov tại điểm cân bằng = 0 nếu với
> 0 bất kỳ bao giờ cũng tồn tại phụ thuộc sao cho nghiệm ( ) của phương trình
(4.3) với điều kiện đầu (0) thỏa mãn:
‖ (0)‖ < ⇒ lim
→
( ) = 0
4.1.4 Phương pháp tuyến tính hóa Lyapunov
Giả sử xung quanh điểm cân bằng , hệ thống phương trình (4.1) có thể tuyến
tính hóa về dạng:
̇ = + (4.4)
Định lý :
- Nếu hệ thống tuyến tính hóa (4.4) ổn định thì hệ phi tuyến (4.1) ổn định tiệm cận tại
điểm cân bằng .
- Nếu hệ thống tuyến tính hóa (4.4) không ổn định thì hệ phi tuyến (4.1) không ổn định
tiệm cận tại điểm cân bằng .

43
- Nếu hệ thống tuyến tính hóa (4.4) ở biên giới ổn định thì không kết luận được gì về
tính ổn định của hệ phi tuyến (4.1) tại điểm cân bằng .
Ví dụ :
Xét hệ con lắc mô tả bởi PTTT:
̇( ) = ( ( ), ( ))
Trong đó : ( , ) =
( )
− ( ) − ( ) + ( )
Xét tính ổn định của hệ thống tại điểm cân bằng.
Giải quyết:
- Mô hình tuyến tính quanh điểm cân bằng: = [0 0] .
̇ = +
=
( , )
= 0 =
( , )
= 1
=
( , )
= − ( )
( , )
= − =
( , )
= −

44
⇒ =
0 1
− −
⇒ PTĐT: det( − ) = det
−1
+
= 0 ⇔ + + = 0
Kết luận: Hệ thống ổn định (theo hệ quả tiêu chuẩn Hurwitz).
- Mô hình tuyến tính quanh điểm cân bằng: = [ 0] .
̇ = +
=
,
= 0 =
,
= 1
=
,
= − ( )
,
= =
,
= −
⇒ =
0 1
−
⇒ PTĐT det( − ) = det
−1
− +
= 0 ⇔ + − = 0
Kết luận: Hệ thống không ổn định (PTĐT không thỏa điều kiện cần).
4.1.5 Phương pháp trực tiếp Lyapunov
4.1.5.1 Định lý ổn định Lyapunov: cho hệ phi tuyến không kích thích mô tả bởi PTTT
(4.3). Giả sử hệ thống có điểm cân bằng = 0. Nếu tồn tại một hàm ( ) sao cho:
- ( ) ≥ 0 , ∀
- (0) = 0
- ̇ ( ) ≤ 0 , ∀ ≠ 0
Thì hệ thống (4.3) ổn định Lyapunov tại điểm .
Nếu ̇ ( ) < 0 , ∀ ≠ 0 thì hệ thống (4.3) ổn định tiệm cận Lyapunov tại điểm .
Chú ý: Hàm ( ) thường được chọn là hàm toàn phương theo biến trạng thái.

45
4.1.5.2 Định lý không ổn định Lyapunov: cho hệ phi tuyến không kích thích mô tả
bởi PTTT (4.3). Giả sử hệ thống có điểm cân bằng = 0. Nếu tồn tại một hàm ( )
sao cho:
- ( ) ≥ 0 , ∀
- (0) = 0
- ̇ ( ) > 0 , ∀ ≠ 0
Thì hệ thống (4.3) không ổn định Lyapunov tại điểm .
Ví dụ 1 : Cho hệ thống mô tả bởi PTTT:
̇ = − + + ( + )
̇ = − − − ( + )
Xác định trạng thái cân bằng của hệ thống và đánh giá tính ổn định của hệ thống tại
trạng thái cân bằng.
Giải quyết:
Trạng thái cân bằng là nghiệm của phương trình:
− + + ( + ) = 0
− − − ( + ) = 0
⇔
= 0
= 0
Đánh giá tính ổn định: Chọn hàm Lyapunov
( ) =
1
2
( + )
Ta có:
- ( ) > 0 , ∀ ≠ 0
- (0) = 0
̇ ( ) = ̇ + ̇
= [− + + ( + )] + [− − − ( + )] = − −
⇒ ̇ ( ) < 0 , ∀ ≠ 0

46
Hệ thống ổn định tiệm cận Lyapunov tại điểm cân bằng.
Ví dụ 2: Cho hệ thống mô tả bởi PTTT:
̇ = + ( + )
̇ = − + ( + )
Xác định trạng thái cân bằng của hệ thống và đánh giá tính ổn định của hệ thống tại
trạng thái cân bằng.
Giải quyết:
Trạng thái cân bằng là nghiệm của phương trình:
+ ( + ) = 0
− + ( + ) = 0
⇔
= 0
= 0
Đánh giá tính ổn định: Chọn hàm Lyapunov
( ) =
1
2
( + )
Ta có :
- ( ) > 0 , ∀ ≠ 0
- (0) = 0
̇ ( ) = ̇ + ̇
= [ + ( + )] + [− + ( + )] = ( + )( + )
⇒ ̇ ( ) > 0 , ∀ ≠ 0
Hệ thống không ổn định tại điểm cân bằng.

47
4.2. Thiết kế bộ điều khiển
Mục tiêu của đề tài này là thiết kế bộ điều khiển sao cho AGV bám theo quỹ đạo
tham chiếu − được xây dựng trong chương 3. Nói cách khác AGV sẽ lần
lượt đi qua tập hợp điểm G3 với một vận tốc mong muốn không đổi . Khi đến đích
AGV tiếp cận pallet và nâng pallet đến vị trí mong muốn.
Bộ điều khiển được thiết kế dựa vào phương pháp Lyapunov. Theo [7] từ (3.5)
chọn hàm Lyapunov như sau:
= ( + ) + (4.5)
̇ = ̇ + ̇ +
̇
(4.6)
Thay (3.6) vào (4.6) ta có:
̇ = ( − + ) + (− + ) +
( − )
= − + + − + (4.7)
Để ̇ mang giá trị âm ta chọn:
= +
= + +
(4.8)
Trong đó , , là những giá trị dương. Thay (4.8) vào (4.7) ta có:
̇ = − − < 0 (4.9)
Từ (4.5) và (4.9) chứng minh rằng các sai số , , hội tụ về không khi thời
gian t tiến ra vô cùng. Do đó bộ điều khiển (4.8) đáp ứng được yêu cầu đặt ra.

48
4.3. Hệ thống đo lường
Mô hình toán học của AGV được thành lập dựa trên một số giả định như AGV di
chuyển trên một mặt phẳng không trượt và bỏ qua ma sát trên các bánh xe. Chuyển
động của AGV khi bám theo quỹ đạo mong muốn được đo bằng encoder trang bị trên
hai bánh xe chủ động thì vận tốc quay có thể được tính như sau:
_ =
( _ _ )
(4.10)
_ =
( _ _ )
(4.11)
Trong đó _ và _ là vận tốc góc của bánh phải và bánh trái đo
được từ encoder.
Vị trí của AGV được ước lượng dựa theo (3.2).
= + _ . Θ
= + _ . Θ
Θ = Θ + _
(4.12)
là thời gian lấy mẫu và , , Θ là tọa độ của AGV ở thời điểm trước đó.
Từ các công thức này để điều khiển AGV theo mục tiêu của đề tài gồm các bước
theo sơ đồ khối hình 4.1.

49
Hình 4.1 Sơ đồ khối của vòng điều khiển AGV bám quỹ đạo G3.
Hình 4.1 Sơ đồ khối của vòng điều khiển AGV bám quỹ đạo G3.
Path Planing
Trajectory G3
Eq (3.7), (3.8), (3.9)
Path Tracking
Wheel Control
AGV
Controller
Eq (4.8)
Eq (3.3)
Right Wheel Controller Left Wheel Controller
Right DC Motor Left DC Motor
Position Estimation
Right Encoder Left Encoder
Eq (4.10), (4.11)
Eq (4.12)
,
, , Θ , ,
, , Θ , ,
_ _

50
Trong hình 4.1 gồm bốn phần:
- Path Planing: Thiết lập quỹ đạo đường đi cho AGV, quỹ đạo này chính là G3
.
- Path Tracking: Dùng bộ điều khiển (4.8) kiểm soát đường đi cho AGV. Từ (4.8) và
(3.3) tính được vận tốc góc mong muốn cho bánh xe phải và trái.
- Wheel Control: Bộ điều khiển động cơ được thiết kế riêng biệt cho hai bánh xe nhằm
điều khiển vận tốc góc của bánh xe theo vận tốc góc tham chiếu ở phần Path Tracking.
Để di chuyển chính xác dọc theo quỹ đạo G3
, AGV phải biết vị trí và góc định hướng
thực tế, chúng được ước tính bằng phương pháp đo sử dụng encoder.
- Position Estimation: Ước lượng vị trí theo (4.10), (4.11), (4.12). Từ các vị trí này sẽ
hồi tiếp về Path Tracking.

51
Chương 5
THIẾT KẾ THI CÔNG AGV VÀ MÔ PHỎNG
5.1 Thiết kế mô hình
Mô hình AGV bao gồm:
- Khung xe với chiều dài khoảng cách cho hai bánh xe là 20 cm và sử dụng mica 5 mm
để làm khung.
- Hai bánh chủ động nằm ở phía sau xe có đường kính 100 mm.
Hình 5.1 Bánh xe dùng cho động cơ có bộ giảm tốc đường kính 100 mm.
- Động cơ DC Tsukasa TG-35F-AG-18-A275 với điện áp 24 VDC, tốc độ không tải
7400 RPM, được trang bị encoder với độ phân giải 90 xung/vòng, được gắn trực tiếp
trên bánh xe.
Hình 5.2 Động cơ DC Tsukasa TG-35F-AG-18-A27.

52
- Một bánh xe omni nhôm nằm phía trước xe, bánh này giúp cho xe được cân bằng.
Hình 5.3 Bánh xe omni nhôm.
Thiết kế mô hình được mô phỏng như sau:
Hình 5.4 Mô hình AGV mô phỏng.

53
5.2 Thiết kế mạch điện
5.2.1 Phương án thiết kế
Hình 5.5 Cấu hình hệ thống điều khiển.
Chức năng từng khối:
- Master bao gồm một vi điều khiển pic 18F4550 dùng để tính toán giải thuật điều
khiển bám cho AGV và truyền tín hiệu điều khiển xuống khối slave bằng giao thức I2C,
tọa độ của AGV lần lượt sẽ được hiển thị trên LCD. Pic giao tiếp với PC bằng giao thức
Rs232, nhiệm vụ của PC là hiển thị tọa độ của AGV trên đồ thị nhằm so sánh với quỹ
đạo mong muốn G3
để đánh giá độ bám của bộ điều khiển (4.8).
- Slave gồm hai vi điều khiển pic 18f4431 nhận tín hiệu điều khiển từ khối master và
giữ vai trò là bộ điều khiển vận tốc cho bánh xe phải và trái.

54
5.2.2 Sơ đồ nguyên lý
- Master:
Hình 5.6 Mạch nguồn 5 VDC.
Hình 5.7 Mạch vi điều khiển Pic 18f4550.

55
Hình 5.8 Khối giao tiếp I2C và RS232.
Hình 5.9 Khối hiển thị.

56
- Slave:
Hình 5.10 Mạch vi điều khiển Pic 18f4431.
Hình 5.11 Mạch kết nối encoder.

57
5.2.3 Thiết bị sử dụng
- Vi điều khiển Pic 18F4550 và 18F4431 nhiệm vụ đã được trình bày ở phần trên.
Hình 5.12 Pic 18F4550 và 18F4431.
- Mạch truyền dữ liệu không dây HM-TR/TTL/433 có nhiệm vụ truyền dữ liệu giữa
pic 18F4550 và PC theo chuẩn rs232.
Hình 5.13 HM-TR/TTL/433.

58
- Mạch công suất cho hai động cơ hay còn gọi là mạch cầu H.
Hình 5.14 Mạch cầu H đôi.
- Pin 18650 dùng làm nguồn cung cấp cho động cơ và mạch điều khiển.
Hình 5.15 Pin 18650.

59
Kết quả: Từ ý tưởng thiết kế mô hình và mạch điện mô hình thực tế được xây dựng
như sau:
Hình 5.16 AGV được thi công.

60
5.3 Kết quả mô phỏng và thực nghiệm
Tiến hành mô phỏng trên máy tính bằng phần mềm Matlab, các hằng số dương
trong bộ điều khiển được chọn:
= 9 , = 4000 , = 1
Bảng 5.1 Thông số mô phỏng.
Parameters Value Unit
b 0.12 m
r 0.051 m
0.1 s
0 m
0 m
Θ 0 rad
0 m/s
0 rad/s
0.05 m/s
0.2 m/s
0.785 rad/s

61
5.3.1 Kết quả mô phỏng trường hợp 1
Quỹ đạo G3
bắt đầu từ ( = 0, = 0, Θ = 0) và điểm đích =
1.4, = 0.5, Θ = .
trường hợp 1.
Hình 5.18 Vec-tơ sai số trường hợp 1.

62
Hình 5.19 Vận tốc góc bánh phải và bánh trái trường hợp 1.
Hình 5.20 Kết quả đo vị trí ước lượng của AGV trường hợp 1.

63
Hình 5.21 e1 được ước lượng trường hợp 1.

64

65
5.3.2 Kết quả mô phỏng trường hợp 2
Quỹ đạo G3
bắt đầu từ ( = 0, = 0, Θ = 0) và điểm đích =
1.4, = 0.8, Θ = .
trường hợp 2.
Hình 5.25 Vec-tơ sai số trường hợp 2.

66
Hình 5.26 Vận tốc góc bánh phải và bánh trái trường hợp 2.
Hình 5.27 Kết quả đo giá trị ước lượng của AGV trường hợp 2.

67

68
5.4 Kết luận và hướng phát triển
Khi tiến hành thực nghiệm chỉ dùng cảm biến đo lường là encoder và encoder có
nhiệm vụ đo vận tốc góc của bánh xe. Tọa độ của AGV chỉ được ước lượng nên việc
AGV bám theo quỹ đạo G3
cũng chưa chính xác và chỉ mang tính tương đối dẫn đến
việc tiếp cận pallet không chính xác.
Trong tương lai sẽ phát triển một hệ thống GPS indoor cho AGV dùng cảm biến
siêu âm và tín hiệu của cảm biến siêu âm sẽ được lọc nhiễu bằng bộ lọc Kalman với
mục đích làm tăng độ chính xác vị trí của AGV. Khi đã giải quyết được vấn đề xác
định về vị trí từ đề tài này có thể phát triển một số đề tài như sau:
- Nghiên cứu và phát triển robot cắt cỏ.
- Nghiên cứu và phát triển robot sơn đường.
- Nghiên cứu và phát triển robot phục vụ trong y tế v.v.

69
Đề tài này đã đưa ra bộ điều khiển phi tuyến dựa trên phương pháp Lyapunov.
Phần thiết kế quỹ đạo sử dụng phương pháp − . AGV có thể di chuyển bám
từ điểm bắt đầu đến điểm kết thúc biết trước. Mô phỏng và kết quả thực nghiệm sai số
tiếp tuyến, sai số phương ngang và sai số định hướng sẽ hội tụ về không. Bộ điều
khiển được đề xuất có thể được áp dụng và thực hiện trong thực tế.

70
TÀI LIỆU THAM KHẢO
[1] Do-Eun Kim, Kyung-Hun Hwang, Dong-Hun Lee, Tae-Young Kuc, A Simple
Ultrasonic GPS System for Indoor Mobile Robot System using Kalman
Filtering, School of Information and Communication Engineering,
SungKyunKwan University, Korea.
[2] Nguyễn Văn Tính, Phạm Thượng Cát, Phạm Minh Tuấn, Bùi Thị Thanh
Quyên, Trajectory planning and control of mobile robot for transportation in
warehouse, National Conference about Control and Automation- VCCA-2011.
[3] Trần Thanh Tùng, Adaptive tracking control of nonholonomic mobile robot,
Khoa Kỹ Thuật- Công Nghệ, Trường Đại Học Phạm Văn Đồng, Quảng Ngãi.
[4] Gregor Klanˇcar, Drago Matko, Saˇso Blaˇziˇc, Mobile Robot Control on a
Reference Path, Proceedings of the 13th
, Mediterranean Conference on
Control and Automation, Limassol, Cyprus, June 27-29, 2005.
[5] Jafar Keighobadi, Yaser Mohamadi, Fuzzy Sliding Mode Control of a Non-
Holonomic Wheeled Mobile Robot, Proceedings of the International
Multiconference of Engineers and Computer scientists 2011 vol II, IMECS
2011, March 16-8, 2011, Hong Kong.
[6] Vũ Thị Gấm, Trần Nguyên Châu, Phạm Hùng Kim Khánh, Nguyễn Hùng,
Controller design for path-following of Automatic Guided Vehicle using
Input-Output feedback linearization technique, HUTECH University.
[7] Trong Hieu Bui, Tan Tien Nguyen, Tan Lam Chung, Sang Bong Kim, A
simple nonlinear control of a two – wheeled welding mobile robot,
International Journal of Control, Automation, and Systems, vol.1, no.1, March
2003.
[8] Ngo Manh Dung, Vo Hoang Duy, Nguyen Thanh Phuong, Sang Bong Kim,
and Myung Suck Oh, Two-Wheeled Welding Mobile Robot for Tracking a
Smooth Curved Welding Path Using Adaptive Sliding-Mode Control

71
Technique, International Journal of Control, Automation, and Systems, vol.5,
no.3,June 2007.
[9] Huỳnh Thái Hoàng (2011). Điều khiển phi tuyến, Lý thuyết điều khiển nâng
cao, Đại học bách khoa, TP.HCM, 55-76.

PHỤ LỤC
1. Giao diện điều khiển
Chú thích:
(1) Control:
- Start: kết nối cổng 232.
- Send: gửi mã lệnh xuống Pic 18f4550.
- Plot: vẽ quỹ đạo đường đi của AGV trên đồ thị.
- Read: đọc tọa độ ước lượng của AGV được gửi từ Pic 18f4550 lên máy tính.
- Exit: thoát chương trình giao diện điều khiển.
- Run: cho phép AGV chạy theo quỹ đạo định trước.
- Line: cho phép AGV bám theo vạch trắng khi tiếp cận mục tiêu.
- Up: cho phép AGV nâng càng xúc pallet.
- Down: cho phép AGV hạ càng xúc pallet.
1
2 3 6
4
5

- F: đi thẳng.
- B: đi lùi.
- L: rẽ trái.
- R: rẽ phải.
(2) Hiển thị tọa độ trục X.
(3) Hiển thị tọa độ trục Y.
(4) Nhập mã lệnh điều khiển.
(5) Hiện thị trạng thái khi kết nối thành công cổng 232.
(6) Đồ thị quỹ đạo đường đi định sẵn và quỹ đạo AGV ước lượng được.
Chương trình Matlab:
function varargout = WMR(varargin)
% WMR M-file for WMR.fig
% WMR, by itself, creates a new WMR or raises the existing
% singleton*.
%
% H = WMR returns the handle to a new WMR or the handle to
% the existing singleton*.
%
% WMR('CALLBACK',hObject,eventData,handles,...) calls the local
% function named CALLBACK in WMR.M with the given input arguments.
%
% WMR('Property','Value',...) creates a new WMR or raises the
% existing singleton*. Starting from the left, property value pairs are
% applied to the GUI before WMR_OpeningFcn gets called. An
% unrecognized property name or invalid value makes property application
% plot. All inputs are passed to WMR_OpeningFcn via varargin.
%
% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one
% instance to run (singleton)".

%
% See also: GUIDE, GUIDATA, GUIHANDLES
% Edit the above text to modify the response to help WMR
% Last Modified by GUIDE v2.5 16-Dec-2013 07:42:54
% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @WMR_OpeningFcn, ...
'gui_OutputFcn', @WMR_OutputFcn, ...
'gui_LayoutFcn', [] , ...
'gui_Callback', []);
if nargin && ischar(varargin{1})
gui_State.gui_Callback = str2func(varargin{1});
end
if nargout
[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT
% --- Executes just before WMR is made visible.
function WMR_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% varargin command line arguments to WMR (see VARARGIN)
% Choose default command line output for WMR

handles.output = hObject;
% Update handles structure
guidata(hObject, handles);
% UIWAIT makes WMR wait for user response (see UIRESUME)
% uiwait(handles.figure1);
IsConnect = 0; % 1 = Connect,0 = Disconnect
%Declare global variable
global hand;
hand = handles;
handles.status_com = IsConnect;
guidata(hObject,handles);
% Initalize for RS232 Object
% --- Outputs from this function are returned to the command line.
function varargout = WMR_OutputFcn(hObject, eventdata, handles)
% varargout cell array for returning output args (see VARARGOUT);
% hObject handle to figure
% Get default command line output from handles structure
varargout{1} = handles.output;
% --- Executes on button press in Start.
function Start_Callback(hObject, eventdata, handles)
% hObject handle to Start (see GCBO)
IsConnect = handles.status_com;
if (IsConnect == 0), % if comport is disconnected so set up and open it.
IsConnect = 1;
% Setting for Serial Object

s = serial('COM3');
s.BytesAvailableFcnMode = 'terminator';
s.BytesAvailableFcn = @BytesAvailable_Callback;
% Open port to operate
fopen(s);
temp = s.status;
if(temp == 'open')
set(handles.editstr4,'String','Connect');
end;
end
% Save all data to all fields of handles.
handles.com = s;
handles.status_com = IsConnect;
%---------------------- Serial_Callback
function BytesAvailable_Callback(obj,event)
global hand i;
if(isvalid(obj))
ind = fscanf(obj);
c = textscan(ind,'%s%s%s%s');
st = char(c{1});
en = char(c{4});
if (st == 's')&&(en == 'e')
x = str2num(char(c{2}));
y = str2num(char(c{3}));
x_c = x*0.0001;
y_c = y*0.0001;
end
set(hand.editstr2,'String',num2str(x_c));

set(hand.editstr3,'String',num2str(y_c));
plot(hand.Trajectory,x_c,y_c,'r s','LineWidth',1.5);
legend(hand.Trajectory,'Reference','Real');
hold(hand.Trajectory,'on');
end
% --- Executes on button press in Plot.
function Plot_Callback(hObject, eventdata, handles)
% hObject handle to Plot (see GCBO)
load D:data1: px_out py_out
global hand;
axis(hand.Trajectory,[0 1.4 0 0.6]);
title(hand.Trajectory,'Trajectory');
xlabel(hand.Trajectory,'Px');
ylabel(hand.Trajectory,'Py');
plot(hand.Trajectory,px_out,py_out,'LineWidth',1.5);
grid(hand.Trajectory,'on');
hold(hand.Trajectory,'on');
% --- Executes on button press in Plot.
% --- Executes on button press in Exit.
function Exit_Callback(hObject, eventdata, handles)
% hObject handle to Exit (see GCBO)
if (IsConnect == 1)
s = handles.com;

%record(s,'off');
fclose(s);
delete(s);
clear s;
end;
close(WMR);
% --- Executes during object creation, after setting all properties.
function Trajectory_CreateFcn(hObject, eventdata, handles)
% hObject handle to Trajectory (see GCBO)
% handles empty - handles not created until after all CreateFcns called
% Hint: place code in OpeningFcn to populate Trajectory
function editstr1_Callback(hObject, eventdata, handles)
% hObject handle to editstr1 (see GCBO)
% Hints: get(hObject,'String') returns contents of editstr1 as text
% str2double(get(hObject,'String')) returns contents of editstr1 as a double
function editstr1_CreateFcn(hObject, eventdata, handles)
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

end
% --- Executes on selection change in Com.
function Com_Callback(hObject, eventdata, handles)
% hObject handle to Com (see GCBO)
% Hints: contents = cellstr(get(hObject,'String')) returns Com contents as cell array
% contents{get(hObject,'Value')} returns selected item from Com
function Com_CreateFcn(hObject, eventdata, handles)
% hObject handle to Com (see GCBO)

% Hint: popupmenu controls usually have a white background on Windows.
end
end

end
% --- Executes on button press in send.
function send_Callback(hObject, eventdata, handles)
% hObject handle to send (see GCBO)
s = handles.com;
if (IsConnect == 1)
send_data = get(handles.editstr1,'String');
send_data = str2num(send_data);
fwrite(s,send_data);
end
handles.com = s;
% --- Executes on button press in pushbutton10.
function pushbutton10_Callback(hObject, eventdata, handles)
% hObject handle to pushbutton10 (see GCBO)

% --- Executes on button press in forward.
function forward_Callback(hObject, eventdata, handles)
% hObject handle to forward (see GCBO)
s = handles.com;
if (IsConnect == 1)
send_data = 105;
end
handles.com = s;
guidata(hObject,handles)
% --- Executes on button press in right.
function right_Callback(hObject, eventdata, handles)
% hObject handle to right (see GCBO)
s = handles.com;
if (IsConnect == 1)
send_data = 107;
end
handles.com = s;
% --- Executes on button press in left.
function left_Callback(hObject, eventdata, handles)

% hObject handle to left (see GCBO)
s = handles.com;
if (IsConnect == 1)
send_data = 106;
end
handles.com = s;
% --- Executes on button press in reserve.
function reserve_Callback(hObject, eventdata, handles)
% hObject handle to reserve (see GCBO)
s = handles.com;
if (IsConnect == 1)
send_data = 108;
end
handles.com = s;
% --- Executes on button press in Up.
function Up_Callback(hObject, eventdata, handles)
% hObject handle to Up (see GCBO)

s = handles.com;
if (IsConnect == 1)
send_data = 103;
end
handles.com = s;
% --- Executes on button press in Down.
function Down_Callback(hObject, eventdata, handles)
% hObject handle to Down (see GCBO)
s = handles.com;
if (IsConnect == 1)
send_data = 104;
end
handles.com = s;
% --- Executes on button press in Run.
function Run_Callback(hObject, eventdata, handles)
% hObject handle to Run (see GCBO)
s = handles.com;
if (IsConnect == 1)

send_data = 100;
end
handles.com = s;
% --- Executes on button press in Line.
function Line_Callback(hObject, eventdata, handles)
% hObject handle to Line (see GCBO)
s = handles.com;
if (IsConnect == 1)
send_data = 102;
end
handles.com = s;
% --- Executes on button press in Read.
function Read_Callback(hObject, eventdata, handles)
% hObject handle to Read (see GCBO)
s = handles.com;
if (IsConnect == 1)
send_data = 101;
end

handles.com = s;
2. Quỹ đạo đường đi định sẵn cho AGV
Chương trình Matlab:
clear all;
close all;
clc;
xa = 0;
ya = 0;
xb = 1.4;
yb = 1.4;
theta_a = 0;
theta_b = 0;
pre_theta_r = 0;
n = sqrt((xa - xb)^2 + (ya - yb)^2);
syms s ;
for i = 0:10
s = i*0.1;
% xac dinh he so px.

a0 = xa;
a1 = n*cos(theta_a);
y1a = n*cos(theta_b);
a2 = 0;
a3 = 0;
a4 = 35*(xb - xa) - 20*a1 - 10*a2 - 4*a3 - 15*y1a;
a5 = -84*(xb - xa) + 45*a1 + 20*a2 + 6*a3 + 39*y1a;
a6 = 70*(xb - xa) - 36*a1 - 15*a2 - 4*a3 - 34*y1a;
a7 = -20*(xb - xa) + 10*a1 + 4*a2 + a3 + 10*y1a;
px = a0 + a1*s + a2*s^2 + a3*s^3 + a4*s^4 + a5*s^5 + a6*s^6 + a7*s^7;
% xac dinh he so py.
b0 = ya;
b1 = n*sin(theta_a);
y1b = n*sin(theta_b);
b2 = 0;
b3 = 0;
b4 = 35*(yb - ya) - 20*b1 - 10*b2 - 4*b3 - 15*y1b;
b5 = -84*(yb - ya) + 45*b1 + 20*b2 + 6*b3 + 39*y1b;
b6 = 70*(yb - ya) - 36*b1 - 15*b2 - 4*b3 - 34*y1b;
b7 = -20*(yb - ya) + 10*b1 + 4*b2 + b3 + 10*y1b;
py = b0 + b1*s + b2*s^2 + b3*s^3 + b4*s^4 + b5*s^5 + b6*s^6 + b7*s^7;
px_out(i+1) = px;
py_out(i+1) = py;
end
save D:data1: px_out py_out
% Ve quy dao duong cong
plot(px_out,py_out,'b', 'LineWidth',2);

title('Trajectory planning');
xlabel('Px');
ylabel('Py');
axis([0 1.5 0 1.5]);
grid on;
3. Giao tiếp I2C và RS232 cho vi điều khiển
Firmware sử dụng CCS - Cấu hình firmware:
Khai báo chip: chọn file <tenchip.h> có trong thu viện CCS, VD : #include
<18F4550.h> Khai báo cấu hình phần cứng: sử dụng từ khóa #fuse bao gồm các
tính năng :
LP, XT, HS, RC,PLL : Chọn kiểu dao dộng
WDT, NOWDT : Kiểm soát Watch Dog Timer.
PROTECT, NOPROTECT : Kiểm soát sao chép firmware
LVP,NOLVP: Kiểm soát lập trình nguồn thấp.
Khai báo cấu hình chức nang sử dụng từ khóa #use bao gồm các tính năng:
delay(clock=24000000) : Khai báo tốc dộ thạch anh
i2c(master, sda=PIN_B0, scl=PIN_B1, FAST): Khai báo cấu hình I2C
rs232(baud=19200, xmit=PIN_C6, rcv=PIN_C7): Khai báo cấu hình cổng COM
Khai báo dịch vụ ngắt, sử dụng từ khóa:
INT_SSP : Ngắt nối tiếp khi thanh ghi dịch vừa thực hiện xong 8 bit s.
INT_EXT : Ngắt ngoài
INT_TIMER1: Ngắt Timer
INT_EEPROM: Ngắt khi có sự kiện truy xuất EEPROM.
Chương trình chính main() Master:
while (TRUE)
{

Nội dung chương trình
}
Ghi dữ liệu tới Slave:
i2c_start(); // Master tạo tín hiệu Start
i2c_write(0xa0); // Gửi dịa chỉ và bít huớng R/W = 0 (Mode write) dến Slave
i2c_write(data); // Gửi dữ liệu byte thứ nhất
i2c_write(data1); // Gửi dữ liệu byte thứ hai
………………….
i2c_write(datan); // Gửi dữ liệu byte thứ n (n<=128)
i2c_stop(); // Master tạo tín hiệu Stop
Ðọc dữ liệu từ Slave:
i2c_start(); // Master tạo tín hiệu Start
i2c_write(0xa1); // Gửi dịa chỉ và bít huớng R/W = 1 (Mode read) dến Slave
buf = i2c_read(); // Ðọc dữ liệu byte thứ nhất
buf2= i2c_read(); // Ðọc dữ liệu byte thứ hai
……………………
buf1 = i2c_read(1,0); // Ðọc dữ liệu byte thứ n (n<=128) dồng thời gửi Not -ACK
i2c_stop(); // Master tạo tín hiệu Stop
Scope: Master phát dịa chỉ "0xa0" và 2 byte có nôi dung "1234"

firmware cho Slave:
Khai báo cấu hình cho Slave:
i2c(SLAVE, SDA=PIN_C4, SCL=PIN_C5, address=0xa0)
INT_SSP
Cấu hình ngắt nối tiếp :
enable_interrupts(GLOBAL);
enable_interrupts(INT_SSP);
Việc đọc, ghi dữ liệu của thiết bị Slave duợc thực thi trong ngắt tùy thuộc vào trạng
thái của đuờng truyền I2C.
state = i2c_isr_state(); // Ðọc trạng thái của I2C Slave sau sự kiện ngắt nối tiếp
State = 0 : Ðọc dịa chỉ và bit R/W
State = 1-0x7F: Ðọc dữ liệu từ Master bằng lệnh i2c_read()
State = 0x80: Ðọc dịa chỉ và bit R/W=1, gửi dữ liệu tới Master bằng lệnh
i2c_write();
State =0x81-0xFF : Nhận duợc ACK từ Master , Gửi dữ liệu tới Master.
Ðọc dữ liệu từ Master:
if(state < 0x80) {
incoming = i2c_read();
if(state == 1) data = incoming;
if(state == 2) data1 = incoming;
………………………………………
if(state == n) datan = incoming;
}
Ghi dữ liệu tới Master:
if(state == 0x80) i2c_write(data);
if(state == 0x81) i2c_write(data1);
…………………………………………
if(state == 0x8n) i2c_write(datan);

Chương trình CCS giao tiếp I2C giữa 2 vi điều khiển Pic 18f4550 và 18f4431:
Master:
/********************************I2C*******************************/
void write_slave(int8 slave_address,signed int8 temp)
{
i2c_start();
i2c_write(slave_address);
i2c_write(temp);
i2c_stop();
delay_us(10);
}
signed int8 read_slave(int8 slave_address1)
{
signed int8 value = 0;
i2c_start();
i2c_write(slave_address1+1);
value = i2c_read(1,0);
i2c_stop();
delay_us(10);
return value;
}
Slave:
/********************************I2C*******************************/
#int_SSP
void SSP_isr(void)
{
BYTE incoming, state;
state = i2c_isr_state();
if(state < 0x80) //Master is sending data

{
incoming = i2c_read();
if(state == 0) //First received byte is address
address = incoming;
if(state == 1) //Second received byte is data
comd = incoming;
}
else
{
if(state == 0x80) //Master is requesting data
{
i2c_write(sen);
}
}
}
Chương trình truyền nhận dữ liệu giữa Pic 18f4550 và máy tính
#INT_RDA
void isr_rda()
{
temp1 = getc();
}
void send()
{
printf("%s %ld %ld %sn","s",x[j],y[j],"e");
delay_ms(100);
}

Đều khiển phi tuyến hệ agv

Recommended

Recommended

More Related Content

What's hot

What's hot (20)

Similar to Đều khiển phi tuyến hệ agv

Similar to Đều khiển phi tuyến hệ agv (20)

More from Man_Ebook

More from Man_Ebook (20)

Recently uploaded

Recently uploaded (20)