Same Cothanh

lOMoARcPSD|38101887
DO AN TOT Nghiep Nguyen Thanh LONG
Đồ án cơ điện tử (Trường Đại học Bách khoa Hà Nội)
Scan to open on Studocu
Studocu is not sponsored or endorsed by any college or university

Downloaded by DHTD14A6HN_06_NGUY?N (nhdat.dhtd14a6hn@sv.uneti.edu.vn)
lOMoARcPSD|38101887
BỘ GIÁO DỤC VÀ ĐÀO TẠO CỘNG HOÀ XÀ HỘI CHỦ NGHĨA VIỆT NAM
TRƯỜNG ĐHBK HÀ NỘI Độc lập - Tự do - Hạnh phúc
VIỆN CƠ KHÍ
BỘ MÔN CSTKM & ROBOT
NHIỆM VỤ
ĐỒ ÁN TỐT NGHIỆP
Họ và tên: NGUYỄN THÀNH LONG
Khóa: 62 Viện : Cơ khí
Bộ môn: CƠ SỞ THIẾT KẾ MÁY VÀ RÔ BỐT
Ngành: CƠ ĐIỆN TỬ
1. Tên đề tài tốt nghiệp
THIẾT KẾ CHẾ TẠO RÔBỐT TỰ HÀNH ĐA HƯỚNG

KIỂU BÁNH XE MECANUM
2. Nội dung các phần thuyết minh và tính toán
Chương 1 Phương trình động học rôbốt tự hành đa hướng kiểu bánh xe
Menanum
Chương 2 Thiết kế bộ điều khiển phản hồi trạng thái tuyến tính cho Rôbốt tự
hành đa hướng kiểu bánh xe Mecanum
Chương 3 Chế tạo thực nghiệm
3. Giáo viên hướng dẫn
Phần Nội dung Giảng viên
Phương trình động học rôbốt tự hành đa

Chương 1 TS. Nguyễn Hồng Thái
hướng kiểu bánh xe Menanum

lOMoARcPSD|38101887
Thiết kế bộ điều khiển phản hồi trạng thái

Chương 2 tuyến tính cho Rôbốt tự hành đa hướng TS. Nguyễn Hồng Thái
kiểu bánh xe Mecanum
Chương 3 Chế tạo thực nghiệm TS. Nguyễn Hồng Thái
4. Ngày giao nhiệm vụ thiết kế Ngày 04 tháng 09 năm 2021

5. Ngày hoàn thành nhiệm vụ Ngày 28 tháng 02 năm 2022
Hà Nội, ngày tháng năm 2022
CHỦ NHIỆM BỘ MÔN CÁN BỘ HƯỚNG DẪN
(ký, ghi rõ hộ tên) (ký, ghi rõ họ tên)
ii

lOMoARcPSD|38101887
BỘ GIÁO DỤC VÀ ĐÀO TẠO

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
VIỆN CƠ KHÍ
BỘ MÔN CƠ SỞ THIẾT KẾ MÁY VÀ ROBOT
-------------------------------------------------------------------------
NHẬN XÉT
CỦA GIẢNG VIÊN HƯỚNG DẪN
Họ và tên sinh viên: Nguyễn Thành Long Ngành: Cơ điện tử Khoá: 62
Người nhận xét: TS. NGUYỄN HỒNG THÁI

Đơn vị công tác: BM. CƠ SỞ THIẾT KẾ MÁY & RÔ BỐT, VIỆN CƠ KHÍ
Địa chỉ: D3-304, ĐHBKHN, Số 1, Đại Cồ Việt
E- mail: thai.nguyenhong@hust.edu.vn
NỘI DUNG NHẬN XÉT
NHẬN XÉT CHUNG

Người hướng dẫn rất hài lòng với kết quả nghiên cứu và tinh thần làm việc của sinh viên:
Nguyễn Thành Long sau thời gian làm đô án, tôi có một số nhận xét sau:
+ Có khả năng đọc tài liệu, tìm tài liệu và tìm hiểu những vấn đề mới.
+ Có kỹ năng thiết kế và lập trình tốt.
+ Có tư duy logic và tư duy tổng hợp.
+ Có khả năng làm việc độc lập và làm việc nhóm.
+ Có sự đam mê, khám phá và tìm tòi và mong muốn tìm hiểu về lĩnh vực rô bốt.
Người hướng dẫn đánh giá chất lượng của đồ án là rất tốt. Đối với trình độ đại học nhìn
về mặt tổng thể có thể tương đương một luận văn thạc sỹ chuyên ngành cơ điện tử bởi vì: có
nghiên cứu lý thuyêt, tính toán mô phỏng và thực nghiệm kiểm chứng một điều mà ít có đồ án
tốt nghiệp đại học đạt được.
Về mặt bố cục, mục tiêu đặt ra và cách giải quyết vẫn đè là logic và mạch lạc. Điều đó
dẫn đến chất lượng đồ án là rất tốt.
Về mặt khối lượng: khối lượng tính toán, thực nghiệm là vượt mong đợi đối với 1 đồ án
tốt nghiệp đại học.
Về mặt khoa học: Tham gia nghiên cứu với thầy giáo và một phần nội dung của đồ án đã
được công bố trong hội nghị quốc tế index Scopus (kèm ở cuối đồ án)
iii

lOMoARcPSD|38101887
KẾT LUẬN
Với chất lượng và kết quả đạt được của đồ án đạt được trong bối cảnh phải nối là dình dập
đi được lên phòng thí nghiệm thì kết quả này ngoài mong đợi. Do đó tôi đề nghị để sinh viên
Nguyễn Thành Long được bảo vệ trước hội đồng đánh giá tốt nghiệp.
Điểm đánh giá: Nguyễn Thành Long 10/10
Hà nội, ngày 17 tháng 02 năm 2022.
Người nhận xét
(Ký, ghi rõ họ tên)
Nguyễn Hồng Thái
iv

lOMoARcPSD|38101887
BỘ GIÁO DỤC VÀ ĐÀO TẠO

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
VIỆN CƠ KHÍ
BỘ MÔN CƠ SỞ THIẾT KẾ MÁY VÀ ROBOT
--------------
NHẬN XÉT
CỦA NGƯỜI PHẢN BIỆN
Họ và tên sinh viên: Nguyễn Thành Long
Ngành: Cơ điện tử Khoá: 62
Với đề tài: THIẾT KẾ CHẾ TẠO RÔBỐT TỰ HÀNH ĐA HƯỚNG KIỂU BÁNH
XE MECANUM
Giảng viên hướng dẫn: TS. NGUYỄN HỒNG THÁI
Đơn vị công tác: BM. CƠ SỞ THIẾT KẾ MÁY & RÔ BỐT, VIỆN CƠ KHÍ
Địa chỉ: D3-304, ĐHBKHN, Số 1, Đại Cồ Việt.
E- mail: thai.nguyenhong@hust.edu.vn
Người nhận xét:
Đơn vị công tác:
NỘI DUNG NHẬN XÉT
1 Nội dung thiết kế tốt nghiệp

...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................

lOMoARcPSD|38101887
2 Nhận xét của người phản biện

...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
...................................................................................................................................
Hà Nội, ngày tháng năm 2022
Người duyệt ký
vi

lOMoARcPSD|38101887
LỜI NÓI ĐẦU
Trong thời gian tìm hiểu và nghiên cứu dưới sự hướng dẫn tận tình của
TS. Nguyễn Hồng Thái và sự góp ý đầy bổ ích của các thầy trong bộ môn Cơ sở Thiết kế
máy và Rô bốt cũng như các thầy cô trong trường Đại Học Bách Khoa Hà Nội, em đã hoàn
thành đồ án tốt nghiệp của mình. Qua quá trình làm đồ án, em đã củng cố được rất nhiều kiến
thức cơ bản, tìm hiểu thêm những vấn đề mới cũng như phương pháp tư duy, tìm tòi khi giải
quyết một vấn đề cụ thể, đặc biệt như tìm hiểu về điều khiển động lực học Rôbốt. Song vì thời
gian có hạn, hạn chế về mặt kiến thức và kinh nghiệm làm việc nên đồ án không tránh khỏi
những sai sót, em rất mong nhận được những ý kiến đóng góp của các thầy và các bạn để em
hoàn thiện hơn về mặt chuyên môn cũng như cung cách làm việc. Qua đây, em xin gửi lời cảm
ơn chân thành đến các thầy trong bộ môn Cơ sở Thiết kế máy và Rô bốt và đặc biệt em xin
bày tỏ lòng biết ơn sâu sắc nhất của mình đến thầy TS. Nguyễn Hồng Thái đã tận tình và
dành rất nhiều tâm huyết cho em. Em xin chúc thầy và gia đình thật nhiều sức khỏe nhiều
niềm vui và hạnh phúc trong cuộc sống.
Hà Nội, ngày ... tháng năm 2022
Sinh viên thực hiện

(ký và ghi rõ họ tên)
Nguyễn Thành Long
vii

lOMoARcPSD|38101887
MỤC LỤC
LỜI NÓI ĐẦU .......................................................................................................................... vii
DANH MỤC CÁC HÌNH VẼ VÀ ĐỒ THỊ ................................................................................ x
DANH MỤC CÁC BẢNG ...................................................................................................... xiii
LỜI MỞ ĐẦU ............................................................................................................................. 1
Chương 1 ..................................................................................................................................... 4
PHƯƠNG TRÌNH ĐỘNG HỌC RÔBỐT TỰ HÀNH ĐA HƯỚNG KIỂU BÁNH XE
MECANUM ................................................................................................................................ 4
1.1 GIỚI THIỆU CHUNG....................................................................................................... 4
1.2 PHƯƠNG TRÌNH ĐỘNG HỌC CỦA AVG ĐIỀU HƯỚNG LINH HOẠT KIỂU
BÁNH XE ĐA HƯỚNG MECANUM ................................................................................... 7
1.2.1 Bài toán động học thuận ........................................................................................... 10
1.2.2 Bài toán động học ngược .......................................................................................... 10
1.3 THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG BẰNG ĐƯỜNG CONG NURBS ............... 11
1.3.1 Đường cong NURBS ................................................................................................ 11
1.3.2 Đường cong và hàm NURBS ................................................................................... 13
1.3.3 Đạo hàm đường cong NURBS ................................................................................. 15
1.3.4 Thiết kế đường cong ................................................................................................. 16
1.4 KHẢO SÁT QUỸ ĐẠO CHUYỂN ĐỘNG CỦA ĐƯỜNG CONG BẤT KỲ .............. 19
1.4.1 Thiết kế quỹ đạo ....................................................................................................... 19
1.4.2 Khảo sát các chuyển động của xe ............................................................................. 20
KẾT LUẬN CHƯƠNG 1 ...................................................................................................... 29
Chương 2 ................................................................................................................................... 30
THIẾT KẾ BỘ ĐIỀU KHIỂN PHẢN HỒI TRẠNG THÁI TUYẾN TÍNH CHO RÔBỐT TỰ
HÀNH ĐA HƯỚNG KIÊU BÁNH XE MECANUM .............................................................. 30
2.1 THIẾT LẬP MÔ HÌNH SAI SỐ ĐỘNG HỌC ............................................................... 30
2.2 THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN PHẢN HỒI TRẠNG THÁI TUYẾN TÍNH.... 31
2.3 THIẾT KẾ QUỸ ĐẠO ĐƯỜNG CONG NURBS .......................................................... 33
2.4 KẾT QUẢ MÔ PHỎNG SIMULINK MATLAB ........................................................... 34
KẾT LUẬN CHƯƠNG 2 ...................................................................................................... 40
Chương 3 ................................................................................................................................... 41
CHẾ TẠO THỰC NGHIỆM ..................................................................................................... 41
3.1 MÔ TẢ CẤU TRÚC CỦA RÔBỐT ............................................................................... 41
3.1.1 Cấu trúc thân xe ........................................................................................................ 41
3.1.2 Phần mềm chương trình............................................................................................ 43
3.1.3 Bộ điều khiển ............................................................................................................ 43
3.2 CẤU TRÚC BỘ DẪN ĐỘNG ........................................................................................ 43
3.3 CẤU TRÚC PHẦN CỨNG CỦA BỘ ĐIỀU KHIỂN ..................................................... 45
3.4 THÔNG SỐ KỸ THUẬT CỦA CÁC THIẾT BỊ PHẦN CỨNG ................................... 46
3.5 CHẾ TẠO THỰC NGHIỆM ........................................................................................... 50
3.6 THỰC NGHIỆM THỬ NGHIỆM ................................................................................... 51
3.6.1 Chế độ bằng tay ........................................................................................................ 51
3.6.2 Chế độ điều khiển tự động bám theo quỹ đạo .......................................................... 56
3.6.3 Thực nghiệm quét bản đồ ......................................................................................... 58
KẾT LUẬN CHƯƠNG 3 ...................................................................................................... 59
Chương 4 ................................................................................................................................... 61
viii

lOMoARcPSD|38101887
LẬP TRÌNH BỘ ĐIỀU KHIỂN ................................................................................................ 61

4.1 HỆ ĐIỀU HÀNH ROS .................................................................................................... 61
4.2 THIẾT KẾ PHẦN MỀM ĐIỀU KHIỂN ĐỘNG CƠ VÀ GIAO TIẾP IMU .................. 62
4.2.1 Sơ đồ kết nối động cơ và IMU với máy tính nhúng ................................................. 62
4.2.2 Thiết kế phần mềm điều khiển động cơ ................................................................... 63
4.2.3 Thiết kế phần mềm giao tiếp với IMU ..................................................................... 65
4.2.4 Chương trình của bo mạch nhúng Arduino Mega 2560 ........................................... 66
4.3 THUẬT TOÁN ĐIỀU KHIỂN........................................................................................ 71
4.4 THU THẬP DỮ LIỆU LIDAR VÀ QUÉT BẢN ĐỒ .................................................... 74
4.5 THỬ NGHIỆM ................................................................................................................ 81
KẾT LUẬN CHƯƠNG 4 ...................................................................................................... 85
KẾT LUẬN ĐỒ ÁN .................................................................................................................. 86
I) Các kết quả đạt được .......................................................................................................... 86
II) Kiến nghị các nội dung cần hoàn thiện............................................................................. 86
DANH MỤC TÀI LIỆU THAM KHẢO .................................................................................. 87
I) Tài liệu tham khảo ............................................................................................................. 87
II) Chi tiết bài báo đã công bố ............................................................................................... 88
ix

lOMoARcPSD|38101887
DANH MỤC CÁC HÌNH VẼ VÀ ĐỒ THỊ
Hinh Nội dung Trang
Hình 1.1 Xe tự hành AGV 4
Hinh 1.2 Bánh răng Mecanum 5

Các phương án điều khiển Rôbốt tự hành đa hướng kiểu bánh xe
Hinh 1.3 6
Mecanum
Hinh 1.4 AGV sử dụng bánh xe Mecanum 6
Hinh 1.5 Vị trí và hướng của Rôbốt tự hành trong hệ quy chiếu cố định 7
Hinh 1.6 Động học AGV đa hướng kiểu bánh xe Mecanum 8
Hinh 1.7 Đường cong NURBS 11
Đường cong NURBS nới n + 1 = 5, k = 3, vector nút mở

Hinh 1.8 13
[X] = [0 0 0 1 2 2 2 2] và [h] = [1 1 h3 1 1]
Hinh 1.9 14
[X] = [0 0 0 1 2 2 2 2] và [h] = [1 1 h3 1 1]
Hinh 1.10 14
[X] = [0 1 2 3 4 5 6 7] và [h] = [1 1 h3 1 1]

Hinh 1.11 15
[X] = [0 1 2 3 4 5 6 7 8] và [h] = [1 1 h3 1 1]
Hinh 1.12 Lộ trình của xe 19
Hinh 1.13 Quỹ đạo của xe theo đường cong NURBS 20
Hinh 1.14 Đồ thị góc quay của xe khi V = 1 m/s 22
Hinh 1.15 Đồ thị biểu diễn bán kính cong khi V = 1 m/s 22
Hinh 1.16 Đồ thị vận tốc góc Ω của xe khi V = 1 m/s 23
Hinh 1.17 Đồ thị vận tốc góc ω1 khi V = 1 m/s 23

lOMoARcPSD|38101887
Hinh 1.21 Đồ thị góc quay của xe khi V = 0,3 m/s 26
Hinh 1.22 Đồ thị biểu diễn bán kính cong khi V = 0,3 m/s 26
Hinh 1.23 Đồ thị vận tốc góc Ω của xe khi V = 0,3 m/s 27
Hinh 1.24 Đồ thị vận tốc góc ω1 khi V = 0,3 m/s 27
Hinh 2.1 Sơ đồ khối hệ thống điều khiển 33
Kết quả mô phỏng quỹ đạo chuyển động của rôbốt theo đường cong
Hinh 2.2 34
NURBS
Sai số tư thế (vị trí và hướng) trung bình khi thay đổi ζ , g = 1; K1 >
Hinh 2.3 35
K2
Sai số tư thế (vị trí và hướng) trung bình khi thay đổi ζ , g = 1; K1 <
Hinh 2.4 35
K2
Sai số tư thế (vị trí và hướng) trung bình khi thay đổi g, ζ = 0,9; K1 >
Hinh 2.5 36
K2
Sai số tư thế (vị trí và hướng) trung bình khi thay đổi g, ζ = 0,9; K1 <
Hinh 2.6 36
K2
Quỹ đạo chuyển động của rôbốt đa hướng với (a) Quỹ đạo thực tế và
Hinh 2.7 quỹ đạo mong muốn, (b) Bán kính cong của quỹ đạo chuyển động 38
mong muốn
Sự di chuyển của robot đa hướng với (a) Tốc độ ban đầu của robot
Hinh 2.8 39
(b) Vận tốc góc thực tế và mong muốn của robot
Hinh 2.9 Vận tốc góc của bánh xe 1,bánh 2, bánh 3, bánh 4 39
Hinh 2.10 Sai số vị trí và hình dáng của robot trong quá trình di chuyển 40
Hinh 3.1 Cấu trúc của Rôbót 41
Hinh 3.2 Tầm Mica đế dày 10 mm 42
Hinh 3.3 Tấm đỡ mạch 43
Hinh 3.4 Kết cấu cụm bánh xe dẫn động 44
xi

lOMoARcPSD|38101887
Hinh 3.5 Bánh xe dẫn động 44
Hinh 3.6 Kích thước gá liên kết động cơ với thân xe 45
Hinh 3.7 Sơ đồ cấu trúc phần cứng của bộ điều khiển 45
Hinh 3.8 Ảnh chụp tấm đế của Rôbốt sau khi chế tạo 50
Hinh 3.9 Ảnh chụp tổng thế Rôbốt sau khi chế tạo 51
Hinh 3.10 Đồ thị mong muốn và thực tế của Rôbốt 57
Hinh 3.11 Sai số vị trí của Rôbốt trong thực nghiệm 57
Hinh 3.12 Kích thước của phỏng kín và vị trí các vật cản trong phòng 59
Hinh 3.13 Bản đồ Rôbốt thiết lập được 59
Hình 4.1 Sơ đồ hệ thống điều khiển của ROS 62
Hình 4.2 Sơ đồ kết nối máy tính nhúng và bo mạch nhúng 63
Hình 4.3 Dạng tín hiệu của 2 kênh Encoder trong một động cơ 64
Hình 4.4 Xử lý dữ liệu của IMU 66

