ĐIỀU KHIỂN ĐỘNG HỌC CÁNH TAY ROBOT DẠNG NGƯỜI DỰA VÀO CẢM BIẾN HÌNH ẢNH 3 CHIỀU MS KINECT

Đồ án tốt nghiệp cơ điện tử điều khiển động học cánh tay robot dạng người 6 bậc tự do dựa vào cảm biến hình ảnh 3 chiều MS Kinect :Với sự phát triển mạnh mẽ của khoa họ c kĩ thuật hiện nay cùng với cuộ c cáchmạnh công nghiệp 4.0, rob ot cùng với trí tuệ nhân tạo đang dần phổ biến hơnkhông chỉ trong sản xuất công n gh iệp mà còn cả trong đời sống giải trí. Rob otngày n ay có sự phát triển vượt bậc, chúng hỗ trợ con người trong rất nh iều lĩnhvực của đời sống, sinh hoạt, khoa họ c, quân sự, y tế...Ở Việt Nam, việc nghiên cứu, chế tạo và phát triển rob ot còn rất mới và còngặp rất nhiều khó khăn. Trong thời gian 2 năm (20162017), Phòng Công nghệxanh (Green Technology Center GTC), thuộ c Viện Điện tử, Tin họ c và Tựđộng hóa của Bộ Công thương đã phối hợp với Viện Cơ khí, Đại họ c B ách KhoaHà Nội và một số tổ chức khác để chế tạo thành công một rob ot dạng người cótên là VIEBOT với hai tiềm năng chính là phục vụ quảng cáo và hỗ trợ ngườikhiếm thị trong đời sống sinh hoạt. Tiếp nối sự thành công của nhóm sinh viênK57, Viện Cơ kh í, Đại họ c Bách Khoa Hà nội với đề tài: Thiết kế, chế tạo vàđiều khiển robot dạng người1, em tiếp tục làm việc, nghiên cứu và phát triểnvới đề tài: Điều khiển động học cánh tay robot dạng người dựa vào cảm biếnhình ảnh 3 chiều MS Kinect.

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

Sinh viên thực hiện: Nguyễn Văn Huy

Mã số sinh viên: 20131782

Lớp KT CĐT 01 - K58Giáo viên hướng dẫn: TS Nguyễn Xuân HạGiáo viên phản biện: ThS Nguyễn Minh Quân

HÀ NỘI 1-2018

LỜI MỞ ĐẦU

Với sự phát triển mạnh mẽ của khoa học kĩ thuật hiện nay cùng với cuộc cáchmạnh công nghiệp 4.0, robot cùng với trí tuệ nhân tạo đang dần phổ biến hơnkhông chỉ trong sản xuất công nghiệp mà còn cả trong đời sống giải trí Robotngày nay có sự phát triển vượt bậc, chúng hỗ trợ con người trong rất nhiều lĩnhvực của đời sống, sinh hoạt, khoa học, quân sự, y tế

Ở Việt Nam, việc nghiên cứu, chế tạo và phát triển robot còn rất mới và còngặp rất nhiều khó khăn Trong thời gian 2 năm (2016-2017), Phòng Công nghệxanh (Green Technology Center - GTC), thuộc Viện Điện tử, Tin học và Tựđộng hóa của Bộ Công thương đã phối hợp với Viện Cơ khí, Đại học Bách Khoa

Hà Nội và một số tổ chức khác để chế tạo thành công một robot dạng người cótên là VIEBOT với hai tiềm năng chính là phục vụ quảng cáo và hỗ trợ ngườikhiếm thị trong đời sống sinh hoạt Tiếp nối sự thành công của nhóm sinh viênK57, Viện Cơ khí, Đại học Bách Khoa Hà nội với đề tài: "Thiết kế, chế tạo vàđiều khiển robot dạng người"[1], em tiếp tục làm việc, nghiên cứu và phát triểnvới đề tài: "Điều khiển động học cánh tay robot dạng người dựa vào cảm biếnhình ảnh 3 chiều - MS Kinect"

Qua đây, em gửi lời cảm ơn đến nhóm sinh viên tác giả đồ án "Thiết kế, chếtạo và điều khiển robot dạng người" làm tiền đề cũng như là tài liệu tham khảocho em tiếp tục thực hiện các nghiên cứu trên VIEBOT Tiếp theo, em xin gửilời cảm ơn chân thành đến PGS.TS Nguyễn Chấn Hùng, Viện phó Viện Điện

tử, Tin học và Tự động hóa thuộc Bộ Công Thương cùng các thành viên PhòngCông Nghệ Xanh (Green Technology Center) đã tạo điều kiện và giúp đỡ em

để em có cơ hội được làm việc trong một dự án robot thật sự tiềm năng Đặcbiệt, em gửi lời cảm ơn sâu sắc tới giảng viên Tiến sỹ Nguyễn Xuân Hạ, người

