BÀI TẬP LỚN KỸ THUẬT ROBOT: Tìm hiểu về robot Puma 500

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN ĐIỆN  BÁO CÁO BÀI TẬP LỚN Đề tài Tìm hiểu về robot Puma 500 Giảng viên hướng dẫn PGS TS Nguyễn Phạm Thục Anh Nhóm sinh viên thực hiện 6 Lớp 129135 Năm học 2021 – 2022 Mục lục Nội dung báo cáo Sinh viên thực hiện Trang 1 Giới thiệu về Robot Puma 500, các ứng dụng trong công nghiệp, kết cấu cơ khí, các thông số kĩ thuật cơ bản Nguyễn Hoàng long 20181611 1 3 2 Tính toán động học thuận vị trí Robot Xây dựng chương trình phần mềm trên matlab để nhập liệu và hiệ.

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

VIỆN ĐIỆN - -

BÁO CÁO BÀI TẬP LỚN

Đề tài: Tìm hiểu về robot Puma 500

Giảng viên hướng dẫn: PGS TS Nguyễn Phạm Thục Anh

Mục lục

1 Giới thiệu về Robot Puma 500,

các ứng dụng trong công nghiệp, kết

cấu cơ khí, các thông số kĩ thuật cơ

Robot Xây dựng chương trình phần

mềm trên matlab để nhập liệu và hiện

thị kết quả

Hoàng Minh Đức 20181402 Nguyễn Hương Ly 20181637 Phạm Yến linh 20181581

3-9

3 Tính toán ma trận Jacoby (thông qua

jH) và viết chương trình trên matlab

Hoàng Minh Đức 20181402 Nguyễn Hương Ly 20181637 Phạm Yến linh 20181581

9-15

4 Tính toán động học đảo vị trí Robot Tống Xuân Sơn 20186313

Vi Hồng Nam 20181671 Nguyễn Văn Sang 20181725

15-22

5 Thiết kế quỹ đạo chuyển động cho

các khớp của robot theo quỹ đạo bậc 3

Tống Xuân Sơn 20186313

Vi Hồng Nam 20181671 Nguyễn Văn Sang 20181725

1 Giới thiệu về Robot Puma 500, các ứng dụng trong công nghiệp, kết cấu cơ khí, các thông số kĩ thuật cơ bản

1.1 Giới thiệu về robot Puma 500

PUMA( P rogramable U niversal M achine for A ssembly hoặc P

rogramable U niversal for A rm)

Được phát triển bởi Giáo sư người

Mĩ Victor David Scheinman tại

công ty robot tiên phong Unimation

Vào những năm 1980 dựa trên

những thiết kế mà ông phát minh ra

khi còn học ở Đại học Stanford

Puma 500 là dòng seri có thể đạt tới

độ cao gần 2m sử dụng ngôn ngữ

lập trình val Thiết kế của nó gồm 2

phần chính: cánh tay cơ và hệ thống

điều khiển Chúng được kết nối với

nhau bằng một hoặc hai dây cáp lớn

nhiều dây dẫn Khi hai dây cáp

được sử dụng, một dây dẫn truyền

lực đến động cơ servo và phanh

trong khi dây thứ hai mang phản

hồi vị trí cho từng khớp trở lại hệ Unimate 500 PUMA (1983), thiếtbị điều thống điều khiển Máy tính điều khiển và thiết bị đầu cuối máy tính khiển dựa trên kiến trúc LSI-11 rất giống với máy tính PDP11 Hệ thống có chương trình khởi động và công cụ gỡ lỗi cơ bản được tải trên chip

ROM Hệ điều hành được tải từ bộ nhớ ngoài thông qua một cổng nối tiếp, thường là từ đĩa mềm

Bộ điều khiển cũng chứa bộ nguồn servo, bảng xử lý phản hồi tương tự và

kỹ thuật số, và hệ thống truyền động servo

1.2 Thông số kĩ thuật

Configuaration 6 degrees of freedom

Power Requirement 110-130 VAC, 50-60Hz,1500 Watts

Maximum static load 25 N

Maximum Straight Line Velocity 51cm/sec

92.2 cm to the flange

Các thông số cơ khí tính bằng inch