Phương pháp quét bản đồ; a) Dữ liệu lidar quét; b) Dữ liệu bản đồ có
Hình 4.5 75
sẵn; c) Vị trí của Lidar trong bản đồ
Hình 4.6 Quỹ dạo thực nghiệm 83
Hình 4.7 Vận tốc và vận tốc góc rôbốt trong thực nghiệm 83
Hình 4.8 Vận tốc góc của các bánh xe trong thực nghiệm 84
Hình 4.9 Sai số vận tốc của thực nghiệm với mong muốn 84
Hình 4.10 Sai số vị trí của thực nghiệm với mong muốn 85
xii

lOMoARcPSD|38101887
DANH MỤC CÁC BẢNG
Bảng Nội dung Trang
Bảng 1.1 Đa giác điều khiển đường cong NURBS 15
Bảng 1.2 Điểm điều khiển đường cong NURBS 20
Bảng 2.1 Các nút điều khiển quỹ đạo đường cong NURBS 33
Bảng 3.1 Các thông số kỹ thuật thiết bị phần cứng của Rôbốt 46
Bảng 3.2 Thực nghiệm khả năng di chuyển theo phương x 52
Bảng 3.3 Thực nghiệm khả năng di chuyển theo phương y 54
Bảng 3.4 Thực nghiệm khả năng thay đổi tư thế của Rôbốt 55
Bảng so sánh các kích thước thực tế và kích thước bản đồ rôbốt thiết
Bảng 3.5 59
lập
Bảng 4.1 Xác định góc quay của một động cơ tại các thời điểm 64
xiii

lOMoARcPSD|38101887
LỜI MỞ ĐẦU
I. TÍNH CẤP THIẾT CỦA ĐỒ ÁN
Ngày nay nền kinh tế thế giới đang bước sang giai đoạn phát triển kinh tế số với cuộc
cách mạng công nghiệp lần thứ 4, trong đó Rôbốt tự hành trở thành một phần tử không thể
thiếu trong các nhà máy sản suất thông minh. Trên thế giới các lĩnh vực được ứng dụng phổ
biến Rôbốt tự hành trong công tác hậu cần như: công nghiệp, logistics, y tế. Một trong những
tính chất đặc biệt làm cho rôbốt tự hành trở nên không thể thay thế là khả năng tự động điều
khiển mà không cần tới sự can thiệp của con người, giúp thay thế con người thực hiện những
công việc nguy hiểm và đặc thù. Trong số các rôbốt đa dạng đang tồn tại, rôbốt tự hành đa
hướng có khả năng di chuyển theo bất kỳ hướng nào. Việc xử lý và di chuyển linh hoạt, thêm
với khả năng kiểm soát thời gian thực, đã trở thành một phần không thể thiếu trong các hệ
thống sản xuất hiện đại.
Nhiều thiết kế xe đa hướng hoặc gần đa hướng đã được đề xuất. Nhìn chung, chúng có
thể được chia thành hai cách tiếp cận: thiết kế bánh xe thông thường và thiết kế bánh xe đặc
biệt. Bánh xe thông thường về mặt cơ học đơn giản, có khả năng chịu tải cao và khả năng
chịu đựng các bề mặt làm việc không đều cao. Tuy nhiên, loại hệ thống này không thực sự đa
hướng. Các thiết kế đã được đề xuất để đạt được khả năng di chuyển gần như đa hướng bằng
cách sử dụng bánh xe thông thường, thiết kế phổ biến nhất là những thiết kế sử dụng bánh lái.
Các xe dựa trên thiết kế này có ít nhất hai bánh chủ động, mỗi bánh có cả cơ cấu dẫn động và
dẫn động lái. Chúng có thể di chuyển theo bất kỳ hướng nào từ bất kỳ cấu hình nào. Tuy
nhiên, loại hệ thống này không thực sự đa hướng vì nó cần dừng lại và định hướng lại bánh xe
theo hướng mong muốn. Hầu hết các thiết kế bánh xe đặc biệt đều dựa trên khái niệm đạt
được lực kéo theo một hướng và cho phép chuyển động thụ động theo hướng khác, do đó cho
phép linh hoạt hơn trong môi trường tắc nghẽn. Cấu trúc ba bánh xe sử dụng 'Omni-wheel' và
cấu trúc bốn bánh sử dụng ‘bánh xe Mecanum’ là những ví dụ tiêu biểu của nền tảng rôbốt đa
hướng. Đặc biệt, rôbốt tự hành đa hướng với Bánh xe Mecanum được sử dụng trong trường
hợp yêu cầu sự ổn định, loại bánh xe này được sử dụng trong các ứng dụng rôbốt đòi hỏi mức
độ cơ động cao. Bánh xe đa hướng có 3 bậc tự do: chuyển động quay của bánh xe, chuyển
động quay của con lăn, và trượt chuyển động về trục thẳng đứng đi qua tiếp điểm [1], [2].
Trong hầu hết các trường hợp, các bánh xe đa hướng được kết nối với thân rôbốt và chúng
không xoay để đánh lái, việc lái xe có thể được thực hiện bằng cách kết hợp các tốc độ quay
của các bánh xe. Bánh xe Mecanum có những ưu điểm sau: thiết kế nhỏ gọn, khả năng chịu tải
cao, điều khiển đơn giản, tốc độ thấp và lực đẩy thấp khi di chuyển theo đường chéo. Nhược
điểm có thể kể đến: tiếp xúc không liên tục, độ nhạy cao với các bất thường bề mặt chuyển

lOMoARcPSD|38101887
động, thiết kế phức tạp [3]. Các rôbốt tự hành đa hướng đã được quan tâm nghiên cứu [4], [5],
[6]. Trong danh mục các rôbốt tự hành này, rôbốt tự hành đa hướng sử dụng bánh xe
Mecanum có thể thực hiện các nhiệm vụ quan trọng trong môi trường đông đúc có chướng
ngại vật tĩnh và động, các lối đi trong nhà, chẳng hạn như văn phòng, xưởng, nhà máy, nhà
kho hoặc được sử dụng để tạo điều kiện chăm sóc người già hoặc bệnh nhân nhập viện, v.v ...
[5], [7]. Chính vì vậy, nội dung đồ án sẽ thiết kế chế tạo rôbốt tự hành bánh xe Mecanum cùng
với một bộ điều khiển phản hồi trạng thái tuyến tính áp dụng vào rôbốt.
Từ những phân tích ở trên, cùng với định hướng của TS. Nguyễn Hồng Thái với mục
đích góp một phần nhỏ bé trong việc thiết kế rôbốt tự hành đa hướng kiểu bánh xe Mecanum
tác giả đã lựa chọn đề tài đồ án “ Thiết kế chế tạo rôbốt tự hành đa hướng kiểu bánh xe
Mecanum”.
II. MỤC TIÊU CỦA ĐỒ ÁN
Đồ án đặt ra các mục tiêu phải giải quyết cụ thể như sau:
(1) Thiết lập mô hình động học rôbốt tự hành đa hướng kiểu bánh xe Mecanum.
(2) Thiết kế và mô phỏng bộ điều khiển phản hồi trạng thái tuyến tính của rôbốt tự hành
đa hướng kiểu bánh xe Mecanum.
(3) Chế tạo thực nghiệm rôbốt đa hướng kiểu bánh xe Mecanum.
III. PHẠM VI NGHIÊN CỨU
Phạm vi nghiên cứu của đồ án gồm các vấn đề sau:

+ Về mặt hình học: Thiết lập phương trình động học cho rôbốt tự hành đa hướng kiểu bánh xe
Mecanum.
+ Trình bày các phương pháp thiết kế bộ điều khiển phản hồi trạng thái tuyến tính cho Rôbốt
tự hành. Thiết kế và mô phỏng bộ điều khiển phản hổi trạng thái tuyến tính cho rôbốt tự hành
đa hướng kiểu bánh xe Mecanum.
IV. Ý NGHĨA KHOA HỌC VÀ THỰC TIỄN
4.1 Ý nghĩa khoa học
Rôbốt tự hành đa hướng là một đề tài nóng được rất nhiều nhà nghiên cứu khoa học
quan tâm, nghiên cứu thiết kế và chế tạo. Các kết quả của đồ án là cơ sở để tiếp tục nghiên cứu
sâu hơn về các phương pháp thiết kế bộ điều khiển của rôbốt đa hướng Mecanum. Kết quả

lOMoARcPSD|38101887
nghiên cứu lý thuyết, mô phỏng và thực nghiệm đã đóng góp một phần vào quá trình xây dựng
tài liệu kỹ thuật cho thiết kế rôbốt tự hành đa hướng Mecanum.
4.2 Ý nghĩa thực tiễn
Trên cơ sở nghiên cứu lý thuyết, mô phỏng và thực nghiệm, đồ án đã thiết kế bộ điều

khiển phản hồi trạng thái tuyến tính cho rôbốt đa hướng kiểu bánh xe Mecanum. Khảo sát và
đánh giá ảnh hưởng của các tham số điều khiển đến độ chính xác của bộ điều khiển, từ đó có
thể lựa chọn các tham số điều khiển để tối ưu hóa, tăng tính chính xác và chất lượng của bộ
điều khiển, từ đó nâng cao khả năng làm việc của Rôbốt tự hành.
V. NỘI DUNG CỦA ĐỒ ÁN
Chương 1: Thiết lập phương trình động học cho rôbốt tự hành đa hướng kiểu bánh
xe Mecanum
Trong chương này đồ án tiến hành thiết lập mô hình động học lý thuyết của rôbốt tự hành đa
hướng kiểu bánh xe Mecanum. Thiết kế quỹ đạo đường cong NURBS cho rôbốt. Kết quả của
chương này là cơ sở cho các tính toán, thiết kế và mô phỏng ở các chương tiếp theo của đồ án.
Chương 2: Thiết kế bộ điều khiển phản hồi trạng thái tuyến tính
Trong chương này đồ án tiến hành trình bày phương pháp thiết kế bộ điều khiển phản hồi
trạng thái tuyến tính. Tiến hành mô phỏng bộ điều khiển bám theo quỹ đạo tham chiếu được
thiết kế trong chương 1.
Chương 3: Chế tạo rôbốt thực nghiệm
Trong chương này đồ án tiến hành thiết kế và chế tạo rôbốt đa hướng kiểu bánh xe Mecanum.
Chương 3: Lập trình bộ điều khiển
Trong chương này đồ án tiến hành lập trình rôbốt hoạt động, đưa bộ điều khiển đã được thiết
kế trong chương 2 vào trong rôbốt thực nghiệm. Kiểm tra mức độ hoạt động của thuật toán.
KẾT LUẬN ĐỒ ÁN
Phần này sẽ tóm tắt các kết quả đạt được của đồ án về lý thuyết, mô phỏng và thực
nghiệm cũng như những đóng góp mà đồ án đã thực hiện và đề ra các vấn đề hạn chế để tiếp
tục nghiên cứu và hoàn thiện trong tương lai.

lOMoARcPSD|38101887
Chương 1
PHƯƠNG TRÌNH ĐỘNG HỌC RÔBỐT TỰ HÀNH ĐA HƯỚNG KIỂU

BÁNH XE MECANUM
1.1 GIỚI THIỆU CHUNG
1.1.1 Xe tự hành AGV
Xe tự hành AGV, robot AGV, tiếng anh là

Automation Guided Vehicle, là loại xe sử dụng các
công nghệ dẫn đường để vận chuyển hàng hóa,
nguyên vật liệu đến những địa điểm đã dược đánh
dấu sẵn mà không cần đến sự can thiệp của con
người. Xe tự hành AGV hay còn được gọi là robot
kéo hàng, robot vận chuyển hàng tự động (Hình
1.1). Xe tự hành AGV là một phần của quá trình
phát triển nhà máy thông minh (Smart Factory)
cũng như Tự động hóa trong công nghiệp.
Cùng với sự mở rộng sản xuất của các nhà Hình 1.1 Xe tự hành AGV
máy, các yêu cầu vận chuyển, bốc dỡ và tích trữ
hàng hóa trở nên ngày càng quan trọng hơn. Với xu hướng tự động hóa thay thế nhân công
trong các lĩnh vực sản xuất, các hệ thống vận chuyển bên trong các nhà máy dưới dạng băng
tải hoặc AGV ngày càng trở nên quan trọng trong toàn bộ lĩnh vực vận chuyển vật liệu. Đặc
biệt là trong các nhà máy sản xuất linh kiện điện tử, sản phẩm khuôn đúc, chi tiết lắp ráp, dược
mỹ phẩm, thực phẩm – đồ uống,… .
Hiện nay xe tự hành AGV được sử dụng trong hoạt động vận chuyển nguyên vật liệu của
nhiều ngành nghề như: Công nghiệp ô tô, điện tử, Logistic, dược phẩm, y tế, hàng tiêu
dùng,…
Lợi ích của xe tự hành AGV, robot AGV được sử dụng trong các nhà máy sản xuất, nhà
kho thông minh mang lại nhiều lợi ích cho doanh nghiệp so với các phương pháp vận chuyển
hàng truyền thống, một số ưu điểm của AGV như sau:
1. Giảm chi phí nhân công
AGV có thể hoạt động hoàn toàn độc lập mà không cần đến sự can thiệp của công nhân
và đáp ứng khối lượng hàng hóa với tải trọng lớn. Ngoài ra máy không cần nghỉ và có thể hoạt
động 24/7, cho phép nhanh chóng thu lại khoản đầu tư ban đầu.

lOMoARcPSD|38101887
2. Tính an toàn
- AGV hoạt động một cách có kiểm soát, với khả năng tăng/ giảm tốc mượt, ổn định
được giám sát.
- AGV được lập trình sự an toàn đặt lên hàng đầu, được trang bị các cảm biến, laser cho
phép chúng hoạt động an toàn xung quanh công nhân và máy móc trong nhà máy
- AGV hoạt động tốt trong các môi trường mà con người không thể tiếp cận và nguy
hiểm như xử lý các chất độc hại, làm việc trong nhiệt độ khắc nghiệt và di chuyển vật liệu
nặng.
3. Nâng cao năng suất lao động
- AGV có thể hoạt động 24/7 và trong điều kiện con người không thể làm việc, điều này
làm tăng năng suất và hiệu quả công việc.
4. Dễ dàng thay đổi và mở rộng modul
Hầu hết các xe AGV có thể thay đổi về modul để đáp ứng yêu cầu của người sử dụng
như: nâng cấp, giảm tải trọng, thay đổi quãng dường,…
Bánh xe đa năng Mecanum
Bánh xe đa năng Mecanum được phát minh bởi Ilon (một
kỹ sư người Thụy Điển) vào năm 1973 tại Công ty Mecanum
và được ứng dụng vào thiết kế Robot tự hành lần đầu tiên vào
năm 1997 (Hình 1.2). Khi truyền momen vào trục bánh xe các
con lăn trên bánh xe sẽ tiếp xúc với sàn hình thành hai thành
 
phần vận tốc: V1 theo hướng di chuyển của bánh xe và V L1 có
phương vuông góc với trục con lăn và có chiều phụ thuộc vào
chiều momen tác dụng. Vì vậy, khi điều khiển phối hợp các
động cơ dẫn động của các bánh xe sẽ tạo thành véc tơ vận tốc
Hình 1.2 Bánh răng Mecanum
tổng hợp đẩy robot tự hành di chuyển theo các hướng khác
nhau làm tăng tính linh hoạt của AGV.

lOMoARcPSD|38101887
Hình 1.3 Các phương án điều khiển Robot tự hành đa hướng kiểu bánh xe Mecanum
Ngày nay, để tăng tính linh hoạt trong vận chuyển vật liệu và hậu cần phục vụ sản xuất
kiểu bánh xe này cũng được ứng dụng trong thiết kế các AGV trong những trường hợp không
đủ không gian dành làm đường đi cho AGV đặc biệt là khoảng không để làm những chỗ quay
vòng
Hình 1.4 AGV sử dụng bánh xe Mecanum
Bài toán động học Robot tự hành

Bài toán động học của Robot tự hành AGV là bài toán chỉ xét đến mô hình toán học mô tả
chuyển động của robot mà không quan tâm đến nguyên nhân gây ra chuyển động (lực hoặc
mô men). Do đó, đây là mô hình toán học thể hiện mối quan hệ giữa các thông số điều khiển

lOMoARcPSD|38101887
và quy luật chuyển động của robot (vị trí và

hướng) trong hệ quy chiếu gắn liền với sàn di
chuyển của robot. Vì vậy, mà bài toán này được
dùng để thiết kế hướng điều khiển robot.
Đối với bài toán động học được chia thành
hai dạng đó là:
 Bài toán động học thuận biết trước
các thông số điều khiển xác định quỹ
đạo chuyển động.
 Bài toán động học ngược biết trước
quỹ đạo chuyển động xác định các Hình 1.5 Vị trí và hướng của Rôbốt tự hành
thông số điều khiển robot. trong hệ quy chiếu cố định
Như vậy, nếu gọi:  f O f , x f , y f  là hệ quy chiếu cố định gắn liền với sàn di chuyển;
R {OR , xR , yR } là hệ quy chiếu gắn trên robot (Hình 5).Giả sử q f  x f (t ) y f (t )  (t )T là các
tham số định vị robot trong hệ quy chiếu  f và q R  xR (t ) yR (t )  (t ) là các tham số
T
trong hệ quy chiếu R thì khi đó:
q f  Q( )q R

 (1.1)
q R  Q( ) q f
T
cos   sin  0
Trong đó: Q( )   sin  cos  0 là ma trận cô sin chỉ hướng của hệ quy chiếu R so với
 0 0 1
hệ quy chiếu  f .
1.2 PHƯƠNG TRÌNH ĐỘNG HỌC CỦA AVG ĐIỀU HƯỚNG LINH HOẠT KIỂU
BÁNH XE ĐA HƯỚNG MECANUM
Xét trong hệ quy chiếu ϑR của AGV. Ta có vận tốc của các bánh xe:
V1 (t )  r1 (t )
V (t )  r (t )
 2 2
 (1.2)
V3 (t )  r3 (t )
V4 (t )  r4 (t )
Với r là bán kính của bánh xe Mecanum

lOMoARcPSD|38101887
Hình 1.6 Động học AGV đa hướng kiểu bánh xe Mecanum
Đối với bánh xe 1 (xem Hình 1.6) ta có:

 L
V1 (t )  VL1 cos   VGx (t )  2 (t )
 d
(1.3)
 VL1 sin   VGy (t )  (t )
 2
 
với  là góc giữa V1 và VL1 .
Khử thành phần vận tốc của con lăn VL1 trong phương trình (1.3) ta có vận tốc bánh xe 1 là:
L  d  1
V1 (t )  VGx (t )  (t )  VGy (t )  (t )  (1.4)
2  2  tg
Tương tự với bánh xe 2,3,4 ta có:

lOMoARcPSD|38101887
 L  d  1
V1 (t )  VGx (t )  2 (t )  VGy (t )  2 (t )  tg
  
V (t )  V (t )  (t )  V (t )  (t )  1
L d
 2 Gx
2 
Gy
2  tg
 (1.5)
V3 (t )  VGx (t )  (t )  VGy (t )  (t )  1
L d
 2  2  tg
 L  d  1
V4 (t )  VGx (t )  (t )  VGy (t )  (t ) 
 2  2  tg
Nếu lấy   45  , phương trình (1.5) viết lại thành:
 L d
V1 (t )  VGx (t )  VGy (t )   2  2 (t )
  
V (t )  V (t )  V (t )    (t )
 L d
 2 Gx Gy
2 2
 (1.6)
V3 (t )  VGx (t )  VGy (t )   L  d (t )
 2 2
 L d
V4 (t )  VGx (t )  VGy (t )    (t )
 2 2
Kết hợp (1.2) và (1.6), sau khi biến đổi về ma trận đại số:
 1 1 
 r r  2r L  d  
