Thiết kế, thi công và lập trình điều khiển robot nhện 6 chân

BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT
THÀNH PHỐ HỒ CHÍ MINH

ĐỒ ÁN TỐT NGHIỆP
NGÀNH CƠNG NGHỆ KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA

THIẾT KẾ, THI CƠNG VÀ LẬP
TRÌNH ĐIỀU KHIỂN ROBOT NHỆN
6 CHÂN

GVHD :
SVTH :
MSSV :
SVTH :
MSSV :

Nguyễn Văn Thái
Đoàn Thanh Sơn
14151126
Huỳnh Đào Thanh Tâm
14151136

SKL 0 0 6 5 7 8

Tp. Hồ Chí Minh, 2018


CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM
Độc lập – Tự do – Hạnh phúc


Tp. Hồ Chí Minh, ngày.... tháng.... năm 2018
NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP
Họ và tên sinh viên: Đoàn Thanh Sơn

MSSV: 14151126

Họ và tên sinh viên: Huỳnh Đào Thanh Tâm

MSSV: 14151136

Ngành: Công nghệ kỹ thuật điều khiển và tự động hóa Lớp: 14151CLC
Giảng viên hướng dẫn: TS. Nguyễn Văn Thái

SĐT: 0902807576

Ngày nhận đề tài:

Ngày nộp đề tài:

1.

Tên đề tài:

THIẾT KẾ, THI CƠNG VÀ LẬP TRÌNH ĐIỀU KHIỂN ROBOT NHỆN 6
CHÂN
Nội dung thực hiện đề tài:

 Nghiên cứu, thiết kế phần cứng robot trên SolidWorks
 Tính tốn phương trình động học thuận, động học nghịch cho robot.
 Nghiên cứu dáng đi của robot và áp dụng trên mô hình.

2.

Sản phẩm:

 Robot 4 chân

TRƯỞNG NGÀNH

GIÁO VIÊN HƯỚNG DẪN

TS. Trương Đình Nhơn

TS. Nguyễn Văn Thái

i


CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM
Độc lập – Tự do – Hạnh Phúc

PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN
Họ và tên Sinh viên: Đoàn Thanh Sơn
Họ và tên Sinh viên: Huỳnh Đào Thanh Tâm

MSSV: 14151126
MSSV: 14151136

Ngành: Công nghệ kỹ thuật điều khiển và tự động hóa

Tên đề tài: THIẾT KẾ, THI CƠNG VÀ LẬP TRÌNH ĐIỀU KHIỂN ROBOT

NHỆN 6 CHÂN
Họ và tên Giảng viên hướng dẫn: TS. Nguyễn Văn Thái
NHẬN XÉT CỦA GIẢNG VIÊN HƯỚNG DẪN
1. Về nội dung đề tài & khối lượng thực hiện:
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
2. Ưu điểm:
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
3. Khuyết điểm:
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
...........................................................................................................................................
4. Đề nghị cho bảo vệ hay không?
...........................................................................................................................................
5. Đánh giá loại: ................................................................................................................
6. Điểm: ............................(Bằng chữ: ...........................................................................)
Tp. Hồ Chí Minh, ngày
tháng
năm 2018
Giảng viên hướng dẫn

ii

CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM
Độc lập – Tự do – Hạnh Phúc

PHIẾU NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN
Họ và tên Sinh viên: Đoàn Thanh Sơn
MSSV: 14151126
Họ và tên Sinh viên: Huỳnh Đào Thanh Tâm
MSSV: 14151136
Ngành: Công nghệ kỹ thuật điều khiển và tự động hóa

Tên đề tài:
THIẾT KẾ, THI CƠNG VÀ LẬP TRÌNH ĐIỀU KHIỂN ROBOT NHỆN 6 CHÂN
Họ và tên Giáo viên phản biện: TS. Nguyễn Minh Tâm
NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN
1. Về nội dung đề tài & khối lượng thực hiện:
..................................................................................................................................................
..................................................................................................................................................
..................................................................................................................................................
..................................................................................................................................................
2. Ưu điểm:
..................................................................................................................................................
..................................................................................................................................................
..................................................................................................................................................
..................................................................................................................................................
3. Khuyết điểm:
..................................................................................................................................................
..................................................................................................................................................
..................................................................................................................................................
..................................................................................................................................................
4. Đề nghị cho bảo vệ hay không?

..................................................................................................................................................
5. Đánh giá loại:
..................................................................................................................................................
6.Điểm:..........................(Bằng chữ: .....................................................................................)
...............................................................................................................................................
Tp. Hồ Chí Minh, ngày
tháng
năm 2018
Giáo viên phản biện

iii


BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

LỜI CẢM ƠN
Đề tài “ Thiết kế, thi cơng và lập trình điều khiển Robot nhện 6 chân” là nội dung
nhóm chọn để nghiên cứu và làm đồ án tốt nghiệp sau bốn năm theo học chương trình
đại học chun ngành Cơng nghệ kỹ thuật điều khiển và tự động hóa tại trường Đại
học Sư phạm kỹ thuật Thành phố Hồ Chí Minh
Để hồn thành q trình nghiên cứu và hồn thiện luận văn này, lời đầu tiên tôi xin
chân thành cảm ơn sâu sắc đến TS. Nguyễn Văn Thái thuộc khoa Điện – Điện tử.
Thầy đã trực tiếp chỉ bảo và hướng dẫn nhóm trong suốt q trình nghiên cứ để tơi
hồn thiện luận văn này. Ngồi ra tơi xin chân thành cảm ơn các Thầy, cô trong Khoa
Điện – Điện tử đã đóng góp ý kiến quý báu cho đồ án này.
Nhân dịp này, tôi cũng xin cảm ơn Khoa Điện – Điện tử Trường Đại học Sư phạm kỹ
thuật Thành phố Hồ Chí Minh, lãnh đạo và các anh chị đang công tác tại trường đã tạo
điều kiện và thời gian cho nhóm trong suốt q trình nghiên cứu