1.3 Ứng dụng Puma 500

Puma 500 được dùng trong các dây truyền sản suất gắp và di chuyển các vật

co khối lượng nhẹ như dây truyền sản xuất đồ uống, được dung trong các bản vẽ kĩ thuật với tốc độ và độ chính xác cao và còn nhiều ứng dụng khác

y2mate.com - Food

application with Puma 500 robot_480p.mp4

y2mate.com - Drawing with the PUMA Robot Arm_480p.mp4Video ứng dụng robot puma 500

2 Tính toán động học thuận vị trí Robot Xây dựng chương trình phần mềm trên matlab để nhập liệu và hiện thị kết quả

2.1 Phương pháp Danevit – Hartenberg (D-H)

Bước 1: Xác định số khớp và thanh nối của Robot

- n khớp (i=1->n): khớp quay + tịnh tiến

- (n+1) thanh nối: 0->n

- thanh nối 0 cố định

- thanh nối n gắn với khâu tác động cuối

- khớp i nối giữa thanh nối (i-1) và i

Bước 2: Khai báo các hệ trục tọa độ trên mỗi thanh nối OiXiYiZi (i=0->n)

- Khớp (i+1) tịnh tiến: Zi là trục mà theo nó khớp (i+1) trượt

- Khớp (i+1) quay: Zi là trục mà xung quanh nó khớp (i+1) quay

+ αi: góc vặn của thanh nối i

+ di: độ lệch với thanh nối (i-1)

+ qi: góc quay của thanh nối i

00cossin

00sincos

i i

i i

100

0010

0001

0100

0010

00

0

0cossin

0

0sincos

0

000

1

i i

i i

00

cossin

0

sinsin

coscos

cossin

cossin

sincos

sincos

i i

i

i i i i i

i i

i i i i i

i i

d a a

1

−

n n

0 0 0 0

0 0 0 0

0 0 0 0

z z z z

y y y y

x x x x

p a s n

p a s n

p a s n

Chú ý rằng, ma trận chuyển vị từ hệ i đến hệ i-1 là hàm của các biến khớp (nếu khớp thứ i là khớp quay) hoặc d (nếu khớp thứ i là khớp trượt)

Một cách tổng quát, quy tắc Denavit – Hartenberg cho phép tổ hợp các

ma trận chuyển vị riêng rẽ thành một mà trận chuyển vị thuần nhất, biểu diễn vị trí và hướng của khâu n so với khâu cơ sở

2.2 Phân tích động học của Robot PUMA

2.2.1 Động học thuận vị trí

Hình 2.2 Gắn hệ trục tọa độ cơ bản và các hệ tọa độ trung gian cho Robot

Áp dụng quy tắc Denavit – Hartenberg ta thu được bảng D-H, biểu diễn dưới bảng:

∗ 𝑐𝑜𝑠(𝑡3) − 𝑎3 ∗ 𝑐𝑜𝑠(𝑡1) ∗ 𝑠𝑖𝑛(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3)

py = 𝑑6 ∗ (𝑠𝑖𝑛(𝑡5) ∗ (𝑐𝑜𝑠(𝑡1) ∗ 𝑠𝑖𝑛(𝑡4) + 𝑐𝑜𝑠(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡4) ∗ 𝑠𝑖𝑛(𝑡1))

+ 𝑠𝑖𝑛(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡5) ∗ 𝑠𝑖𝑛(𝑡1)) − 𝑑2 ∗ 𝑐𝑜𝑠(𝑡1) + 𝑑4 ∗ 𝑠𝑖𝑛(𝑡2 + 𝑡3) ∗ 𝑠𝑖𝑛(𝑡1) + 𝑎2 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑠𝑖𝑛(𝑡1) + 𝑎3 ∗ 𝑐𝑜𝑠(𝑡2) ∗ 𝑐𝑜𝑠(𝑡3)

∗ 𝑠𝑖𝑛(𝑡1) − 𝑎3 ∗ 𝑠𝑖𝑛(𝑡1) ∗ 𝑠𝑖𝑛(𝑡2) ∗ 𝑠𝑖𝑛(𝑡3)