đã trực tiếp hướng dẫn và truyền cho em những kiến thức tuyệt vời trong suốtquá trình làm đồ án này

Ngày 10 tháng 1 năm 2018

Sinh viên

Nguyễn Văn Huy

TÓM TẮT NỘI DUNG

Nội dung của đồ án này được chia theo 5 phần:

• Phần thứ nhất Tổng quan về robot dạng người Trong phần này giớithiệu khái quát về tình hình phát triển của robot dạng người đã có hiệnnay

• Phần thứ hai là Tổng quan về VIEBOT Phần này giới thiệu về VIEBOT,giới thiệu khái quát về thiết kế cơ khí, điều khiển và các tính năng đã cótrên VIEBOT đồng thời cũng chỉ ra những vấn đề cần giải quyết trongkhuôn khổ đồ án này

• Tiếp theo là phần Động học và động lực học cánh tay robot Trongphần này trình bày ngắn gọn về phương pháp tính động học ngược và cácbước tính toán, mô hình hóa trong bài toán động lực học cánh tay robot 6bậc tự do

• Phần thứ tư là Điều khiển robot thực hiện thao tác với mục tiêu cốđịnh Phần này trình bày về phương pháp thực hiện, thuật toán và kết quảthực hiện nhiệm vụ điều khiển robot thực hiện thao tác nhấn nút thangmáy

• Và cuối cùng là phần Kết luận trình bày những gì đã làm được trongkhuôn khổ đồ án này, những hạn chế và hướng phát triển trong tương lai

DANH MỤC KÝ HIỆU VÀ CÁC CHỮ VIẾT TẮT

θi Góc quay từ trục xi−1 đến trục xi quanh zi−1

di Khoảng cách trục xi−1 đến trục xi dọc theo trục zi−1

ai Khoảng cách trục zi−1 đến trục zi dọc theo trục xi

αi Góc quay từ trục zi−1 đến trục zi quanh xi

qi, i = 1 6 Các góc khớp trên cánh tay

i Aj Ma trận thuần nhất thể hiện vị trí và hướng của hệ tọa độ

thứ j đối với hệ tọa độ thứ i

Ci, i = 1 6 Cos(θi)

Si, i = 1 6 Sin(θi)

q i Tọa độ của khâu thao tác đối với hệ tọa độ vai

Mục lục

1.1 Khái niệm robot dạng người 1

1.2 Các ứng dụng của robot dạng người 1

2 TỔNG QUAN VỀ VIEBOT 5 2.1 Giới thiệu về VIEBOT 5

2.2 Thiết kế hệ thống tự động đặt lại gốc "0" và giới hạn chống va đập cơ khí cho các khớp trên cánh tay 8

2.2.1 Phân tích vấn đề 8

2.2.2 Thiết kế đồ gá 9

2.2.3 Thuật toán điều khiển 9

2.3 Phạm vi nghiên cứu của đồ án 13

3 ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC CÁNH TAY ROBOT 14 3.1 Mô hình động học cánh tay robot 14

3.2 Các ma trận biến đổi thuần nhất 15

3.3 Bài toán động học robot 16

3.3.1 Động học thuận 16

3.3.2 Động học ngược 17

3.4 Động lực học cánh tay robot 22

3.4.1 Cơ sở lý thuyết 22

3.4.2 Mô hình hóa 24

3.4.3 Tính toán 30

3.4.4 Kết luận 33

4 ĐIỀU KHIỂN ROBOT THỰC HIỆN THAO TÁC VỚI MỤC TIÊU CỐ ĐỊNH 36 4.1 Phân tích vấn đề 36

4.2 Giới thiệu về Aruco Marker và OpenCV 37

4.2.1 Thư viện OpenCV 37

4.2.2 ArUco Marker 37

4.2.3 Tạo điểm đánh dấu 38

4.2.4 Phát hiện điểm đánh dấu 39

4.3 Giới thiệu về cảm biến chiều sâu của MS Kinect 40

4.3.1 Kinect v2 40

4.3.2 Cảm biến chiều sâu 40

4.3.3 Đo khoảng cách 40

4.3.4 Dựng ảnh độ sâu 42

4.3.5 Đồng chỉnh cảm biến chiều sâu và màu 43

4.4 Thuật toán điều khiển 43

4.4.1 Mã hóa nút bấm 43

4.4.2 Xác định vị trí nút bấm 43

4.4.3 Chuyển tọa độ từ hệ tọa độ camera sang hệ tọa độ robot 45 4.4.4 Điều khiển chân đế 46

4.4.5 Điều khiển cánh tay 48

4.5 Kết quả thực nghiệm và đánh giá 48

5 KẾT LUẬN 51 5.1 Kết quả đạt được 51

5.2 Những hạn chế 51

5.3 Hướng phát triển trong tương lai 52

Danh sách hình vẽ

1.1 Robot ASIMO 2