Cuối cùng, nhóm xin cảm ơn đến cha mẹ đã ln động viên khích lệ và tạo điều kiện
giúp đỡ trong suốt q trình thực hiện để nhóm có thể hồn thành đồ án một cách tốt
nhất.
Nhóm xin chân thành cảm ơn!

i


BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

TÓM TẮT
Trong đề tài này, nhóm thực hiện việc thiết kế và thi công Robot nhện 6 chân với mỗi
chân như là một cánh tay robot 3 bậc tự do. Robot có thể tự di chuyển một cách độc
lập trong địa hình bằng phẳng. Nhóm sẽ thiết kế phần cứng Robot nhện 6 chân trên
phần mềm SolidWorks. Sau đó tính tốn động học thuận và nghịch dựa trên các thông
số thiết kế. Việc lựa chọn vật liệu, thiết bị để thi công robot là một phần quan trọng
trong sự thành công của Robot.
Sau khi hồn tất việc thi cơng mơ hình, nhóm tiến hành xử lý các thuật toán di chuyển
của Robot để phù hợp với dáng đi thực tế.
Kết quả cuối cùng nhóm thu được là mơ hình Robot đúng như thiết kế ban đầu và
Robot hoạt động đúng như mục tiêu mong muốn.

ii


BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

DANH SÁCH HÌNH ẢNH VÀ BẢNG BIỂU
Hình 2.1: Chiều dài và góc xoắn của một khâu .......................................................... 4
Hình 2.2 : Các thơng số của khâu : d, a, θ và α .......................................................... 5
Hình 2.3: Quy tắc bàn tay phải ................................................................................... 6
Hình 2.4: Thơng số động học cho 6 chân robot ......................................................... 7
Hình 2.5: Hệ tọa độ khơng gian cho chân robot ......................................................... 10
Hình 2.6: Chuyển động xoay theo trục ....................................................................... 12
Hình 2.7: Phép xoay thân giữa robot .......................................................................... 13
Hình 2.8: Phép tịnh tiến cho thân giữa robot.............................................................. 14
Hình 2.9: Sơ đồ bước chân của dáng đi liên tục......................................................... 16
Hình 2.10: Sơ đồ bước chân của dáng đi khơng liên tục............................................ 18
Hình 3.1: Thân trên và thân dưới của robot nhện 6 chân ........................................... 19
Hình 3.2: Link 1 của robot nhện ................................................................................. 20
Hình 3.3: Đế đỡ động cơ servo ................................................................................... 20
Hình 3.4: Link 2 của robot nhện ................................................................................. 20
Hình 3.5: Link 3 của robot nhện ................................................................................. 20
Hình 3.6: Mơ hình robot nhện 6 chân trên SolidWork 2015 ...................................... 21
Hình 3.7: Board Arduino Mega 2560 ......................................................................... 22
Hình 3.8: Hình ảnh chi tiết trong board Arduino Mega 2560 .................................... 23
Hình 3.9: Sơ đồ nguyên lý board Arduino Mega 2560 .............................................. 23
Hình 3.10: Arduino Mega Sensor Shield ................................................................... 24
Hình 3.11: Sơ đồ chức năng các chân của Mega Sensor Shield ................................ 24
Hình 3.12: Cấu tạo bên trong của RC Servo .............................................................. 25
Hình 3.13: RC Servo Digital RC FR1501 .................................................................. 27
Hình 3.14: Mạch giảm áp ........................................................................................... 27
Hình 3.15: Pin Lipo 3cell ........................................................................................... 28
Hình 4.1: Mơ hình robot nhện 6 chân thực tế............................................................. 29
Hình 4.2: Mơ hình kết nối chân robot với Arduino .................................................... 30

iii


BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

Hình 5.1: Hướng phát triển Robot 6 chân trong tương lai ......................................... 33

DANH SÁCH BẢNG BIỂU
Bảng 2.1: Bảng thông số DH ...................................................................................... 7
Bảng 2.2: Bảng thông số D-H cho từng chân robot ................................................... 8
Bảng 3.1: Tóm tắt thơng số Arduino Mega 2560 ....................................................... 22
Bảng 3.2: Thông số kỹ thuật của mạch giảm áp ........................................................ 25
Bảng 3.3: Thông số kỹ thuật của pin Lipo 2s ............................................................. 26

iv


BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

MỤC LỤC
NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP................................................................................ i
PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN............................................... ii
PHIẾU NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN ................................................. iii
LỜI CẢM ƠN .................................................................................................................. i
TÓM TẮT .................................................................................................................. ii
DANH SÁCH HÌNH ẢNH VÀ BẢNG BIỂU .............................................................. iii

DANH SÁCH BẢNG BIỂU ......................................................................................... iv
MỤC LỤC .......................................................................................................................v
CHƯƠNG 1 TỔNG QUAN ............................................................................................1
1.1.

Đặt vấn đề .......................................................................................................1

1.2.

Mục tiêu đề tài ................................................................................................2

1.3.

Giới hạn đề tài ................................................................................................2

1.4.