1
1 (t )   1 
 (t )  
1 1
 L  d   VGx (t ) 
 2  r r 2r  VGy (t ) (1.7)
 3 (t )   1 1 1
    
   r r 2r L d
  (t ) 
 4 (t )  1 
 
1

1
L  d 
r r 2r 
 1 1 
 r r  2r L  d  
1
 1 
 
1 1
L  d  
Đặt ω  1 (t ) 2 (t ) 3 (t ) 4 (t ) và J   r r 2r 
T
 1 1 1 L  d  
 r r 2r 
1 
 L  d 
1 1
 
r r 2r 
Khi đó (1.7) được viết lại:
ω  Jq R (1.8)
Khi cho trước ω thì q R được xác định bởi:

lOMoARcPSD|38101887
q R  J *ω (1.9)
Trong đó J * là ma trận tựa nghịch đảo cho bởi:
 
1 1 1 1
r 
* T
 
1 T
J  J J J   1
4
1 1 1  (1.10)
2 2 2 2 
  
 Ld Ld Ld Ld
1.2.1 Bài toán động học thuận
Khi cho trước ω , cần xác định q f  x f (t )  

y f (t )  (t ) để giải quyết vấn đề này, ta
T
đưa phương trình (1.9) về hệ quy chiếu cố định  f thông qua phương trình (1.1) ta có:
q f  Q( )q R  Q  J T J   1

JT ω (1.11)
Từ (1.11) ta tìm được vị trí và hướng của AGV đa hướng kiểu bánh xe Mecanum:
 t
0 1 (t )  2 (t )  3 (t )  4 (t )cos  (t )   1 (t )  2 (t )  3 (t )  4 (t )sin  (t )dt

r
 f x (t ) 
 4
 t
 y f (t )   1 (t )  2 (t )  3 (t )  4 (t )  cos  (t )  1 (t )  2 (t )  3 (t )  4 (t ) sin  (t ) dt (1.1

r
 40
 t
r
 1 (t )  2 (t )  3 (t )  4 (t ) dt
2L  d  0
 (t ) 

2)
1.2.2 Bài toán động học ngược


Từ quỹ đạo  x f (t ), y f (t ) và VG xác định được q f  x f (t )  
y f (t )  (t ) trong hệ quy
T
chiếu  f thay vào (1.1) sau đó kết hợp với (1.8) ta có:
ω  JQ  q f
T
(1.13)
Từ (1.13) ta xác định được tham số điều khiển biến các biến khớp của bánh Mecanum
10

lOMoARcPSD|38101887
 1 Ld 
1  r  cos   sin  x f (t )  cos   sin   y f (t )  2  (t ) 
  
 1 Ld 
2   cos   sin  x f (t )  cos   sin   y f (t )   (t ) 
 r 2 
 (1.14)
  1  cos  sin  x (t )  cos  sin   y (t )  L  d  (t ) 
 3 r f f
2 

  1  cos   sin  x (t )  cos   sin   y (t )  L  d  (t ) 
 4 r  f f
2 
Phương trình (1.14) xác định các thông số điều khiển 4 bánh Mecanum để AGV đa hướng đi

theo quỹ đạo  x f (t ), y f (t ) và VG cho trước.
1.3 THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG BẰNG ĐƯỜNG CONG NURBS
1.3.1 Đường cong NURBS
NURBS, viết tắt của cụm từ tiếng anh: “Non-uniform

rational B-Spline” là một mô hình toán học được sử dụng
trong kỹ thuật đồ họa máy tính để biểu diễn đường cong và
bề mặt.
Một trong những khác biệt chính giữa các đối tượng
Bézier và các đối tượng NURBS là các đường cong Bézier
là gần đúng. Ví dụ: vòng tròn Bézier là một xấp xỉ của một
vòng tròn, trong khi vòng tròn NURBS là một vòng tròn
Hình 1.7 Đường cong NURBS
chính xác.
Một đường cong B-Spline hữu tỷ (NURBS) là hình chiếu của một đường cong B-Spline
vô tỷ xác định trên không gian 4D lên không gian vật lý 3D. Cụ thể :
n 1
P(t)   Bih N i,k (t) (1.15)
i 1
Tại đây Bih là các đỉnh đa giác điều khiển đồng nhất 4D của đường cong B-Spline vô tỷ 4D.
N i ,k (t ) là các hàm B-Spline vô tỷ cơ sở được xác định bằng công thức đệ qui:
 1 xi  t  xi 1
 N i,1(t)  
 0
 (1.16)
 N (t)  t  xi N i,k 1(t)   xi  k t  t N i 1,k 1(t)
 i,k xi  k 1  xi xi  k  xi 1

11

lOMoARcPSD|38101887
Vectơ nút X được xác định bởi
 xi  0 1 i  k

 xi  i  k k 1  i  n 1 (1.17)
x  n  k  2 n  2  i  nk  1
 i
Chiếu đường cong B-Spline vô tỷ xác định trên không gian 4D lên không gian vật lý 3D bằng
cách chia phương trình đường cong B-Spline vô tỷ cho tọa độ đồng nhất:
n 1
BhN i i i,k (t ) n 1
P (t )  i 1
n 1
  Bi Ri , k (t ) (1.18)
h N
i 1
i i,k (t ) i 1
Tại đây Bi là các đỉnh của đa giác khống chế trong không gian 3D của đường cong B-Spline
hữu tỷ và
hi N i ,k (t )
Ri ,k (t )  n 1
(1.19)
h N
j 1
j j ,k (t )
là hàm B-Spline hữu tỷ cơ sở với hi ≥ 0 với mọi giá trị của i.
Tính chất của NURBS

Hàm cơ sở và đường cong hàm B-Spline hữu tỷ là một sự tổng quát hóa của hàm cơ sở và
đường cong hàm B-Spline vô tỷ. Do đó, chúng có gần như toàn bộ tính chất của hàm B-Spline
vô tỷ. Cụ thể :
- Tổng của các hàm cơ sở B-Spline hữu tỷ đối với giá trị tham số là:
n 1
R
i 1
i,k (t )  1 (1.20)
- Mỗi hàm cơ sở sẽ dương hoặc bằng 0 tức N k,i ≥ 0

- Mỗi hàm cơ sở có một giá trị cực đại, ngoại trừ các hàm cơ sở bậc 1.
- Bậc lớn nhất của đường cong bằng số đỉnh của đa giác điều khiển.
- Với hi  0 đường cong nằm trong đường bao lồi của đa giác điều khiển.
- Hình dạng đường cong phụ thuộc đa giác điều khiển.

- Đường cong và hàm B-Spline vô tỷ là trường hợp đặc biệt của đường cong và hàm
BSpline hữu tỷ khi hi  1
12

lOMoARcPSD|38101887
- Khi k = n, đường cong NURBS là đường cong Bezier hữu tỷ. Khi mọi hi  1 đường
cong Bezier hữu tỷ trở thành đường cong Bezier vô tỷ. Do đó cả 2 loại đường cong
Bezier hữu tỷ và vô tỷ đều là trường hợp đặc biệt của đường cong NURBS.
Bởi các đường cong NURBS là sự suy rộng 4 chiều của các hàm B-Spline vô tỷ, nên các giải
thuật cho sự nâng bậc, chia nhỏ, ước lượng đường cong của đường cong B-Spline vô tỷ đúng
cho đường cong NURBS bằng cách áp dụng cho các đỉnh khống chế 4 chiều.
1.3.2 Đường cong và hàm NURBS
Đường cong và đương NURBS mở

Ở đây, một vector nút mở X = 0 0 0 1 2 3 3 3 ( n +1 = 5, k = 3 ) được dùng
vector tọa độ thuần nhất hi  1 , i ≠ 3. Giá trị của h3 thay đổi trong khoảng từ 0 đến 5. Hình
1.8
thể hiện đường cong NURBS. Với h3 =1 đường cong NURBS trở thành đường cong B-Spline
vô
tỷ tương ứng. Chú ý rằng với h3 = 0 thì R3,3 = 0 . Do đó đỉnh của đa giác tương ứng, B3 ,
không
ảnh hưởng đến hình dạng của đường cong. Tác động này được thể hiện ở Hình 1.8, tại đây các
đỉnh B2 và B4 được nối bằng một đường thẳng. Khi h3 tăng, đường cong bị kéo lại gần B3
Hình 1.8 Đường cong NURBS với n+1=5, k=3, vector nút mở
[X] = [0 0 0 1 2 2 2 2] và [h] = [1 1 h3 1 1]
Tương tự với đương cong NURBS bậc 4 trong Hình 1.8

Với n+1=5, k=4 vector nút mở là X = 0 0 0 1 2 2 2 2
13

lOMoARcPSD|38101887
Tuy nhiên với đường cong NURBS cấp cao như ta thấy trong Hình 1.9 với h3 =0 , đường cong
không biến đổi thành một đường thẳng giữa B2 và B4 .
Tương tự đường cong B-Spline mở, điểm đầu và điểm cuối của đường cong NURBS mở trùng
với đỉnh đầu và cuối của đa giác điều khiển
P(0) = B1 và P(t max )  P(n  k  2  Bn1 )
X= 0 0 0 1 2 2 2 2 và h  = 1 1 h3 1 1
Đường cong và hàm NURBS có chu kì
X= 0 1 2 3 4 5 6 7 và h  = 1 1 h3 1 1
Hình 1.10 thể hiện đường cong NURBS với n+1=5, k=3, vector nút có chu kì
14

lOMoARcPSD|38101887
X= 0 1 2 3 4 5 6 7 và h  = 1 1 h3 1 1 với giá trị của h3 thay đổi trong

khoảng từ 0 đến 5.
Ở đây, với đường cong có chu kỳ, giới hạn sử dụng của tham số là 2 ≤ t ≤ 5
Với h3 =1 đường cong NURBS lại xác định đường cong B-Spline vô tỷ tương ứng. Tuy nhiên
với h3 ≠ 1 các hàm cơ sở không còn có chu kì.
Như có thể thấy trên hình, các đường cong NURBS có điểm đầu và điểm cuối trùng nhau và
không trùng với đỉnh đầu và cuối của đa giác điều khiển.
Tương tự với đường cong NURBS bậc 4 trong hình 1.11.
Với n+1=5, k=4, vector nút chu kỳ là X = 0 1 2 3 4 5 6 7 8
X= 0 1 2 3 4 5 6 7 8 và h  = 1 1 h3 1 1
Ở đây, các điểm đầu và các điểm cuối của một đường cong cùng nằm trên một đường thẳng.
1.3.3 Đạo hàm đường cong NURBS
 f ( x  x)  f ( x) 
Từ định nghĩa tính đạo hàm f ' ( x)  lim   ta tính được:
x  0
 x 
 P (t  t )  P (t ) 
P (t )  lim   (1.21)
t 0
 t 
 
(t )  lim  P(t  t )  P(t ) 
P (1.22)
t 0  t 
 
15

lOMoARcPSD|38101887
1.3.4 Thiết kế đường cong
Ví dụ 1.1:
Cho đa giác điều khiển bởi Bảng 1.1 :
Bảng 1.1 Đa giác điều khiển đường cong NURBS
B1 B2 B3 B4 B5
T
5 
0 0
T
1 2 T
 2 0 4 2
T
5 0
T

3
Xác định điểm tại t  thuộc đường cong NURBS mở bậc 3 ( k = 3) với hệ số tọa độ thuần
2
nhất cho bởi h = 1 1 h3 1 1 , h3 =0,

1
,1,5
4
Với n+1=5 , k=3, áp dụng công thức (1.17) ta có vector nút là:
X= 0 0 0 1 2 3 3 3
Tham số trong khoảng 0 ≤ t < 3 . Đường cong bao gồm 3 phương trình hữu tỷ bậc 2, mỗi
phương trình tương ứng một khoảng tham số trong vector nút. Sử dụng công thức (1.16) trong
khoảng tham số 1≤ t < 2 , các hàm B-Spline cơ sở là:
1≤ t < 2
N 4,1 (t )  1 ; N i ,1 (t )  0 , i≠4
N 3, 2 (t )  2  t ; N 4, 2 (t )  t  1 , N i , 2 (t )  0 , i ≠ 3,4
N 2,3 (t ) 
2  t 2 ; N 3,3 (t ) 
t 2  t  (3  t )(t  1)

2 2 2
N 4,3 (t ) 
t  12 ; N i ,3 (t )  0 , i ≠ 2,3,4
2
Từ phương trình (1.19):
n 1
S   hi N i ,k (t )  h1 N1,3 (t )  h2 N 2,3 (t )  h3 N 3,3 (t )  h4 N 4,3 (t )  h5 N 5,3 (t )
i 1
Các hàm NURBS cơ sở :

1≤ t < 2
h3  0
16

lOMoARcPSD|38101887
(2  t ) 2 (t  1) 2 (2  t ) 2
S  h2 N 2,3 (t )  h4 N 4,3 (t )    2
2 2 2t  6t  5
R1,3 (t )  0
h2 N 2,3 (t ) (2  t ) 2
R2,3 (t )  
S 2t 2  6t  5
R3,3 (t )  0
h2 N 4,3 (t ) (t  1) 2
R4,3 (t )   2
S 2t  6t  5
R5,3 (t )  0
1
h3 
4
S  h2 N 2,3 (t )  h3 N 3,3 (t )  h4 N 4,3 (t )
(2  t ) 2 t (2  t ) (3  t )(t  1) (t  1) 2
   
2 8 8 2
6t  18t  17
2

2t 2  6t  5
R1,3 (t )  0
h2 N 2,3 (t ) 4(2  t ) 2
R2,3 (t )   2
S 6t  18t  17
h3 N 3,3 (t ) t (2  t )  (3  t )(t  1)  2t 2  6t  3
R3,3 (t )    2
S 6t 2  18t  17 6t  18t  17
h4 N 4,3 (t ) 4(t  1) 2
R4,3 (t )   2
S 6t  18t  17
R5,3 (t )  0
h3  1
S=1
R1,3 (t )  0
(2  t ) 2
R2,3 (t )  N 2,3 (t ) 
2
t (2  t )) (3  t )(t  1)  2t 2  6t  3
R3,3 (t )  N 3,3 (t )  
t 2 6t 2  18t  17
17

lOMoARcPSD|38101887
(t  1) 2
R4,3 (t )  N 4,3 
2
R5,3 (t )  0
h3  5
S  h2 N 2,3 (t )  h3 N 3,3 (t )  h4 N 4,3 (t )

(2  t ) 2 5t (2  t ) 5(3  t )(t  1) (t  1) 2
   
2 2 2 2
 4t  12t  5
2
R1,3 (t )  0
h2 N 2,3 (t ) (2  t ) 2
R2,3 (t )  
S 2(4t 2  12t  5)
h3 N 3,3 (t ) 5t (2  t )  5(3  t )(t  1) (5  2t 2  6t  3)
R3,3 (t )   
S 2(4t 2  12t  5) 2(4t 2  12t  5)
h4 N 4,3 (t ) (t  1) 2
R4,3 (t )  
S 2(4t 2  12t  5)
R5,3 (t )  0
3
Tính toán những kết quả trên tại t= ta thu được:
2
3 3 1 3 3 1 3
h3  0 R1,3 ( )  0 R2,3 ( )  R3,3 ( )  0 R4,3 ( )  R5,3 ( )  0
2 2 2 2 2 2 2
1 3 3 2 3 3 3 2 3
h3  R1,3 ( )  0 R2,3 ( )  R3,3 ( )  R4,3 ( )  R5,3 ( )  0
4 2 2 7 2 7 2 7 2
3 3 1 3 3 3 1 3
h3  1 R1,3 ( )  0 R2,3 ( )  R3,3 ( )  R4,3 ( )  R5,3 ( )  0
2 2 8 2 4 2 8 2
3 3 1 3 15 3 1 3
h3  5 R1,3 ( )  0 R2,3 ( )  R3,3 ( )  R4,3 ( )  R5,3 ( )  0
2 2 32 2 16 2 32 2
Tọa độ các điểm tương ứng trên đường cong NURBS:
h3  0
3 1 5 
P   1 2  4 2  
1
2
2 2 2 2 
18

lOMoARcPSD|38101887
1
h3 
4
3 2 3 5  2 5 8
P   1 2   0  4 2  
2 7 7 2  7 2 7 
h3  1
3 1 3 5  1 5 1
P   1 2   0  4 2  
2 8 4 2  8 2 2 
h3  5
3 1 15  5  1 5 1
P   1 2   0  4 2  
 2  32 16  2  32 2 8 
1.4 KHẢO SÁT QUỸ ĐẠO CHUYỂN ĐỘNG CỦA ĐƯỜNG CONG BẤT KỲ
1.4.1 Thiết kế quỹ đạo
Ví dụ 1.2 Xét lộ trình của xe như Hình 1.12
Hình 1.12 Lộ trình của xe
19

lOMoARcPSD|38101887
Ta có đa giác điều khiển quỹ đạo của xe theo Bảng 1.2:

Bảng 1.2 Điểm điều khiển đường cong NURBS
B1 B2 B3 B4 B5 B6 B7
[0 0] [-6,5 0] [-6,5 5] [-3 5] [-3 8] [-6,5 8] [-6,5 13]
B8 B9 B10 B11 B12 B13 B14
[6,5 13] [6,5 8] [3 8] [3 5] [6,5 5] [6,5 0] [0 0]
Sau khi sử dụng phương pháp thiết kế quỹ đạo bằng đường cong NURBS ta có quỹ đạo
cảu xe như Hình 1.13
Hình 1.13 Quỹ đạo của xe theo đường cong NURBS
1.4.2 Khảo sát các chuyển động của xe
Tại mỗi vị trí ta có hướng chuyển động của xe là  (t )
 y ' (t ) 
 (t )  arctan  0    2  (1.23)
 x' (t ) 
Bán kính cong tại t được tính theo công thức:
20

lOMoARcPSD|38101887
R
x 2
 y2 
3/ 2
(1.24)
xy  yx
Khi xe chuyển động, vận tốc góc của xe là:

V
 (1.25)
R
Dựa vào công thức (1.14) suy ra vận tốc góc của mỗi bánh Mecanum:
 1 Ld 
1  r  cos   sin  VGx (t )  cos   sin  VGy (t )  2 (t ) 
  
 1 Ld 
2   cos   sin  VGx (t )  cos   sin  VGy (t )  (t ) 
 r 2 
 (1.26)
  1  cos   sin  V (t )  cos   sin  V (t )  L  d (t ) 
 3 r Gx Gy
2 

  1  cos   sin  V (t )  cos   sin  V (t )  L  d (t ) 
 4 r  Gx Gy
2


Trong đó:
VGx (t )  VG cos (t ) 
 (1.27)
VGy (t )  VG sin  (t ) 
Quãng đường di chuyển tính được bằng tổng mỗi quãng đường nhỏ Pi Pi 1
S   Pi Pi1 (m) (1.28)
Thời gian chuyển động bẳng tổng độ dài quãng đường chia cho vận tốc V
S
 (s) (1.29)
V
Ví dụ 1.3 Lấy quỹ đạo chuyển động như trên ví dụ 1.2, các thông số bổ sung thêm:
Kích thước xe: L = 0.5(m) ; d = 0,8(m)
Bán kính bánh xe Mecanum r = 0,05(m)
Vận tốc của xe V = 1 m/s.
Dựa vào công thức (1.27) và (1.28) ta tính được:
Quãng đường di chuyển là S = 54,8232 (m)
Thời gian di chuyển là τ = 54,8232 (s)
Đồ thị các giá trị  , R, , 1 , 2 , 3 , 4 được biểu diễn từ Hình 1.14 - 1.20.
21