1.2 Robot Nao 3

1.3 TOPIO 3.0 tại triển lãm Robot Quốc tế, Tokyo-Nhật Bản[2] 4

2.1 Bộ khung của VIEBOT 6

2.2 Cấu trúc động học và các công nghệ sử dụng trên VIEBOT 6

2.3 Cấu trúc tổng thể của VIEBOT 7

2.4 Sơ đồ nguyên lý và vị trí các động cơ trên cánh tay robot 8

2.5 Sơ đồ hệ thống điều khiển cánh tay robot 9

2.6 Các loại cảm biến được sử dụng 10

2.7 Mô hình 3D đồ gá và vị trí CTHT sử dụng cho khớp 1 10

2.8 Vị trí lắp thực tế CTHT cho khớp 1 11

2.9 Công tắc hành trình cho khớp 2 11

2.10 Thuật toán điều khiển đặt lại điểm "0" và giới hạn chống va đập cơ khí 12

3.1 Mô hình động học cánh tay robot 14

3.2 Sơ đồ cơ chế điều khiển động học ngược robot 19

3.3 Hệ tọa độ bàn tay 19

3.4 Các bước giải bài toán động lực học robot 24

3.5 Mô hình khâu 1 25

3.6 Các thuộc tính động học của khâu 1 26

3.7 Mô hình khâu 2 27

3.8 Mô hình khâu 3 28

3.9 Mô hình khâu 4 29

3.10 Mô hình khâu 5 30

3.11 Mô hình khâu 6 31

3.12 Đồ thị thể hiện giá trị các biến khớp 34

3.13 Đồ thị thể hiện giá trị lực tác dụng tại các biến khớp 35

4.1 Ví dụ về ArUco marker 38

4.2 Camera phát hiện các ArUco marker 39

4.3 Vị trí các cảm biến của Kinect 41

4.4 Các thông số kĩ thuật cảm biến chiều sâu của Kinect 41

4.5 Nguyên lý đo khoảng cách 41

4.6 Chuyển từ khoảng cách điểm ảnh sang độ sâu điểm ảnh 42

4.7 Các điểm mù của camera độ sâu và camera màu 42

4.8 Các bước điều khiển bấm nút thang máy 43

4.9 Mã hóa nút bấm 44

4.10 Chuyển tọa độ từ hệ tọa độ ảnh màu sang hệ tọa độ ảnh độ sâu 44 4.11 Kết quả phát hiện vị trí nút bấm trong không gian 3 chiều 45

4.12 Vị trí hệ tọa độ Kinect và hệ tọa độ cánh tay 46

4.13 Thuật toán điều khiển chân đế đến vị trí nhấn nút 47

4.14 Đồ thị so sánh kết quả đo chiều sâu bằng Kinect và đo bằng laser 49 4.15 Đồ thị phân bố vị trí điểm tác động cuối các lần thử nghiệm nhấn nút (đơn vị: cm) 49

Danh sách bảng

3.1 Bảng động học D-H 153.2 Bảng động học D-H 314.1 Bảng so sánh kết quả đo chiều sâu bằng Kinect và đo bằng laser 48

Chương 1

TỔNG QUAN VỀ ROBOT DẠNG NGƯỜI

Robot dạng người là một robot với hình dạng của nó được xây dựng gầngiống hoặc giống với hình dạng con người Thiết kế có thể cho các mục đíchchức năng, như tương tác với các công cụ và môi trường của con người, cho cácmục đích thí nghiệm như nghiên cứu các chuyển động hoặc cho các mục đíchkhác Nói chung, robot dạng người có thân, đầu, hai cánh tay và hai chân nhưcon người, hoặc một số dạng robot hình người chỉ mô phỏng một phần của cơthể ví dụ như từ thắt lưng trở lên Một số dạng robot hình người cũng có phầnđầu được thiết kế để sao chép các tính năng trên khuôn mặt con người như mắt,miệng để thể hiện các trạng thái biểu cảm.[3]

Robot dạng người hiện nay được sử dụng như một công cụ trong một vài lĩnhvực nghiên cứu Các nhà nghiên cứu cần hiểu cấu trúc cơ thể con người và cácứng xử để xây dựng và nghiên cứu các robot dạng người Mặt khác, cố gắng môphỏng cơ thể con người để có hiểu biết tốt hơn về nó Nhận thức con người làmột lĩnh vực để nghiên cứu tập trung vào cách con người học hỏi thông tin cảmquan để có được các kỹ năng nhận thức và vận động Kiến thức này được dùng

để phát triển mô hình tính toán của hành vi con người và nó đã được cải thiệntheo thời gian

Bên cạnh việc nghiên cứu, các robot dạng người đang được phát triển để thựchiện các nhiệm vụ của con người như trợ giúp các cá nhân như giúp đỡ người

ốm, người cao tuổi, người khuyến tật hay là thay con người làm các công việc

