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

TRƯỜNG ĐẠI HỌC PHENIKAA

_________________

BÁO CÁO TỔNG KẾT

Thiết kế bộ điều khiển cho mô hình Robot

4 bậc tự do

Sinh viên thực hiện: Lê Viết Đạt MSSV: 19010206

Trần Mạch Tuấn Kiệt 19010214

HÀ NỘI, 12/2022
 MỤC LỤC
1. LỜI MỞ ĐẦU................................................................................................2
2. TỔNG QUAN TÌNH HÌNH NGHIÊN CỨU..................................................3
2.1. Tổng quan tình hình nghiên cứu và lí do lựa chọn đề tài..........................3
2.2. Mục tiêu, nội dung và phương pháp nghiên cứu......................................4
2.3. Đối tượng và phạm vi nghiên cứu............................................................5
3. CỞ SỞ LÝ THUYẾT.....................................................................................7
3.1. Cấu trúc robot và tay máy công nghiệp....................................................7
3.2. Thiết lập hệ tọa độ khảo sát và lập bảng D – H........................................9
3.3. Vùng hoạt động của khâu tác động cuối.................................................12
3.4. Bài toán động học ngược........................................................................14
4. PHÂN TÍCH THIẾT KẾ HỆ THỐNG.........................................................15
4.1. Ý tưởng thiết kế.....................................................................................15
4.2. Phần cứng...............................................................................................15
4.2.1. Động cơ Step KV4234.......................................................................15
4.2.2. Driver A4988.....................................................................................17
4.2.3. Board điều khiển Arduino Uno........................................................18
4.2.4. Board CNC Shield V3.......................................................................20
4.2.5. Servo MG90S....................................................................................21
4.3. Phần mềm...............................................................................................22
4.3.1. Arduino IDE......................................................................................22
4.3.2. LabVIEW...........................................................................................23
4.3.3. MATLAB..........................................................................................25
4.4. Giải thuật điều khiển cho động cơ Step..................................................26
4.5. Giải thuật điều khiển cho động cơ Servo................................................30
4.6. Thành lập các phương trình động học....................................................32
4.6.1. Xây dựng bài toán động học thuận tay máy......................................32

1
 4.6.2. Xây dựng bài toán động học ngược tay máy......................................35
5. TRIỂN KHAI TRÊN THỰC NGHIỆM VÀ KÊT QUẢ...............................38
5.1. Thiết kế và lắp ráp robot........................................................................38
5.2. Kiểm tra thuật toán điều khiển bằng mô phỏng......................................40
5.3. Vùng làm việc của robot........................................................................42
5.4. Thuật toán trên Arduino.........................................................................43
5.5. Ứng dụng điều khiển trên LabVIEW.....................................................44
5.6. Robot và ứng dụng thực tế.....................................................................47
6. KẾT LUẬN VÀ KIẾN NGHỊ......................................................................50
TÀI LIỆU THAM KHẢO..................................................................................51

2
 MỤC LỤC HÌNH ẢNH
Hình 1 Sơ đồ cấu trúc của robot công nghiệp........................................................10
Hình 2 Trường công tác của robot.........................................................................12
Hình 3 Các hệ tọa độ trên cánh tay robot...............................................................13
Hình 4 Chiều dài và góc của một khâu..................................................................13
Hình 5 Bộ thông số Denavit – Hartenberg.............................................................14
Hình 6 Sơ đồ chuyển vị của khâu cuối so với cơ sở...............................................15
Hình 7 Mô tả không gian làm việc của cơ cấu hai khâu phẳng..............................16
Hình 8 Sơ đồ khối thiết kế Robot...........................................................................18
Hình 9 Động cơ Step KV4234...............................................................................19
Hình 10 Driver A4988...........................................................................................20
Hình 11 Module ESP32-WROOM-32...................................................................21
Hình 12 Board CNC Shield V3..............................................................................23
Hình 13 Servo MG90S...........................................................................................24
Hình 14 Giao diện phần mềm lập trình Arduino IDE............................................25
Hình 15 Giao diện phần mềm lập trình LabVIEW.................................................27
Hình 16 Giao diện phần mềm lập trình MATLAB................................................29
Hình 17 Đồ thị góc quay và vận tốc với chu kỳ cấp xung cố định.........................30
Hình 18 Qũy đạo mong muốn của động cơ............................................................31
Hình 19 Nguyên lý hoạt động của động cơ servo..................................................33
Hình 20 Thuật toán điều khiển động cơ servo........................................................34
Hình 21 Hệ tọa độ trên tay máy 4 bậc tự do...........................................................35
Hình 22 Động học ngược tay máy trong không gian Oxyz....................................38
Hình 23 Động học ngược tay máy trong mặt phẳng zOO 1 zO 1:...........................39
Hình 24 Robot IBR 460.........................................................................................41
Hình 25 Mẫu robot được sử dụng..........................................................................42
Hình 26 Sơ đồ ghép nối mạch điều khiển..............................................................43
Hình 27 Kết quả mô phỏng thuật toán điều khiển động cơ....................................44
Hình 28 Kết quả mô phỏng Robot trong không gian ba chiều...............................45
Hình 29. Vùng làm việc của robot.........................................................................46
Hình 30 Cấu trúc thuật toán trên Arduino..............................................................47
Hình 31 Mô tả hoạt động của phần mềm điều khiển trên labVIEW.......................48
Hình 32 Giao diện điều khiển trên phần mềm LabVIEW......................................49
Hình 33 Khối xử lý phương trình động học thuận (bên trái) và nghịch (bên phải) 49

3
Hình 34 Cấu trúc thuật toán trên Arduino..............................................................51
Hình 35 Hình ảnh thử nghiệm robot di chuyển không giật....................................51
Hình 36 Hình ảnh robot gắp thả vật theo tọa độ đặt bởi người dùng......................52

MỤC LỤC BẢNG BIỂU

Bảng 1 Bảng tham số động học D – H...................................................................15
Bảng 2 Thông số hoạt động chính của động cơ Step KV4234...............................19
Bảng 3 Thông số hoạt động chính của Driver A4988............................................21
Bảng 4 Các thông số kỹ thuật chính của Arduino Uno..........................................22
Bảng 5 Các thông số kỹ thuật chính của Board CNC Shield V3............................23
Bảng 6 Các thông số kỹ thuật chính của Servo MG90S.........................................24
Bảng 7 Thông số D-H của robot............................................................................36

4
1. LỜI MỞ ĐẦU
Robot nói chung và robot công nghiệp nói riêng đã tiến những bước dài để
xâm nhập vào đời sống xã hội cũng như hoạt động sản xuất của con người chúng
ta. Việc ứng dụng robot trở nên quan trọng trong ngành tự động hóa nhằm nâng cao
năng suất và chất lượng, hạn chế và cải thiện sức lao động của con người. Robot
giúp con người hoạt động ở những môi trường nguy hiểm với tính mạng như khu
vực hầm mỏ, khư vực nhiễm phóng xạ,... hoặc duy trì hoạt động trong những dây
chuyền 24/7. Robot và đặc biệt là cánh tay robot đã có ứng dụng rộng khắp ở các
nhà máy trên khắp thế giới. Ngày này robot vẫn luôn là một đối tượng được quan
tâm nhiều không những trong lĩnh vực sản xuất mà còn lĩnh vực nghiên cứu để có
thể cải tiến, tối ưu hoạt động của nó hoặc được sử dụng để giảng dạy.
Đồ án chuyên ngành lần này nhằm nghiên cứu chuyên sâu về robot, chúng
tôi tập trung vào việc tính toán lý thuyết và xây dựng thuật toán điều khiển cũng
như giao diện điều khiển đối với cánh tay robot 4 bậc tự do. Cơ sở lý thuyết của
robot được xây dựng dựa trên phương trình động học thuận, phương trình động học
ngược, phương trình động lực của robot được nhóm tác giả tính toán và mô phỏng
trên phần mềm MATLAB. Từ đó xây dựng thuật toán điều khiển vận tốc theo bậc
thang và thiết kế phần mềm LabVIEW để điều khiển. Đề tài có sử dụng một kết
cấu phần cứng được in 3D bằng nhựa để điều khiển. Kết cấu này không đáng tin
trong thực tế nhưng trong khuôn khổ đề tài này có thể được sử dụng để mô tả một
khâu hay một giai đoạn của quá trình sản xuất thực tế.
Mô hình thuật toán điều khiển hoạt động tốt trong giới hạn làm việc của
robot khi mà động cơ chạy mượt mà và không xảy ra hiện tượng giật. Thử nghiệm
khi đặt một ly nước nhỏ trên khâu tác động cuối, nước không bị rơi vãi ra khỏi ly
trong suốt quá trình hoạt động.