Nội dung đề tài ...............................................................................................3

CHƯƠNG 2 CƠ SỞ LÍ THUYẾT ..................................................................................4
2.1.

Giới thiệu chung .............................................................................................4

2.2.

Bộ thơng số Denavit-Hartenberg (DH) ..........................................................4

2.3.

Bài tốn động học thuận robot........................................................................7

2.4.

Bài toán động học nghịch robot....................................................................10

2.5.

Phương pháp điều khiển thân giữa của robot ...............................................12

2.6.

Kiểu dáng di chuyển cho robot .....................................................................15

2.6.1.

Di chuyển liên tục ..................................................................................15

2.6.2.

Di chuyển không liên tục .......................................................................17

CHƯƠNG 3 THIẾT KẾ VÀ LỰA CHỌN THIẾT BỊ ..................................................19
3.1.

Thiết kế robot................................................................................................19

3.2.

Lựa chọn thiết bị ...........................................................................................21

3.2.1.

Bộ điều khiển board arduino mega 2560 ...............................................21

3.2.2.

Shield Mega Sensor V2 .........................................................................24

3.2.3.

Động cơ RC Servo Digital RC FR1501.................................................24

3.2.4.

Mạch giảm áp DC ..................................................................................27

3.2.5.

Pin Lipo..................................................................................................28

CHƯƠNG 4 THI CÔNG VÀ KẾT QUẢ......................................................................29
4.1.

Thi công ........................................................................................................29

v


BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

4.2.

GVHD:TS.Nguyễn Văn Thái

Điều khiển.....................................................................................................30

CHƯƠNG 5 KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN ...............................................32
5.1.

Kết luận.........................................................................................................32

5.1. 1.

Những công việc đã thực hiện ...............................................................32

5.1.2.

Những mặt hạn chế: ...............................................................................32

5.2.

Hướng phát triển của để tài ..........................................................................33

TÀI LIỆU THAM KHẢO .............................................................................................34
Phụ lục ...........................................................................................................................35

vi


BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

vii


BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