CHƯƠNG 1 TỔNG QUAN VỀ ROBOT DẠNG NGƯỜI

trong môi trường ô nhiễm, nhàm chán hoặc nguy hiểm Nó cũng được sử dụng

để thay thế con người trong các công việc của nhân viên lễ tân hoặc nhân viêntrong các dây chuyền sản xuất tự động Về bản chất, vì nó được thiết kế theohình dạng con người nên nó có thể thực hiện bất kì nhiệm vụ nào của con ngườimiễn là có phần mềm phù hợp Tuy nhiên để thực hiện được các công việc phứctập cần rất nhiều sự tính toán và lập trình tỉ mỉ

Trong sự phát triển của nền công nghiệp thông minh hiện nay, bên cạnh cácứng dụng trong công nghiệp sản xuất và trợ giúp đời sống Con người hiện naycũng có nhu cầu giải trí cao Vì vậy robot dạng người cũng đang dần phổ biếntrong lĩnh vực giải trí

Một số lĩnh vực ứng dụng của robot dạng người cụ thể như:

• Robot phục vụ quảng cáo

• Robot phục vụ trong y tế

• Robot trong quân sự

• Robot dịch vụ trong gia đình

Hình 1.1: Robot ASIMO

Xa hơn nữa, robot dạng người đặc biệt với các thuật toán trí tuệ nhân tạothông minh có thể hữu ích cho các nhiệm vụ thăm dò vụ trụ nguy hiểm và xa

CHƯƠNG 1 TỔNG QUAN VỀ ROBOT DẠNG NGƯỜI

xôi trong tương lai, mà không cần quay trở lại Trái Đất hoặc quay trở lại khinhiệm vụ hoàn thành.[3]

Trên thế giới và tại Việt Nam đã có rất nhiều công ty nghiên cứu chế tạothành công robot như:

• ASIMO (Hình1.1) của Honda, là robot đầu tiên có thế giới có khả năng leocầu thang và đi lại một cách độc lập Không chỉ vậy nó còn có thể nhận diệnkhuôn mặt, giọng nói, cử chỉ điệu bộ và có thể điều khiển được qua giọngnói Với đôi tay của mình, ASIMO có thể bật tắt công tắc đèn, mở cửa,mang vác, đẩy xe và vô vàn công việc khác miễn là được lập trình trước

• Nao (Hình 1.2) được phát triển bởi Aldebaran Robotics, một công ty vềrobotics ở Pháp Nao phục vụ trong lĩnh vực giải trí, cụ thể là nó được

sử dụng trong các cuộc thi robot đá bóng thế giới (RoboCup StandardPlatform League - SPL) Bên cạnh đó, một vài phiên bản của Nao đã đượcphát triển cho các trường đại học và các phòng thí nghiệm cho mục đíchnghiên cứu và giáo dục

Hình 1.2: Robot Nao

CHƯƠNG 1 TỔNG QUAN VỀ ROBOT DẠNG NGƯỜI

• TOPIO (TOSY Ping Pong Playing Robot) (Hình 1.3) là Robot dạng người

có khả năng chơi bóng bàn với con người Được phát triển từ năm 2005bởi công ty TOSY của Việt Nam TOPIO có 3 phiên bản là TOPIO 1.0,TOPIO 2.0, TOPIO 3.0 TOPIO sử dụng các công nghệ như: dùng các máyảnh tốc độ cao để ghi lại quỹ đạo của các đối tượng di chuyển rất nhanh,tích hợp trí tuệ nhân tạo, điều khiển di chuyển nhanh và chính xác, đi lạicân bằng trên cả hai chân.[2]

Hình 1.3: TOPIO 3.0 tại triển lãm Robot Quốc tế, Tokyo-Nhật Bản[2]

Robot dạng người nói riêng và robot nói chung trên thế giới hiện nay pháttriển rất đa dạng, từ đơn giản đến rất phức tạp, ở mọi lĩnh vực Tuy nhiên ởViệt Nam hiện nay, robot phát triển rất hạn chế và ở trình độ tương đối hạnchế so với thế giới VIEBOT là một trong những robot dạng người đầu tiên ởViệt Nam được tích hợp rất nhiều công nghệ mới, tuy nó còn đơn giản, sơ khai

so với những robot đã có trên thế giới, nhưng chắc chắn sẽ là nền móng, là tiền

đề để Việt Nam tiến sâu hơn vào lĩnh vực robot trong tương lai Các phần saucủa đồ án này sẽ giới thiệu tổng thể về robot và chi tiết vào điều khiển độnghọc cánh tay robot VIEBOT cũng như những đóng góp mới của em trong dự

án VIEBOT này

Chương 2

TỔNG QUAN VỀ

VIEBOT