lOMoARcPSD|38101887
Hình 1.14 Đồ thị góc quay của xe khi V = 1 m/s
Hình 1.15 Đồ thị biểu diễn bán kính cong khi V = 1 m/s
22

lOMoARcPSD|38101887
Hình 1.16 Đồ thị vận tốc góc Ω của xe khi V = 1 m/s
Hình 1.17 Đồ thị vận tốc góc ω1 khi V = 1 m/s
23

lOMoARcPSD|38101887
24

lOMoARcPSD|38101887
Nhận xét:
- Vận tốc góc của các bánh luôn thay đổi đối xứng nhau (ω1. ω4 tăng thì ω2, ω3 giảm)
nhằm giữ cho vận tốc của rôbốt theo phương x của rôbốt không đổi và vận tốc theo
phương y của rôbốt bằng 0.
- Tại các vị trí bánh xe có vận tốc góc lớn nhất hoặc nhỏ nhất, là các vị trí rôbốt có biên độ
vận tốc góc là lớn nhất.
Ví dụ 1.4: Xét chuyển động và các thông số của rôbốt như ví dụ 1.3 nhưng thay vận tốc
chuyển động V = 0,3(m/s) ta tính được:
Quãng đường di chuyển là S=54,8232 (m)
Thời gian di chuyển là τ = 182,7441 (s)
Khi đó đồ thị các giá trị  , R, , 1 , 2 , 3 , 4 tương tự như ví dụ 1.3 nhưng thời gian thực hiện
quá trình thay đổi. Cụ thể  , R, , 1 , 2 , 3 , 4 được biểu diễn từ Hình 1.21-1.27
25

lOMoARcPSD|38101887
Hình 1.21 Đồ thị góc quay của xe khi V=0,3 m/s
Hình 1.22 Đồ thị biểu diễn bán kính cong khi V=0, 3 m/s
26

lOMoARcPSD|38101887
Hình 1.23 Đồ thị vận tốc góc Ω của xe khi V=0,3 m/s
Hình 1.24 Đồ thị vận tốc góc 1 khi V=0,3 m/s
27

lOMoARcPSD|38101887
Hình 1.25 Đồ thị vận tốc góc  2 khi V = 0,3 m/s
Hình 1.26 Đồ thị vận tốc góc 3 khi V = 0,3 m/s
28

lOMoARcPSD|38101887
Hình 1.27 Đồ thị vận tốc góc  4 khi V = 0,3 m/s
Nhận xét:
- Từ các Hình 1.21-1.27 (trường hợp V = 1 m/s) so với các Hình 1.14-1.20 (trường hợp V =
0,3 m/s) nhận thấy khi thay đổi giá trị vận tốc, các giá trị vận tốc góc của các bánh xe thay đổi,
tuy nhiên dạng đồ thị vận tốc góc theo thời gian t không đổi, nguyên nhân là do vận tốc thay
đổi thì thời gian và vận tốc góc từng bánh thay đổi nhằm đáp ứng theo quỹ đạo chuyển động
của rôbốt.
KẾT LUẬN CHƯƠNG 1
Chương này đã thu được kết quả như sau:

(1) Thiết lập được phương trình động học của rôbốt tự hành điều hướng kiểu bánh xe
Mecanum
(2) Đã thực hiện mô phỏng điều khiển động học trên Matlab
29

lOMoARcPSD|38101887
Chương 2
THIẾT KẾ BỘ ĐIỀU KHIỂN PHẢN HỒI TRẠNG THÁI TUYẾN TÍNH

CHO RÔBỐT TỰ HÀNH ĐA HƯỚNG KIÊU BÁNH XE MECANUM
2.1 THIẾT LẬP MÔ HÌNH SAI SỐ ĐỘNG HỌC
Véctơ sai số dịch chuyển e(t) cho bởi:

e(t )  e x ey e 
T
 q d (t )  q(t ) (2.1)
Đạo hàm phương trình (1.7) ta có phương trình sai số tốc độ:
e (t )  q d (t )  q (t ) (2.2)
Trong đó:
q d  xd y d  d T q  x y  T .
Ta có phương trình sai số trong hệ quy chiếu gắn với Robot R {OR x R y R z R } cho bởi:
e R  QT ( )e (2.3)
Với Q là ma trận quay biến đổi từ hệ quy chiếu R {OR x R y R z R } sang hệ quy chiếu quán tính
 f {O f x f y f z f } một góc θd cho bởi:
cos   sin  0
Q   sin  cos  0 (2.4)
 0 1
 0
Đạo hàm phương trình (2.3) ta có:

 T ( )e
e R  Q T ( )e  Q (2.5)
Kết hợp phương trình (2.5) và (2.2, 2.3 , 2.1):
 T ( )Q( )e
e R  QT ( )(q d  q )  Q (2.6)
R
Khai triển (2.6) ta có:

 e Rx   cos  sin  0 cos  d  sin  d 0 V xd  cos   sin  0 V x  
    
 e Ry    sin  cos  0  sin  d cos  d 0 V yd    sin  cos  0 V y   
e R   0
  0 1  0 0 1  d   0 0 1    
30

lOMoARcPSD|38101887
  sin  cos 
0 cos   sin  0  eRx 
 
   cos   sin  0  sin  cos  0  eRy  (2.7)
 0 0 1  0 0 1 eR 
Hay:
  
 eRx   Vxd cos d  Vyd sin d   Vx cos   Vy sin   cos   Vxd sin d  Vyd cos d   Vx sin   Vy cos   sin   eRy  
  
   
 eRy    Vxd cos d  Vyd sin d   Vx cos   Vy sin   sin   Vxd sin d  Vyd cos d   Vx sin   Vy cos   cos   eRx 

eR   d   
 
(2.8)
Rút gọn (2.7) ta được:

 eRx  Vxd cos e  V yd sin e  Vx  eRy 
   
 eRy   Vxd sin e  V yd cos e  V y  eRx  (2.9)
eR   d   
 
Phương trình (2.8) viết dưới dạng ma trận:
 0 (t ) 0  eRx  cos e  sin e 0 Vxd (t )  VGx (t ) 

  
eR   (t ) 0 0  eRy    sin e cos e 0 V yd (t )  VGy (t ) (2.10)
 0 0 0 eR   0 0 1  d (t )  (t ) 
2.2 THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN PHẢN HỒI TRẠNG THÁI TUYẾN TÍNH
Mô hình (2.10) là mô hình sai số phi tuyến của Rôbốt. Theo [8] ta có thể xấp xỉ mô hình
(2.10) về mô hình tuyết tính cho phạm vi lân cận giá trị của điểm tham chiếu xd, yd, φd kết hợp
với (2.3) ta được:
 0 d 0  e Rx   1  e R 0 V xd (t )  VGx (t )
e R    d 0 0  e Ry   e R 1 0 V yd (t )  VGy (t ) (2.11)
    
 0
 0 0 e R   0 0 1  d (t )  (t ) 

Rút gọn (2.11) ta được:

 0 d  V yd 
e R    d 0 V xd e R  I 33 u (2.12)
 0 0 0 

Với:
u  [eVx , eVy , e ]T  [Vxd (t )  VGx (t ),V yd (t )  VGy (t ),  d  ]T (2.13)
là các biến điều khiển.
31

lOMoARcPSD|38101887
Phương trình (2.12) được viết lại thành:

e R  Ae R  Bu (2.14)
 0 d  V yd 
Với A    d 0 V xd  , B=I3˟3
 0 0 0 

Bộ điều khiển được thiết kế gồm 3 trạng thái x(t), y(t), φ(t) và hai đầu vào là u1, u2, u3.Sử dụng
bộ điều khiển không gian trạng thái tuyến tính [9] với phương trình tổng quát:
u=KeR(t) (2.15)
Do đó luật điều khiển kiểm soát phản hồi như sau:
 u1   k1 0 0   e Rx 
u 2    0  k2 0   e Ry  (2.16)
u   0  
 3  0  k 3  e R 
 
Trong đó k1,k2,k3 là các hệ số của bộ điều khiển. Trong mô hình này, sự kết hợp giữa điều
khiển truyền thẳng và điều khiển phản hồi tuyến tính dựa trên cách tiếp cận vị trí, do đó, đầu
vào điều khiển mới được tạo ra:
V x (t )  V xd  u1
V (t )  V  u
 y yd 2 (2.17)
 (t )   d  u 3
Vd
Ωd được xác định theo Ωd = trong đó ρ là bán kính cong được xác định từ quỹ đạo đường

cong NURBS
Thay (2.15) vào (2.14) ta có:
e R  Ae R  BKe R  A  BK e R (2.18)
Từ (2.12) và (2.14) suy ra đa thức đặc trưng của hệ vòng kín:
 0 d  V yd   k1 0 0 
det( sI 33  ( A  BK ))  sI 33    d 0 Vxd    0  k2 0 
(2.19)
 0 0 0   0 0  k3 

 s  k3  s 2  k1  k 2 s  k1k 2   2d 
So với phương trình đặc trưng mong muốn:
(s+2ωnξ)(s2+2 ωnξs+ωn2) (2.20)
Trong đó 0 < ξ < 1,  n   d2  gVd2 và Vd  Vxd2  Vyd2
Xác định được mối quan hệ các hệ số điều khiển:
32

lOMoARcPSD|38101887
k1  k 2  2 n

k1 k 2   n   d
2 2
(2.21)
k 3  2 n
Giải hệ (2.21) ta được:

k    2  gV 2   2  2  gV 2  2  1
 1 d d d d  

k 2    d  gV d    d  gVd   1
2 2 2 2 2 2
  (2.22)

k 3  2  d  gV d
2 2
Với:
2
g
1   2  max
2
(2.23)
Hình 2.1 Sơ đồ khối hệ thống điều khiển
2.3 THIẾT KẾ QUỸ ĐẠO ĐƯỜNG CONG NURBS
Từ phương pháp thiết kế đường cong NURBS với các điểm điều khiển được cho dưới Bảng
2.1 có được quỹ đạo NURBS được thể hiện trong Hình 2.2
Bảng 2.1. Các nút điều khiển quỹ đạo đường cong NURBS
B1 B2 B3 B4 B5 B6 B7 B8 B9 B10
1  2 2 3 3 1  1 0  0  1 
0 0 1  1 2 2 1 1 0 0
33

lOMoARcPSD|38101887
Hình 2.2 Kết quả mô phỏng quỹ đạo chuyển động của robot
theo đường cong NURBS
2.4 KẾT QUẢ MÔ PHỎNG SIMULINK MATLAB
Với các thông số của robot đa hướng và bánh xe như sau: L = 316(mm), d = 270(mm),
bán kính bánh xe r = 30(mm). Cài đặt các thông số như sau:
Bước 1: Quỹ đạo chuyển động của robot đa hướng là đường cong NURBS trên Hình 2.2 với
cơ sở dữ liệu trong Bảng 2.1
Bước 2: Các thông số chuyển động: (i) Vận tốc góc mong muốn Vd=0,3(m/s) và(ii) Vận tốc
góc của robot đa hướng Ω được xác định như sau :Ωd=Vd / ρ với ρ là bán kính cong của quỹ
đạo NURBS
T
V  0 0 1 

Đặt Vxd V yd d 
T
 T  d  với T   
 d  1 0 0
Bước 3: Lựa chọn tham số bộ điều khiển
Lần lượt thay đổi các giá trị của g và ζ, chọn g và ζ sao cho các giá trị sai số trung bình trên
toàn quỹ đạo là nhỏ nhất.
Tiến hành cho g = 1 và ζ chạy từ 0,1 đến 0,9, ta thu được đồ thị thể hiện sai số trung bình
trong các Hình 2.3, 2.4:
34

lOMoARcPSD|38101887
Hình 2.3 Sai số tư thế (vị trí và hướng) trung bình khi thay đổi ζ , g = 1; K1 > K2
Hình 2.4 Sai số tư thế (vị trí và hướng) trung bình khi thay đổi ζ , g = 1; K1 < K2
35

lOMoARcPSD|38101887
Từ Hình 2.3 và Hinh 2.4 ta thấy các giá trị sai số giảm dần khi ζ tăng từ 0,1 đến 0,9, từ đó
chọn chọn ζ = 0,9. Với ζ = 0,9 và ρmax = 4 (m) thay vào (2.23) xác định 0 ≤ g ≤ 0,266
Hình 2.5 Sai số tư thế (vị trí và hướng) trung bình khi thay đổi g, ζ = 0,9; K1 > K2
Hình 2.6 Sai số tư thế (vị trí và hướng) trung bình khi thay đổi g, ζ = 0,9; K1 < K2
36

lOMoARcPSD|38101887
Từ Hình 2.5 và Hình 2.6 ta thấy sai số quỹ đạo của rôbốt giảm dần khi g tăng từ 0 dến
0,226 chọn chọn g = 0,266.
Bước 4: Sau đánh giá sai số trung bình chọn ξ=0,9, g=0,266 thay vào phương trình (2.22) các
tham số k1 ,k2, k3 của bộ điều khiển có thể xác định được. Hình 2.7 bên dưới là quỹ đạo chuyển
động của robot được điều khiển theo dõi quỹ đạo NURBS mong muốn. Robot đa hướng vận
chuyển NURBS mong muốn được miêu tả trong Hình 2.7 với các thông số trên.Sau đó Hình
2.10 là tốc độ di chuyển của robot bao gồm cả vận tốc V và vận tốc góc Ω của rôbot khi bán
kính cong ρ của quỹ đạo ξ mong muốn thay đổi từ 0.358 (m) đến 4 (m) ( Hình 2.7b).
Từ Hình 2.7 và Hình 2.10, từ vận tốc góc mong muốn Ω ta xác định được V và ρ, sai số
chuyển vị trí góc của robot tại mỗi thời điểm ΔΩ‒Ω‒Ωd từ 0 rad/s đến 0,8 rad/s (Hình 2.8b).
Lý do tại các vị trí  , ,, , ,  là vị trí của robot thay đổi hướng từ cùng chiều kim
đồng hồ sang ngược chiều kim đồng hồ để đi theo quỹ đạo mong muốn như được mô tả trong
Hình 2.7a. Sai số theo dõi giữa quỹ đạo chuyển động và quỹ đạo mong muốn được thể hiện
trong Hình 2.10.
Từ Hình 2.7a và Hình 2.10 nhận thấy một điều khi robot di chuyển: (1) Từ điểm bắt đầu
đến điểm  sẽ có sự thay đổi hướng từ cùng chiều kim đồng hồ sang ngược chiều kim đồng
hồ và theo chiều ngược lại, theo dõi từ trong ra ngoài quỹ đạo mong muốn với sai số vị trí là
1mm và sai số phương hướng khoảng 1˚; (2) Từ vị trí  đến vị trí  trên quỹ đạo, robot bắt
đầu theo dõi từ bên ngoài vào bên trong quỹ đạo mong muốn là đưa trực tiếp vào bên trong bắt
đầu từ vị trí  cùng với vị trí sai số giảm từ 1.2 mm xuống 2.8 mm và sai số phương hướng
vào khoảng 4,6˚; (3) Từ vị trí  đến vị trí  robot đảo ngược hướng theo dõi từ ngoài vào
trong theo quỹ đạo mong muốn và ngược lại ở vị trí  sai số theo phương x là 1,8mm tại vị trí
(15) là hướng ngược lại với sai số góc là 4,59˚ tạo ra sai số phương y là 2,2 mm; (4) Từ vị trí
 sang vị trí  robot sẽ theo dõi từ ngoài vào trong quỹ đạo mong muốn với sai số tối đa là
1,5 mm theo phương x, 0,8mm theo hướng y và 8,6˚ theo hướng ngược lại; (5) Từ vị trí  đến
vị trí  robot theo dõi từ bên ngoài vào bên trong của quỹ đạo mong muốn với sai số max
=1,9 mm theo phương x và 2,4mm theo phương y, 1,1˚ trong các tư thế đảo ngược; (6) Từ vị
trí  đến vị trí  robot sẽ theo dõi hướng từ ngoài vào trong của quỹ đạo mong muốn thông
qua các điểm và (18). Điểm  đảo ngược hướng với sai số góc là 5,16˚ làm cho điểm có sai
số theo phương x là 1,8mm; (7) Từ điểm  đến điểm xuất phát, rôbốt sẽ theo dõi từ trong ra
ngoài của quỹ đạo mong muốn.
37

lOMoARcPSD|38101887
Hình 2.7 Quỹ đạo chuyển động của robot đa hướng với (a) Quỹ đạo thực tế và quỹ đạo
mong muốn, (b) Bán kính cong của quỹ đạo chuyển động mong muốn
38

lOMoARcPSD|38101887
Hình 2.8 Sự di chuyển của robot đa hướng với (a) Tốc độ ban đầu của robot
(b) Vận tốc góc thực tế và mong muốn của robot
Hình 2.9 Vận tốc góc của bánh xe 1,bánh 2, bánh 3, bánh 4
39

lOMoARcPSD|38101887
Hình 2.10 Sai số vị trí và hình dáng của robot trong quá trình di chuyển
Như vậy đồ thị Hình 2.9 là thông số điều khiển tốc độ của bốn bánh xe để robot đa hướng
có thể chuyển động theo quỹ đạo NURBS mong muốn.

(1) Tính toán thiết kế được bộ điều khiển phản hồi trạng thái tuyến tính đồi với
rôbốt đa hướng kiểu bánh xe Mecanum.
(2) Thực hiện mô phỏng bộ điều khiển phản hồi trạng thái tuyến tính trên Matlab
Simulink.
40

lOMoARcPSD|38101887
Chương 3
CHẾ TẠO THỰC NGHIỆM
3.1 MÔ TẢ CẤU TRÚC CỦA RÔBỐT
Cấu trúc Rôbốt tự hành điều hướng kiểu bánh xe mecanum như được mô tả trên Hình 3.1
dưới đây.
Hình 3.1 Cấu trúc của Rôbốt
Trong đó: Cụm dẫn động bao gồm bánh xe mecanum ❶ và động cơ hộp giảm tốc hành
tinh ❷; Công tắc ❸; Báo dung lượng pin ❹; Nguồn điện pin Lithium ❺; Mạch công suất điều
khiển động cơ ❻; Bộ xử lý trung tâm máy tính nhúng ❼; Cảm biến IMU ❽; Mạch trung gian
Arduino Mega ❾; Máy quét Lidar ❿.
3.1.1 Cấu trúc thân xe
Thân Rôbốt được làm bằng vật liệu Mica nhằm đảm bảo nhẹ có thông số kích thước như
sau.
Tấm đế: có kích thước 260mm × 380mm, dày 10 mm và được mô tả trên Hình 3.2
41

lOMoARcPSD|38101887
Hình 3.2 Tầm Mica đế dày 10 mm
Chức năng của tầm Mica đế:

 Đảm bảo độ cứng vững.
 Đảm bảo nhẹ.
 Đỡ được tất cả các thiết bị như: 4 bánh xe dẫn động và động cơ (đỡ toàn bộ hệ thống), pin
Lithium, mạch công suất, công tắc, báo dung lượng pin, v.v.
Ngoài ra, còn có 6 trụ đứng cọc đồng ϕ6 lắp ghép bằng bulông M4 nối giữa đế với phần kết
cấu phía trên đỡ mạch điều khiển và các thiết bị ngoại vi khác.
42