CHƯƠNG 1 TỔNG QUAN
Cùng với sự phát triển mạnh mẽ của các hệ thống Cơ-Điện tử, robot vượt địa hình
ngày một được hồn thiện và càng cho thấy lợi ích của nó trong qn sự, trong nghiên
cứu. Nó có khả năng thăm dị những khu vực mà con người không đến được thậm chí
trong khám phá vũ trụ. Với sự phát triển mạnh mẽ của khoa học kĩ thuật. Robot di
động hay còn gọi là Robot tự hành. Chúng là loại Mobile robot có khả năng tự hoạt
động, thực thi nhiệm vụ mà không cần sự can thiệp của con ngưới. Với những cảm
biến, chúng có khả năng nhận biết về mơi trường xung quanh.
Thực tế Robot dạng chân có nhiều loại như: Hệ thống robot 2 chân (Bipeds robot)
giống như con người hoặc những con chim, Robot 4 chân (Quadrupeds Robot) như
động vật có vú và bị sát, Robot 6 chân (Hexapod Robot) như côn trùng và Robot 10
chân (Octopods robot. Thế nhưng trong đề tài này chỉ nghiên cứu tập trung vào robot 6
chân (Hexapod Robot) về phương pháp tính và các kiểu dáng đi, thứ tự bước chân, vị
trí đặt chân.

1.1. Đặt vấn đề
Theo dự báo trong vòng 20 năm nữa mỗi người sẽ có nhu cầu sử dụng một robot cá
nhân như cần một máy tính PC hiện nay và robot sẽ là tâm điểm của một cuộc cách
mạng công nghệ lớn sau Internet. Với xu thế này, cùng với các ứng dụng truyền thống
khác của robot trong công nghiệp, y tế, giáo dục và đào tạo, giải trí và đặc biệt trong

an ninh quốc phịng thì thị trường robot và các dịch vụ ăn theo robot sẽ vô cùng lớn
Robot tổng hợp trong nó cả khoa học và công nghệ. Để thiết kế và chế tạo được robot,
ta cần có các tri thức của tốn học, cơ học, vật lý, điện tử, lý thuyết điều khiển, khoa
học tính tốn và nhiều tri thức khác. Để có thể ứng dụng được robot, ta cần biết rõ về
đối tượng ứng dụng. Robot là sản phẩm tích hợp cả khoa học và cơng nghệ với độ
phức tạp cao.
Robot đã có những tiến bộ đáng kể trong hơn nửa thế kỷ qua. Robot đầu tiền được ứng
dụng trong công nghiệp vào những năm 60 để thay thế con người làm các công việc
nặng nhọc, nguy hiểm trong môi trường độc hại. Do nhu cầu sử dụng ngày càng nhiều
trong các quá trình sản xuất phức tạp nên robot cơng nghiệp cần có những khả năng
thích ứng linh hoạt và thơng minh hơn. Ngày nay, ngoài ứng dụng sơ khai ban đầu của
robot trong chế tạo máy thì các ứng dụng khác như trong y tế, chăm sóc sức khỏe,
nơng nghiệp, đóng tàu, xây dựng, an ninh quốc phòng và gia đinh đang có nhu cầu
tăng đang là động lực cho các robot địa hình và robot dịch vụ phát triển
Ngày nay hệ thống Robot được phân loại và tổng quát thành 2 nhóm chính:

1


BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

 Robot có gốc vị trí cố định (Manipulation robotics)
 Robot di động
Robot di động là robot có thể di chuyển xung quanh một môi trường nhất định không
bị ràng buộc bởi một góc tọa độ nhất định:
 Robot di chuyển bằng chân (Legged Robots)
 Robot di chuyển bằng bánh xe (Wheeled Robots)
Thực tế robot dạng chân có nhiều ại như : robot 2 chân (Bipeds robot) giống như con

người hoặc những con chim , robot 4 chân (Quadruped Robot) như động vật có vú và
bị sát, robot 6 chân (Hexapod Robot) như côn trùng và robot 10 chân (Octopods
Robot). Thế nhưng trong đề tài này chỉ nghiên cứu tập trung vào Robot 6 chân
(Hexapod Robot) về phương pháp tính động học và các kiểu dáng đi, thứ tự bước các
chân và vị trí đặt chân.

1.2. Mục tiêu đề tài
Trong thời đại cơng nghiệp hiện đại hóa hiện nay nói chung và sự phát triển mạnh mẽ
của Robot để thay thế con người thực hiện những cơng việc địi hỏi sự chính xác trong
môi trường nguy hiểm và độc hại trong các lĩnh vực công nghiệp, quân sự, y tế. Để đạt
các yêu cầu đặt ra của con người chúng ta phải nghiên cứu, phát triển và hồn thiện
từng bước. Vì vật nhóm đã nghiên cứu và chọn đề tài” Thiết kế, thi cơng và lập trình
điều khiển Robot nhện 6 chân”
Với đề tài này, nhóm nghiên cứu sẽ tìm hiểu Robot 6 chân về :





Nghiên cứu phương pháp tính động học thuận, động học nghịch của robot.
Nghiên cứu thiết kế quỹ đạo chuyển động của cánh tay robot.
Nghiên cứu các dáng đi của robot, vi trí đặt chân, phương pháp chuyển hướng.
Nghiên cứu về vi điều khiển để lập trình robot..

1.3. Giới hạn đề tài
Với đề tài này, nhóm đã thiết kế robot 6 chân với mỗi chân sử dụng 3 động cơ RC
servo. Vì động cơ chỉ quay được 180o nên ta có một số giới hạn về hành trình di
chuyển của robot. Vì vậy trong quá trình điều khiển robot này ta có một số giới hạn
nhất định :
 Robot chỉ di chuyển theo đường thẳng.

 Robot chỉ di chuyển tốt trên mặt phẳng nằm ngang.
 Chưa thể thêm các hoạt động khác cho robot.

2


BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

1.4. Nội dung đề tài
Đề tài “Thiết kế, thi cơng và lập trình điều khiển Robot nhện 6 chân” bao gồm các
chương sau :
Chương I: Tổng quan: Chương này trình bày tổng quan sơ bộ về các yêu cầu của cuốn
báo cáo như đặt vấn đề, mục tiêu, giới hạn và nội dung đề tài.
Chuơng II: Cơ sở lý thuyết: Chương này trình bày phương pháp tính độc học thuận,
động học nghịch cánh tay máy. Bên cạnh đó là lí thuyết về các phương pháp di chuyển
Robot.
Chương III: Thiết kế và lựa chọn thiết bị: Chương này trình bày thiết kế và thiết kế và
lựa chọn thiết bị phần cứng cho robot.
Chương IV: Thi cơng và kết quả: Chương này nhóm sẽ tiến hành thi cơng theo mơ
hình đã được thiết kế và kết quả đạt được.
Chương V: Kết luận và hướng phát triển: Kết luận chung về ưu điểm và hạn chế của
đề tài, khẳng định những kết quả đóng góp đạt được, đề xuất ý kiến để cải thiện khuyết
điểm và định hướng phát triển đề tài.

3


BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

CHƯƠNG 2 CƠ SỞ LÍ THUYẾT
2.1. Giới thiệu chung
Nghiên cứu về động học của robot là bước quan trọng để điều khiển sự chuyển động
của robot. Bất kỳ một Robot nào cũng có thể coi là một tập hợp các khâu (links) gắn
liền với các khớp (joints). Ta sẽ đặt trên mỗi khâu của Robot một hệ tọa độ.Sau đó sử
dụng các phép biến đổi để mơ tả vị trí tương đối và hướng giữa các hệ tọa độ này bằng
các bài toán động học. Bài toán động học bao gồm động học thuận và động học nghịch.

2.2. Bộ thông số Denavit-Hartenberg (DH)
Denavit & Hartenberg (D-H) đã đề xuất một phương pháp để mô tả quan hệ giữa hai
khâu liên tiếp trong cơ cấu không gian dùng ma trận thuần nhất 4x4, các phần tử trong
ma trận chứa thông tin về thông số của khớp. Bất kỳ khâu nào cũng được đặc trưng
bơi hai kích thước:
Độ dài pháp tuyến chung : an
Góc giữa các trục trong mặt phẳng vng góc với an : αn
Thông thường, người ta gọi là an là chiều dài và αn là góc xoắn của khâu (hình 2.1).
Phổ biến là hai khâu liên kết với nhau ở chính trục của khớp (hình 2.2)

Hình 2.1: Chiều dài và góc xoắn của một khâu
Mỗi trục sẽ có hai pháp tuyến vng góc với nó, mỗi pháp tuyến dùng cho mỗi khâu
(trước và sau một khớp). Vị trí tương đối của hai khâu liên tiếp như thế được xác định
bởi dn là khoảng cách giữa các pháp tuyến đo dọc theo trục khớp n và θn là góc giữa
các pháp tuyến đo trong mặt phẳng vng góc với trục. dn và θn thường được gõi là
khoảng cách và góc giữa các khâu.

4

BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

Hình 2.2 : Các thông số của khâu : d, a, θ và α
Để mô tả mỗi quan hệ giữa các khâu ta gắn vào mỗi khâu một hệ tọa độ. Nguyên tắc
chung để gắn hệ tọa độ lên các khâu như sau :
Gốc của hệ tọa độ gắn lên khâu thứ n đặt tại giao điểm của pháp tuyến an với khớp thứ
n+1. Trường hợp hai trục khớp cắt nhau, gốc tọa độ sẽ đặt tại chính điểm cắt đó. Nếu
các trục khớp song song với nhau, gốc tọa độ được chọn trên trục khớp của khâu kế
tiếp, tại điểm thích hợp
Trục z của hệ tọa độ gắn lên khâu thứ n đặt dọc theo trục khớp thứ n+1
Trục x thường đặt đặt theo dọc pháp tuyến chung và hướng từ khớp n đến n+1

Mơ hình D-H chỉ ra mối quan hệ giữa hệ tọa độ i so với hệ tọa độ i+1 bằng biểu thức
tích các ma trận thuần nhất sau:
𝑖
𝑖+1𝑇=

R(z, 𝜃) × T(0, 0, d) × T(a, 0, 0) × R(x, 𝛼)

+ R(z, 𝜃) : Trục x[i] quay quanh trục z[i+1] một góc 𝜃 so với x[i+1].
+ T(0, 0, d) : Gốc O[i] tịnh tiến dọc trục z[i+1] so với gốc O[i+1] một đoạn d.
+ T(a, 0, 0) : Gốc O[i] Tịnh tiến dọc trục x[i+1] (đã trùng với x[i]) so với gốc O[i+1]
một đoạn a.
+ R(x, 𝛼) : Trục z[i] quay quanh trục x[i+1] một góc 𝛼 so với z[i+1].

5

BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

Ma trận riêng cho từng phép biến đổi:
𝑐𝜃
𝑠𝜃
R(z, 𝜃) = [
0
0

−𝑠𝜃
𝑐𝜃
0
0

1
0
T(0, 0, d) = [
0
0

0
1
0
0

0
0

1
0

0
0
]
𝑑
1

1
0
T(a, 0, 0) = [
0
0

0
1
0
0

0
0
1
0

𝑎
0
]
0
1

1
0
R(x, 𝛼) = [
0
0

0
𝑐𝛼
𝑠𝛼
0

0
0
1
0

0
0
]
0
1

0
−𝑠𝛼
𝑐𝛼
0

0
0

]
0
1

Từ đó ta có ma trận tổng quát cho
𝑐𝜃
𝑠𝜃
𝑖
𝑖+1𝑇 = [
0
0

−𝑠𝜃𝑐𝛼
𝑐𝜃𝑐𝛼
𝑠𝛼
0

𝑠𝜃𝑠𝛼
−𝑐𝜃𝑠𝛼
𝑐𝛼
0

𝑖
𝑖+1𝑇

:

𝑎𝑐𝜃
𝑎𝑠𝜃
]

𝑑
1

Việc xác định hệ trục tọa độ phải phù hợp với các phép biến đổi của ma trận
lần lượt các trục được xác định:
- Khi gắn hệ trục tọa độ phải tuân theo mơ hình ma trận

𝑖
𝑖+1𝑇.

Nên

𝑖
𝑖+1𝑇.

-Trục z[i] phải chọn cùng phương với trục khớp động cơ i+1.
- Chọn trục x[i] trùng với trục của link [i], dùng quy tắc bàn tay phải để có trục y[i], ta
đặt được trục cho robot như hình vẽ.

6


BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

Hình 2.3: Quy tắc bàn tay phải
Quy tắc bàn tay phải:

Từ đó có được bảng thông số D-H:

Bảng 2.1: Bảng thông số D-H
Joint (i)

𝑎𝑖 (chiều dài link)

𝛼𝑖 (góc xoắn)

𝑑𝑖 (độ lệch)

𝜃𝑖 (góc khớp)

1

0

90

D

𝜃1

2

𝑎2

0

0

𝜃2

3

𝑎3

0

0

𝜃3

4

𝑎4

0

0

𝜃4

2.3. Bài toán động học thuận robot
Quy tắc đặt hệ tọa độ được xây dựng cho mơ hình robot 6 chân được thể hiện trong
hình.

7


BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

Hình 2.4: Thơng số động học cho 6 chân robot
Chúng ta xem một chân của robot là một cánh tay 3 bậc tự do. Bảng thông số D-H cho
các phép biến đổi:

Bảng 2.2: Bảng thông số D-H cho từng chân robot
Link

a

α

d

θ

1

a1

-90o

0

*θ1

2

a2

0

0

*θ2

3

a3

0

0

*θ3

8


BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

Để mô tả mối quan hệ về hướng và vị trí của hệ tọa độ gắn trên hai khâu liền kề nhau
(Khâu thứ I về khâu i-1) ta dùng ma trận An, đó là ma trận thể hiện mối quan hệ về tọa
độ của các khâu nối tiếp nhau và được thực hiện bởi các phép quay về tịnh tiến sau
đây:
-

Quay quanh zn-1 một góc θn
Tịnh tiến theo trục zn-1 một khoảng dn
Tịnh tiến dọc theo xn-1 một đoạn an
Quay quanh xn một góc xoắn αn

Bốn phép biến đổi thuần nhất này thể hiện quan hê của hệ tọa độ thuộc khâu n so với
hệ tọa độ thuộc khâu n-1 và tích của chúng được gọi là ma trận An.
𝐴𝑛 = 𝑅(𝑧, 𝜃). 𝑃(0,0, 𝑑 ). 𝑃(𝑎, 0,0). 𝑅(𝑥, 𝛼)
𝑐𝑜𝑠𝜃𝑛
𝑠𝑖𝑛𝜃𝑛
𝐴𝑛 = [
0
0

−𝑠𝑖𝑛𝜃𝑛
𝑐𝑜𝑠𝜃𝑛
0
0

0
0
1
0

0 1
0 0
][
0 0
1 0

𝑐𝑜𝑠𝜃𝑛
𝑠𝑖𝑛𝜃𝑛
𝐴𝑛 = [
0
0

−𝑠𝑖𝑛𝜃𝑛 𝑐𝑜𝑠𝛼𝑛
𝑐𝑜𝑠𝜃𝑛 𝑐𝑜𝑠𝛼𝑛
𝑠𝑖𝑛𝛼𝑛
0

0
1
0
0

0
0
1
0

𝑎𝑛 1
0 0
][
𝑑𝑛 0
1 0

𝑠𝑖𝑛𝜃𝑛 𝑠𝑖𝑛𝛼𝑛
−𝑐𝑜𝑠𝜃𝑛 𝑠𝑖𝑛𝛼𝑛
𝑐𝑜𝑠𝛼𝑛

0

0
𝑐𝑜𝑠𝛼𝑛
𝑠𝑖𝑛𝛼𝑛
0

0
−𝑠𝑖𝑛𝛼𝑛
𝑐𝑜𝑠𝛼𝑛
0

0
0
]
0
1

𝑎𝑛 . 𝑐𝑜𝑠𝜃𝑛
𝑎𝑛 . 𝑠𝑖𝑛𝜃𝑛
]
𝑑𝑛
1

Ta kí hiệu ma trận chuyển đổi từ khâu n-1 sang n như sau :
𝑛−1
𝑛𝑇

= 𝐴𝑛

Quy ước:
𝑆1 = 𝑠𝑖𝑛𝜃1 ; 𝐶1 = 𝑐𝑜𝑠𝜃1 ; 𝑆12 = 𝑠𝑖𝑛(𝜃1 + 𝜃2 ) ; 𝐶12 = cos(𝜃1 + 𝜃2 )
Thay các thông số trong bảng thông số D-H vào ma trận An ta có :
𝑐𝜃1
𝑠𝜃1
0
1𝑇 = [
0
0

0
0
−1
0

−𝑠𝜃1
𝑐𝜃1
0
0

𝑎1 𝑐𝜃1
𝑎1 𝑠𝜃1
]
0
1

𝑐𝜃2
𝑠𝜃2
1
2𝑇 = [

0
0

−𝑠𝜃2
𝑐𝜃2
0
0

0
0
1
0

𝑎2 𝑐𝜃2
𝑎2 𝑠𝜃2
]
0
1

𝑐𝜃3
𝑠𝜃3
2
3𝑇 = [
0
0

−𝑠𝜃3
𝑐𝜃3
0
0

0
0
1
0

𝑎3 𝑐𝜃3
𝑎3 𝑠𝜃3
]
0
1

9


BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

Ma trận chuyển đổi từ gốc tọa độ tới điểm cuối của chân robot :
𝑇11
𝑇
21
0
0
1
2
3𝑇 = 1𝑇 × 2𝑇 × 3𝑇 = [
𝑇31
0

𝑇12
𝑇22
𝑇32
0

𝑇13
𝑇23
𝑇33
0

𝑃𝑥
𝑃𝑦
]
𝑃𝑧
1

Từ hệ phương trình động học thuận ta tìm được vị trí của điểm cuối robot :
𝑃𝑥
𝑐𝜃1 (𝑎1 + 𝑎2 𝑐𝜃2 + 𝑎3 𝑐 (𝜃2 + 𝜃3 ))
P = [𝑃𝑦 ] = [𝑠𝜃1 (𝑎1 + 𝑎2 𝑐𝜃2 + 𝑎3 𝑐 (𝜃2 + 𝜃3 ))]
𝑃𝑧
− 𝑎2 𝑠𝜃2 − 𝑎3 𝑠(𝜃2 + 𝜃3 )

2.4. Bài toán động học nghịch robot
Cho trước tọa độ P (𝑃𝑥 , 𝑃𝑦 , 𝑃𝑧 ) và bảng thông số D-H như trong bài toán động học
thuận, ta đi tìm 𝜃1 , 𝜃2 , 𝜃3

Hình 2.5: Hệ tọa độ khơng gian cho chân robot
0

3𝑇

= 01𝑇 × 12𝑇 × 23𝑇

Suy ra 03𝑇 × 01𝑇 −1 = ( 01𝑇 × 12𝑇 × 23𝑇 ) × 01𝑇 −1 = 12𝑇 × 23𝑇
Ta có: 03𝑇 × 01𝑇 −1

𝐴11
𝐴
= [ 21
𝐴31
0

𝐴12
𝐴22
𝐴32
0

𝐴13
𝐴23
𝐴33
0

𝑛1
𝑛2
]
𝑛3
1

10

BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

Với 𝑛1 = - 𝑎1 + 𝑃𝑥 c𝜃1 + 𝑃𝑦 s𝜃1
𝑛2 = - 𝑃𝑧
𝑛3 = - 𝑃𝑥 s𝜃1 + 𝑃𝑦 c𝜃1
𝐵11
𝐵
Ta cũng có 12𝑇 × 23𝑇 = [ 21
𝐵31
0

𝐵12
𝐵22
𝐵32
0

𝐵13
𝐵23
𝐵33
0

𝑚1
𝑚2
]
𝑚3
1

Với 𝑚1 = 𝑎2 c𝜃2 + 𝑎3 c(𝜃2 + 𝜃3 )
𝑚2 = 𝑎2 s𝜃2 + 𝑎3 s(𝜃2 + 𝜃3 )
𝑚3 = 0
Cho 𝑛1 = 𝑚1 , 𝑛2 = 𝑚2 , 𝑛3 = 𝑚3 , ta được các phương trình:
- 𝑃𝑥 s𝜃1 + 𝑃𝑦 c𝜃1 = 0 → 𝜃1 = atan2d(𝑃𝑥 , 𝑃𝑦 )
Ta có: {

− 𝑎1 + 𝑃𝑥 c𝜃1 + 𝑃𝑦 s𝜃1 = 𝑎2 c𝜃2 + 𝑎3 c(𝜃2 + 𝜃3 )
− 𝑃𝑧 = 𝑎2 s𝜃2 + 𝑎3 s(𝜃2 + 𝜃3 )

Đặt A = - 𝑎1 + 𝑃𝑥 c𝜃1 + 𝑃𝑦 s𝜃1
B = - 𝑃𝑧
Suy ra {

A = 𝑎2 c𝜃2 + 𝑎3 c(𝜃2 + 𝜃3 )
𝐴2 = (𝑎2 c𝜃2 + 𝑎3 c(𝜃2 + 𝜃3 ) )2
→{ 2
B = 𝑎2 s𝜃2 + 𝑎3 s(𝜃2 + 𝜃3 )
𝐵 = (𝑎2 s𝜃2 + 𝑎3 s(𝜃2 + 𝜃3 ) )2

→ 𝐴2 + 𝐵2 = 𝑎22 + 𝑎32 + 2𝑎2 𝑎3 (c𝜃2 ∗ c(𝜃2 + 𝜃3 ) + s𝜃2 ∗ s(𝜃2 + 𝜃3 ))
= 𝑎22 + 𝑎32 + 2𝑎2 𝑎3 c𝜃3
(𝐴2 + 𝐵2 )−(𝑎22 + 𝑎32 )

→ c𝜃3 =

2𝑎2 𝑎3

→ 𝜃3 = atan2d(√1 − (𝑐𝜃3 )2 , 𝑐𝜃3 )

Ta có : {
→{

A = 𝑎2 c𝜃2 + 𝑎3 (c𝜃2 ∗ 𝑐𝜃3 − 𝑠𝜃2 ∗ 𝑠𝜃3 )
B = 𝑎2 s𝜃2 + 𝑎3 (c𝜃2 ∗ 𝑠𝜃3 + 𝑠𝜃2 ∗ 𝑐𝜃3 )

A = 𝑐𝜃2 (𝑎2 + 𝑎3 ∗ 𝑐𝜃3 ) − 𝑎3 𝑠𝜃2 𝑠𝜃3
B = 𝑠𝜃2 (𝑎2 + 𝑎3 ∗ 𝑐𝜃3 ) + 𝑎3 𝑐𝜃2 𝑠𝜃3

→{

𝑐𝜃2 =
𝑠𝜃2 =

→ 𝑐𝜃2 =

𝐴 + 𝑎3 𝑠𝜃2 𝑠𝜃3
𝑎3 𝑐𝜃3 + 𝑎2
𝐵 − 𝑎3 𝑠𝜃3 𝑐𝜃2
𝑎3 𝑐𝜃3 + 𝑎2

𝐴 + 𝑎3 𝑠𝜃3

𝐵 − 𝑎3 𝑠𝜃3 𝑐𝜃2
𝑎3 𝑐𝜃3 + 𝑎2

𝑎3 𝑐𝜃3 + 𝑎2

11

BÁO CÁO ĐỒ ÁN TỐT NGHIỆP
→ 𝑐𝜃2 =
→ 𝑐𝜃2 =
→ 𝑐𝜃2 =

GVHD:TS.Nguyễn Văn Thái

𝐴(𝑎3 𝑐𝜃3 + 𝑎2 ) + 𝑎3 𝑠𝜃3 (𝐵 − 𝑎3 𝑠𝜃3 𝑐𝜃2 )
(𝑎3 𝑐𝜃3 + 𝑎2 )2
𝐴(𝑎3 𝑐𝜃3 + 𝑎2 ) + 𝑎3 𝑠𝜃3 𝐵 − 𝑎32 (𝑠𝜃3 )2 𝑐𝜃2
(𝑎3 𝑐𝜃3 + 𝑎2 )2
𝐴(𝑎3 𝑐𝜃3 + 𝑎2 ) + 𝑎3 𝑠𝜃3 𝐵
(𝑎3 𝑐𝜃3 + 𝑎2 )2

→ 𝑐𝜃2 [1 +

𝑎32 (𝑠𝜃3 )2
(𝑎3 𝑐𝜃3 + 𝑎2 )2

]=

-

𝑎32 (𝑠𝜃3 )2 𝑐𝜃2
(𝑎3 𝑐𝜃3 + 𝑎2 )2

𝐴(𝑎3 𝑐𝜃3 + 𝑎2 ) + 𝑎3 𝑠𝜃3 𝐵
(𝑎3 𝑐𝜃3 + 𝑎2 )2

→ 𝑐𝜃2 [(𝑎3 𝑐𝜃3 + 𝑎2 )2 + 𝑎32 (𝑠𝜃3 )2 ] = 𝐴(𝑎3 𝑐𝜃3 + 𝑎2 ) + 𝑎3 𝑠𝜃3 𝐵
→ 𝑐𝜃2 =

𝐴(𝑎3 𝑐𝜃3 + 𝑎2 ) + 𝑎3 𝑠𝜃3 𝐵
(𝑎3 𝑐𝜃3 + 𝑎2 )2 + 𝑎32 (𝑠𝜃3 )2

Ta có: 𝑠𝜃2 =
→ 𝑠𝜃2 =

𝐵 − 𝑎3 𝑠𝜃3 𝑐𝜃2
𝑎3 𝑐𝜃3 + 𝑎2

→ 𝑠𝜃2 =

𝐵 − 𝑎3 𝑠𝜃3

𝐴(𝑎3𝑐𝜃3 + 𝑎2 ) + 𝑎3 𝑠𝜃3 𝐵
2
(𝑎3 𝑐𝜃3 + 𝑎2)2 + 𝑎2
3 (𝑠𝜃3 )

𝑎3 𝑐𝜃3 + 𝑎2

𝐵(𝑎3 𝑐𝜃3 + 𝑎2 ) − 𝑎3 𝑠𝜃3 𝐴
(𝑎3 𝑐𝜃3 + 𝑎2 )2 + 𝑎32 (𝑠𝜃3 )2

→ tan(𝜃2 ) =

𝐵(𝑎3 𝑐𝜃3 + 𝑎2 ) − 𝑎3 𝑠𝜃3 𝐴
𝐴(𝑎3 𝑐𝜃3 + 𝑎2 ) + 𝑎3 𝑠𝜃3 𝐵

=

𝐶
𝐷

𝜃2 = atan2d(C, 𝐷)

2.5. Phương pháp điều khiển thân giữa của robot
Với việc giải bài tốn động học thuận và nghịch ta đã có thể tiến hành lập trình chuyển
động cho chân robot, nhưng để di chuyển được trong mơi trường thực tế thì ta cần phải
nghiên cứu tới những dáng di chuyển của robot, và việc này cần tới sự phối hợp
chuyển động của thân Robot 6 chân.
Với chuyển động xoay ta có 3 hướng theo từng trục x , y , z được gọi lần lượt là roll,
pitch và yaw như hình 2.6:

Hình 2.6: Chuyển động xoay theo trục

12


BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

Góc roll và pitch sẽ có phương pháp tính giống nhau và được thể hiện như hình 2.7:

Hình 2.7: Phép xoay thân giữa robot

Áp dụng ma trận xoay trong mặt phẳng Oxz :

𝑐𝑜𝑠𝑟
[𝑥′] = [
𝑠𝑖𝑛𝑟
𝑧′
{

−𝑠𝑖𝑛𝑟 𝑥
][ ]
𝑐𝑜𝑠𝑟 𝑧

𝑎 = 𝑥′ = 𝑥 ∗ 𝑐𝑜𝑠(𝑟) − 𝑧 ∗ 𝑠𝑖𝑛(𝑟)
𝑏 = 𝑧′ = 𝑥 ∗ 𝑠𝑖𝑛(𝑟) + 𝑧 ∗ 𝑐𝑜𝑠(𝑟)

Tọa độ mới của chân khi thực hiện phép xoay
Px ′ = Px + (R − a)
{
Pz ′ = Pz + b
Tương tự với các chân cịn lại, khi 6 chân được cập nhập vị trí mới thì sẽ tạo ra chuyển
động xoay.
Khi robot xoay theo góc yaw:
Với góc yaw thì cả 6 chân đồng thời sẽ nhân với một ma trận xoay theo trục Z của
thân giữa và cập nhật vị trí mới để tạo ra chuyển động xoay.
cos(𝑦𝑎𝑤)
𝑥′
[ ]=[
𝑦′
sin(𝑦𝑎𝑤)
{

−sin(𝑦𝑎𝑤) 𝑥

][ ]
cos(𝑦𝑎𝑤) 𝑦

𝑥′ = 𝑥 ∗ 𝑐𝑜𝑠(𝑦𝑎𝑤 ) − 𝑦 ∗ 𝑠𝑖𝑛(𝑦𝑎𝑤)
𝑦′ = 𝑥 ∗ 𝑠𝑖𝑛(𝑦𝑎𝑤 ) + 𝑦 ∗ 𝑐𝑜𝑠(𝑦𝑎𝑤)

Tọa độ mới của chân khi thực hiện phép xoay:
{

Px ′ = x′
Py ′ = y′

Phép tịnh tiến cho thân giữa robot :

13


BÁO CÁO ĐỒ ÁN TỐT NGHIỆP

GVHD:TS.Nguyễn Văn Thái

x6
y6
x1
y0

y1

x5

x2

y2
x0
y5
x4
x3

y4

y3
Hình 2.8: Phép tịnh tiến cho thân giữa robot

Giả sử tịnh tiến theo trục Yo một đoạn m:
Tọa độ mới các chân :
P’y1 = Py1 + m
P’x2 = Px2 - m
P’y3 = Py3 + m
P’x4 = Px4 - m
P’y5 = Py5 + m
P’x6 = Px6 - m

Tương tự với việc tịnh tiến theo trục Xo

14