VIETBOT là một robot dạng người - dự án nghiên cứu của phòng Công nghệxanh thuộc Viện Điện tử, Tin học và Tự động hóa thuộc Bộ Công Thương phốihợp với Viện Cơ khí, Đại học Bách Khoa Hà Nội và một số cơ quan khác Bộkhung thật của robot được cho ở Hình 2.1 Cấu trúc động học và các công nghệ

sử dụng trên VIEBOT được mô tả ở Hình 2.2 Cấu trúc tổng thể của VIEBOTđược thể hiện ở Hình 2.3

Tại Việt Nam, VIEBOT được coi là một thế hệ robot mới được thừa hưởng

từ một số công nghệ trong nền công nghiệp 4.0 như Cloud Analytics, cấu trúcphần mềm dựa trên dịch vụ Micro và in 3D

Ngoài ra, VIEBOT được thiết kế theo định hướng thương mại, do đó nó cóthể linh hoạt chuyển thành các loại robot khác với các chức năng khác nhau như

xe tự hành trong bệnh viện, robot dịch vụ, robot quảng cáo hoặc robot giáo dụcvới chi phí thấp Với mục đích đầu tiên là trở thành một robot quảng cáo, nó cóhình dáng như một phụ nữ trẻ với chiều cao 1.6m và trọng lượng 55kg cả pin

Bộ não của robot là một bộ vi xử lý Intel Core i7 Mini-PC với 16GB RAM chạy

MS Windows 10 Robot kết nối đám mây MS Azure thông qua mạng di động4G LTE để sử dụng các dịch vụ đám mây Azure tiên tiến bao gồm Face API,Computer Vision và Machine Learning hỗ trợ VIEBOT nhận dạng khuôn mặt,giới tính, tuổi tác và các vật xung quanh Nhờ kết nối không dây tốc độ cao của

nó, VIEBOT cho phép người dùng từ xa tương tác với những người dùng trongkhu vực hoạt động của VIEBOT

Cấu trúc cơ khí robot gồm 2 phân hệ:

1 Phân hệ chân - thân - đầu

CHƯƠNG 2 TỔNG QUAN VỀ VIEBOT

Hình 2.1: Bộ khung của VIEBOT

Hình 2.2: Cấu trúc động học và các công nghệ sử dụng trên VIEBOT

CHƯƠNG 2 TỔNG QUAN VỀ VIEBOT

Hình 2.3: Cấu trúc tổng thể của VIEBOT

• Chân: di chuyển bằng 3 bánh xe, trong đó có một bánh có trục quaytùy động và 2 bánh được dẫn động bằng 2 động cơ bước độc lập chophép chân đế thực hiện di chuyển linh hoạt như tiến, lùi, quay trái,quay phải, quay tại chỗ, đi theo đường cong Tuy nhiên hạn chế của cấutrúc này là chỉ di chuyển được trên mặt sàn phẳng

• Thân: có thể quay so với chân đế nhờ cấu trúc ổ bi ở giữa thân và chân

đế, cho phép thân robot quay để tạo các hướng nhìn khác nhau trongkhi chân đế vẫn đang di chuyển thẳng

• Đầu: gồm 2 động cơ trong đó có một động cơ xoay đầu và một động cơgật đầu

2 Phân hệ cánh tay

• 2 cánh tay mỗi cánh tay có 6 bậc tự do (BTD)

• 2 bàn tay mỗi bàn tay có một động cơ điều khiển đồng thời cả 5 ngóngiúp bàn tay thực hiện nắm-mở bàn tay

Với việc có 6 bậc tự do chính và một bậc tự do phụ ở mỗi cánh tay, cùng vớiyêu cầu cánh tay phải có hình dạng gần giống với tay người, nhỏ gọn và linhhoạt đòi hỏi quá trình thiết kế, tính toán một cách tỉ mỉ Trên cánh tay sử dụng

4 dòng động cơ chính sau:

• Họ động cơ RMCS220x: Sử dụng tại 2 bậc tự do khớp vai và 1 bậc tự do ởkhuỷu tay

CHƯƠNG 2 TỔNG QUAN VỀ VIEBOT

(a) (b)

Hình 2.4: Sơ đồ nguyên lý và vị trí các động cơ trên cánh tay robot

• Họ động cơ MX: Động cơ MX64 ở khuỷu tay và MX28 ở cổ tay

• Động cơ Faulhaber DC 12V Coreless Motor 16002 ở cổ tay

• Động cơ servo FR1501 nắm mở bàn tay

Hệ thống điều khiển các động cơ ở cánh tay được thể hiện như Hình 2.5 Toàn

bộ vi điều khiển trên robot sử dụng là các mạch vi điều khiển adruino

Với thực tế kết cấu cơ khí ở các khớp và các loại động cơ đang sử dụng trongcánh tay của VIEBOT Có 3 khớp ở mỗi cánh tay đó là khớp số 1, 2, 3 nhưtrong Hình 2.4b sử dụng động cơ RMCS 220x với đặc điểm là nó không có điểm