lOMoARcPSD|38101887
Hình 3.3 Tầm đỡ mạch
Tấm đỡ mạch: có kích thước 260 mm × 480mm, dày 5 mm và được thiết kế như mô tả trên
Hình 3.3. Tấm này có chức năng chứa các bộ phận: mạch điều khiển động cơ, máy tính nhúng,
Lidar, v.v..
3.1.2 Phần mềm chương trình
Sử dụng hệ thống ROS để kết nối và điều khiển rôbốt từ xa thông qua kết nối không dây
Wifi giữa máy tính (người dùng) và máy tính nhúng Raspbery Pi 4B (trên rôbốt).
Trên các máy tính (máy tính nhúng và máy tính người điều khiển) cài hệ điều hành ROS
NOETIC, trong đó máy tính nhúng trên rôbốt có vai trò làm master (bộ quản lý trung tâm), các
máy tính người điều khiển còn lại sẽ kết nối chung với master. Trong máy tính nhúng trung
tâm và máy tính người điều khiển có các Nodes là các nút chương trình phục vụ cho mục đích
tính toán. Các nodes gửi và nhận các tin nhắn với nhau nhằm giao tiếp giữa máy tính người
điều khiển, máy tính nhúng và các thiết bị ngoại vi trên rôbốt.
3.1.3 Bộ điều khiển
Bộ điều khiển được thiết kế là bộ điều khiển phản hồi trạng thái tuyến tính có tham số
điều khiển biến đổi theo thời gian được thiết kế trong Chương 2.
3.2 CẤU TRÚC BỘ DẪN ĐỘNG
43

lOMoARcPSD|38101887
Hệ dẫn động được mô tả trên Hình 3.4 có chức năng dẫn động cho Rôbốt. Trong đó:
Hình 3.4. Kết cấu cụm bánh xe đẫn động
1. Động cơ DC-Hộp giảm tốc có thông số như sau: Động cơ DC 12V, hộp giảm tốc bánh răng
hành tinh có tỷ số truyền 1:14.
2. Bánh xe dẫn động
Bánh xe dẫn động mecanum cho Rôbốt có kết cấu như mô tả trên Hình 3.5. Trong đó vật liệu
bằng nhôm hợp kim với các con lăn là nhựa PP và PE để đảm bảo ma sát với mặt đường với
bánh xe là loại có góc nghiêng β = 45°.
Hình 3.5. Bánh xe đẫn động
3. Giá liên kết động cơ và thân xe có kích thước như mô tả trên Hình 3.6.
4. Đế mica
Đế mica cố định cụm dẫn động
44

lOMoARcPSD|38101887
Hình 3.6. Kích thước gá liên kết động cơ với thân xe
3.3 CẤU TRÚC PHẦN CỨNG CỦA BỘ ĐIỀU KHIỂN
Cấu trúc phần cứng của Rôbốt được mô tả trên Hình 3.7.
Hình 3.7. Sơ đồ cấu trúc phần cứng của bộ điều khiển
45

lOMoARcPSD|38101887
Trong đó:
 Pin Lithium có thông số kỹ thuật: Điện áp hoạt động 2,5-3,65V, dung lượng 15Ah có chức
năng nuôi hệ thống mạch đièu khiển và mạch động lực.
 Máy tính nhúng đóng vai trò là bộ xử lý trung tâm có chức năng nhận các dữ liệu từ các bộ
phận chấp hành, xử lý dữ liệu và gửi tín hiệu điều khiển để điều khiển các bộ phận khác.
 Arduino Mega 2560 có vai trò là mạch điều khiển trung gian giữa máy tính nhúng với
động cơ và các đơn vị phản hồi để , có chắc năng nhận dữ liệu phản hồi từ IMU và encoder
gửi về bộ xử lý trung tâm và nhận tín hiệu điều khiển để điều khiển động cơ.
 Mạch công suất có chức năng điều chỉnh dòng điện và điện áp để điều khiển tốc độ của
động cơ theo tín hiệu điều khiển nhận từ mạch arduino.
 Lidar có chức năng xác định khoảng cách các vật xung quanh và gửi dữ liệu về bộ xử lý để
xử lý và điều khiển tránh vật cản.
 IMU có chức năng xác định các gia tốc quán tính và vận tốc góc của Rôbốt và gửi dữ liệu
về bộ xử lý trung tâm để xử lý và điều khiển.
3.4 THÔNG SỐ KỸ THUẬT CỦA CÁC THIẾT BỊ PHẦN CỨNG
Bảng 3.1 dưới đây là các thông số kỹ thuật của thiết bị phần cứng của Rôbốt.
Bảng 3.1 Các thông số kỹ thuật thiết bị phần cứng của Rôbốt
Tên thiết bị Thông số kỹ thuật Hình mô tả
GP36E13CPR 1:14
 Điện áp sử dụng: 12VDC.
 Dòng tiêu thụ: 3A
 Stalling current: 7A (dòng khi động
cơ bị kẹt hoặc quá tải đứng im).
 Encoder quang 500CPR (Count Per
Round), 2 kênh A-B.
Động cơ Servo-tích  Điện áp cấp cho Encoder: 5VDC, tín
hợp hộp giảm tốc hiệu xuất ra 2 kênh TTL 0/5VDC
hành tinh  Hộp số kim loại giảm tốc hành tinh
Planetary.
 Tỉ số giảm tốc 1:14.
 Moment lực kéo:
6.2Kg.cm (1:14 gear ratio) - 280rpm
12.2Kg.cm (1:27 gear ratio) - 145rpm
 Motor: Đen - Đỏ cấp nguồn 12VDC.
 Encoder:
Đỏ: 5VDC.
46

lOMoARcPSD|38101887
Đen: GND - 0VDC.

Vàng: kênh A.
Trắng: kênh B.
Raspberry Pi 4 Model b
 Broadcom BCM2711, Quad core
Cortex-A72 (ARM v8) 64-bit SoC @
1.5GHz.
 RAM: 4GB LPDDR4-2400 SDRAM.
 Wifi chuẩn 2.4 GHz và 5.0 GHz
IEEE 802.11ac. Bluetooth 5.0, BLE.
 Cổng mạng Gigabit Ethernet 2 cổng
USB 3.0 và 2 cổng USB 2.0.
 Chuẩn 40 chân GPIO, tương thích với
các phiên bản trước.
 Hỗ trợ 2 cổng ra màn hình chuẩn
Micro HDMI với độ phân giải lên tới
4K.
Máy tính nhúng
 Cổng MIPI DSI.
 Cổng MIPI CSI.
 Cổng AV 4 chân.
 H.265 (4kp60 decode), H264
(1080p60 decode, 1080p30 encode).
 OpenGL ES 3.0 graphics.
 Khe cắm Micro-SD cho hệ điều hành
và lưu trữ.
 Nguồn điện DC 5V – 3A DC chuẩn
USB-C.
 5V DC via GPIO header (minimum
3A*).
 Hỗ trợ Power over Ethernet (PoE)
(yêu cầu có PoE HAT).
47

lOMoARcPSD|38101887
Arduino Mega 2560

 Vi điều khiển chính CH340.
 IC nạp và giao tiếp UART:
ATmega16U2.
 Nguồn nuôi mạch: 5VDC từ cổng
USB hoặc nguồn ngoài cắm từ giắc
tròn DC.
 Số chân Digital: 54 (15 chân PWM).
 Số chân Analog: 16.
Arduino Mega 2560  Giao tiếp UART : 4 bộ UART.
 Giao tiếp SPI : 1 bộ ( chân 50 -> 53 )
dùng với thư viện SPI của Arduino.
 Giao tiếp I2C : 1 bộ.
 Ngắt ngoài : 6 chân.
 Bộ nhớ Flash: 256 KB, 8KB sử dụng
cho Bootloader.
 SRAM: 8 KB.
 EEPROM: 4 KB.
 Xung clock: 16 MHz.
BTS7960 43A
 Nguồn: 6 ~ 27V.
 Dòng điện tải mach: 43A (Tải trở)
hoặc 15A (Tải cảm).
 Tín hiệu logic điều khiển: 3.3 đến 5V.
Mạch công suất điều  Tần số điều khiển tối đa: 25KHz.
khiển động cơ
 Tự động tắt khi điện áp thấp: Nhỏ
hơn 5.5V module sẽ tự động tắt.
 Bảo vệ quá nhiệt: BTS7960 bảo vệ
chống quá nhiệt bằng cảm biến nhiệt
tích hợp bên trong. Đầu ra sẽ bị ngắt
khi có hiện tượng quá nhiệt.
48

lOMoARcPSD|38101887
 Kích thước: 40 x 50 x12mm.
Máy quét Lidar Delta 2A

 Phạm vi: 0,15m ~ 8m (độ phản xạ
80).
 Tốc độ lấy mẫu: 2~5k/s.
 Tần số quét: 4 ~ 10Hz.
 Bước song laser: 780nm.
 Công suất laser: 3mW ( công suất tốt
đa).
 Độ chính xác đo: <1%@8m.
 Độ phân giải góc: 0,3 ~ 0,8 ○.
 Giao diện truyền thong: UART (3.3V
TTL).
Lidar  Công suất tiêu thụ định mức: 2.5W.

 Điện áp làm việc: DC 5V.
 Ổ đĩa động cơ: DC 3.3V.
 Dòng điện khởi động: 600mA.
 Dòng điện làm việc: 500mA.
 Âm lượng: Φ108mm*76mm*51mm.
 Trọng lượng: 185 ± 2g.
 Cấp độ : <1○.
 Nhiệt độ làm việc: 0 ~ 45 ○C.
 Cường độ ánh sang xung quanh:
<1000lux.
 Độ ẩm xung quang: <90%.
49

lOMoARcPSD|38101887
Pin Lithium Lifepo4 33140

 Điện áp hoạt động 2,5-3,65V.
 Dung lượng 15Ah.
 Điện trở 2-3mΩ.
Pin Lithium
 Kích thước 33mm × 140mm.
 Dòng xả tối đa 20C.
 Dòng xả liên tục 2C.
 Trọng lượng 0,25kg.
3.5 CHẾ TẠO THỰC NGHIỆM
Sau khi chế tạo thực nghiệm mô hình, rôbốt đa hướng kiểu bánh Mecanum được mô tả
trên Hình 3.8 và Hình 3.9 dưới đây:
Hình 3.8. Ảnh chụp tấm đế của Rôbốt sau khi chế tạo
50

lOMoARcPSD|38101887
Hình 3.9. Ảnh chụp tổng thể Rôbốt sau khi chế tạo
Với các thông số kỹ thuật:

1. Kích thước: 0,48 x 0,35 x 0,16 m.
2. Tốc độ di chuyển tối đa: 1 m/s (tương đương 36 km/h)
3. Chuyển động: đa hướng với 4 bánh dẫn động là bánh đa hướng.
4. Chức năng: có hai chức năng đó là (1) Điều khiển bằng tay và (2) Điều khiển theo chương
trình.
5. Truyền thông: truyền thông không dây qua hệ thống WIFI.
6. Rôbốt có chức năng điều khiển và nhận dữ liệu quét bản đồ, chuyển động bám quỹ đạo cho
trước và tránh vật cản.
3.6 THỰC NGHIỆM THỬ NGHIỆM
3.6.1 Chế độ bằng tay
Thử nghiệm di chuyển và điều khiển từ máy tính theo các tình huống sau:
Trình tự thì nghiệm:
51

lOMoARcPSD|38101887
Bước 1: Vạch 2 đường thẳng thế hiện 2 trục x,y của rôbốt trên sàn để xác định điểm bắt đầu
của rôbốt.
Bước 2: Cài đặt các thông số VGx, VGy, Ω, thời gian chạy t
Bước 3: Thu thập dữ liệu từ IMU để xác định vị trí của rôbốt sau khi chạy so với vị trí bất đầu,
ta có kết quả được thể hiện trong Bảng 3.2, Bảng 3.3, Bảng 3.4 qua mỗi lần thì nghiệm.
(1) Chạy thẳng
Thí nghiệm 1: Khả năng chạy thẳng theo phương x
Ta tiến hành cho rôbốt di chuyển thẳng với quảng đường S = 1 m, rôbốt di chuyển trên bề
mặt sàn gỗ phẳng nhẵn, trong quá trình di chuyển không có vật cản. Tiến hành thử nghiệm với
các tốc độ đặt VGx lần lượt là 0.1 m/s, 0.2 m/s, 0.25 m/s, 0.4 m/s tương ứng với các thời gian
chạy là 10 s, 5 s, 4 s, 2.5 s. Với mỗi tốc độ đặt như trên ta thử nghiệm 5 lần. Kết quả thu được
được thể hiện trong Bảng 3.2 bao gồm vận tốc đặt (m/s), vận tốc trung bình thực tế được tính
S
theo v = (m/s), khoảng cách đặt (m), quãng đường đi được S (m), góc lệch γ = φd – φ =
t
atan2(∆y, ∆x) (với φd = 0° là góc đặt, φ là góc của rôbốt sau khi di chuyển) và sai số ∆ =
1 S
(%) ứng với từng vận tốc.
1
Bảng 3.2 Thưc nghiệm khả năng di chuyển theo phương x
Lần Vận Thời gian Vận tốc Khoảng Khoảng Góc lệch Sai số
chạy tốc đặt chạy thực cách đặt cách thực (°) (%)
(m/s) (s) (m/s) (m) (m)
1 0,1 10 0,095 1 0,95 1 5
2 0,1 10 0,090 1 0,90 3 10
3 0,1 10 0,102 1 1,02 1 2
4 0,1 10 0,097 1 0,97 -1 3
5 0,1 10 0,095 1 0,95 -4 5
6 0,2 5 0,198 1 0,99 2 1
7 0,2 5 0,204 1 1,02 2 2
8 0,2 5 0,210 1 1,05 -1 5
9 0,2 5 0,200 1 1,00 1 0
52

lOMoARcPSD|38101887
10 0,2 5 0,198 1 0,99 0 1
11 0,25 4 0,258 1 1,03 2 3
12 0,25 4 0,243 1 0,97 3 3
13 0,25 4 0,248 1 0,99 0 1
14 0,25 4 0,253 1 1,01 -1 1
15 0,25 4 0,265 1 1,06 1 6
16 0,4 2.5 0,408 1 1,02 1 2
17 0,4 2.5 0,420 1 1,05 3 5
18 0,4 2.5 0,420 1 1,05 -2 5
19 0,4 2.5 0,412 1 1,03 1 3
20 0,4 2.5 0,416 1 1,04 2 4
Nhận xét:
- Với yêu cầu rôbốt tự hành di chuyển một khoảng cách S = 1 m, trong các lần thử với các vận
tốc khác nhau, sai số lớn nhất là 0,1 m (tương ứng với 10%) ( ở vận tốc 0,1 m/s). Điều này
cho thấy khi di chuyển trong thực tế, quỹ đạo của rôbốt luôn sẽ sai lệch so với lý thuyết được
thiết lập trên máy tính. Những sai lệch này xảy ra là do sai số trong quá trính điều khiển, sai số
của IMU và sai số khi bánh xe trượt. Điều đó cho thấy khi hoạt động phải hiệu chỉnh lại các hệ
số của bộ điều khiển, hiệu chỉnh IMU đảm bảo kết quả được chính xác nhất ứng với mỗi môi
trường khác nhau.
- Đối với vận tốc của rôbốt, sai lệch lớn nhất là 0,02 m/s (ở vận tốc 0,4 m/s). Nguyên nhân của
sai lệch này là do đáp ứng của bộ điều khiển PID và trượt của bánh xe trên sàn. Do đó cần
hiệu chỉnh các hệ số của bộ PID để giảm tối thiểu sai lệch gây ra.
(2) Chạy ngang
Thí nghiệm 2: Khả năng chạy thẳng theo phương y
Ta tiến hành cho rôbốt di chuyển ngang với quãng đường S = 1 m, rôbốt di chuyển trên
bề mặt sàn gỗ phẳng nhẵn, trong quá trình di chuyển không có vật cản. Tiến hành thử nghiệm
với các tốc độ đặt VGy lần lượt là 0.1 m/s, 0.2 m/s, 0.25 m/s, 0.4 m/s tương ứng với các thời
gian chạy là 10 s, 5 s, 4 s, 2.5 s. Với mỗi tốc độ đặt như trên ta thử nghiệm 5 lần. Kết quả thu
được được thể hiện trong Bảng 3.3 bao gồm vận tốc đặt (m/s), vận tốc trung bình thực tế được
S
tính theo v = (m/s), khoảng cách đặt (m), quãng đường đi được S (m), góc lệch γ = φd – φ =
t
53

lOMoARcPSD|38101887
atan2(∆y, ∆x) (với φd = 90° là góc đặt, φ là góc của rôbốt sau khi di chuyển) và sai số ∆ =
1 S
(%) ứng với từng vận tốc.
1
Bảng 3.3 Thưc nghiệm khả năng di chuyển theo phương y
Lần Vận Thời gian Vận tốc Khoảng Khoảng Góc lệch Sai số
chạy tốc đặt chạy thực cách đặt cách thực (°) (%)
(m/s) (s) (m/s) (m) (m)
1 0,1 10 0.042 1 0.42 40 58

2 0,1 10 0.050 1 0.5 36 50
3 0,1 10 0.044 1 0.44 38 56
4 0,1 10 0.042 1 0.42 40 58
5 0,1 10 0.046 1 0.46 37 54
6 0,2 5 0.122 1 0.61 84 39
7 0,2 5 0.118 1 0.59 78 41
8 0,2 5 0.132 1 0.66 80 34
9 0,2 5 0.128 1 0.64 82 36
10 0,2 5 0.128 1 0.64 80 36
11 0,25 4 0.153 1 0.61 42 39
12 0,25 4 0.168 1 0.67 40 33
13 0,25 4 0.165 1 0.66 41 34
14 0,25 4 0.160 1 0.64 44 36
15 0,25 4 0.155 1 0.62 44 38
16 0,4 2.5 0.244 1 0.61 30 39
17 0,4 2.5 0.252 1 0.63 28 37
18 0,4 2.5 0.264 1 0.66 28 34
19 0,4 2.5 0.256 1 0.64 30 36
20 0,4 2.5 0.252 1 0.63 31 37
Nhận xét:
- Sau các lần thử với các vận tốc khác nhau, ta nhận thấy sai số của rôbốt khi di chuyển theo
phương y là rất lớn 58% (ở 0,1 m/s), hướng di chuyển của rôbốt cũng bị lệch một góc lớn 82°
54

lOMoARcPSD|38101887
(ở 0,2 m/s). Nguyên nhân dẫn đến sai số lớn là do mặt phẳng sàn không phẳng đồng đều
(chênh lệch độ cao giữa các viên đá lát) dẫn đến trượt của các bánh xe không đồng đều, rôbốt
di chuyển không đúng hướng so với lý thuyết.
- Qua thí nghiệm này cho thấy khi thiết kế chuyển động nên thêm các bộ điều khiển phản hồi
để giảm sai số về hướng đi cũng như vận tốc của rôbốt khi di chuyển ngang.
(3) Quay tại chỗ
Thí nghiệm 3: Khả năng điều hướng của rôbốt
Ta tiến hành cho rôbốt xoay tại chỗ một góc φ = 2π (rad) = 360(°), rôbốt di chuyển trên
bề mặt sàn đá phẳng nhẵn, trong quá trình di chuyển không có vật cản. Tiến hành thử nghiệm
với các tốc độ đặt Ωd lần lượt là π/10 rad/s, π/5 rad/s, π/3 rad/s, π/2 rad/s tương ứng với các
thời gian chạy là 20 s, 10 s, 6 s, 4 s. Với mỗi tốc độ đặt như trên ta thử nghiệm 5 lần. Kết quả
thu được được thể hiện trong Bảng 3.4 bao gồm vận tốc góc đặt (rad/s), vận tốc góc trung bình