pz = 𝑑4 ∗ 𝑐𝑜𝑠(𝑡2 + 𝑡3) − 𝑎3 ∗ 𝑠𝑖𝑛(𝑡2 + 𝑡3) − 𝑎2 ∗ 𝑠𝑖𝑛(𝑡2) + 𝑑6 ∗ (𝑐𝑜𝑠(𝑡2

+ 𝑡3) ∗ 𝑐𝑜𝑠(𝑡5) − 𝑠𝑖𝑛(𝑡2 + 𝑡3) ∗ 𝑐𝑜𝑠(𝑡4) ∗ 𝑠𝑖𝑛(𝑡5))

3.Tính toán ma trận jacoby thông qua ma trận Jh và viết

chương trình trên matlab

3.1 Ma trận Jacoby

Các bước tính toán ma trận Jacoby sử dụng thuật toán ma trận JH

Bước 1: Xác định ma trận Ti

n (i=0->n-1)

Với Robot Puma, có 6 DOFs và cả 6 khớp đều là khớp quay, ta có:

Các hệ số của 0T6 được xác định ở phần động lực thuận vị trí

- Tiếp theo ta sẽ tính toán ma trận JH bằng việc tính các ma trận theo từng cột , tương ứng với từng khớp của robot

3.2 Viết chương trình Matlab tính ma trận Jacoby thông qua ma trận JH

3.2.1 Chương trình matlab tính toán động học thuận và ma trận JH

function[J_so]=kinematic(value_t1, value _t2, value _t3, value _t4, value _t5, value _6, value _a2, value _a3, value _d2, value _d4, value _d6)

syms thetadalpha

A = [cos(theta) -cos(alph)*sin(theta) sin(alph)*sin(theta) a*cos(theta)

sin(theta) cos(alph)*cos(theta) -sin(alph)*cos(theta) a*sin(theta)

J51=simplify(T06(3,2));

J61=simplify(T06(3,3));

J12=simplify(T16(2,1)*T16(1,4)-T16(1,1)*T16(2,4)); J22=simplify(T16(2,2)*T16(1,4)-T16(1,2)*T16(2,4)); J32=simplify(T16(2,3)*T16(1,4)-T16(1,3)*T16(2,4)); J42=simplify(T16(3,1));

J52=simplify(T16(3,2));

J62=simplify(T16(3,3));

J13=simplify(T26(2,1)*T26(1,4)-T26(1,1)*T26(2,4)); J23=simplify(T26(2,2)*T26(1,4)-T26(1,2)*T26(2,4)); J33=simplify(T26(2,3)*T26(1,4)-T26(1,3)*T26(2,4)); J43=simplify(T26(3,1));

J53=simplify(T26(3,2));

end

3.2 Giao diện chương trình động học thuận và ma trận jacoby

Hình 3.2 Giao diện chương trình tính toán động học thuận và ma trận Jacoby

4 ĐỘNG HỌC NGƯỢC VỊ TRÍ ROBOT PUMA

Ta có thể tách bài toán động học ngược thành hai bài toán đơn giản hơn là động học ngược vị trí và động học ngược hướng :

− Tìm vị trí giao điểm các trục cổ tay (tâm cổ tay)

− Tìm hướng của cổ tay

Ta sẽ biểu diễn hướng và vị trí của cổ tay, cổ tay sẽ được biểu diễn đối với hệ tọa độ cố định bên ngoài (world coordinate system – Hệ tọa độ 𝑂0) Ta phải giải bài toán trên đối với các ẩn 𝜃1, … , 𝜃6

Phương pháp giải :

Vị trí của tâm cổ tay RC xác định qua vị trí công cụ (The given tool position – so với hệ tọa độ 𝑂0) và phương của Tool pointing (Z6) Do đó vị trí của tâm cổ tay phụ thuộc vào 3 biến khớp đầu tiên

Các biến khớp 𝜃1, 𝜃2, 𝜃3 xác định từ ma trận định hướng cổ tay (The arm orientation) 𝑇30 và ma trận định hướng công cụ (The given tool orientation) 𝑇60 Vị trí C so với 𝑂0 sẽ bằng vị trí của C so với hệ tọa độ 𝑂3 nhân với ma trận biến đổi đồng nhất của 𝑂3 trong tọa độ 𝑂0 (𝑃𝑐0 = 𝑇30𝑃𝑐3) Tương tự vị trí C so với 𝑂0 sẽ bằng

vị trí C so với hệ tọa độ 𝑂6 nhân với ma trận biến đổi đồng nhất của 𝑂6 trong tọa độ

𝑂0 (𝑃𝑐0 = 𝑇60𝑃𝑐6)

Các biến khớp 𝜃4, 𝜃5, 𝜃6 xác định từ ma trận 𝑇63(𝜃1, 𝜃2, 𝜃3) và ma trận

𝑇63 = 𝑇30−1(𝜃1, 𝜃2, 𝜃3) ∗ 𝑇60

Ta phân thành 2 công việc chính như sau:

• Thiết lập ma trận vị trí 𝑃𝑐= cột cuối cùng của 𝑇30

1 2 3( ,  , ) và 𝑇60 để tìm ra các biến khớp    Tính 𝑇1, 2, 3 63 = 𝑇30−1𝑇60 sau khi đã thay giá trị của các biến khớp vào

1, 2, 3

   vào 𝑇30( ,  1 2, 3)

• So sánh 𝑇63 và 𝑇63(𝜃4, 𝜃5, 𝜃6) để rút ra   4, 5, 6

Ta giả sử C là điếm cổ tay máy

Ta chia bài toán thành 2 phần:

• Tìm vị trí giao điểm trục cổ tay

1 Bài toán tìm vị trí giao điểm trục cổ tay

Tọa độ C trong hệ tọa độ O6 là:

𝑃𝑐6 = [

00

−𝑑61]

Tọa độ C trong hệ tọa độ O0 là:

−𝑑61] = [

−𝑎𝑥𝑑6+ 𝑝𝑥

−𝑎𝑦𝑑6+ 𝑝𝑦

−𝑎𝑧𝑑6+ 𝑝𝑧1

00

𝑐230

−𝑎3𝑠23− 𝑎2𝑠2 1

𝑑41

00

𝑐230

−𝑎3𝑠23− 𝑎2𝑠2 1

] [

00

𝑑41]