"0" cố định mà mỗi lần cấp điện thì nó lấy luôn vị trí lúc đó làm điểm "0" Vì

CHƯƠNG 2 TỔNG QUAN VỀ VIEBOT

Hình 2.5: Sơ đồ hệ thống điều khiển cánh tay robot

vậy, ở các khớp này cần có các cơ chế để tự lấy lại điểm "0" mỗi lúc cấp điệnhoặc lúc cần thiết và các giới hạn để chống va đập cơ khí tại các khớp này.Với ý tưởng tạo nên cơ cấu để báo cho động cơ biết ở đâu là vị trí "0", ở đâu

là giới hạn lớn nhất mà động cơ có thể quay được Tuy nhiên trong các khớpquay của cánh tay robot này yêu cầu kích thước nhỏ trong không gian chật hẹp

Vì vậy em đã sử dụng cảm biến mắt đọc hồng ngoại hình chữ U và công tắchành trình loại nhỏ Hình 2.6 để phù hợp với mỗi vị trí khác nhau

Ở khớp 2 chỉ cần sử dụng công tắc hành trình HT1418 (Hình 2.9)

2.2.3 Thuật toán điều khiển

Với yêu cầu cụ thể của vấn đề này như sau:

CHƯƠNG 2 TỔNG QUAN VỀ VIEBOT

(a) Cấu tạo mắt đọc encoder (b) Mô-đun mắt đọc

encoder

(c) Mô-đun công tắc hành trình

Hình 2.6: Các loại cảm biến được sử dụng

(a) (b)

Hình 2.7: Mô hình 3D đồ gá và vị trí CTHT sử dụng cho khớp 1

CHƯƠNG 2 TỔNG QUAN VỀ VIEBOT

(a) (b)

Hình 2.8: Vị trí lắp thực tế CTHT cho khớp 1

Hình 2.9: Công tắc hành trình cho khớp 2

CHƯƠNG 2 TỔNG QUAN VỀ VIEBOT

Hình 2.10: Thuật toán điều khiển đặt lại điểm "0" và giới hạn chống va đập cơkhí

• Động cơ tự quay khớp về vị trí "0" và đặt gốc tại đó mỗi khi bật nguồnhoặc nhấn nút khởi động lại hệ thống vi điều khiển

• Động cơ dừng lại ở vị trí cao nhất khi chạm CTHT ở vị trí cao nhất đểtránh va chạm cơ khí

Với các yêu cầu trên, một thuật toán thích hợp để điều khiển được mô tả nhưtrong Hình 2.10

Thuật toán trên được hiện thực hóa bằng chương trình điều khiển arduino vớitín hiệu của 2 vị trí đặt CTHT được đưa về vào 2 chân ngắt 2 và 3 của arduinopro mini và sử dụng lệnh ngắt theo sườn xuống (FALLING) Việc sử dụng lệnhngắt theo sườn xuống sẽ nảy sinh một vấn đề là khi cánh tay chạm vào CTHT

và giữ nguyên ở đó thì hàm ngắt không phát hiện được Do đó, cần phải có tínhiệu để báo cho arduino biết rằng CTHT có đang được kích hoạt hay không

Vì vậy, tín hiệu từ CTHT gửi về sẽ vừa vào chân ngắt 2,3 vừa được trích sang

CHƯƠNG 2 TỔNG QUAN VỀ VIEBOT

chân INPUT 7,8 để kiểm tra trạng thái của CTHT

Chương trình được lập trình, lắp đặt và đã hoạt động ổn định theo thời gian.*

Các đặc tính kĩ thuật của cánh tay robot như sau:

• Phần cánh tay được chia làm 3 cụm: cụm bả vai, cụm bắp tay và khuỷutay, cụm cẳng tay và cổ tay

• Bàn tay có 5 ngón có thể nắm, mở được theo tay người

• Cánh tay có khả năng bắt chước cử chỉ người và thực hiện một số thao táckhác như nhấn nút thang máy

• Trên các khớp của cánh tay có cơ chế tự động đặt vị trí điểm 0 và các giớihạn chống va đập cơ khí

Kế thừa và phát triển từ đồ án "Thiết kế, chế tạo và điều khiển mô hình robotdạng người phân hệ cánh tay - bàn tay.[1]"

Đồ án trên dừng lại ở việc thiết kế cơ khí, tính chọn các động cơ, tính toán

và điều khiển động học cho cánh tay thực hiện bắt chước cử chỉ người Trongkhuôn khổ nội dung Đồ án tốt nghiệp này, em sẽ thực hiện nhiệm vụ: "Điềukhiển động học cánh tay robot dựa vào cảm biến hình ảnh 3 chiều MS Kinect" Phạm vi nghiên cứu trong đồ án này của em là:

• Cải tiến kết cấu cơ khí và điều khiển tự động đặt lại điểm "0" và giới hạn