5
2. TỔNG QUAN TÌNH HÌNH NGHIÊN CỨU
2.1.Tổng quan tình hình nghiên cứu và lí do lựa chọn đề tài
Những ý niệm đầu tiên về những vật tương tự robot đã xuất hiện từ thời cổ
đại ngày chính trong các câu chuyện thần thoại xưa. Những vật này được tạo ra
nhằm mục đích phục vụ chủ nhân của nó một cách tự động, có khả năng tương tác
và nghe lệnh con người. Tuy nhiên đến năm 1921, thuật ngữ “robotics” mới lần
đầu tiên xuất hiện và được đề cập trong vở kịch “Rossum’s Universal Robots” của
nhà viết kịch viễn tưởng nổi tiếng người Czech, Karel Capek. Ông sử dụng từ
“robot” được biến thể từ một tiếng Czech cổ là “robota” có nghĩa là lao động
cưỡng bức để gán vào một chiếc máy mà Rossum và con trai đã chế tạo ra để phục
vụ con người. [1] Vào năm 40 của thế kỷ trước, nhà văn Nga, Issac Asimov đưa ra
những mô tả gần hơn của robot đối với khái niệm ngày này khi ông thêm vào việc
điều khiển bằng một hệ thần kinh khả trình gọi là “Positron” do chính con người
lập trình. Ông cũng đã đề ra 3 nguyên tắc cơ bản mà một robot cần phải có đó là:
“Không tổn hại con người”, “Tuân theo nguyên tắc do con người đặt ra”, và “Cần
phải tự bảo vệ mình nhưng không vi phạm hai quy tắc trước”. [2] Từ đó mà
“robot” dần vượt ra khỏi biên giới của sự hư cấu trừu tượng để trở thành niềm cảm
hứng thiết kế của những kỹ sư cơ khí với những cơ cấu, thiết bị mang dáng dấp con
người, được điều khiển và phục vụ trong cả đời sống và công nghiệp.
Mặc dù vậy, có một thực tế rằng robot được sử dụng chủ yếu nhất công
nghiệp, được gọi là robot công nghiệp. Về mặt kỹ thuật chúng có nguồn gốc từ hai
lĩnh vực đã phát triển từ lâu là cơ cấu điều khiển từ xa và các máy công cụ điều
khiển số. Các người thao tác được tách biệt khỏi môi trường làm việc vì những lý
do chẳng hạn như an toàn hoặc chế tạo chi tiết. Những robot đầu tiên là sự nối kết
giữa các thiết bị điều khiển cơ khí từ xa và khả năng lập trình của các máy điều
khiển. Tuy vậy ngày nay robot công nghiệp phải có các kết cấu, chức năng phải
phù hợp với những quy chuẩn và định nghĩa được quy định ngày nay. Trong đó
một định nghĩa được sử dụng nhiều nhất có thể kể đến được viện nghiên cứu Robot
của Mỹ đề xuất: “Robot công nghiệp là tay máy vạn năng hoạt động theo chương
trình và có thể lập trình lại để hoàn thành và nâng cao hiện quả hoàn thành các
nhiệm vụ khác nhau trong công nghiệp, như vận chuyển nguyên vật liệu, chi tiết,

6
sản phầm, dụng cụ hoặc các thiết bị chuyên dụng khác”. Một robot cần phải thể
hiện được 2 đặc trưng cơ bản là thiết bị vạn năng và có thể lập trình được thì mới
được gọi là robot công nghiệp. Ngoài ra robot còn phải đảm bảo chức năng điều
khiển được trong quá trình sản xuất. Một robot công nghiệp bao gồm các thiết bị
thừa hành như tay máy và có thể lập trình hoặc điều khiển đề hoàn thành chức năng
vận động. Với các đặc trưng trên robot công nghiệp là thiết bị tự động hóa khả trình
mà không thể thiếu trong các hệ thống sản xuất linh hoạt hiện nay.
Robotics phát triển mạnh mẽ từ những năm sau chiến tranh thế giới thứ hai
với nhiều thành tự lớn như robot T3 có thể nâng khối lượng lên đến 40Kg vào
những năm 1974 của công ty Cincinnati đến từ Mỹ. Hay hiện nay có dự án nghiên
cứu robot trong chiến tranh mô phỏng những con chó gồ hàng hoặc người lính của
công ty Boston Dynamic. Trong lĩnh vực nghiên cứu, năm 1967, Đại học Stanford
đã cho ra đời một mẫu robot trang bị các cảm biến để có thể nhận biết và định
hướng đối tượng vật kẹp trên bàn kép. Ngoài ra còn nhiều lĩnh vực được ứng dụng
robot nhất là công nghiệp vũ trụ, chế tạo xe, máy móc,...
Ở Việt Nam, kỹ thuật robot mới bắt đầu được du nhập nhiều vào ngành công
nghiệp nước nhà từ năm 1990 khi các cơ sở công nghiệp bắt đầu đổi mới quy trình
và nhập khẩu nhiều loại robot phục vụ các mục đích công nghiệp. Năm 2002, công
ty TOSY của giám đốc người Việt Nam thành lập và bắt đầu sản xuất robot giải trí,
công nghiệp đã đem đến nhiều giải thưởng trong và ngoài nước. Bên cạnh đó còn
có các đơn vị sản xuất robot nước ngoài có trụ sở tại Việt Nam như ABB. Ngày
12/10/2003, sự ra đời của Hội khoa học và công nghệ Robot Việt Nam
(Vietnamese Assiciation of Robotics – VAR) đã thúc đẩy lĩnh vực nghiên cứu và
ứng dụng Robot tại Việt Nam phát triển hơn nữa. Ngày nay hầu hết các trường kỹ
thuật tại Việt Nam đã trang bị môn kỹ thuật Robot vào các chương trình giảng dạy
chính giúp sinh viên tiếp cận tốt hơn và phát triển các khía cạnh như kỹ thuật điều
khiển, mô phỏng được phát triển tốt hơn.
Robot cánh tay công nghiệp là loại robot phổ biến nhất trong công nghiệp
hiện nay, vì thế nghiên cứu về nó giúp nhóm tác giả có thể tiếp cận tốt hơn những
gì đang diễn ra trong công nghiệp. Ngoài ra vì ngành robot vẫn chưa phát triển ở

7
Việt Nam nên nhóm tác giả muốn xây dựng một mô hình để sinh viên có thể thực
nghiệm và nghiên cứu tốt hơn, hỗ trợ tối đa cho giảng viên trong việc giảng dạy.

2.2.Mục tiêu, nội dung và phương pháp nghiên cứu

Từ những phân tích nêu trên, đề tài nghiên cứu tập trung thiết kế, xây dựng
và phát triển bộ điều khiển dùng cho mô hình Robot 4 bậc tự do.
Mục tiêu:
- Nghiên cứu cánh tay robot được dùng trong công nghiệp và giảng dạy
- Hoàn thành mô hình cánh tay robot dùng động cơ step
- Hoàn thiện giao diện điều khiển mô hình trên máy tính
- Phân tích, tính toán, lựa chọn giải thuật điều khiển thích hợp cho robot.
Nội dung nghiên cứu:
- Tìm hiểu về các loại tay máy được dùng nhiều trong công nghiệp và chọn
lựa mô hình 3D của tay máy dựa theo các open source.
- Tính toán lựa chọn các linh kiện cho tay máy bao gồm động cơ, vi điều
khiển, driver, các chi tiết cơ khí… và lắp ráp sản phẩm thực tế.
- Xác định không gian làm việc của tay máy
- Tính toán các phương trình động học và động lực học cho tay máy.
- Thiết lập giải thuật điều khiển vận tốc theo dạng hình thang cho động cơ step
- Kiểm tra lại các thuật toán và phương trình bằng mô phỏng trên Matlab
- Kiểm tra thuật toán trên mô hình robot đã xây dựng,
- Xây dựng giao diện và các chế độ điều khiển cho tay máy bằng LabView,
- Đánh giá sản phẩm hoàn thành với một số tiêu chí đề ra,
- Chạy thử Robot cho một vài ứng dụng thực tế.
Phương pháp nghiên cứu:
- Thu thập, tham khảo và phân tích các tài liệu về các lĩnh vực, đề tài nghiên
cứu liên quan, và các linh kiện thiết bị được sử dụng,
- Nghiên cứu, chỉnh sửa lại Robot cho phù hợp yêu cầu đặt ra,
- Tìm hiểu về kỹ thuật lập trình, xây dựng giải thuật và áp dụng các phần mềm
lập trình thông dụng,

8
 - Kiểm tra thực nghiệm thiết robot trong môi trường thực tế,
- Tổng hợp, phân tích, đánh giá kết quả đạt được,
- So sánh và đánh giá sản phẩm hoàn thành với một số thiết bị có sẵn trên thị
trường.

2.3.Đối tượng và phạm vi nghiên cứu

Trong nghiên cứu này, tác giả sẽ tập trung thiết kế và chế tạo một bộ điều khiển
dành cho Robot 4 bậc tự do sử dụng động cơ Step không phản hồi. Bộ điều khiển này
được phát triển dựa trên cơ sở các phương trình động học của Robot để đưa ra thuật toán.
Phần mềm Matlab dùng để mô phỏng, LabVIEW để điều khiển Robot bốn bậc tự do và
phần cơ khí để chế tạo cánh tay Robot. Tối ưu sản phẩm và ứng dụng các thuật toán trí
tuệ nhân tạo trong xử lý ảnh sẽ được nhóm tác giả tiếp tục nghiên cứu phát triển trong
thời gian tới.

9
3. CỞ SỞ LÝ THUYẾT
3.1.Cấu trúc robot và tay máy công nghiệp
Cấu trúc của một mô hình robot công nghiệp có hệ thống cơ cấu chấp hành
(Actuator) giúp tạo chuyển động cho các tay máy, có thể kế đến là các loại động cơ điện,
thủy lực, khí nén,... Hệ thống cảm biến (Sensor) gồm các sensor và thiết bị chuyển đổi tín
hiệu. Các cảm biến này giúp cho robot nhận biết được trạng thái của bản thân trong môi
trường xung quanh. Hệ thống điều khiển (Controller) thường là máy tính có khả năng tái
lập trình để người dùng có thể điều khiển tùy vào mục đích và kịch bản hoạt động. Cuối
cùng là tay máy (Manipulator), các cơ cấu cơ khí gồm các khâu, khớp giúp hoàn thiện
chuyển động.

Hình 1 Sơ đồ cấu trúc của robot công nghiệp

10
 Tay máy là thành phần quan trọng của robot có các đặc điểm chung về kết cấu gốc