thực tế được tính theo Ω = (rad/s), góc quay đặt (°), góc quay thực (°) và sai số (%) ứng
t
với từng vận tốc.
Bảng 3.4 Thực nghiệm khả năng thay đổi tư thế của Rôbốt
Lần Vận tốc Thời gian Vận tốc Góc đặt Góc thực Sai số
chạy góc đặt chạy góc thực (°) (°) (%)
(rad/s) (s) (rad/s)
1 π/10 20 0,3124 360 358 0,56
2 π/10 20 0,3115 360 357 0,83
3 π/10 20 0,3124 360 358 0,56
4 π/10 20 0,3124 360 358 0,56
5 π/10 20 0,3133 360 359 0,28
6 π/5 10 0,6213 360 356 1,11
7 π/5 10 0,6196 360 355 1,39
8 π/5 10 0,6196 360 355 1.39
9 π/5 10 0,6231 360 357 0,83
10 π/5 10 0,6213 360 356 1,11
11 π/3 6 1,0620 360 365 1,39
55

lOMoARcPSD|38101887
12 π/3 6 1,0530 360 362 0,56
13 π/3 6 1,0620 360 365 1,39
14 π/3 6 1,0588 360 364 1,11
15 π/3 6 1,0620 360 365 1,39
16 π/2 4 1,6057 360 368 2,22
17 π/2 4 1,6144 360 370 2,78
18 π/2 4 1,6057 360 368 2,22
19 π/2 4 1,6101 360 369 2,50
20 π/2 4 1,6188 360 371 3,06
Nhận xét:
- Sau 20 lần thử nghiệm với 4 vận tốc góc khác nhau, ta nhận thấy sai lệch về góc quay lớn
nhất là 3,06 % ( 11° ở vận tốc góc π/2 rad/s), các lần thử với vận tốc góc càng lớn thì góc quay
thực tế càng lớn hơn so với góc đặt (11° ở vận tốc góc π/2 rad/s), vận tốc góc càng nhỏ thì góc
quay thưc tế càng nhỏ so với góc đặt (3° ở vận tốc góc π/10 rad/s). Nguyên nhân là do ma sát
giữa sàn và bánh xe chưa xét đến và chưa xét đến lực quán tính. Ngoài ra, sai số của kết cấu
cũng như bộ truyền động chưa được đánh giá.
- Qua thí nghiệm này có thể cho thấy khi thiết kế quỹ đạo chuyển động, cần tránh việc quay
nhanh một góc hay tăng tốc độ góc đột ngột.
3.6.2 Chế độ điều khiển tự động bám theo quỹ đạo
Sử dụng dữ liệu đường cong NURBS được thiết kế trong Chương 2, điều khiển robot đi theo
quỹ đạo cho trước.
Thí nghiệm 4: Thực nghiệm chạy theo quỹ đạo cho trước của rôbốt
Trình tự thì nghiệm:
Bước 1: Xác định vị trí bắt đầu của rôbốt, đảm bảo quỹ đạo di chuyển của rôbốt phẳng, không
có vật cản trên đường đi.
Bước 2: Đưa tệp dữ liệu của đường cong Nurbs đã thiết kế vào trong chương trình chạy.
Bước 3: Theo dõi sự di chuyển của rôbốt bằng IMU và lưu vị trí của rôbốt vào tệp văn bản
mỗi 0,1s.
Từ tệp văn bản dữ liệu có ở Bước 3 kết hợp với xử lý so sánh trên phần mềm MATLAB, kết
quả quỹ đạo thu được và sai số được thế hiện trong Hình 3.10 và Hình 3.11
56

lOMoARcPSD|38101887
Hình 3.10 Đồ thị quỹ đạo mong muốn và thực tế của rôbốt
Hình 3.11 Sai số vị trí của rôbốt trong thực nghiệm
Nhận xét:
- Qua đồ thị quỹ đạo di chuyển trong hình 3.10, ta thấy rằng quỹ đạo thực tế của rôbốt bám sát
chưa hoàn toàn. Cần thay đổi các tham số của bộ điều khiển để đáp ứng tốt hơn trong các môi
trường khác nhau.
- Có thế thấy rằng sai số thực nghiệm trong Hình 3.11 lớn nhất là 0.075m, lớn hơn sai số khi
mô phỏng ở chương 2. Lý do là do những nguyên nhân sau: Bài toán tính lý thuyết và bộ điều
khiển ở chương 2 chưa tính đến lực, các sai số của cảm biến IMU, sai số của Encoder.
57

lOMoARcPSD|38101887
3.6.3 Thực nghiệm quét bản đồ
Dữ liệu Lidar được thu thập về máy tính nhúng, sử dụng bộ lọc điểm để kiểm tra mức độ
tương ứng dữ liệu của 2 lần quét kế tiếp. Từ đó suy ra vị trí của rôbốt trong hệ quy chiếu cố
định gắn với sàn nhà.
Các bước thực hiện:
Bước 1: Quét dự liệu lidar lần đầu để xác định vị trí ban đầu của rôbốt
Bước 2: Lấy dữ liệu của lidar trong lần sau đó. So sánh sự trùng lặp giữa 2 lần quét, đưa
ra vị trí tâm của rôbốt ở 2 lần quét liên tiếp
Bước 3: So sánh dữ liệu để xác định vị trí của lần quét hiện tại so với lần quét đầu tiên, vẽ
lên bản đồ.
Thí nghiệm 5: Thực nghiệm khả năng quét bản đồ của rôbốt
Trong thí nghiệm này, tiến hành điều khiển rôbốt di chuyển trong phỏng kín có các thông
số kích thước và vật cản được thể hiện trong Hình 3.12
Hình 3.12 Kích thước của phòng kín và vị trí các vật cản trong phòng
Sau khi quét khu vực phòng kín, ta thiết lập được bản đồ và thi được các kích thước như
Hình 3.13 so sánh với các kích thước thực tế trong Bảng 3.6
58

lOMoARcPSD|38101887
Hình 3.13 Bản đồ rôbốt thiết lập được
Bảng 3.5 Bảng so sánh các kích thước thực tế và kích thước bản đồ rôbốt thiết lập
STT Kích thước thực Kích thước rôbốt Sai lệch Tỷ lệ sai lệch
tế (m) thiết lập (m) (m) (%)
1 4 4,02 0,02 0,5
2 9 9,02 0,02 0,22
3 5,2 5,18 0,02 0,38
4 1,2 1,21 0,01 0,83
Nhận xét:
Bản đồ thu được có sai lệch so với thực tế, tuy nhiên sai lệch nhỏ ( tối đa 0,83 %) không
gây ảnh hưởng nhiếu đến quá trình vận hành rôbốt. Nguyên nhân gây ra sai lệch do trong
phòng có một số vật cản đặc biệt, có tính phản quang tốt dẫn đến sai số của lidar; do thuật toán
xử lý dữ liệu chưa được tốt, khoảng cách giữa các pixel điểm lưu lại còn sai lệch.

(1) Đã thiết kế, chế tạo thực tế được mô hình rôbốt đa hướng kiểu bánh Mecanum
(2) Đã tiến hành thực nghiệm các khả năng hoạt động của rôbốt:
i) Xác định sai số khi rôbốt di chuyển thẳng, ngang, quay và hiệu chỉnh PID điều
khiển động cơ, hiệu chỉnh các thông số cài đặt để giảm sai số
ii) Đã tiến hành điều khiển chạy theo quỹ đạo cho trước.
59

lOMoARcPSD|38101887
iii) Thử nghiệm khả năng tạo bản đồ bằng lidar.

Các kết quả của đồ án có ý nghĩa quan trọng trong việc thử nghiệm các thuật toán điều
khiển và tiến tới chế tạo và phát triển các AGV và AMR trong lĩnh vực công nghiệp, y tế và
dịch vụ.
60

lOMoARcPSD|38101887
Chương 4
LẬP TRÌNH BỘ ĐIỀU KHIỂN
4.1 HỆ ĐIỀU HÀNH ROS
Hệ điều hành ROS (Robot Opera Systems) là một bộ phần mềm trung gian người máy mã
nguồn mở. Mặc dù ROS không phải là một hệ điều hành hoàn chỉnh mà là một tập hợp các
khung phần mềm để phát triển phần mềm rô bốt, nó cung cấp các dịch vụ được thiết kế cho
một cụm máy tính không đồng nhất như trừu tượng hóa phần cứng, điều khiển thiết bị cấp
thấp, ...
Lợi ích của hệ điều hành ROS:
- Quản lý phần cứng bằng cách viết trình điều khiển.
- Quản lý bộ nhớ và các quy trình.
- Quản lý đồng thời, song song, hợp nhất dữ liệu.
- Cung cấp các thuật toán trừu tượng, sử dụng trí tuệ nhân tạo.
Có nhiều hệ điều hành để điều khiển rôbốt như Microsoft Robotics Developer Studio,
NAOQi, URBI, ROS … . Lý do đồ án chọn chọn ROS để lập trình bộ điều khiển rôbốt:
- Mã nguồn mở và miễn phí: Phần lõi của ROS được phát hành theo giấy phép BSD,
cho phép sử dụng thương mại và phi thương mại. ROS truyền dữ liệu giữa các mô-đun bằng
cách sử dụng giao tiếp (IPC), có nghĩa là các hệ thống được xây dựng bằng ROS có thể có
giấy phép chi tiết cho các thành phần khác nhau của chúng.
- Đa ngôn ngữ: ROS là ngôn ngữ trung lập và có thể được lập trình bằng nhiều ngôn ngữ
khác nhau. Đặc tả ROS hoạt động ở lớp nhắn tin. Các kết nối ngang hàng được đàm phán
trong XML-RPC, tồn tại trong một số lượng lớn ngôn ngữ. Để hỗ trợ một ngôn ngữ mới, ví
dụ, các lớp C ++ được bọc lại (được thực hiện cho ứng dụng khách Octave chẳng hạn) hoặc
các lớp được viết cho phép tạo ra các thông điệp. Các thông báo này được mô tả bằng IDL
(Ngôn ngữ định nghĩa giao diện).
- Có nhiều công cụ hỗ trợ: Thay vì môi trường thời gian chạy nguyên khối, ROS đã áp
dụng thiết kế vi hạt, sử dụng một số lượng lớn các công cụ nhỏ để xây dựng và chạy các thành
phần ROS khác nhau. Khi bạn trình bày các hướng dẫn của ROS, bạn sẽ học cách sử dụng một
số lệnh được sử dụng để thao tác với các nút và thông báo. Mỗi lệnh trong thực tế là một thực
thi. Ưu điểm của hệ thống này là một vấn đề với một thực thi không ảnh hưởng đến các hệ
thống khác, điều này làm cho hệ thống mạnh mẽ và linh hoạt hơn một hệ thống dựa trên môi
trường thời gian chạy tập trung.
61

lOMoARcPSD|38101887
Tổng quan sơ đồ điều khiển:
Hình 4.1 Sơ đồ hệ thống điều khiển của ROS
Giải thích các khối trong Hình 4.1:

- Package: Các gói là các thành phần cơ bản của ROS, nó chứa các quá trình hoạt động,
các thư viện, các tệp, các chương trình chỉnh sửa.
- Node: Các nút quy trình tính toán, mỗi node được viết bằng các ngôn ngữ lập trình như
c/c++, python. Trong một rôbốt có thể có nhiều nút quy trình để thực hiện nhiều nhiệm vụ
khác nhau, chúng có thể giao tiếp, trao đổi thông tin với nhau thông qua các phương thức giao
tiếp trong ROS.
- ROS Master: Cung cấp việc đăng ký kết nối và quản lý các nút, là trung tâm của ROS.
Các nút sẽ không thể trao đổi tin nhắn với nhau nếu không chung một ROS Master.
Đồ án sử dụng hệ điều hành Ubuntu Server 20.01 và cài đặt ROS Noetic theo hướng dẫn cài
đặt trong [10].
4.2 THIẾT KẾ PHẦN MỀM ĐIỀU KHIỂN ĐỘNG CƠ VÀ GIAO TIẾP IMU
4.2.1 Sơ đồ kết nối động cơ và IMU với máy tính nhúng
Đồ án sử dụng một bo mạch nhúng Arduino Mega 2560 có chức năng trung gian trao đổi
tín hiệu giữa máy tính nhúng và các thiết bị ngoại vi động cơ, mạch điều khiển động cơ, IMU .
Hình 4.2 là sơ đồ kết nối giữa máy tính nhúng và bo mạch nhúng.
62

lOMoARcPSD|38101887
Hình 4.2 Sơ đồ kết nối máy tính nhúng và bo mạch nhúng
4.2.2 Thiết kế phần mềm điều khiển động cơ
Đồ án sử dụng thuật toán PID để điều khiển động cơ, thuật toán PID là một cơ chế phản
hồi vòng kín điều khiển được sử dụng rất rộng rãi trong các bộ điều khiển động cơ. Trong đó
thuật toán PID sẽ kết hợp dữ liệu vận tốc góc của mỗi bánh xe được lấy từ Encoder, sau đó
đưa ra tín hiệu Analog phù hợp để động cơ hoạt động đúng với vận tốc góc mong muốn.
Encoder trên mỗi động cơ là loại 2 kênh, 182 xung/1 vòng quay động cơ (bao gồm cả hộp số),
Mỗi kênh của Encoder nối vào một chân I/O của bo mạch nhúng. Trên Hình 4.3 là dạng tín
hiệu 2 kênh A B của một Encoder khi đọc dữ liệu.
63

lOMoARcPSD|38101887
Hình 4.3 Dạng tín hiệu của 2 kênh Encoder trong một động cơ
Sử dụng Timer của bo mạch nhúng đọc đồng thời tín hiệu của các Encoder và xác định
góc quay dựa theo Bảng 4.1
Bảng 4.1 Xác định góc quay của một động cơ tại các thời điểm
STT Kênh A Kênh B Kênh A hiện Kênh B hiện Biến đếm

trước trước tại tại xung tăng
1 0 0 0 0 0
2 1 0 0 0 +1
3 0 1 0 0 -1
4 1 1 0 0 +2
5 0 0 1 0 -1
6 1 0 1 0 0
7 0 1 1 0 -2
64

lOMoARcPSD|38101887
8 1 1 1 0 +1
9 0 0 0 1 +1
10 1 0 0 1 -2
11 0 1 0 1 0
12 1 1 0 1 -1
13 0 0 1 1 +2
14 1 0 1 1 -1
15 0 1 1 1 +1
16 1 1 1 1 0
Trong đó, dữ liệu đầu vào là tín hiệu xung của lần đọc hiện tại cà lần đọc trước đó của mỗi
kênh, từ đó tính đước sự tăng giảm của biến đếm xung của các động cơ. Với phương pháp đọc
như trên, ứng với một vòng của đầu ra động cơ (sau hộp số) thì số lượng xung đếm được là
182×4 = 728 (xung).
Bộ điều khiển PID nằm trong thư viện có sẵn của Arduino, bằng cách thay đổi, hiệu chỉnh các
thông số và thực nghiệm, bộ số cho bộ điều khiển PID là kp = 0.5, ki = 0.4, kd = 0.2, với bộ số
điều khiển này, rôbốt đi được ổn định và đáp ứng được tốc độ cài đặt.
4.2.3 Thiết kế phần mềm giao tiếp với IMU
IMU 10 DOF được thiết kế bao gồm:

- MPU9255:3-trục con quay hồi chuyển, 3 trục gia tốc, và 3-Trục La bàn/từ kế
 Nội bộ Kỹ Thuật Số Chuyển Động Chế Biến™ (DMP™) động cơ, giảm tải
làm phức tạp Fusion tính toán, cảm biến đồng bộ hóa, cử chỉ công nhận, vv.
 So sánh với MPU6050, Các MPU9255 là tiêu thụ điện năng thấp, và phù
hợp hơn cho mặc các thiết bị
- BMP180: Áp suất không khí cảm biến
 Nhúng cảm biến nhiệt độ cho nhiệt độ bồi thường
Giao tiếp với IMU thông qua giao tiếp I2C của bo mạch. Thư viện đọc dữ liệu của IMU
có sẵn trong thư viện của Arduino IDE.
Xét rôbốt chuyển động trên mặt phẳng, các dữ liệu IMU cần sử dụng là:
65

lOMoARcPSD|38101887
- Gia tốc theo trục x: ax

- Gia tốc theo trục y: ay
- Vận tốc quay theo trục z: ωz
Từ các dữ liệu đã lấy được từ cảm biến IMU trên, ta đưa dữ liệu gốc vào 1 bộ lọc tần số
cao để lọc đi các loại nhiễu tần số cao: khi xảy ra các chuyển động đột ngột khiến các loại
IMU bị nhiễu do sung lực, bộ lọc sẽ có tác dụng loại bỏ dữ liệu mà gây ra sự tăng vọt về giá
trị. Theo như dữ liệu đã nhận được từ IMU, xác định được ax, ay và . ωz, tuy nhiên để thực
hiện khả năng định hướng cần dữ liệu về vị trí x và y, dữ liệu về góc quay φ theo trục z của
rôbốt theo thời gian. Để nhận về dữ liệu góc quay φ và vị trí x, y, bộ lọc Extended Kalman
Fillter (EKF, [12]) được sử dụng (Hình 4.4). Với các đầu vào là ax, ay và ωz được vào liên tục
theo thời gian, thuật toán sẽ đưa ra được x, y và φ tương ứng bằng cách tích phân 2 lần theo
thời gian với giá trị ax, ay và tích phân 1 lần theo thời gian với giá trị của ωz, kết hợp với các
thông số tùy chỉnh sẽ thu được kết quả cuối cùng là vị trí và góc quay của rôbốt so với vị trí
ban đầu.
Hình 4.4 Xử lý dữ liệu của IMU
4.2.4 Chương trình của bo mạch nhúng Arduino Mega 2560
Chương trình được viết bằng ngôn ngữ C/C++ và được nạp vào Arduino Mega 2560 bằng
phần mềm Arduino IDE
#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
#include "ros.h"
#include "ros/time.h"
#include "lino_msgs/Velocities.h"
#include "geometry_msgs/Twist.h"
#include "lino_msgs/PID.h"
#include "lino_msgs/Imu.h"
#include "geometry_msgs/Point.h"
#include "Imu.h"
#include <Adafruit_NeoPixel.h>
#define ENCODER1 ((PINH>>PH0&0x01)<<2)| ((PINH>>PH1&0x01)<<3)

#define ENCODER2 ((PINJ>>PJ1&0x01)<<2)| ((PINJ>>PJ0&0x01)<<3)
66

lOMoARcPSD|38101887
#define ENCODER3 ((PINA>>PA0&0x01)<<2)| ((PINA>>PA1&0x01)<<3)

#define ENCODER4 ((PIND>>PD2&0x01)<<2)| ((PIND>>PD3&0x01)<<3)
#define IMU_PUBLISH_RATE 20 //hz

#define COMMAND_RATE 20 //hz
#define DEBUG_RATE 5
Adafruit_NeoPixel led(70, 13, NEO_GRB + NEO_KHZ800);
float g_req_linear_vel_x = 0;
float g_req_linear_vel_y = 0;
float g_req_angular_vel_z = 0;
unsigned long g_prev_command_time = 0;