cơ khí chống va đập cho các khớp trên cánh tay robot

• Tối ưu tính toán và điều khiển động học cánh tay

• Ứng dụng điều khiển thao tác với mục tiêu cố định (Thực hiện thao tácnhấn nút thang máy)

• Tính toán và mô hình hóa động lực học cánh tay robot

* Sự hoạt động ổn định của hệ thống đặt lại điểm gốc "0" và giới hạn chống va đập cơ khí này được kiểm nghiệm và công nhận trong quá trình vận hành robot của trưởng dự án VIEBOT, kĩ sư Trần Xuân Đức cùng các đồng nghiệp trong dự án của phòng Công Nghệ Xanh, Viện Điện tử, Tin học và Tự động hóa Bộ Công thương.

Chương 3

ĐỘNG HỌC VÀ ĐỘNG

LỰC HỌC CÁNH TAY

ROBOT

Cánh tay robot gồm có 6 khớp quay được mô hình hóa và gắn hệ tọa độ

Denavit– Hartenberg như hình dưới:

(a) Sơ đồ động học cánh tay (b) Mô hình động học cánh tay và các hệ tọa độ D-H

Hình 3.1: Mô hình động học cánh tay robot

CHƯƠNG 3 ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC CÁNH TAY ROBOT

Dạng tổng quát của ma trận biến đổi thuần nhất từ hệ tọa độ khâu thứ i-1sang khâu thứ i theo tài liệu [4] như sau:

CHƯƠNG 3 ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC CÁNH TAY ROBOT

◦ nx= C6(S4(C1C3 + S1S2S3) + C2C4S1) + S6(S5(C4(C1C3 + S1S2S3) + C2S1S4) + C5(C1S3 − C3S1S2))

◦ ny = C6(S4(C3S1 − C1S2S3) − C1C2C4) − S6(S5(C4(C3S1 + C1S2S3) + C1C2S4) + C5(C1S3 + C1C3S2))

◦ n z = C6(S5(S2S4 + C2C4S3) − C2C3C5) + C6(C4S2 − C2S3S4).

◦ sx = −C5(C4(C1C3 + S1S2S3) − C2S1S4) − S5(C1S3 − C3S1S2).

CHƯƠNG 3 ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC CÁNH TAY ROBOT

◦ sy = −C5(C4(C3S1 − C1S2S3) + C1C2S4) − S5(S1S3 + C1C3S2).

◦ sz = C5(S2S4 + C2C4S3) − C2C3S5.

◦ a x = S6(S4(C1C3 + S1S2S3) + C2C4S1) + C6(S5(C4(C1C3 + S1S2S3) − C2S1S4) + C5(C1S3 − C3S1S2)).

◦ ay = S6(S4(C3S1 − C1S2S3) − C1C2C4) + C6(S5(C4(S1C3 − C1S1S3) + C1C2S4) + C5(S1S3 + C1C3S2)).

S5(C1S3 − C3S1S2)).

◦ py = d3(S3(C3S1 − C1S2S3) − C1C2C3) − d2C1C2 + a6C6(S3(C3S1 − C1S2S3)−C1C2C3)−a6S6(C5(C3(C3S1−C1S2S3)+C1C2S3)+S5(S1S3+ C1C3S2)).

◦ pz = d3(C3S2−C2S3S3)+d2S2+a6C6(C3S2−C2S3S3)+a6S6(C5(S2S3+ C2C3S3) − C2C3S5).

Hệ 12 phương trình trên chính là hệ phương trình động học thuận của cánh tayrobot Từ hệ phương trình động học thuận, vị trí và hướng của bàn tay robothoàn toàn xác định khi biết các giá trị góc các biến khớp

3.3.2 Động học ngược

Bài toán động học ngược là bài toán khi ta biết trước quỹ đạo và hướngchuyển động của khâu thao tác cuối, cần tìm quy luật chuyển động của các biếnkhớp Ở trong bài toán cụ thể với VIEBOT, dữ liệu đầu vào từ Kinect cho ta

vị trí và hướng của bàn tay Vì vậy, cần giải bài toán động học ngược để giải

CHƯƠNG 3 ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC CÁNH TAY ROBOT

ra giá trị các biến khớp tại từng thời điểm để đưa ra lệnh điều khiển cho từngđộng cơ độc lập tại các khớp