các khâu được nối với nhau bằng các khớp để hình thành một chuỗi động học hở tính từ
thân đến công tác. Các khớp giúp cho robot chuyển động với hai chuyển động cơ bản là
tịnh tiến và quay. Trong thiết kế tay máy, người ta thường quan tầm các thông số hình –
động học ảnh hưởng khả năng làm việc như sức nâng, trường làm việc, số bậc tự do ảnh
hưởng đến sự khéo léo, tính ứng dụng của tay máy. Số lượng và cách sử dụng các khớp
tay máy có thể phân loại ra các kiểu tọa độ đề các, tọa độ trụ, tọa độ cầu, ... Thông số bậc
tự do đặc trưng cho khả năng linh hoạt của chuyển động, số bậc càng lớn robot càng linh
hoạt nhưng độ phức tạp lại tăng lên. Ngày nay tính toán các bậc tự do dựa vào định lý
Gruebler là phổ biến nhất, định lí này được phát biểu ở công thức (1.1)
g
f =λ .(n−1)−∑ ❑ ( λ−f i )−f 0 (1.1)
i=1

Trong đó:
- f là số bậc tự do
- λ là bậc tự do của một vật rắn không chịu liên kết trong không gian làm việc của
robot, λ = 6 tương ứng với không gian làm việc trong không gian
- n là số khâu
- f i là số bậc tự do khớp i
- g là tổng số khớp
- f 0 là số bậc tự do thừa

Vì thế mà tính toán số bậc tự do để lựa chọn robot phù hợp mục đích là một nhiệm
vụ quan trọng.
Ảnh hướng của số bậc tự do có thể kể đến trường công tác của robot. Hay còn gọi
là không gian làm việc, trường công tác là toàn bộ thể tích được quét bởi khâu chấp hành
cuối khi robot chuyển động.

11
 Hình 2 Trường công tác của robot
Robot được xem là chuỗi động học hở các khâu (links) gắn liền với các khớp
(joints). Một đầu của chuỗi nối với giá đỡ, đầu còn lại là phần cơ cấu chấp hành. Mỗi
khâu khớp hình thành nên một cặp khâu – khớp đảm bảo nó có khả năng chuyển động
nhất định. Các cặp khâu khớp được đặc trưng bởi hai thông số là tham số và biến khớp.
Tham số là thông số không thay đổi trong quá trình hoạt động, biến khớp là các thông số
thay đổi. Ta có thể đặt trên mỗi khâu của robot một hệ tọa độ và sử dụng phép biến đổi
thuần nhất để có thể mô tả mối quan hệ giữa các tọa độ này. Các biến đổi thuần nhất này
được mô tả dưới một ma trận A . Giả sử A1 mô tả vị trí và hướng của khâu thứ nhất, A21
mô tả vị trí và hướng của khâu thứ hai so với khâu thứ nhất nếu vậy ta có thể suy ra
A2= A 21∗A1 là vị trí và hướng của khâu thứ hai so với hệ tọa độ gốc.

3.2.Thiết lập hệ tọa độ khảo sát và lập bảng D – H

Bài toán đông học thuận nhằm mô tả vị trí và hướng của khâu tác động cuối dưới
dạng hàm số của các biến khớp. Một phương pháp có thể sử dụng là tính trực tiếp hình
học giải tích các phép biến đổi tọa độ. Tuy nhiên phương pháp này chỉ dành cho các cơ
cấu đơn giản. Các bài toán tổng quát cần một giải thuật tổng quát hơn, và một trong các
giải thuật đấy được xuất phát từ quy tắc Denavit – Hartenberg, được Denavit và
Hartenberg xây dựng vào năm 1955. Bài toán này yêu cầu đặt hệ tọa độ tương ứng với
từng cặp khâu – khớp. Hệ tọa độ gắn với khâu cơ bản gọi là hệ tọa độ chuẩn như hình 3
mô tả. Các hệ tọa độ trung gian gắn với các khâu động khác. Các hệ tọa độ cần phải tuân
theo quy tắc bàn tay phải để có thể gắn chiều các trục một cách chính xác. Dựa trên hệ tọa
độ này có thể mô tả các cặp khâu – khớp bằng các tham số và biến khớp, từ đó tính toán

12
vị trí và hướng của khâu tác động bằng một phương trình tổng quát cho bài toán động học
thuận này.

Hình 3 Các hệ tọa độ trên cánh tay robot

Quy ước hệ tọa độ theo Denavit – Hartenberg:
- Trục z i được chọn dọc theo trục của khớp thứ i+1
- Trục x i được xác định dọc theo đường vuông góc chung giữa trục khớp thứ i và
i+1
- Trục y i xac định theo quy tắc bàn tay phải
Góc chuẩn (base) của robot là khâu số 0, khâu thứ i được nối với khâu trước đó
bằng khớp i và đặc trưng bởi hai kích thước, độ dài pháp tuyến chung là a n và góc giữa
các trục trong mặt phẳng vuông góc với a n gọi là α n như hình 4 mô tả.

Hình 4 Chiều dài và góc của một khâu

Mỗi trục sẽ có hai pháp tuyến ứng với cặp khâu – khớp trước và sau. Vị trí tương
đối giữa hai cặp khâu – khớp liên tiếp được xác định bởi d n là khoảng cách giữa các pháp
tuyến dọc theo trục khớp và θn là góc giữa các pháp tuyến đo trong mặt phẳng vuông góc
với trục.

13
 Hình 5 Bộ thông số Denavit – Hartenberg
Vị trí tương đối giữa hai hệ tọa độ liên tiếp n và n−1 được mô tả với
- d n dịch chuyển tịnh tiến giữa hai đường vuông góc chung của hai trục, theo trục
z n−1 để x n−1 nằm trên mặt phẳng pháp tuyến của z n−1 chứa x n
- θn góc giữa hai đường vuông góc chung. Quay quanh trục z n−1 để trục x n−1 cùng
phương với x n
- a n tịnh tiến theo trục x n−1 để hai O trùng nhau
- α n quay quanh trục x n−1 để z n−1 trùng với z i

Như vậy đồng nhất từ hệ tọa độ n−1 qua hệ tọa độ n được thực hiện bởi các phép
tịnh tiến và quay các hệ trục tọa độ. Tương ứng với nó là các ma trận biến đổi thuần nhất
được gọi là H n−1
n

−1
H i−1=Trans z ( d n ) Rot z ( θn ) Trans x ( a n ) Rot x ( α n )
n−1 n−1 n−1 n−1

[ ]
cos ⁡θn −sin ⁡θn cos ⁡α n sin ⁡θn sin ⁡α n a n cos ⁡θn
sin ⁡θn cos ⁡θn cos ⁡α n −cos ⁡θn sin ⁡α n an sin ⁡θn
H n−1
n = (2.2)
0 sin ⁡α n cos ⁡α n dn
0 0 0 1

Từ ma trận biến đổi thuần nhất này ta dễ dàng tìm vị trí và hướng của khâu tác
động thứ i đối với base bằng cách nhân các ma trận thuần nhất lại như công thức (2.3)
như biểu diễn trên hình 6

14
 0 0 1 i−1
Di =H 1 H 2 … H i (2.3)
Việc tìm ra từng ma trận thuần nhất cũng dễ dàng hơn bằng cách điền vào bảng D
– H đủ các tham số mà một tay máy robot có như bảng 1
Khâu dn θn an αn

1
2
3
...
i

Bảng 1 Bảng tham số động học D – H

Hình 6 Sơ đồ chuyển vị của khâu cuối so với cơ sở

3.3.Vùng hoạt động của khâu tác động cuối

Vùng hoạt động được định nghĩa là một vùng mà trong đó tay máy có thể hoạt
động theo đúng chức năng của nó. Vùng hoạt động của robot phụ thuộc vào số lượng các

15
bậc tự do của robot, kích thước các cánh tay máy và được đại diện bởi vị trí của phần tác
động cuối đối với hệ toạ độ chung, ta gọi chung là vector ⃗p. Hướng của phần công tác này
được mô tả qua bộ tham số MRO gọi chung là vector σ⃗ . Vùng làm việc (Workspace) là
một thông số quan trọng của tay máy, và được phân ra hai loại là vùng làm việc không
tính đến sự định hướng (Reachable Workspace) và có tính đến sự định hướng của khâu
tác động cuối (Dexterous Workspace).
Reachable Workspace là vùng hoạt động mà khi không có các kết cấu như dây nối,
tay máy có thể với xa nhất hoặc vào gần nhất ở trong vùng đấy. Tuy nhiên trên thực tế,
các tay máy được cấp điện bằng các đường điện dẫn bên thân hoặc bên trong thân máy,
những dây điện này có thể quấn quanh thân máy trong quá trình hoạt động làm cho sự
hoạt động diễn ra không như ý. Vì thế mà Dexterous Workspace cần phải khảo sát để có
thể định hướng khâu tác động cuối trong khoảng cho phép của robot. Thể tích và hình
dạng của vùng làm việc phụ thuốc vào kết cấu và giới hạn của biến khớp. Đối với tay máy
có n bậc tự do, vùng làm việc là tập hợp mọi vị trí có thể của khâu tác động cuối được mô
tả bằng phương trình
p= p ( q ) ; qℑ ≤q ℑ ;i=1… n

Trong đó q ℑ và q ℑ là giới hạn dưới và trên của mỗi biến khớp

Ngoài ra vùng làm việc còn có các tính chất về cố giới hạn, khép kín và liên thông.

Hình 7 Mô tả không gian làm việc của cơ cấu hai khâu phẳng

16
 3.4.Bài toán động học ngược