unsigned long t_pos = 0;
float x = 0;
float y = 0;
float phi = 0;
class motor
{
private:
int pinA, pinB, encoderA, encoderB;
bool a,b,la,lb;
byte state;
double e_speed = 0;
double e_speed_pre = 0;
double e_speed_sum = 0;
double kp = 0.5;
double ki = 0.4;
double kd = 0.2;
unsigned long t_pid = millis();
public:
double pwm_pulse = 0; //this value is 0~255
double pv_speed = 0;
volatile long encoder = 0;
void set(int pinA_motor, int pinB_motor, int enA, int enB)
{
pinA = pinA_motor;
pinB = pinB_motor;
encoderA = enA;
encoderB = enB;
pinMode(pinA, OUTPUT);
pinMode(pinB, OUTPUT);
pinMode(encoderA, INPUT_PULLUP);
pinMode(encoderB, INPUT_PULLUP);
}
void PID(float& set_speed)
{
pv_speed = 60.0 * (encoder / 728.0) / (millis()-t_pid) * 1000;
t_pid = millis();
e_speed = set_speed - pv_speed;
pwm_pulse = e_speed * kp + e_speed_sum * ki + (e_speed - e_speed_pre) * kd;
e_speed_pre = e_speed; //save last (previous) error
e_speed_sum += e_speed; //sum of error
if (e_speed_sum > 4000) e_speed_sum = 4000;
if (e_speed_sum < -4000) e_speed_sum = -4000;
encoder = 0;
67

lOMoARcPSD|38101887
out(pwm_pulse);
return;
}
void exit()
{
e_speed_sum = 0;
e_speed_pre = 0;
}
void out(int pwm_pulse)
{
if (pwm_pulse > 0) {
if (pwm_pulse>250) pwm_pulse=60;
analogWrite(pinA, 0);
analogWrite(pinB, abs(pwm_pulse));
}
else {
if (pwm_pulse<-250) pwm_pulse=60;
analogWrite(pinA, abs(pwm_pulse));
analogWrite(pinB, 0);
}
}
void update(uint8_t s)
{
s |= state & 3;
switch (s) {
case 0: case 5: case 10: case 15:
break;
encoder++; break;
encoder--; break;
case 3: case 12:
encoder += 2; break;
default:
encoder -= 2; break;
}
state = (s >> 2);
}
} motor1, motor2, motor3, motor4;
void commandCallback(const geometry_msgs::Twist& cmd_msg);

ros::NodeHandle nh;
ros::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", commandCallback);
lino_msgs::Imu raw_imu_msg;
ros::Publisher raw_imu_pub("raw_imu", &raw_imu_msg);
lino_msgs::Velocities raw_vel_msg;
ros::Publisher raw_vel_pub("raw_vel", &raw_vel_msg);
geometry_msgs::Point raw_pos_msg;
ros::Publisher raw_pos_pub("raw_pos", &raw_pos_msg);
void setup()
{
led.begin();
led.clear();
68

lOMoARcPSD|38101887
for(int i=0; i<70; i++) {

led.setPixelColor(i, led.Color(0, 0, 2));
}
led.show();
motor1.set(4,5,17,16);
motor2.set(7,6,14,15);
motor3.set(2,10,22,23);
motor4.set(8,9,19,18);
TCCR1B = TCCR1B & 0b11111000 | 1;
nh.initNode();
nh.getHardware()->setBaud(57600);
nh.subscribe(cmd_sub);
nh.advertise(raw_vel_pub);
nh.advertise(raw_imu_pub);
nh.advertise(raw_pos_pub);
while (!nh.connected())
{
nh.spinOnce();
}
nh.loginfo("LINOBASE CONNECTED");
delay(1);
for(int i=0; i<70; i++) {

led.setPixelColor(i, led.Color(20, 0, 0));
}
led.show();
cli();
TCCR1A = 0;
TCCR1B = 0;
TIMSK1 = 0;
TCCR1B |= (1 << CS11) ;
TCNT1= 65000;
TIMSK1 |= (1 << TOIE1);
sei();
}
ISR (TIMER1_OVF_vect)
{
TCNT1= 65400;
motor1.update(ENCODER1);
}
void loop()
{
static unsigned long prev_control_time = 0;
static unsigned long prev_imu_time = 0;
static unsigned long prev_debug_time = 0;
static bool imu_is_initialized;
if ((millis() - prev_control_time) >= (1000 / COMMAND_RATE))

{
moveBase();
prev_control_time = millis();
}
69

lOMoARcPSD|38101887
if ((millis() - g_prev_command_time) >= 500)

{
stopBase(millis() - g_prev_command_time);
}
if ((millis() - prev_imu_time) >= (1000 / IMU_PUBLISH_RATE))

{
if (!imu_is_initialized)
{
imu_is_initialized = initIMU();
if(imu_is_initialized)
nh.loginfo("IMU Initialized");
else
nh.logfatal("IMU failed to initialize. Check your IMU connection.");
}
else
{
publishIMU();
}
prev_imu_time = millis();
}
if(DEBUG)
{
if ((millis() - prev_debug_time) >= (1000 / DEBUG_RATE))
{
printDebug();
prev_debug_time = millis();
}
}
nh.spinOnce();
}
void commandCallback(const geometry_msgs::Twist& cmd_msg)

{
g_req_linear_vel_x = cmd_msg.linear.x;
g_req_linear_vel_y = cmd_msg.linear.y;
g_req_angular_vel_z = cmd_msg.angular.z;
g_prev_command_time = millis();
}
void moveBase()
{
float w1 = 1/WHEEL_DIAMETER*2 * (g_req_linear_vel_x - g_req_linear_vel_y -
(LR_WHEELS_DISTANCE+FR_WHEELS_DISTANCE)/2*g_req_angular_vel_z) *30/M_PI;
float w2 = 1/WHEEL_DIAMETER*2 * (g_req_linear_vel_x + g_req_linear_vel_y +
float w3 = 1/WHEEL_DIAMETER*2 * (g_req_linear_vel_x - g_req_linear_vel_y +
float w4 = 1/WHEEL_DIAMETER*2 * (g_req_linear_vel_x + g_req_linear_vel_y -
motor1.PID(w1);
motor2.PID(w2);
motor3.PID(w3);
motor4.PID(w4);
70

lOMoARcPSD|38101887
raw_vel_msg.linear_x = WHEEL_DIAMETER/2/4*(motor1.pv_speed + motor2.pv_speed +

motor3.pv_speed + motor4.pv_speed)*M_PI/30;
raw_vel_msg.linear_y = WHEEL_DIAMETER/2/4*(-motor1.pv_speed + motor2.pv_speed -
motor3.pv_speed + motor4.pv_speed)*M_PI/30;
raw_vel_msg.angular_z = WHEEL_DIAMETER/2/2/(LR_WHEELS_DISTANCE+FR_WHEELS_DISTANCE)
* (-motor1.pv_speed + motor2.pv_speed + motor3.pv_speed - motor4.pv_speed)*M_PI/30;
raw_vel_pub.publish(&raw_vel_msg);
float drx = raw_vel_msg.linear_x * (millis()-t_pos)/1000.000;

float dphi = raw_vel_msg.angular_z * (millis()-t_pos)/1000.000;
t_pos = millis();
phi += dphi;
x = x + drx*cos(phi);
y = y + drx*sin(phi);
raw_pos_msg.x = x;
raw_pos_msg.y = y;
raw_pos_msg.z = phi*180/M_PI;
raw_pos_pub.publish(&raw_pos_msg);
}
void stopBase(unsigned long timeout)

{
g_req_linear_vel_x = 0;
g_req_linear_vel_y = 0;
g_req_angular_vel_z = 0;
if (timeout > 2000)
{
motor1.exit();
motor2.exit();
motor3.exit();
motor4.exit();
}
}
void publishIMU()
{
raw_imu_msg.linear_acceleration = readAccelerometer();
raw_imu_msg.angular_velocity = readGyroscope();
raw_imu_msg.magnetic_field = readMagnetometer();
raw_imu_pub.publish(&raw_imu_msg);
}
4.3 THUẬT TOÁN ĐIỀU KHIỂN
Đồ án sử dụng thuật toán điều khiển phản hồi trạng thái tuyến tính được thiết kế ở
Chương 2 của Đồ án. Dữ liệu vị trí được lấy từ topic /tf và gửi đi thông qua topic /cmd_vel để
điều khiển Rôbốt
Chương trình của bộ điều khiển viết bằng ngôn ngữ C++
#define _CRT_SECURE_NO_DEPRECATE
#include <ros/ros.h>
#include <iostream>
#include <fstream>
#include <sstream>
71

lOMoARcPSD|38101887
#include "Queue_adv.h"
using namespace std;

struct data_vel
{
float vx = 0;
float vy = 0;
float o = 0;
};
void docfile();
data_vel xu_ly(float time, float x, float y, float phi, float vx, float vy, float o,
float imuax, float imuay, float imuaz, float imugx, float imugy, float imugz, float
imumx, float imumy, float imumz);
bool start = false;

bool run = true;
bool savefile = false;
int startPoint = 0;
float time0;
float xStart = 0;
float yStart = 0;
float phiStart = 0;
Queue_adv full_data;
data_pos Ldata, Ndata;
ofstream myWriteFile1;
void docfile()
{
data_pos data;
std::ifstream myReadFile;
myReadFile.open("src/rb_chay_1m/sdk/Long_025_cong.txt", std::ios_base::in);
if (!myReadFile.is_open()) return;
std::string line;
std::getline(myReadFile, line);
cout << line << endl;
while (std::getline(myReadFile, line))
{
std::istringstream iss(line);
if (!(iss >> data.t >> data.x >> data.y >> data.phi >> data.bk >>
data.V>>data.x1>>data.y1>>data.x2>>data.y2)) break;
full_data.Add(data);
}
myReadFile.close();
}
data_vel xu_ly(float time, float x, float y, float phi, float vx, float vy, float o,
float imuax, float imuay, float imuaz, float imugx, float imugy, float imugz, float
imumx, float imumy, float imumz)
{
data_vel curr_vel;
if (startPoint < 20) {
startPoint++;
return curr_vel;
}
if (!start)
{
myWriteFile1.open("src/rb_chay_1m/sdk/data_save.txt",
std::ios_base::out);
72

lOMoARcPSD|38101887
myWriteFile1<<
"t\txd\tyd\tphid\tx\ty\tphi\tvx\tvy\to\timuax\timuay\timuaz\timugx\timugy\timugz\timumx
\timugy\timugz\n";
time0 = time;
xStart = x;
yStart = y;
phiStart = phi;
start = true;
run = full_data.Getitem(Ldata);
run = full_data.Getitem(Ndata);
}
float dx = x-xStart;
float dy = y-yStart;
float dphi = phiStart;
x = dx*cos(dphi) + dy*sin(dphi);
y = -dx*sin(dphi) + dy*cos(dphi);
phi = phi - phiStart;
float t = time - time0;
cout << "t: " <<setprecision(4)<<fixed << t << "\tx: " << x << "\ty: "<< y <<
"\tphi: " << phi*180/M_PI << endl;
if (run)
{
if (t >= Ndata.t) {
Ldata = Ndata;
run = full_data.Getitem(Ndata);
}
float xd = Ldata.x;
float yd = Ldata.y;
float phid = Ldata.phi;
float vd = Ldata.V;
float od = Ldata.V/Ldata.bk*((Ndata.phi-Ldata.phi)>0?1:-1);
cout << "t: " <<setprecision(4)<<fixed << t << "\txd: " << xd << "\tyd:
"<< yd << "\tphid: " << phid*180/M_PI << endl;
float ex = x - xd;
float ey = y - yd;
float ephi = phi-phid;
while (ephi > M_PI) ephi -= 2*M_PI;
while (ephi <-M_PI) ephi += 2*M_PI;
float eRx = ex*cos(phi) + ey*sin(phi);

float eRy = -ex*sin(phi) + ey*cos(phi);
float eRphi = ephi;
float e = sqrt(eRx*eRx + eRy*eRy);
float zeta = 0.9;
float g = 0.266;
float k1 = zeta * sqrt(od * od + g * vd * vd) + sqrt(zeta * zeta * od *

od + g * vd * vd * (zeta * zeta - 1));
float k2 = zeta * sqrt(od * od + g * vd * vd) - sqrt(zeta * zeta * od *
od + g * vd * vd * (zeta * zeta - 1));
float k3 = 2 * zeta * sqrt(od * od + g * vd * vd);
float u1 = -k1*eRx;
float u2 = -k2*eRy;
float u3 = -k3*eRphi;
curr_vel.vx = vd - u1;
73

lOMoARcPSD|38101887
curr_vel.vy = 0 - u2;
curr_vel.o = od - u3;
myWriteFile1 << setprecision(6)<<fixed << t << "\t" << xd << "\t"<< yd <<
"\t" << phid << "\t" << x << "\t" << y << "\t" << phi << "\t" ;
myWriteFile1 << setprecision(6)<<fixed << vx << "\t" << vy << "\t"<< o <<
"\t" ;
myWriteFile1 << setprecision(6)<<fixed << imuax << "\t" << imuay <<
"\t"<< imuaz << "\t" ;
myWriteFile1 << setprecision(6)<<fixed << imugx << "\t" << imugy <<
"\t"<< imugz << "\t" ;
myWriteFile1 << setprecision(6)<<fixed << imumx << "\t" << imumy <<
"\t"<< imumz << "\t" ;
myWriteFile1 << '\n';
}
else if (!savefile) {
myWriteFile1.close();
cout<< "save OK" <<endl;
savefile = true;
}
return curr_vel;
}
4.4 THU THẬP DỮ LIỆU LIDAR VÀ QUÉT BẢN ĐỒ
Lidar được sử dụng là RPLidar, chương trình đọc dữ liệu từ Lidar [11] và hiệu chỉnh lại
các thông số để hoạt động đúng cổng USB của máy tính nhúng.
Chương trình xây dựng bản đồ dựa trên phương pháp so sánh vị trí các điểm của 2 lần
quét liên tiếp, tự đó tìm ra vị trí có số điểm quét trùng nhau nhiều nhất, ứng với mỗi điểm
được chia lưới trong ma trận bản đồ.
74

lOMoARcPSD|38101887
Hình 4.5 Phương pháp quét bản đồ; a) Dữ liệu lidar quét; b) Dữ liệu bản đồ có sẵn;
c) Vị trí của Lidar trong bản đồ
Theo phương pháp được nêu trên Hình 4.5, mỗi lần liên tiếp sẽ được ghi lại vào ma trận
bản đồ như Hình 4.5c, các điểm màu xanh lá là điểm mới thêm vào bản đồ, từ đó ta có ma trận
bản đồ hoàn chỉnh.
Chương trình quét tạo bản đồ được viết dưới dạng C++:
#define _CRT_SECURE_NO_DEPRECATE
#include <iostream>
#include <math.h>
#include <vector>
#include<fstream>
#include <stdio.h>
#include <thread>
75

lOMoARcPSD|38101887
const double lidar1_x = -0.0, lidar1_y = -0.18;

const double lidar2_x = 0.0, lidar2_y = 0.18;
const double pi = 3.14159265359;
const double deg_rad = pi / 180;
#define DEG2RAD(x) ((x)*deg_rad)

#pragma once
struct position
{
double x;
double y;
long t;
};
class scan_matching
{
private:
FILE* pFile;
unsigned char arr_convert[10240][1280];
void rline(long x1, long y1, long x2, long y2);
public:
unsigned long x_min = 10240, y_min = 10240, x_max = 0, y_max = 0;
double _scale;
unsigned char arrout[10240][10240];
long _countmax;
scan_matching(double limit);
void draw(double ranges[], position pos);
position match(double ranges[]);
position match(double ranges[], double _limit, position pos);
void save();
bool load();
};
position p;
double scale;
double _cos[1440];
double _sin[1440];
unsigned char arr[10240][10240];
unsigned long countmax = 0;
scan_matching::scan_matching(double meshing)
{
scale = 1 / meshing;
_scale = scale;
for (int i = 0; i < 1440; i++) _cos[i] = scale * cos(DEG2RAD(i));
for (int i = 0; i < 1440; i++) _sin[i] = scale * sin(DEG2RAD(i));
}
void scan_matching::rline(long x1, long y1, long x2, long y2)

{
x2 = x1 + (x2-x1);
y2 = y1 + (y2-y1);
double k = abs((y2-y1)*1./(x2-x1));
if (k < 1)
for (int i=0 ; i <= abs(x2-x1); i++)
{
long x = (long) ((x2 - x1) > 0 ? 1 : -1 ) * i + x1;
long y = (long) ((y2 - y1) > 0 ? 1 : -1 ) * i * k + y1;
76

lOMoARcPSD|38101887
arr[x][y] = 30;
arrout[x][y] = 30;
}
else
for (int i=0 ; i <= abs(y2-y1); i++)
{
long x = (long) ((x2-x1)>0?1:-1) * i/k + x1;
long y = ((y2-y1)>0?1:-1) * i + y1;
arr[x][y] = 30;
arrout[x][y] = 30;
}
}
void scan_matching::draw(double ranges[], position pos)
{
long ox = lidar1_x * _cos[pos.t] + lidar1_y * _sin[pos.t] + scale * pos.x;
long oy = lidar1_x * _sin[pos.t] - lidar1_y * _cos[pos.t] + scale * pos.y;
for (int i = 0; i < 180; i++)
{
if (ranges[i] > 0)
{
unsigned long _x = 5120 + ranges[i] * _cos[i + pos.t] + ox;
unsigned long _y = 5120 + ranges[i] * _sin[i + pos.t] + oy;
if (_x < x_min) x_min = _x;
if (_x > x_max) x_max = _x;
if (_y < y_min) y_min = _y;
if (_y > y_max) y_max = _y;
if (_x >= 10240 || _y >= 10240) continue;
rline(5120 + ox, 5120 + oy, _x, _y);
arr[_x][_y] = 130;
arrout[_x][_y] = 130;
arrout[_x][_y] = 130;
arrout[_x-1][_y] = 130;
arrout[_x+1][_y] = 130;
arrout[_x][_y-1] = 130;
arrout[_x][_y+1] = 130;
}
}
ox = lidar2_x * _cos[pos.t] + lidar2_y * _sin[pos.t] + scale * pos.x;
oy = lidar2_x * _sin[pos.t] - lidar2_y * _cos[pos.t] + scale * pos.y;
for (int i = 180; i < 360; i++)
{
if (ranges[i] > 0)
{
unsigned long _x = 5120 + ranges[i] * _cos[i + pos.t] + ox;
unsigned long _y = 5120 + ranges[i] * _sin[i + pos.t] + oy;
if (_x < x_min) x_min = _x;
if (_x > x_max) x_max = _x;
if (_y < y_min) y_min = _y;
if (_y > y_max) y_max = _y;
if (_x >= 10240 || _y >= 10240) continue;
rline(5120 + ox, 5120 + oy, _x, _y);
arr[_x][_y] = 130;
arrout[_x][_y] = 130;
arrout[_x-1][_y] = 130;
arrout[_x+1][_y] = 130;
arrout[_x][_y-1] = 130;
arrout[_x][_y+1] = 130;
}
77

lOMoARcPSD|38101887
}
return;
}
void calc_F(double ranges[], long x1, long x2, long y1, long y2)
{
for (long dx = x1; dx < x2; dx+=3)
{
for (long dy = y1; dy < y2; dy+=3)
{
for (long dt = 0; dt < 360; dt+=4)
{
unsigned long count = 0;
for (int i = 0; i < 180; i+=2)
{
if (ranges[i] > 0)
{
unsigned long _x = ranges[i] * _cos[i + dt] + lidar1_x *
_cos[dt] + lidar1_y * _sin[dt] + dx;
unsigned long _y = ranges[i] * _sin[i + dt] + lidar1_x *
_sin[dt] - lidar1_y * _cos[dt] + dy;
if (_x < 0 || _y < 0 || _x >= 10240 || _y >= 10240) continue;
if (arr[_x][_y]==130) count++;
}
}
for (int i = 180; i < 360; i+=2)
{
if (ranges[i] > 0)
{
unsigned long _x = ranges[i] * _cos[i + dt] + lidar2_x *
unsigned long _y = ranges[i] * _sin[i + dt] + lidar2_x *
if (_x < 0 || _y < 0 || _x >= 10240 || _y >= 10240) continue;
if (arr[_x][_y] == 130) count++;
}
}
if (count > countmax)
{
countmax = count;
p.x = (dx - 5120) / scale;
p.y = (dy - 5120) / scale;
p.t = dt;
}
}
}
}
}
void match_F(double ranges[], long x1, long x2, long y1, long y2)
{
calc_F(ranges, x1, x2, y1, y2);
}
position scan_matching::match(double ranges[])