Việc giải bài toán động học ngược hiện nay có hai phương pháp đó là: Giảibằng phương pháp số sử dụng vòng lặp (ví dụ vòng lặp Newton Raphson) đểgiải Ưu điểm của phương pháp này là điều kiện đầu vào đơn giản, các nghiệmsau phụ thuộc vào các nghiệm trước đó nên việc chọn nghiệm hầu như khôngphải giải quyết Tuy nhiên, với các cánh tay robot nhiều bậc tự do thì bản thâncông thức động học nó rất dài và việc giải bằng phương pháp số mất rất nhiềuthời gian và cần máy tính có bộ vi xử lý mạnh Và nó chỉ có thể phù hợp với cácchương trình offline (Chương trình được tính toán một lần trên máy tính sau

đó lấy kết quả cho vào robot để robot thực hiện) Đối với VIEBOT, mỗi cánhtay có 6 bậc tự do và yêu cầu điều khiển online (theo thời gian thực) cho nênphương án dùng phương pháp số là không khả thi Vì vậy, phương án được sửdụng ở đây là dùng các biến đổi giải tích và hình học để giải ra nghiệm tườngminh của từng biến khớp Tuy phương pháp này cho ra nghiệm tường minh

ta có thể sử dụng trong chương trình điều khiển online nhưng nhược điểm củaphương pháp này là phải giải bằng tay các phương trình phức tạp, không thểdùng chung phương pháp giải cho các dạng cánh tay robot khác nhau mà nó chỉ

có thể sử dụng cho từng dạng cánh tay đặc biệt

Hình 3.2 cho thấy sơ đồ chính của hệ thống điều khiển động học ngược củaVIEBOT Hệ thống có 4 mô-đun chức năng bao gồm: Xác định các khớp - IJP,Tính toán động học ngược - CIK, Đánh giá chất lượng bên ngoài - EQA) và bộđiều khiển chuyển động khớp - JMC

Trong nghiên cứu này, robot có 2 cánh tay với 6 bậc tự do ở mỗi cánh tay Dođiều kiện hoạt động theo thời gian thực của các cánh tay, một giải pháp dạngđóng (close-form solution) cho các biến khớp là một lợi thế Do đó, chúng tôi sửdụng và thích ứng phương biến đổi ngược [5] bằng việc đảo ngược các ma trậntruyền của phương trình động học để tìm ra giải pháp dạng đóng cho các biếnkhớp

Biểu thức bên trái là nhân các ma trận truyền có các biến khớp qi(i = 1 ÷ 6)

và bên phải là ma trận mô tả vị trí và hướng của bản tay

Do cấu trúc cơ khí thực tế của cánh tay và chuyển động tay người, các biếnkhớp bị giới hạn bởi các góc nhất định như công thức 3.1

CHƯƠNG 3 ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC CÁNH TAY ROBOT

Hình 3.2: Sơ đồ cơ chế điều khiển động học ngược robot

CHƯƠNG 3 ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC CÁNH TAY ROBOT

cánh tay, chúng ta tìm được ma trận khâu thao tác cuối

ψ = arctan p

0 z

d 2 s 4

CHƯƠNG 3 ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC CÁNH TAY ROBOT

Để chọn một biến phù hợp cho trường hợp này, chúng tôi đề xuất sử dụng

vị trí vai D 0 và khuỷu tay D 4 làm nhân tố quyết định để đạt được nghiệmmong muốn

CHƯƠNG 3 ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC CÁNH TAY ROBOT

Việc giải quyết bài toán động lực học là rất quan trọng trong vấn đề tínhtoán và điều khiển robot Động lực học là mối liên hệ giữa lực/mô men đặt vàomột vật và các thông số động học như là vị trí, vận tốc, gia tốc Động lực họctrong robot được chia thành 2 bài toán lớn đó là bài toán động lực học thuận vàbài toán động lực học ngược Bài toán động lực học thuận là khi cho 1 lực/mômen tác động và một khớp, ta xét vị trí, vận tốc và gia tốc của khớp đó sau khitác dụng lực Bài toán động học ngược là ứng với mỗi vị trí, vận tốc và gia tốccho trước, ta cố gắng tìm ra lực để có thể tạo ra được các thông số động học đó.Nhờ đó, bài toán động lực học giúp chúng ta có thể điều khiển cánh tay robotmột cách linh hoạt với độ chính xác cao Trong khuôn khổ đồ án này, em trìnhbày phương pháp sử dụng phương trình vi phân Lagrange loại II cho bài toánđộng lực học ngược để tìm ra các lực điều khiển cần thiết cho các khớp của cánhtay của robot

3.4.1 Cơ sở lý thuyết

Phương trình vi phân chuyển động của robot được xây dựng theo phươngtrình Lagrange loại II có dạng tổng quát như sau:

d dt

• M (q) là ma trận khối lượng

• C(q, ˙q) ˙q là ma trận đặc trưng cho lực quán tính và lực Coriolis

• G(q) là lực suy rộng của các lực có thế

• Q là lực suy rộng của các lực không thế ứng với các tọa độ suy rộng

• U là lực điều khiển ứng với các tọa độ suy rộng

a Ten-xơ quán tính khối

Ma trận quán tính của khâu i đối với khối tâm của chính nó trong hệ tọa độđộng.[10]

Θrci=







Iixxr −Iixyr −Iixzr

−Iiyxr Iiyyr −Iiyzr

−Iizxr −Iizyr Iizzr