Bài toán động học thuận đã giải quyết bài toán xác định phần cộng tác và vùng
làm việc khi có giá trị của các biến khớp. Tuy nhiên trong thực tế, việc điều khiển các
biến khớp có thể khó hiểu đối với người dùng. Việc sử dụng bài toán động học ngược
nhằm xác định các thông số động học khi đã biết tọa độ của khâu tác động cuối bên
trong vùng làm việc là cần thiết trong việc thiết kế điều khiển. Từ đó mà người sử dụng
có thể điều khiển khâu tác động cuối theo các tọa độ cho sẵn trong không gian làm việc.
Động học ngược có một tính chất gọi là tính đa nghiệm. Tính đa nghiệm này phụ
thuộc vào số lượng các biến khớp (số bậc tự do) và cả vào số lượng các tham số khác.
Khi số bậc tự do càng lớn nghiệm càng nhiều, robot có thể di chuyển theo nhiều quỹ đạo
khác nhau từ đó. Từ đó mà việc tìm kiếm nghiệm phù hợp để có thể tối ưu đường đi của
robot cũng là một bài toán để nghiên cứu. Tuy nhiên trong giới hạn nghiên cứu lần này
của nhóm tác giả, vì sử dụng robot với 3 bậc từ do (không tính khâu tác động cuối), bài
toán động học ngược giải ra chỉ có duy nhất một nghiệm.

17
4. PHÂN TÍCH THIẾT KẾ HỆ THỐNG
4.1.Ý tưởng thiết kế
Hình 2.1 thể hiện sơ đồ khối các thành phần chính của Robot. Sản phẩm bao gồm
ba động động cơ Step để điều khiển các khâu của robot, driver A4988 kết nối với động cơ
để điều khiển góc quay, board điều khiển Arduino Uno cho phép kết nối và xử lý tín hiệu
điều khiển được gửi từ một máy tính cá nhân qua Serial đến driver và nguồn điện cung
cấp năng lượng cho các khối có thể hoạt động.

Hình 8 Sơ đồ khối thiết kế Robot.

4.2.Phần cứng
4.2.1. Động cơ Step KV4234

Hình 9 Động cơ Step KV4234

18
 Do có khả năng cung cấp mô men xoắn lớn ở dải vận tốc trung bình và thấp và có
thể điều khiển chính xác góc quay nên KV4234 là một loại động cơ bước được sử dụng
nhiều trong các dự án về Robot hay các loại máy CNC, máy in 3D,… Nó hoạt động theo
cách chuyển đổi các chuỗi xung điện tại các cuộn kích của chúng thành trục quay cơ học.
Khi một chuỗi xung được gửi, động cơ sẽ bước theo xung. Góc mà trục động cơ bước
quay cho mỗi xung được gọi là góc bước. Số lượng xung vào quyết định góc bước. Góc
bước càng nhỏ thì số bước càng lớn. Các thông số kỹ thuật cơ bản của động cơ được
đưa ra trong Bảng 2.
Thông số Giá trị

Kích thước 42x42x34mm

Đường kính trục 5mm
Nguồn 2.76V
Dòng từng pha 1.2A

Điện trở từng pha 2.3 Ohm

Bước 1.8 độ

Pha 2

Mô men xoắn 260Nm

Quán tính 42gcm^2

Trọng lượng 210g

Bảng 2 Thông số hoạt động chính của động cơ Step KV4234

19
 4.2.2. Driver A4988

Hình 10 Driver A4988

A4988 là một trình điều khiển động cơ microstepping hoàn chỉnh với bộ phiên
dịch tích hợp sẵn để vận hành dễ dàng. Nó được thiết kế để vận hành động cơ bước lưỡng
cực ở các chế độ bước đầy đủ, nửa, phần tư, bước thứ tám và thứ mười sáu. A4988 bao
gồm một bộ điều chỉnh dòng điện thời gian cố định có khả năng hoạt động ở các chế độ
chậm hoặc hỗn hợp. Chỉ cần nhập một xung vào đầu vào STEP cho động cơ chạy một
bước. Không cần đến bảng thứ tự pha, đường điều khiển tần số cao hoặc giao diện phức
tạp để lập trình.. Các thông số kỹ thuật cơ bản của cảm biến được đưa ra trong
Bảng 3.
Thông số Giá trị

Nguồn điện áp nuôi 35 V

Dòng dầu ra 2A

Điện áp logic đầu vào -0.3-5.5V

Điện áp đầu ra motor -0.3-37V

Nhiệt độ hoạt động -20-85 độ C

Chu kỳ cấp xung tối thiểu 2us

20
 Chế độ điều khiển 1, 1/2, 1/4, 1/8, 1/16

Bảng 3 Thông số hoạt động chính của Driver A4988

4.2.3. Board điều khiển Arduino Uno

Hình 11 Arduino Uno

Arduino Uno là một bo mạch vi điều khiển nguồn mở dựa trên vi điều khiển
Microchip ATmega328P và được phát triển bởi Arduino.cc và được phát hành lần đầu
vào năm 2010. Bo mạch được trang bị bộ đầu vào/đầu ra kỹ thuật số và tương tự (I/ O) có
thể được giao tiếp với các bảng mở rộng khác nhau (shiled) và các mạch khác. Bo mạch
có 14 chân I/O kỹ thuật số (sáu chân có khả năng xuất PWM ), 6 chân I/O tương tự. Đi
cùng với nó là một môi trường phát triển tích hợp (IDE) chạy trên các máy tính cá nhân
thông thường và cho phép người dùng viết các chương trình cho mạch bằng ngôn ngữ
Arduino, một ngôn ngữ riêng được phát triển dựa trên C/C++. thông qua cáp USB loại B.
Nó có thể được cấp nguồn bằng cáp USB hoặc bằng pin 9 vôn bên ngoài , mặc dù nó chấp
nhận điện áp từ 7 đến 20 vôn. Nó tương tự như Arduino Nano và Leonardo. Các thông số
cơ bản của Arduino Uno được đưa ra trong bảng 4.

21
 Thông số Giá trị

Chip ATmega328

Điện áp cấp nguồn 5V

Điện áp đầu vào (input)

7-12V
(kiến nghị )

Điện áp đầu vào(giới hạn) 6-20V

14 (có 6 chân điều chế độ rộng xung

Số chân Digital I/O
PWM)

Số chân Analog (Input ) 6

DC Current per I/O Pin 40 Ma

DC Current for 3.3V Pin 50 mA

32KB(ATmega328)với 0.5KB sử dụng

Flash Memory
bootloader

SRAM 2 KB (ATmega328)

EEPROM 1 KB (ATmega328)

Xung nhịp 16 MHz

Bảng 4 Các thông số kỹ thuật chính của Arduino Uno

22
 4.2.4. Board CNC Shield V3

Hình 12 Board CNC Shield V3

Mạch CNC Shield V3 là board mở rộng của Arduino UNO R3 được sử dụng kết
hợp với Driver động cơ bước (A4988, DRV8825,...) giúp bạn có thể dễ dàng điều khiển
động cơ bước, công tắc hành trình để setup hệ thống điều khiển trong các ứng dụng CNC
cỡ nhỏ 1 cách dễ dàng, Arduino CNC Shield V3 có 4 khay dùng để cắm các mô đun điều
khiển động cơ bước A4988, khi đó board có thể điều khiển 3 trục X, Y, Z và thêm một
trục thứ 4 tùy chọn và có cách sử dụng đơn giản, chất lượng gia công tốt, độ bền cao. Các
thông số cơ bản của module được đưa ra trong Bảng 5.
Thông số Giá trị
Trục hỗ trợ 4
Driver tương thích A4988/ DRV8825
Điện áp đầu vào(giới hạn) 12-36V
Điện áp đầu vào(giới hạn) 6-20V
Điều chỉnh độ phân giải 1, 1/2, 1/4, 1/8, 1/16

Bảng 5 Các thông số kỹ thuật chính của Board CNC Shield V3

23
 4.2.5. Servo MG90S

Hình 13 Servo MG90S

Động cơ RC Servo MG90S là phiên bản nâng cấp của động cơ RC Servo 9G với
các bánh răng được làm bằng kim loại cho lực khéo khỏe và độ bền cao, động cơ có kích
thước nhỏ gọn, cách điều khiển giống như các động cơ RC Servo phổ biến trên thị trường
hiện nay: MG996, MG995, 9G,...Phù hợp cho nhiều ứng dụng khác nhau: Robot cánh tay
máy, robot nhện, cơ cấu chuyển hướng, cơ cấu quay góc,... Các thông số cơ bản của
module được đưa ra trong Bảng 6
Thông số Giá trị
Điện áp hoạt động 4.8 ~ 6VDC
Mô men xoắn 1.8kg/cm(4.8V ),2.2kg/cm(6V)
Tốc độ quay 600 ~ 750 độ/s
Bánh răng Kim loại
Trọng lượng 13.4g

Bảng 6 Các thông số kỹ thuật chính của Servo MG90S

24
 4.3.Phần mềm
4.3.1. Arduino IDE
Phần mềm Arduino IDE được sử dụng để soạn thảo code, kiểm tra lỗi và upload
code cho Arduino Uno.

Hình 14 Giao diện phần mềm lập trình Arduino IDE

Một số chức năng chính của phần mềm được nêu ra dưới đây.
Arduino Toolbar có một số button và chức năng của chúng như sau :

- Verify : kiểm tra code có lỗi hay không

- Upload: nạp code đang soạn thảo vào Arduino

- New, Open, Save: Tạo mới, mở và Save sketch

25
 - Serial Monitor: màn hình hiển thị dữ liệu từ Arduino gửi lên máy tính

Arduino IDE Menu:

Trong thanh Menu, chúng ta quan tâm tới mục Examples đây là nơi chứa code mẫu
ví dụ như: cách sử dụng các chân digital, analog, sensor …

Sketch menu:
- Verify/ Compile: chức năng kiểm tra lỗi code.
- Show Sketch Folder: hiển thị nơi code được lưu.
- Add File: thêm vào một Tap code mới.
- Import Library: thêm thư viện cho IDE