2 Bài toán tìm hướng của cổ tay

Sau khi giải xong bài toán tìm vị trí giao điểm của các trục cổ tay ta đi đến bài toán tìm hướng sử dụng ma trận 𝑇63 để tính toán

𝑠5𝑠60

𝑐50

𝑑4+ 𝑑6𝑐51] (4.3)

Mặt khác 𝐴1𝐴2𝐴3𝐴4𝐴5𝐴6 = 𝑇60 ↔ 𝑇30𝑇63 = 𝑇60 ↔ 𝑇63 = 𝑇30−1𝑇60

Mà

𝑠6 = 𝑜𝑧𝑐23+ 𝑜𝑥𝑠23𝑐1+ 𝑜𝑦𝑠1𝑠23

𝑐6 = −𝑛𝑧𝑐23+ 𝑛𝑥𝑐1𝑠23+ 𝑛𝑦𝑠1𝑠23

𝑠5 → 𝜃4 = 𝐴𝑇𝐴𝑁2(𝑠4, 𝑐4) → 𝜃6 = 𝐴𝑇𝐴𝑁2(𝑠6, 𝑐6)

0

00

0

00

𝜃1 = 𝐴𝑇𝐴𝑁2(−𝑎𝑥𝑑6+ 𝑝𝑥, 𝑎𝑦𝑑6− 𝑝𝑦) + 𝐴𝑇𝐴𝑁2(±√(−𝑎𝑥𝑑6+ 𝑝𝑥)2+ (𝑎𝑦𝑑6− 𝑝𝑦)2− 𝑑22, 𝑑2)

𝜃2 = 𝐴𝑇𝐴𝑁2(𝑘1𝑀 − 𝑘2𝑁, 𝑘1𝑁 + 𝑘2𝑀)

𝜃3 = 𝐴𝑇𝐴𝑁2(2𝑑4𝑎2, 2𝑎2𝑎3) + 𝐴𝑇𝐴𝑁2(±√(2𝑎3𝑎2)2+ (2𝑑4𝑎2)2− 𝑋2, 𝑋)