{
countmax = 0;
std::thread t1(match_F, ranges, x_min, (x_max + x_min) / 2, y_min, (y_max + y_min)
78

lOMoARcPSD|38101887
/ 2);
std::thread t2(match_F, ranges, x_min, (x_max + x_min) / 2, (y_max + y_min) / 2,
y_max);
std::thread t3(match_F, ranges, (x_max + x_min) / 2, x_max, y_min, (y_max + y_min)
/ 2);
std::thread t4(match_F, ranges, (x_max + x_min) / 2, x_max, (y_max + y_min) / 2,
y_max);
t1.join();
t2.join();
t3.join();
t4.join();
_countmax = countmax;
return p;
}
void calc(double ranges[], long x1, long x2, long y1, long y2, position pos)
{
long x = 5120 + scale * pos.x;
long y = 5120 + scale * pos.y;
for (long dx = x1; dx < x2; dx++)
{
for (long dy = y1; dy < y2; dy++)
{
for (long dt = 0+pos.t; dt < 360+pos.t; dt++)
{
unsigned long count = 0;
for (int i = 0; i < 180; i++)
{
if (ranges[i] > 0)
{
unsigned long _x = x + ranges[i] * _cos[i + dt] + lidar1_x *
unsigned long _y = y + ranges[i] * _sin[i + dt] + lidar1_x *
if (_x >= 10240 || _y >= 10240) continue;
}
}
for (int i = 180; i < 360; i++)
{
if (ranges[i] > 0)
{
unsigned long _x = x + ranges[i] * _cos[i + dt] + lidar2_x *
unsigned long _y = y + ranges[i] * _sin[i + dt] + lidar2_x *
if (_x >= 10240 || _y >= 10240) continue;
}
}
if (count > countmax)
{
countmax = count;
p.x = dx / scale + pos.x;
p.y = dy / scale + pos.y;
p.t = dt;
while (p.t >= 360) p.t -= 360;
}
79

lOMoARcPSD|38101887
}
}
}
}
void match1(double ranges[], long _limit, position pos)
{
calc(ranges, - _limit, 0, - _limit, 0, pos);
}

{
calc(ranges, 0, _limit, - _limit, 0, pos);
}
{
calc(ranges, - _limit, 0, 0, _limit, pos);
}

{
calc(ranges, 0, _limit, 0, _limit, pos);
}
position scan_matching::match(double ranges[], double limit, position pos)

{
countmax = 0;
long _limit = scale * limit;
std::thread t1(match1, ranges, _limit, pos);
t1.join();
t2.join();
t3.join();
t4.join();
//std::cout << dem << '\t' << countmax << std::endl;
//dem ++;
_countmax = countmax;
return p;
}
void scan_matching::save()
{
std::cout << "save ... ";
for (int i = 0; i < 10240; i++)
{
for (int j = 0; j < 1280; j++)
{
char c = '\0';
for (int k = 0; k < 8; k++) c = c << 1 | arr[i][j * 8 + k];
arr_convert[i][j] = c;
}
}
pFile = fopen("file.txt", "wb");
fwrite(arr_convert, 1, 10240 * 1280 * sizeof(unsigned char), pFile);
fclose(pFile);
std::cout << "OK" << std::endl;
}
80

lOMoARcPSD|38101887
bool scan_matching::load()
{
std::cout << "load ... ";
x_min = 10240, y_min = 10240, x_max = 0, y_max = 0;
pFile = fopen("file.txt", "rb");
if (pFile == NULL) return 0;
fread(arr_convert, 1, 10240 * 1280 * sizeof(unsigned char), pFile);
fclose(pFile);
for (int i = 0; i < 10240; i++)
{
for (int j = 0; j < 1280; j++)
{
char c = arr_convert[i][j];
for (int k = 0; k < 8; k++)
{
unsigned char ch = (c & 0x80) > 0;
arr[i][j * 8 + k] = ch;
c = c << 1;
if (ch == 1)
{
if (i < x_min) x_min = i;
if (i > x_max) x_max = i;
if (j * 8 + k < y_min) y_min = j * 8 + k;
if (j * 8 + k > y_max) y_max = j * 8 + k;
}
}
}
}
std::cout << "OK" << std::endl;
return 1;
}
4.5 THỬ NGHIỆM
Tiến hành cho rôbốt chạy thử nghiệm bám theo quỹ đạo được lưu trong tệp “text”. Bộ
điều khiển được sử dụng là BĐK phản hồi trạng thái tuyến tính
Chương trình điều khiển của rôbốt
#include <iostream>
#include <ros/ros.h>
#include "ros/time.h"
#include "lino_msgs/Velocities.h"
#include "geometry_msgs/Twist.h"
#include "lino_msgs/Imu.h"
#include "geometry_msgs/Point.h"
#include "xu_ly.h"
float x = 0, y = 0, phi = 0;
float vx = 0, vy = 0, o = 0;
float imuax = 0, imuay = 0, imuaz = 0;
float imugx = 0, imugy = 0, imugz = 0;
float imumx = 0, imumy = 0, imumz = 0;
void Call_Imu( const lino_msgs::Imu::ConstPtr& Imu_input)
81

lOMoARcPSD|38101887
{
imuax = Imu_input->linear_acceleration.x;
imuay = Imu_input->linear_acceleration.y;
imuaz = Imu_input->linear_acceleration.z;
imugx = Imu_input->angular_velocity.x;
imugy = Imu_input->angular_velocity.y;
imugz = Imu_input->angular_velocity.z;
imumx = Imu_input->magnetic_field.x;
imumy = Imu_input->magnetic_field.y;
imumz = Imu_input->magnetic_field.z;
}
void Call_Vel( const lino_msgs::Velocities::ConstPtr& Vel_input)

{
vx = Vel_input->linear_x;
vy = Vel_input->linear_y;
o = Vel_input->angular_z;
}
void Call_Pos( const geometry_msgs::Point::ConstPtr& Pos_input)

{
x = Pos_input->x;
y = Pos_input->y;
phi = Pos_input->z;
}
int main(int argc, char **argv)

{
ros::init(argc, argv, "dlh_thien");
ros::NodeHandle nh;
ros::Subscriber sub1 = nh.subscribe("/raw_imu", 1000, Call_Imu);
ros::Subscriber sub2 = nh.subscribe("/raw_vel", 1000, Call_Vel);
ros::Subscriber sub = nh.subscribe("/tf", 1000, Call_Pos);
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1000);
docfile();
ros::Rate loop_rate(20);
ros::Time start_time = ros::Time::now();
while (ros::ok()) {
geometry_msgs::Twist cmd;
data_vel out = xu_ly((ros::Time::now()-start_time).toSec(),x,y, vx, vy,
o, imuax, imuay, imuaz, imugx, imugy, imugz, imumx, imumy, imumz);
cmd.linear.x = out.vx;
cmd.linear.y = out.vy;
cmd.angular.z = out.o;
std::cout << out.vx << '\t' << out.vy << '\t' << out.o << '\n';
pub.publish(cmd);
ros::spinOnce();
loop_rate.sleep();
}
ros::spin();
return 0;
}
Kết quả thu được thể hiện ở các Hình 4.6 – Hình 4.10
82

lOMoARcPSD|38101887
Hình 4.6 Quỹ dạo thực nghiệm
Hình 4.7 Vận tốc và vận tốc góc rôbốt trong thực nghiệm
83

lOMoARcPSD|38101887
Hình 4.8 Vận tốc góc của các bánh xe trong thực nghiệm
Hình 4.9 Sai số vận tốc của thực nghiệm với mong muốn
84

lOMoARcPSD|38101887
Hình 4.10 Sai số vị trí của thực nghiệm với mong muốn
Nhận xét:
- Rôbốt đã bám tương đối theo quỹ đạo cho trước với sai số không quá lớn (Hình 4.6),
không bị lệch hoàn toàn khỏi quỹ đạo mong muốn. Có khả năng tự giảm sai số khi di chuyển,
tuy nhiên cần mất một khoảng thời giàn lớn để giảm lượng sai số đó.
- Vận tốc của Rôbốt đã bám được theo vân tốc mong muốn, nhưng biên độ dao động
quanh vận tốc mong muốn vẫn còn lớn (Hình 4.8, Hình 4.9).
- Nguyên nhân là do phần cơ khí chưa được tối ưu, ma sát trượt với sàn nhà nhỏ, khả
năng đáp ứng chưa được tốt. Cần tối ưu hóa lại chương trình, tùy chỉnh lại các hệ số ở bộ điều
khiển.

(1) Đã thiết kế, kết nối, lập trình phần mạch điện trong rôbốt.
(2) Đã tiến hành thực nghiệm kiểm tra khả năng hoạt động các phần của rôbốt.
85

lOMoARcPSD|38101887
KẾT LUẬN ĐỒ ÁN
I) Các kết quả đạt được
Trong đồ án này em đã thực hiện được các kết quả sau:

1) Thiét lập được phương trình động học của rôbốt tự hành đa hướng kiểu bánh xe
Mecanum.
2) Đã thiết lập được mô hình sai số động học để từ đó thiết kế bộ diều khiển phản hồi
trạng thái tuyến tính với tham số thay đổi theo thời gian.
3) Đã thiết kế chế tạo và thử nghiệm luật điều khiển phản hồi trạng thái tuyến tính trong
việc điều khiển bám quỹ đạo với đường cong bất kỳ. Các tham số của bộ điều khiển cần xác
định và hiệu chỉnh từ thí nghiệm.
4) Đã thực hiện xây dựng bản đồ xung quanh rôbốt bằng lidar
5) Về mặt khoa học: Tham gia nghiên cứu với thầy giáo và một phần nội dung của đồ án
đã được công bố trong hội nghị quốc tế index Scopus.
II) Kiến nghị các nội dung cần hoàn thiện
+ Nghiên cứu bài toán động lực học và điều khiển.

+ Xây dựng các luật điều khiển dộng lực học và tương tác với môi trường
+ Thiết kế và tính toán kết cấu cơ khí theo khả năng tải để có thể phát triển thành các xe
tự hành AGV phục vụ hậu cần trong công nghiệp, y tế và rôbốt dịch vụ.
86

lOMoARcPSD|38101887
DANH MỤC TÀI LIỆU THAM KHẢO
I) Tài liệu tham khảo
[1] P. Muir, C. Neuman “Kinematic modeling for feedback control of an omnidirectional

wheeled mobile robot”, Proceedings, IEEE International Conference in Robotics and
Automation, Vol 4. pp. 1772-1778, 1987.
[2] J.B. Song, K.S. Byun, “Design and Control of a Four-Wheeled Omnidirectional Mobile
Robot with Steerable Omnidirectional Wheels”, Journal of Robotic Systems, Vol. 21, No.4
(2004), pp. 193-208.
[3] I. Doroftei, V. Grosu, V. Spinu, “Omnidirectional Mobile Robot - Design and
Implementation”, Book-Chapter, Bioinspiration and Robotics - Climbing and Walking
Robots, Editor Maki K. Habib, Published by Advanced Robotic Systems International
(Vienna) and ITech, 2007, pp. 511-529.
[4] L.C. Lin, H.Y. Shih “Modeling and Adaptive Control of an OmniMecanum-Wheeled
Robot” Intelligent Control and Automation, 4 (2013), pp. 166-179.
[5] K.-L. Han, O. K. Choi, I. Lee, I. Hwang, J. S. Lee, S. Choi “Design and Control of Omni-
Directional Mobile Robot for Mobile Haptic Interface“, International Conference on
Control, Automation and Systems 2008, Oct. 14-17, 2008 in COEX, Seoul, Korea,
pp.1290-1295.
[6] I. Doroftei, B. Stirbu, “Design, Modeling and Control of an Omnidirectional Mobile
Robot”, Solid State Phenomena Vols. 166-167, (2010), pp. 173-178.
[7] F. Adascalitei, I. Doroftei, “Practical Applications for Mobile Robots based on Mecanum
Wheels - a Systematic Survey”, Proceedings of International Conference On Innovations,
Recent Trends And Challenges In Mechatronics,Mechanical Engineering And New
HighTech Products Development – MECAHITECH’11, Vol. 3, 2011, pp. 112-123
[8] Nguyen Hong Thai, Trinh Thi Khanh Ly, Nguyen Thanh Long, and Le Quoc Dzung,
(2022) “Trajectory Tracking Using Linear State Feedback Controller for a Mecanum
Wheel Omnidirectional ASIAN MMS 2021”, MMS 113, pp. 411–421.
[9] Nguyễn Doãn Phước, (2009) “Lý thuyết điều khiển tuyến tính”, Nhà xuất bản Khoa
học và Kỹ thuật.
[10] Ubuntu install of ROS Noetic (http://wiki.ros.org/noetic/Installation/Ubuntu)
[11] RPLIDAR ROS package (https://github.com/Slamtec/rplidar_ros)
[12] Robot_localization package (http://wiki.ros.org/robot_localization)
87

lOMoARcPSD|38101887
II) Chi tiết bài báo đã công bố
DOI: 10.1007/978-3-030-91892-7_39
Tên: Trajectory Tracking Using Linear State Feedback Controller for a Mecanum Wheel
Omnidirectional
Ngày xuất bản: 2021/12/15
Hội thảo: IFToMM Asian conference on Mechanism and Machine Science
Trang: 411-421
Nhà xuất bản: Springer, Cham
Nội dung bài báo:
88

lOMoARcPSD|38101887
89

lOMoARcPSD|38101887
90

lOMoARcPSD|38101887
91

lOMoARcPSD|38101887
92

lOMoARcPSD|38101887
93

lOMoARcPSD|38101887
94

lOMoARcPSD|38101887
95

lOMoARcPSD|38101887
96

lOMoARcPSD|38101887
97

lOMoARcPSD|38101887
98

lOMoARcPSD|38101887
99

Same Cothanh

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Same Cothanh

Uploaded by

Copyright:

Available Formats

lOMoARcPSD|38101887

DO AN TOT Nghiep Nguyen Thanh LONG

Đồ án cơ điện tử (Trường Đại học Bách khoa Hà Nội)

Scan to open on Studocu

Studocu is not sponsored or endorsed by any college or university

Họ và tên: NGUYỄN THÀNH LONG

Khóa: 62 Viện : Cơ khí

Bộ môn: CƠ SỞ THIẾT KẾ MÁY VÀ RÔ BỐT

1. Tên đề tài tốt nghiệp

THIẾT KẾ CHẾ TẠO RÔBỐT TỰ HÀNH ĐA HƯỚNG

Phương trình động học rôbốt tự hành đa

Downloaded by DHTD14A6HN_06_NGUY?N (nhdat.dhtd14a6hn@sv.uneti.edu.vn)

Thiết kế bộ điều khiển phản hồi trạng thái

Chương 3 Chế tạo thực nghiệm TS. Nguyễn Hồng Thái

4. Ngày giao nhiệm vụ thiết kế Ngày 04 tháng 09 năm 2021

Hà Nội, ngày tháng năm 2022

CHỦ NHIỆM BỘ MÔN CÁN BỘ HƯỚNG DẪN

(ký, ghi rõ hộ tên) (ký, ghi rõ họ tên)

Downloaded by DHTD14A6HN_06_NGUY?N (nhdat.dhtd14a6hn@sv.uneti.edu.vn)

BỘ GIÁO DỤC VÀ ĐÀO TẠO

Người nhận xét: TS. NGUYỄN HỒNG THÁI

NỘI DUNG NHẬN XÉT

NHẬN XÉT CHUNG

Downloaded by DHTD14A6HN_06_NGUY?N (nhdat.dhtd14a6hn@sv.uneti.edu.vn)

Nguyễn Hồng Thái

Downloaded by DHTD14A6HN_06_NGUY?N (nhdat.dhtd14a6hn@sv.uneti.edu.vn)

BỘ GIÁO DỤC VÀ ĐÀO TẠO

NỘI DUNG NHẬN XÉT

1 Nội dung thiết kế tốt nghiệp

Downloaded by DHTD14A6HN_06_NGUY?N (nhdat.dhtd14a6hn@sv.uneti.edu.vn)

2 Nhận xét của người phản biện

Downloaded by DHTD14A6HN_06_NGUY?N (nhdat.dhtd14a6hn@sv.uneti.edu.vn)

LỜI NÓI ĐẦU

Hà Nội, ngày ... tháng năm 2022

Sinh viên thực hiện

Nguyễn Thành Long

Downloaded by DHTD14A6HN_06_NGUY?N (nhdat.dhtd14a6hn@sv.uneti.edu.vn)

Downloaded by DHTD14A6HN_06_NGUY?N (nhdat.dhtd14a6hn@sv.uneti.edu.vn)

LẬP TRÌNH BỘ ĐIỀU KHIỂN ................................................................................................ 61

Downloaded by DHTD14A6HN_06_NGUY?N (nhdat.dhtd14a6hn@sv.uneti.edu.vn)

DANH MỤC CÁC HÌNH VẼ VÀ ĐỒ THỊ

Hinh Nội dung Trang

Hình 1.1 Xe tự hành AGV 4

Hinh 1.2 Bánh răng Mecanum 5

Hinh 1.4 AGV sử dụng bánh xe Mecanum 6

Hinh 1.6 Động học AGV đa hướng kiểu bánh xe Mecanum 8

Hinh 1.7 Đường cong NURBS 11

Đường cong NURBS nới n + 1 = 5, k = 3, vector nút mở

Đường cong NURBS nới n + 1 = 5, k = 4, vector nút mở

Hinh 1.12 Lộ trình của xe 19

Hinh 1.13 Quỹ đạo của xe theo đường cong NURBS 20

Hinh 1.14 Đồ thị góc quay của xe khi V = 1 m/s 22

Hinh 1.16 Đồ thị vận tốc góc Ω của xe khi V = 1 m/s 23

Hinh 1.17 Đồ thị vận tốc góc ω1 khi V = 1 m/s 23

Hinh 1.18 Đồ thị vận tốc góc ω2 khi V = 1 m/s 24

Hinh 1.19 Đồ thị vận tốc góc ω3 khi V = 1 m/s 24

Hinh 1.20 Đồ thị vận tốc góc ω4 khi V = 1 m/s 25

Downloaded by DHTD14A6HN_06_NGUY?N (nhdat.dhtd14a6hn@sv.uneti.edu.vn)

Hinh 1.21 Đồ thị góc quay của xe khi V = 0,3 m/s 26

Hinh 1.24 Đồ thị vận tốc góc ω1 khi V = 0,3 m/s 27

Hinh 1.25 Đồ thị vận tốc góc ω2 khi V = 0,3 m/s 28

Hinh 1.26 Đồ thị vận tốc góc ω3 khi V = 0,3 m/s 28

Hinh 1.27 Đồ thị vận tốc góc ω4 khi V = 0,3 m/s 29

Hinh 2.1 Sơ đồ khối hệ thống điều khiển 33