Tool menu:
- Board: lựa chọn bo mạch cho phù hợp với loại bo sử dụng
- Serial Port: đây là nơi lựa chọn cổng COM. Khi chúng ta cài đặt driver thì máy
tính sẽ hiện thông báo tên cổng Com của là bao nhiêu, ta chỉ việc vào Serial Port
chọn đúng cổng Com để nạp code, nếu chọn sai thì không thể nạp code cho ESP
được.

4.3.2. LabVIEW
LabVIEW là một môi trường lập trình đồ họa của National Instrument có thể sử
dụng để tạo các ứng dụng với giao diện người dùng chuyên nghiệp một cách nhanh chóng
và hiệu quả. Hàng triệu kỹ sư và nhà khoa học sử dụng LabVIEW để phát triển các ứng
dụng đo lường, kiểm thử, và điều khiển tinh vi bằng cách sử dụng các biểu tượng trực
quan và dây nối tín hiệu. Ngoài ra, LabVIEW có thể được mở rộng cho nhiều nền tảng
phẩn cứng và hệ điều hành khác nhau. Trong thực tế, nền tảng LabVIEW có khả năng tích
hợp với hàng nghìn thiết bị phần cứng và cung cấp hàng trăm thư viện được xây dựng sẵn
để phân tích nâng cao và hiển thị dữ liệu giúp tạo ra các thiết bị ảo có thể tùy chỉnh theo
26
nhu cầu của người dùng. LabVIEW đã tích hợp sẵn các chức năng cho các tác vụ phổ
biến như thu thập và phân tích dữ liệu, cho đến các chức năng chuyên biệt hơn cho các
ứng dụng như thiết kế điều khiển, mô phỏng hoặc thiết kế RF.

Hình 15 Giao diện phần mềm lập trình LabVIEW

LabVIEW được dùng nhiều trong các phòng thí nghiệm, lĩnh vực khoa học kỹ
thuật như tự động hóa, điều khiển, điện tử, cơ điện tử, hàng không, hóa sinh, điện tử y
sinh,... Hiện tại ngoài phiên bản LabVIEW cho các hệ điều hành Windows, Linux, Hãng
NI đã phát triển các mô-đun LabVIEW cho máy hỗ trợ cá nhân (PDA). Các chức năng
chính của LabVIEW có thể tóm tắt như sau:
 Thu thập tín hiệu từ các thiết bị bên ngoài như cảm biến nhiệt độ, hình ảnh
từ webcam, vận tốc của động cơ,...
 Giao tiếp với các thiết bị ngoại vi thông qua nhiều chuẩn giao tiếp thông qua các
cổng giao tiếp: RS232, RS485, USB, PCI, Ethernet
 Mô phỏng và xử lý các tín hiệu thu nhận được để phục vụ các mục đích nghiên cứu
hay mục đích của hệ thống mà người lập trình mong muốn
 Xây dựng các giao diện người dùng một cách nhanh chóng và thẩm mỹ hơn nhiều
so với các ngôn ngữ khác như Visual Basic, Matlab,..
 Cho phép thực hiện các thuật toán điều khiển như PID, Logic mờ (Fuzzy Logic),
một cách nhanh chóng thông qua các chức năng tích hợp sẵn trong LabVIEW.
 Cho phép kết hợp với nhiều ngôn ngữ lập trình truyền thống như C, C++,...

27
 4.3.3. MATLAB
MATLAB (viết tắt của MATrix LABoratory) là phần mềm cung cấp môi trường
tính toán số và lập trình, do công ty MathWorks thiết kế. MATLAB cho phép tính toán số
với ma trận, vẽ đồ thị hàm số hay biểu đồ thông tin, thực hiện thuật toán, tạo các giao diện
người dùng và liên kết với những chương trình máy tính viết trên nhiều ngôn ngữ lập
trình khác. Chương trình MATLAB triển khai ngôn ngữ MATLAB và cung cấp một thư
viện rất phong phú các hàm được xác định trước để thực hiện các tác vụ lập trình kỹ thuật
dễ dàng và hiệu quả hơn. Các chức năng cực kỳ đa dạng này giúp giải quyết các vấn đề
kỹ thuật trong MATLAB dễ dàng hơn nhiều so với các ngôn ngữ khác như Fortran hoặc
C. Với thư viện Toolbox, MATLAB cho phép mô phỏng tính toán, thực nghiệm nhiều mô
hình trong thực tế và kỹ thuật.
Các chức năng chính của MATLAB có thể kể đến như:
• Tự động hoàn thành câu lệnh
• Hỗ trợ các dòng lệnh tích hợp
• Không gian làm việc chia thành nhiều cột khác nhau
• Phát hiện lỗi cú pháp thời gian thực và sửa nhanh
• Nhận biết từ giống nhau
• Hỗ trợ file excel:
• Chức năng gỡ lỗi
• Hỗ trợ nhiều phím tắt (có thể tùy chỉnh theo ý thích)

28
 Hình 16 Giao diện phần mềm lập trình MATLAB
Bằng các tính năng đa dạng, phần mềm MATLAB có thể được ứng dụng trong
nhiều trường hợp. Một vài ví dụ có thể kể đến như:
 Thiết kế, mô phỏng và thử nghiệm hệ thống lái xe tự động.
 Phân tích, lập mô hình dữ liệu và hệ thống sinh học.
 Thiết kế, kiểm tra và triển khai các hệ thống điều khiển.
 Khám phá dữ liệu, phân tích dự đoán để áp dụng trong máy học (Machine
learning).
 Phát triển và triển khai phần mềm giám sát và dự đoán bảo trì.

4.4.Giải thuật điều khiển cho động cơ Step

Theo nguyên lý hoạt động của động cơ bước và driver, khi ta cấp cho driver một
xung, tương ứng động cơ sẽ quay một góc 1.8 độ (với loại động cơ đã chọn).khi đó, nếu
chu kỳ của xung là cố định, ta sẽ có đồ thị góc quay và vận tốc như hình sau:

29
 100

position (degree)
50

0
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
Time (seconds)
velocity (degree/s)

0.04

0.02

0
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
Time (seconds)

Hình 17 Đồ thị góc quay và vận tốc với chu kỳ cấp xung cố định
Tại giá trị khi bắt đầu cấp xung, vận tốc tăng đột ngột từ 0 đến w trong khoảng thời
gian cực ngắn. Chu kỳ cấp xung càng nhỏ vận tốc của động cơ càng cao. Khi đó gia tốc
góc được tính bằng đạo hàm của w sẽ là một số rất lớn khiến cho mômen cần thiết tại
động cơ cũng tăng cao theo, dẫn đến việc những động cơ vừa và nhỏ không đáp ứng
được. Bên cạnh đó nếu động cơ có thể chịu được mômen xoắn gây ra tại các điểm khởi
động này thì lại gây nên hiện tượng “jerk” khiến cho robot không chạy mượt mà. Điều
này dẫn đến việc cần phải thiết kế một thuật toán điều khiển vận tốc và gia tốc cho động
cơ để không gây ra các bất lợi trên mà vẫn đạt đượt thời gian chuyển động mong muốn.
Trong đồ án này, quỹ đạo chuyển động của động cơ là dạng 2-1-2, hay còn gọi là
quỹ đạo theo quy luật vận tốc hình thang. [3] Đây chính là quỹ đạo thường được dùng
trong các robot công nghiệp. Quỹ đạo này chia làm ba phần rõ rệt:
- Khởi động (tăng tốc) với gia tốc không đổi.
- Chuyển động tiếp với vận tốc không đổi.
- Về địch (giảm tốc) với gia tốc không đổi.

30
 Hình 18 Qũy đạo mong muốn của động cơ
Thời gian trễ t giữa các xung động cơ bước điều khiển tốc độ. Những khoảng
thời gian trễ này phải được tính toán để làm cho tốc độ của động cơ bước theo sát
đường dốc tốc độ nhất có thể.
Gọi t t là khoảng thời gian mà ngắt timer xảy ra trong vi điều khiển,
α là góc quay nhỏ nhất mà động cơ có thể đạt được
θ là góc quay của động cơ
ω là vận tốc góc của động cơ
ω̇ là gia tốc góc của động cơ

Ta có tốc độ góc của động cơ tại một thời điểm t bất kỳ là:
t
ω ( t )= ∫ ω̇ dτ =ω̇ t
τ =0

Vị trí của động cơ tại thời điểm đó:

t
1
θ ( t )= ∫ ω( τ)dτ = ω̇ t 2
τ =0 2

Đặt θ=nα ta có:

31
 1 2
nα = ω̇ t
2
Khoảng thời gian của xung thứ n tại thời điểm θ=nα là:

t n=
√ 2 nα
ω̇
Do đó khoảng thời gian giữa hai bước của động cơ là:

c n t t =t n+1−t n=
√ 2 nα (
ω̇
√n+1−√ n )

Với c n là số tầm timer nhảy vào ngắt, hay còn gọi là counter ta có:

c n=
√
1 2nα
t t ω̇
( √ n+1−√ n )

Gọi c 0 là counter đầu tiên ta có:

c 0=
√
1 2α
t t ω̇
⇒ c n=c 0 ( √ n+1−√ n )

Do khả năng tính toán của một bộ vi điều khiển bị hạn chế và việc tính toán hai
căn bậc hai rất tốn thời gian. Do đó, ta cần phải xấp xỉ với độ phức tạp của c n thành
dạng đơn giản hơn.
Sử dụng xấp xỉ bằng chuỗi Taylor ta có:

√ 1
n
1 1
2n 8 n
1
1± =1 ± − 2 +O 3
n ( )
⇒
cn
=
c ( √ n+1−√ n )
0
=
1+ −
1
2n 8n
1
+O
( n )
1
−1
=
2
4 n−1
3

c ( √ n− √ n−1 )
( 2n 8 n ( n ))
c n−1 0 1 1 1 4 n+1
1− 1− − +O 2 3