𝜃4 = 𝐴𝑇𝐴𝑁2(𝑠4, 𝑐4)

𝜃5 = 𝐴𝑇𝐴𝑁2 (±√1 − 𝑘32, 𝑘3)

𝜃6 = 𝐴𝑇𝐴𝑁2(𝑠6, 𝑐6)

Từ những kết quả trên ta thay thông số thực tế của robot PUMA vào ta được bảng

kết quả như sau:

𝜃1 156.65 156.65 156.65 156.65 0.52 0.52 0.52 0.52

𝜃2 88.06 88.06 89.52 89.52 87.76 87.76 89.2 89.2

𝜃3 174.21 174.21 0.59 0.59 173.56 173.56 1.24 1.24

𝜃4 -2.74 177.35 -2.73 177.36 -0.0090 180.08 -0.0090 180.08

𝜃5 85.47 -85.47 88.48 -88.48 85.48 -85.48 88.47 -88.47

𝜃6 -179.87 0.22 -180.02 0.08 -180.09 0.0007 -180.09 0.00025

II THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CHO ROBOT

1 Giới thiệu và cơ sở thiết kế quỹ đạo

Thiết kế quỹ đạo chuyển động của robot có liên quan mật thiết đến bài toán điều

khiển robot di chuyển từ vị trí này sang vị trí khác trong không gian làm việc Đường

đi và quỹ đạo được thiết kế là đại lượng đặc trưng cho hệ thống điều khiển vị trí của

robot Do đó độ chính xác của quỹ đạo sẽ ảnh hưởng đến chất lượng di chuyển của

robot

Yêu cầu thiết kế quỹ đạo chuyển động của Robot là:

− Khâu chấp hành phải đảm bảo đi qua lần lượt các điểm trong không gian

làm việc hoặc di chuyển theo một quỹ đạo xác định

− Quỹ đạo của Robot phải là đường liên tục về vị trí trong một khoảng các

nhất định

− Không có bước nhảy về vận tốc, gia tốc

Quỹ đạo là các đường cong có dạng:

− Đa thức bậc 2: 𝑥(𝑡) = 𝑎 + 𝑏𝑡 + 𝑐𝑡2

− Đa thức bậc 3: 𝑥(𝑡) = 𝑎 + 𝑏𝑡 + 𝑐𝑡2+ 𝑑𝑡3

− Đa thức bậc cao: 𝑥(𝑡) = 𝑎 + 𝑏𝑡 + 𝑐𝑡2+ ⋯ + 𝑘𝑡𝑛

Ở đây ta sẽ sử dụng quỹ đạo dạng đa thức bậc 3 để thiết kế cho robot PUMA

2.Thiết kế quỹ đạo trong không gian khớp

Chọn hai điểm A, B bất kỳ trong không gian làm việc, biết tọa độ (Px,P y, Pz)

và hướng của các khâu thao tác Thiết kế quỹ đạo chuyển động bất kỳ từ A đến B Theo bài toán động học ta xác định được các biến khớp 𝜃1, 𝜃2, 𝜃3, 𝜃4, 𝜃5,𝜃6 tại A

và B

Chọn quỹ đạo thiết kế là hàm đa thức bậc 3 theo thời gian như sau:

𝜃𝑖(𝑡) = 𝑎𝑖 + 𝑏𝑖𝑡 + 𝑐𝑖𝑡2+ 𝑑𝑖𝑡3

Với: i =1÷6 tương ứng với 6 biến khớp

Ta được hệ phương trình sau:

Ta suy ra hệ số phương trình của quỹ đạo cho khâu 1 là:

𝜃1(𝑡) = 7,5 𝑡2− 0,833 𝑡3

𝜃̇1(𝑡) = 7,5.2 𝑡 − 0,833.3 𝑡2𝜃̈1(𝑡) = 7,5.2 − 0,833.3.2 𝑡

Đồ thị quỹ đạo chuyển động bậc 3 của khớp 1:

5.Điều khiển chuyển động cho robot theo thuật toán PID

5.1 Phương trình động lực học

Hình 1 Sơ đồ cấu trúc bộ điều khiển PID

5.2 Thiết kế bộ điều khiển PID