Cuối cùng, biểu thức cho bộ counter có thể được tính gần đúng như sau:
2c n−1
c n=c n−1 −
4 n+1
Phép tính này nhanh hơn nhiều so với căn bậc hai, nhưng đưa ra sai số 0,44 tại
n = 1. Ta bù cho lỗi này bằng cách nhân c ❑0 với 0,676.

32
 4.5.Giải thuật điều khiển cho động cơ Servo
Chế độ hoạt động servo được hình thành bởi những hệ thống hồi tiếp vòng kín.
Động cơ servo nhận một tín hiệu xung điện (PWM) từ bộ điều khiển để hoạt động và
được kiểm soát bằng bộ mã hóa (encoder).
Khi động cơ vận hành thì vận tốc và vị trí sẽ được hồi tiếp về mạch điều khiển này
thông qua bộ mã hóa (encoder). Khi đó bất kỳ lý do nào ngăn cản chuyển động và làm sai
lệch tốc độ cũng như vị trí mong muốn, cơ cấu hồi tiếp sẽ phản hồi tín hiệu về bộ điều
khiển. Từ tín hiệu phản hồi về, bộ điều khiển servo sẽ so sánh với tín hiệu lệnh và đưa ra
điều chỉnh phù hợp, đảm bảo động cơ servo hoạt động đúng theo yêu cầu đạt được tốc độ
và vị trí chính xác nhất.

Hình 19 Nguyên lý hoạt động của động cơ servo

Theo datasheet của nhà sản xuất, khi ta cấp xung có tần số 50Hz và khoảng
Duty Cycle bằng 1.5ms thì động cơ sẽ quay góc 0 độ. Tương tự với Duty Cycle bằng
2ms thì động cơ sẽ quay góc 90 độ. Ta có lưu đồ thuật toán với đầu ra out là tín hiệu
xung cấp cho động cơ như hình 2.15:

33
 Hình 20 Thuật toán điều khiển động cơ servo
Giả sử t t là thời gian để timer trong vi điểu khiển xảy ra ngắt, α là góc quay của
động cơ, ta tính đượcgiá trị của high_time và cycle_time như sau:

(
high time= 1.5+ α ×
2−1.5
90−0
×
1
)
tt
−3
20∗10
cycle time=
tt

34
4.6.Thành lập các phương trình động học
4.6.1. Xây dựng bài toán động học thuận tay máy

Hình 21 Hệ tọa độ trên tay máy 4 bậc tự do

Thiết lậpbbn thông số D-H
Khâu di θi ai αi

1 l1 q1 0 π/2

2 0 q2 l2 0

3 0 q3 l3 0

4 0 q4 l4 0

35
 Bảng 7 Thông số D-H của robot

Sau khi đã thành lập bảng D-H, ta tiến hành xác định các ma trận biển đổi tọa độ
thuần nhất.
Ma trận biến đổi từ hệ tọa độ 0 sang hệ tọa độ 1:

[ ]
cos ⁡(q 1) 0 sin ⁡(q 1) 0
0 sin ⁡( q 1) 0 −cos ⁡(q 1) 0
H 1=
0 1 1 l1
0 0 0 1

Ma trận biến đổi từ hệ tọa độ 1 sang hệ tọa độ 2:

[ ]
cos ⁡( q 2) −sin ⁡(q 2) 0 l 2∗cos ⁡( q 2)
H 12= sin ⁡( q 2) cos ⁡(q 2) 0 l2∗sin ⁡(q 2)
0 0 1 0
0 0 0 1

Ma trận biến đổi từ hệ tọa độ 2 sang hệ tọa độ 3:

[ ]
cos ⁡(q 3)−sin ⁡(q 3) 0 l 3∗cos ⁡(q 3)
2 sin ⁡( q 3) cos ⁡(q 3) 0 l 3∗sin ⁡(q 3)
H 3=
0 0 1 0
0 0 0 1

Ma trận biến đổi từ hệ tọa độ 3 sang hệ tọa độ 4:

[ ]
cos ⁡(q 4) −sin ⁡(q 4) 0 l 4∗cos ⁡(q 4)
3 sin ⁡(q 4) cos ⁡( q 4 ) 0 l 4∗sin ⁡( q 4 )
H4=
0 0 1 0
0 0 0 1

   Ma trận biến đổi thuần nhất giữa hệ tọa độ 0 và hệ tọa độ gắn với điểm tác động
là:

[ ]
σ 3 cos(q 1) −σ 1 cos ⁡(q 1) sin ⁡(q 1) cos ⁡(q 1)σ 2
0 0 1 2 3 σ sin ⁡( q 1) −σ 1 sin ⁡(q 1) −cos ⁡(q 1) sin ⁡(q 1)σ 2
→ H 4=H 1∗H 2∗H 3∗H 4 = 3
σ1 σ3 0 l 1 +l 3 sin ( q 2+ q 3 ) +l 2 sin ( q 2 )+l 4 σ 1
0 0 0 1

36
Với :
σ 3=cos ( q 2+ q 3+q 4 )

σ 1=sin ⁡( q 2+q 3+q 4)

σ 2=l 3 cos ( q 3+q 2 ) +l 2 cos ( q 2 ) +l 4 σ 3

Theo cấu tạo robot q 4=−q 2−q 3, thay vào ma trận ta có:

[ ]
cos( q 1) 0 sin ⁡(q 1) cos ( q 1 ) [l 4 +l 3 cos ( q 3+ q 2 ) +l 2 cos ( q 2 ) ]
0 sin ⁡(q 1) 0 −cos ⁡(q 1) sin ( q 1 ) [l 4 +l 3 cos ( q 3+ q 2 )+ l2 cos ( q 2 )]
H4=
σ1 1 0 l 1+ l 3 sin ( q 3+ q 2 )+ l 2 sin ⁡(q 2)
0 0 0 1

   
Phương trình động học tay máy xác định vị trí điểm tác động cuối:

{
x p =[l 4 +l 3 cos ⁡(q 3+q 2)+l 2 cos ⁡(q 2)]cos ⁡(q 1)
y p=[l 4 +l 3 cos ( q 3+ q 2 ) +l 2 cos ( q 2 ) ]sin ⁡(q 1) (2.1)
z p=l 1 +l 3 sin ( q 3+ q 2 ) +l 2 sin ⁡(q 2)

37
4.6.2. Xây dựng bài toán động học ngược tay máy

Hình 22 Động học ngược tay máy trong không gian Oxyz
Ta có:

q 1=arctan
( )
yp
xp

Xét mặt phẳng z O O1 z ❑O ::

1

38
 Hình 23 Động học ngược tay máy trong mặt phẳng z O O1 z ❑O : 1

q 2=q 23+q 32=arctan

( O1 z p
z O −l 4
1
)
+arctan ⁡¿

(√ )
z p−l 1
⇒q 2=arctan +arctan ⁡¿
x + y 2p−l 4
2
p

Từ (2.1) :

{
x−l 4∗cos ⁡(q 1)=[l 3∗cos ⁡( q 3+q 2)+l 2∗cos ⁡(q 2)]cos ⁡(q 1)
⇒ y−l 4∗sin ⁡(q 1)=[l 3∗cos ⁡(q 3+ q 2)+l 2∗cos ⁡(q 2)]sin ⁡(q 1)
z−l 1=l 3∗sin ( q 3+q 2 )+l 2∗sin ⁡(q 2)
2 2 2 2
⇒ [ x−l 4 cos ( q 1 ) ] + [ y−l 4 sin ( q 1 ) ] + ( z−l 1 ) = [l 3 cos ( q 3+ q 2 ) +l 2 cos ( q 2 ) ] + [ l 3 sin ( q 3+ q 2 ) +l 2 sin ( q 2 ) ] =
2

2 2
[ x−l4 cos ( q 1 ) ] + [ y −l4 sin ( q 1 ) ] + ( z−l1 )2−l22−l23
⇒ cos ( q 3 )=
2l 2 l3

( )
2 2
[ x−l 4 cos ( q 1 ) ] + [ y−l4 sin ( q 1 ) ] + ( z−l1 )2 −l22−l23
⇒ q 3=arccos ⁡
2 l2 l3

Ta có phương trình động học ngược:

¿

39
5. TRIỂN KHAI TRÊN THỰC NGHIỆM VÀ KÊT QUẢ
5.1.Thiết kế và lắp ráp robot
Trong đồ án này, mẫu robot được làm mô hình theo là IRB 460. Mẫu 3D của mô
hình này được lấy theo một open source (bỏ tham khảo vào chỗ này) sau đó được chỉnh
sửa khâu tác động cuối thành động cơ servo để giảm thiểu khối lượng.

Hình 24 Robot IBR 460

40
 Hình 25 Mẫu robot được sử dụng
Về phần bộ điều khiển sẽ được ghép nối theo sơ đồ

Hình 26 Sơ đồ ghép nối mạch điều khiển

41
5.2.Kiểm tra thuật toán điều khiển bằng mô phỏng
Như vậy từ công thức nêu trong phần 2.4, có thể thấy khoảng thời gian giữa hai
lần cấp xung liên tiếp phụ thuộc vào ba thông số t t , ω̇ , α . Trong đó α là tham số cố
định phụ thuộc vào động cơ (xem trong phần phụ lục), gia tốc góc ω̇ do người dùng
cài đặt, còn khoảng thời gian mà ngắt timer xảy ra trong vi điều khiển t t phụ thuộc vào
tốc độ quay tối đa của động cơ và thời gian tính toán khi trong trạng thái ngắt của vi
điều khiến.
Sau khi kiểm tra lại thuật toán bằng Matlab với các thông số trong bảng dưới, ta
có đồ thị sau:
Tham số Giá trị
α 0.45 °
2
ω̇ 1200 ° /s
ω max 1500 ° /s 2
tt 100 μs

42
 position (degree)
2000

0
0 0.5 1 1.5 2 2.5 3 3.5
Time (seconds)
velocity (degree/s)

2000

1000

0
0 0.5 1 1.5 2 2.5 3 3.5
Time (seconds)
accelator (degree2 /s)

1000

-1000

0 0.5 1 1.5 2 2.5 3 3.5

Time (seconds)

Hình 27 Kết quả mô phỏng thuật toán điều khiển động cơ

Theo như kết quả được thể hiện trên đồ thị ta thấy thuật toán hoạt động tốt khi
cho ra kết quả vận tốc có dạng bậc thang và tại các điểm khởi động, giá trị của gia tốc
góc không bị tăng vọt lên quá lớn và nhanh chóng ổn định sau khoảng thời gian rất
ngắn
Với các giá trị nhập vào là các góc quay của Robot kết hợp với bài toán động
học, ta có thể thiết kế cho Robot di chuyển theo như mong muốn như trong hình 28
với đường màu đỏ là đường tham chiếu, đường đường màu xanh là đường đi của khâu
tác động cuối của robot.

43
 Hình 28 Kết quả mô phỏng Robot trong không gian ba chiều

5.3.Vùng làm việc của robot

Dựa vào các phương trình động học thuận và bảng giá trị thông số từng biến khớp
của robot (xem trong phần phụ lục), ta có thể tính ra không gian làm việc của Robot như
trong hình 29.

44
 Hình 29. Vùng làm việc của robot

5.4.Thuật toán trên Arduino

Phần sụn trên Arduino có nhiệm vụ chính là nhận tín hiệu điều khiển từ LabVIEW
bao gồm các góc quay của động cơ và điều khiển Robot sao cho đạt được trạng thái đặt
của tín hiệu điều khiển. Cấu trúc thuật toán như sau:

45
 Hình 30 Cấu trúc thuật toán trên Arduino
Chương trình chính có tác dụng cấu hình cho các ngoại vi và ngắt, sau đó đọc lệnh
gửi đến từ Serial để chọn chế độ điều khiển phù hợp. Nếu cần điều khiển động cơ step,
timer1 bắt đầu chạy để tính toán các khoảng thời gian trễ cần thiết giữa hai lần cấp xung
bằng giải thuật trong phần 2.4. Bằng cách thử nghiệm, nhóm tác giả nhận thấy với chu kỳ
lặp lại là =100μs, động cơ hoạt động đúng như mong muốn. Bên cạnh đó timer2 được
dùng để cấp xung liên tiếp với khoảng thời gian duty cycle được tính như trong phần 2.5
chu kỳ lặp lại là =16μs.

5.5.Ứng dụng điều khiển trên LabVIEW

LabVIEW là một phần mềm thiết kế giao diện và lập trình dựa trên ngôn ngữ đồ
họa được sử dụng rộng rãi và phổ biến trong các lĩnh vực đo lường dữ liệu, điều khiển
thiết bị và tự động hóa trong công nghiệp. LabVIEW hỗ trợ nhiều module dùng để giao
tiếp với các thiết bị ngoại vi một cách đơn giản và chính xác nhất. Giao diện lập trình đồ
họa, diễn đạt cú pháp thông qua các hình ảnh trực quan giúp môi trường soạn thảo trực
quan hơn trong quá trình thiết kế thuật toán điều khiển.

46
 Bên dưới hình 8 mô tả lưu đồ thuật toán của quá trình điều khiển sử dụng
LabVIEW. Trong phần mềm này, người dùng có thể lựa chọn hai chế độ điều khiển là
Auto và Manual. Ở chế độ Manual người dùng có thể điều khiển thông qua các nút nhấn
điều hướng có trên hình 9 hoặc người dùng có thể điều khiển bằng cách nhập tọa độ
x , y , z hoặc góc của các khớp q 1 , q 2 , q 3 , q 4 và sử dụng lệnh “Move to point”. Ở chế độ
Auto người dùng không thể điều khiển thông qua các nút điều hướng mà chỉ có thể nhập
các giá trị vào các ô text và sử dụng nút “Move to point” để điều khiển. Sau khi nhấn
Run, LabVIEW sẽ gửi một lệnh giúp robot nhận biết được nó bắt đầu phải chạy tự động,
LabVIEW sẽ load ra những điểm nó đã lưu và bắt đầu ghi vào Arduino. Giao diện này
cũng cung cấp việc lưu các điểm dữ liệu đã cài đặt vào một file trên máy tính, giúp thuận
tiện cho việc tái sử dụng sau này. Ngoài ra thì chức năng mở file cũng được trang bị để
load dữ liệu từ file text ra bộ nhớ lệnh trong LabVIEW.

Hình 31 Mô tả hoạt động của phần mềm điều khiển trên labVIEW
Phương trình động học thuận và phương trình động học ngược cũng được tính toán
trong phần mềm điều khiển, giúp cho việc đồng bộ khi người dùng nhập điều khiển thông
qua tọa độ khâu cuối hoặc giá trị của góc quay. Các khối xử lý này sử dụng Module
Formula Node được hỗ trợ bởi LabVIEW giúp quá trình tính toán trở nên dễ dàng hơn,
mô tả của khối xuất hiện trong hình 10.

47
 Hình 32 Giao diện điều khiển trên phần mềm LabVIEW

Hình 33 Khối xử lý phương trình động học thuận (bên trái) và nghịch (bên
phải)
Giao tiếp giữa Arduino và LabVIEW sẽ thông qua cổng Serial. Tốc độ của cổng
Serial đủ nhanh để đảm bảo dữ liệu được ghi và đọc một cách kịp thời. Dữ liệu điều khiển

48
được chuyển lên là các câu lệnh theo định dạng được quy định trên board Arduino tương
ứng với từng hành vi mà Robot sẽ hoạt động. Arduino cũng sẽ trả về một lệnh để xác thực
rằng nó đã nhận được mệnh lệnh điều khiển từ LabVIEW. LabVIEW sẽ sử dụng module
VISA Read để ghi lại những dữ liệu này

5.6.Robot và ứng dụng thực tế

Hình ảnh phiên bản đầu tiên của thiết bị đo được đưa ra trong Hình 34. Các thành
phần chính của bộ điều khiển được lắp ráp bên trong hộp nhựa. Phần khung robot chế tạo
bởi công nghệ in 3D, sử dụng chất liệu nhựa in PLA. Các động cơ được lắp đặt trên thân
robot dựa theo những thông số đã chọn và được nối dây sao cho không làm ảnh hưởng
đến quá trình hoạt động của robot khi đang chạy trong vùng làm việc. Để cấp nguồn cho
robot, người dùng có thể sử dụng đầu nguồn 12V.
Một số hình ảnh ứng dụng của robot khi chạy lượt được thể hiện trên Hình 35 và
36. Các thông số đo về bị trí của khâu tác động cuối cũng như góc quay của từng động cơ
đã được hiển thị trên giao diện phần mềm LabVIEW giúp người dùng có thể dễ dàng điều
khiển và thao tác. Robot có thể được ứng dụng nhiều trong các phòng thí nghiệm hóa chất
để di chuyển các ống ngiệm mà không làm đổ dung dịch ra ngoài, trong các mô hình dây
chuyền bang tải vừa và nhỏ giúp gắp thả vật theo đúng nơi quy định hay hoạt động trong
môi trường độc hại mà con người khó có thể làm việc. Bên cạnh đó, ta còn có thể sử dụng
robot trong môi trường giáo dục để giảng dạy, tạo cảm giác hứng thú cho người học với
bộ môn điều khiển robot. Robot sẽ tiếp tục được nghiên cứu hoàn thiện để nâng cao độ
chính xác, tăng thẩm mỹ, và hạ giá thành.

49
 Hình 34 Cấu trúc thuật toán trên Arduino

Hình 35 Hình ảnh thử nghiệm robot di chuyển không giật

50
Hình 36 Hình ảnh robot gắp thả vật theo tọa độ đặt bởi người dùng

51
6. KẾT LUẬN VÀ KIẾN NGHỊ
Kết luận:
Đề tài nghiên cứu đã hoàn thành một số nội dung chính sau đây:
- Thiết kế và lắp ráp được một phiên bản đầu tiên robot bốn bậc tự do, có thể ứng
dụng trong các phòng thí nghiệm hoặc giảng dạy.
- Xây dựng được giải thuật điều khiển vận tốc theo dạng hình thang cho động cơ
step.
- Hoàn thành mô phỏng robot trên MATLAB
- Xây dựng được giao diện điều khiển cho robot bằng LabVIEW
- Bên cạnh đó, đề tài cũng đã giúp cho tác giả củng cố các kiến thức, hiểu biết và
vận dụng một số phương pháp, phần mềm kỹ thuật, như quy trình chế tạo sản phẩm điện
tử, lập trình vi điều khiển, in 3D… thông qua thu thập, phân tích, tổng hợp, đánh giá các
tài liệu tham khảo về chủ đề nghiên cứu và các linh kiện, thiết bị đã được sử dụng.
Kiến nghị:
Tác giả mong muốn được tiếp tục thực hiện đề tài để phát triển và tối ưu sản phẩm,
bổ sung thêm phần ứng dụng trí tuệ nhân tạo kết hợp xử lý ảnh để giúp robot trở nên
thông minh hơn, chính xác hơn và có nhiều ứng dụng thực tế hơn.

52
 TÀI LIỆU THAM KHẢO

[ J. M. Jordan, “The Czech Play That Gave Us the Word ‘Robot’,” The MIT
1] Press Reader, 19 July 2019. [Trực tuyến]. Available:
https://thereader.mitpress.mit.edu/origin-word-robot-rur/#:~:text=The%20word
%20itself%20derives%20from,were%20neither%20metallic%20nor%20mechanical..
[Đã truy cập 27 December 2022].
[2] A. Tikkanen., “Article History: Three laws of robotics,” Britannica, [Trực
tuyến]. Available: https://www.britannica.com/topic/Three-Laws-of-Robotics.
[3] K. Minh, Giáo trình Robot công nghiệp, Hanoi: Khoa học kỹ thuật, 2019.

53
 Phụ lục I: tính toán động lực học cho robot
- Tính động năng tay máy
Tay máy gồm 4 khâu, tổng động năng tay máy là:
K= K 1+ K 2+ K 3+ K 4
Trong đó:
K 1 : động năng của khâu 1 quay quanh trục cố định z 0
1 2 1 2
K 1= J 0 ω 1= J 0 q̇1
2 2
K 2 : động năng của khâu 2 quay quanh trục di động z 1
1 1
K 2= J C ω22 + m2 v 2C
2 2 2 2

Với:
vC2 =xC2 + y 2C + z 2C

[ ][ ][ ]
2 2 2 2

0 1
x
cos ( q 1 ) 0 sin ( q 1 ) 0 x C
C2 2
0 1
y 0 0 yC
= sin ( q 1 ) −cos ( q 1 )
0 0 1
r C =H 1 r C ⇔ C2 2
2
z 0 1
2 0
1 l 1 z1
C2 C
0
2

1 0 0 1 1

[ ][ ][ ]
x 0C
cos ⁡( q 1) 0 sin ⁡(q 1) 0 r 2 cos ⁡(q 2)
2

y 0C
sin ⁡( q 1) 0 −cos ⁡(q 1) 0 r 2 sin ⁡(q 2)
⇔ 0 = 2

zC 0 1 1 l1 0
0
2

1 0 0 1 1

[ ][ ]
0
xCr 2 cos (q 1) cos( q2 )
2

y 0C
⇔ 0 = r 2 cos(q 2) sin( q1)
2

zC 2
l 1 +r 2 sin(q 2)
1 1

[ ][ ]
ẋ 0C
−q̇1 r 2 cos(q 2) sin( q1)−q̇ 2 r 2 cos( q1 )sin(q2 )
2
0
⇔ ẏ = q̇1 r 2 cos(q 1)cos( q2 )−q˙2 r 2 sin(q1)sin(q2 )
C2

ż 0
C2
q̇2 r 2 cos(q2)

[]
2
( ẋ 0C ) 2

( ẏ 0C ) =¿
2
⇔ 2

2
( ż 0C ) 2

54
 2 2
⇔ v 2C =( q̇1 r 2 cos ( q 2) sin ( q 1 ) + q̇ 2 r 2 cos(q1 )sin(q2 ) ) + ( q˙1 r 2 cos(q1 )cos (q 2)− q̇2 r 2 sin (q1 )sin (q 2) ) +
2

K 3 : động năng của khâu 3 quay quanh trục di động z 2

1 2 1 2
K 3= J C ω3 + m3 v C
2 2 3 3

Với:
2 2 2 2
vC =x C + y C + z C
3 3 3 3

2 2
2 q˙1 r 3 cos(2 ( q 2+ q3 ) ) l 2 q˙1 cos (2 q2 )
2 2 2 2 2 2
2 q̇ r 1 32 2 2 2 l 2 q˙1 2 2
⇔v = C3 + q̇2 r 3 + q̇3 r 3 + +l 2 q˙2 +2 q˙2 q˙3 r 3 + + +
2 2 2 2
K 4 : động năng của khâu 4 quay quanh trục di động z 3
1 2 1 2
K 4 = J C ω 4+ m 4 v C
2 2 4 4

Với:
vC2 =x 2C + y 2C + z 2C
4 4 4 4

2 2
⇔ v 2C =( l 3 cos ( ( q2 +q3 ) ) ( q̇ 2+ q̇3 ) +l 2 q2 cos( q2 )) + ( cos (q1 ) σ 1+ q1 sin(q 1)σ 2 ) + ( sin (q1 ) σ 1−q˙1 cos (q
4

Trong đó:
σ 1=l 3 sin ( ( q2 +q 3 ) ) ( q˙2+ q̇3 ) +l 2 q̇2 sin ( q 2)
σ 2=r 4 +l 3 cos ( ( q 2+ q3 ) )+ l 2 cos (q2 )

- Tính thế năng tay máy

P=P1 + P2 + P3 + P4 =m1 g r 1 +m2 g [ r 2 sin ( q 2) + l1 ] + m3 g ¿

- Tính các lực không thế:

¿
Q 1=M m −b1 q̇1 1

¿
Q 2=M m −b2 q̇2 2

¿
Q =M m −b3 q̇3
3 3

¿
Q 4 =M m −b4 q˙4 4

Trong đó M m , M m , M m , M m là mômen tại các khớp 1, 2, 3 và 4

1 2 3 4

b 1 , b2 , b3 , b4 là hệ số cản nhớt tại các khớp 1, 2, 3 và 4

- Phương trình Lagarange biểu diễn động lực học tay máy:

55
 dt ∂ q̇i( )
d ∂K ∂K ∂P
− =
∂ qi ∂ qi
¿
+Q i (3)

- Tính các đạo hàm riêng

∂K
=0
∂ q1
∂ K −1
dq 1 (l 3 m4 sin (2 q2 +2 q3 )+m3 r 3 sin(2 q2 +2 q 3)+l 2 m3 sin(2 q2 )+l 2 m 4 sin(2 q 2)+m2 r 2 sin(2 q 2
2 2 2 2 2 2
=
∂ q2 2
2 2 2 2 2
∂ K −dq1 l 3 m4 sin(2 q 2+ 2q 3) dq 1 m 3 r 3 sin(2 q 2+2 q 3) 2 dq1 l 2 l 3 m4 sin (q3
= − −dq 1 l 3 m4 r 4 sin(q 2+ q3 )−
∂ q3 2 2 2
∂K
=0
∂ q4

( )
d ∂ K −1
dt ∂ q̇1
=
2
dq (2 dq 2 l 2 m3 sin(2 q2 )+2 dq2 l 2 m4 sin(2 q2 )+2 dq2 m2 r 2 sin (2 q2 )+l 3 m4 sin(2 q2 +2 q 3)2
1
2 2 2 2

dt ( ∂ q̇ )
d ∂K 2 2 2
=−dq l l m sin(q )−dq l m r sin( q )−dq l m r sin(q )−2 dq dq l l m sin (q )−2 dq
3 2 3 4 3 2 2 4 4 2 3 2 3 3 3 2 3 2 3 4 3
2

dt ( ∂ q̇ )
d ∂K 2
=−dq l m r sin(q )−dq dq l l m sin(q )−dq dq l m r sin (q )−dq l m r sin (q +q ) ( d
2 2 4 4 2 2 3 2 3 4 3 2 3 2 3 3 3 2 3 4 4 2 3
3

dt ( ∂ q˙ )
d ∂K
=−m r (l sin(q + q ) ( dq +dq ) + dq l sin( q ))
2 2
4 4 3 2 3 2 3 2 2 2
4

∂P
=0
∂ q1
∂P
=g m4 ( l 3 cos (q 2+ q3 )+ l 2 cos (q 2) ) + g m3 ( r 3 cos (q 2+ q3 )+ l2 cos (q2 ) ) + g m2 r 2 cos( q2 )
∂ q2

∂P
=g cos (q2 +q 3) ( l 3 m4 +m3 r 3 )
∂ q3
∂P
=0
∂ q4

Thay tất cả vào phương trình (3):

Khâu 1:

Mm =1
d ∂K
( )−
∂K ∂P
−
dt ∂ q˙1 ∂ q1 ∂ q1
+ b1 q̇1=
−1
2
dq ( 2 dq2 l 2 m3 sin(2q 2)+2 dq 2 l 2 m 4 sin(2q 2)+2 dq 2 m 2 r 2 sin(2 q
1
2 2 2

56
 Khâu 2:

Mm =
2 ( )
d ∂K
−
∂K ∂P
−
dt ∂ q˙2 ∂ q2 ∂ q2
+ b2 q̇2=−dq32 l 2 l 3 m4 sin ( q3 ) −dq22 l2 m4 r 4 sin ( q 2 )−dq32 l 2 m 3 r 3 sin ( q 3 )−2 dq2

Khâu 3:

Mm =
3
( )
d ∂K
−
∂K ∂P
−
dt ∂ q˙3 ∂ q3 ∂ q3
+b3 q˙3=−dq 22 l 2 m4 r 4 sin ( q2 ) −dq2 dq 3 l 2 l 3 m 4 sin ( q3 ) −dq 2 dq 3 l 2 m3 r 3 sin ( q 3 )−

Khâu 4:

Mm =
4 ( )
d ∂K
−
∂ K ∂P
−
dt ∂ q̇4 ∂ q4 ∂ q 4
+ b4 q˙4 =−m4 r 4 ( l 3 sin ( q 2+ q3 ) ( dq 2+ dq3 ) +dq 22 l 2 sin ( q2 ) )+ b4 q˙3 +b4 q˙2
2

Do trong công thức tính mômen tại các khớp có sự xuất hiện của hệ số ma sát,
trong mô phỏng ta sẽ giả sử các giá trị này bằng 0, còn khi làm mô hình phần cứng, các
chỉ số này không kiểm soát được do nhóm tác giả gặp hạn chế về thiết kế cơ khí nên khối
lượng vật nâng tối đa trong mô hình thực sẽ được xác định bằng phương pháp thực
nghiệm.

Phụ lục II: Thiết kế và các thông số của robot

57
58
59
Thông số robot:
Tham số Giá trị
l1 150mm
l2 140mm

l3 140mm

l4 54mm

q1 -90~90 độ
q2 0~120 độ
q3 -136~33 độ
Khối lượng vật nâng tối đa 85g
Vận tốc tối đa 600 rpm
Gia tốc tối đa 2500 độ/sq(s)

60