Với các tham số Kp=450, Ki=600, Kd=20

Dựa vào đặc điểm ảnh hưởng của các tham số tới đáp ứng đầu ra của

mô hình ta có thể chỉnh định các tham số sao cho đàu ra bám giá trị đặt và

đạt một số các tiêu chí về độ quá độ và thời gian đáp ứng cũng như sai lệch

tĩnh

6.1 Mô hình Robot

1.1 Mô hình 3D Robot trên SolidWork

Mô hình Robot PUMA 560 cùng các khớp được thiết kế trên Solidworks như các hình dưới

Hình 2 Khớp 1 của robot

6.XÂY DỰNG MÔ HÌNH ROBOT TRÊN

SIMMECHANICS/MATLAB

Hình 3 Khớp 2 của robot

Hình 4 Khớp 3 của robot

Hình 5 Khớp 4 của robot

Hình 6 Khớp 5 của robot

Hình 7 Khớp 6 của robot

Hình 8 Robot PUMA hoàn chỉnh

6.2 Sơ đồ khối của Robot trên Matlab Simulink

- Để tiến hành mô phỏng Robot sau khi đã vẽ bằng SolidWorks dựa trên Toolbox Simscape của Matlab, chúng ta cần cài đặt “Simscape Multibody Link” Ở đây ta chọn Simscape Multibody Link 6.1 dành cho R2019a (Matlab2019a)

- Sau khi download được hai file “smlink.r2019a.win64.zip” và “install_addon.m”,

ta mở Matlab để tiến hành cài đặt Mở thư mục chứa 2 file trên rồi gõ lệnh

“install_addon('smlink.r2019a.win64.zip')” để tiến hành cài dặt

- Khi quá trình cài đặt kết thúc, ta mở Solidworks lên click vào mục Add-Ins → Chọn Simscape Multibody Link

- Sau đó ở mục tool Tool chọn Simscape Multibody Link → Export→ Simscape Multibody… để tiến hành xuất Solidworks dưới dạng file xml

- Sau khi hoàn thành xuất file, ta mở Matlab, mở thư mục chứa file xml, ở cửa sổ Command gõ lệnh Smimport để xuất file xml sang file slx của Simulink

- Cuối cùng robot Puma sau khi chuyển sang Simulink ta được mô hình sau:

Hình 9 Robot PUMA Model trên Simulink

Tiếp theo ta đi thiết kế bộ điều khiển cho robot PUMA

6.3 Bộ điều khiển PID

Hình 10 Sơ đồ điều khiển PID

Mô hình trên được biểu diễn dựa trên phương trình sau

𝑀 = 𝐾𝑝𝐸 − 𝐾𝐷𝑄 + 𝐾𝐼∫ 𝐸 𝑑𝑡 Trong đó:

E là sai lệch giữa 𝑄𝑑 (Q đặt) hay 𝑄𝑟𝑒𝑓 (Q reference) như hình:

Với 𝐸 = 𝑄𝑑− 𝑄

𝑄𝑑 là Vị trí biến khớp mong muốn và 𝑄𝑑 = [45 90 120 90 135 180]𝑇

𝐾𝑝, 𝐾, 𝐾𝐼 lần lượt là các bộ số của bộ điều khiển PID

Hình 10 Sơ đồ Bộ điều khiển PID cho từng khớp

Hình 11 Sơ đồ Bộ điều khiển PID cho Robot PUMA

Hình 12 PUMA RBOT ARM với Bộ điều khiển PID đã thiết kế

1.4 Đồ thị đáp ứng đầu ra của các khớp

Hình 13 Đồ thị vị trí đáp ứng đầu ra của khớp 1

Hình 14 Đồ thị vị trí đáp ứng đầu ra của khớp 2

Hình 15 Đồ thị vị trí đáp ứng đầu ra của khớp 3

Hình 16 Đồ thị vị trí đáp ứng đầu ra của khớp 4

Hình 17 Đồ thị vị trí đáp ứng đầu ra của khớp 5

Hình 18 Đồ thị vị trí đáp ứng đầu ra của khớp 6

Hình 19 Sơ đồ cấu trúc bộ PID trên

Simulink