Báo cáo BTL Kỹ thuật Robot_Đề tài: Robot MH3BM​ (Full matlab + solid work)

Mục lục I Giới thiệu 3 I 1 Ứng dụng 3 I 2 Kết cấu 3 I 2 1 Đặc điểm chính 3 I 2 2 Thông số kỹ thuật 3 I 2 3 Thông số cơ khí 3 II Tính toán động học thuận vị trí 4 III Tính ma trận Jacoby 8 IV Động họ. I. Giới thiệu I.1. Ứng dụng Robot MH3BM được thiết kế để ứng dụng trong những công việc mà ở đó tải nhỏ và nhẹ ví dụ như hệ thống dọn dẹp tự động, dây chuyền sản xuất thuốc hay trong các quá trình thử nghiệm lâm sàng và pha chế dược phẩm. I.2. Kết cấu I.2.1. Đặc điểm chính  Robot có 6 bậc tự do, có độ linh hoạt cao  Kích thước nhỏ gọn, sử dụng điều hướng bằng điện và khí nén  Có thể gắn trên sàn, lên tường hoặc trần  Hỗ trợ các tiêu chuẩn MotoPlus và MotomanPlus  Có cấu trúc mở dễ dàng lập trình. I.2.2. Thông số kỹ thuật  Lớp phủ ngoài đặc biệt, bề mặt được xử lý, ốc vít làm bằng thép không gỉ.  Tiêu chuẩn bảo vệ IP65 cho phần thân và IP67 cho phần khớp  Hệ thống kết nối cáp và thủy lực ở phía dưới bề mặt cố định cũng được bảo vệ  Chiều rộng tối thiểu mặt phẳng để cố định robot là 200mm  Tải trọng quy định là 3 kg, tầm với theo dọc chiều robot là 804mm, theo ngang chiều robot là 532 mm  Có khả năng kết nối nhiều robot khác cùng lúc để tăng hiệu quả làm việc

Mục lục

I Giới thiệu 3

I.1 Ứng dụng 3

I.2 Kết cấu 3

I.2.1 Đặc điểm chính 3

I.2.2 Thông số kỹ thuật 3

I.2.3 Thông số cơ khí 3

II Tính toán động học thuận vị trí 4

III Tính ma trận Jacoby 8

IV Động học đảo vị trí 9

V Thiết kế quỹ đạo chuyển động bậc 3 cho từng biến khớp 10

VI Mô hình động lực học cho đối tượng trên Simmechanics 11

VI.1 Vẽ các chi tiết của robot trên SolidWorks 2010 11

VI.2 Xuất sang file Matlab 14

I Giới thiệu

I.1 Ứng dụng

Robot MH3BM được thiết kế để ứng dụng trong những công việc mà ở đó tải nhỏ và nhẹ ví dụ như hệ thống dọn dẹp tự động, dây chuyền sản xuất thuốc hay trong các quá trình thử nghiệm lâm sàng và pha chế dược phẩm

I.2 Kết cấu

I.2.1 Đặc điểm chính

 Robot có 6 bậc tự do, có độ linh hoạt cao

 Kích thước nhỏ gọn, sử dụng điều hướng bằng điện và khí nén

 Có thể gắn trên sàn, lên tường hoặc trần

 Hỗ trợ các tiêu chuẩn MotoPlus và MotomanPlus

 Có cấu trúc mở dễ dàng lập trình

I.2.2 Thông số kỹ thuật

 Lớp phủ ngoài đặc biệt, bề mặt được xử lý, ốc vít làm bằng thép không gỉ

 Tiêu chuẩn bảo vệ IP65 cho phần thân và IP67 cho phần khớp

 Hệ thống kết nối cáp và thủy lực ở phía dưới bề mặt cố định cũng được bảo vệ

 Chiều rộng tối thiểu mặt phẳng để cố định robot là 200mm

 Tải trọng quy định là 3 kg, tầm với theo dọc chiều robot là 804mm, theo ngang chiều robot là 532 mm

 Có khả năng kết nối nhiều robot khác cùng lúc để tăng hiệu quả làm việc

I.2.3 Thông số cơ khí

Hình chiếu mặt bên của robot

Hình chiếu mặt trên của robot MH3BM

Hình chiếu mặt trước của robot MH3BM

II Tính toán động học thuận vị trí

Bảng D-H:

i α i a i θ i d i

o

o

o

Áp dụng công thức:

T i i−1

=A i=Rot(z ,θ i).Trans(z, d i).Trans(x i , a i) Rot (x , a i)

Code Matlab:

function output = TinhA(al,a,t,d)

% Ma tran xac tinh huong va vi tri cua mot khau theo bang DH

X = [cos(t), -cos(al)*sin(t), sin(al)*sin(t), a*cos(t);

sin(t), cos(al)*cos(t), -sin(al)*cos(t), a*sin(t);

output = X;

syms t1 t2 t3 t4 t5 t6 a1 a2 a4 a6

% sym(pi/2) de ra dung so pi

A1 = TinhA(sym(pi/2), 0, t1, a1);

A2 = TinhA(0, a2, t2, 0);

A3 = TinhA(sym(pi/2), 0, t3 + sym(pi/2), 0);

A4 = TinhA(sym(-pi/2), 0, t4, a4);

A5 = TinhA(sym(pi/2), 0, t5, 0);

A6 = TinhA(0, 0, t6, a6);

→ T i i−1

%Tinh dong hoc thuan vi tri

T06 =

simplify(expand(A1*A2*A3*A4*A5*A6)); T16

= simplify(expand(A2*A3*A4*A5*A6)); T26 =

simplify(expand(A3*A4*A5*A6));

T36 =

simplify(expand(A4*A5*A6)); T46

= simplify(expand(A5*A6)); T56 =

simplify(expand(A6));

T06 =

III Tính ma trận Jacoby

Để tính ma trận Jacoby cho robot 6 bậc tự do ta sử dụng thuật toán tính toán ma trận J H Xét ma trận :

J n H=∂ H X

∂ Q =[∂ H p x

∂ H p x

∂ q n

∂ H θ z

∂ H θ z

∂ q n ]

Do cả 6 khớp đều là khớp quay nên ta có ma trận J H có các cột:

∂θ i =¿ ¿

Xác định ma trận J=[R n0 0

0 R n0]J H

Chương trình matlab :

function output = Jh(T1, T2, T3, T4, T5, T6)

%Tinh ma tran Jh

Y = [jh(T1), jh(T2), jh(T3), jh(T4), jh(T5), jh(T6)];

output = Y;

end

function result = jh(T)

nx = T(1,1); ny = T(2,1); nz = T(3,1);

ox = T(1,2); oy = T(2,2); oz = T(3,2);

ax = T(1,3); ay = T(2,3); az = T(3,3);

px = T(1,4); py = T(2,4);

X = [ny*px - nx*py;

oy*px - ox*py;

ay*px - ax*py;

nz;

oz;

az];

result = X;

end

%Tinh ma tran Jacobi Jh

JH = simplify(expand(Jh(T06, T16, T26, T36, T46, T56)));

% Dung ham expand() de khai trien ma tran

R06 = T06(1:3, 1:3);

J_1 = [ R06 [0] [0] [0]

[0] [0] [0] R06 ] ;

J = J_1*JH ;

IV Động học đảo vị trí

Giả sử biết trước ma trận :

T60

=A1A2A3A4A5A6=[n x o x a x p x

T60=A2 A3 A4A5 A6(¿)

Ta có : R 1(3,4) R 1(3,3) = a6 =L1(3,4) L1(3,3) ¿px s 1− p yc 1

Ta giải được 2 nghiệm q1 :

q1 = atan2(py-a6.ay;px-a6.az)

q1 = atan2(py-a6.ay;px-a6.az) + π

Sau khi tìm được q1 thì ta biết được ma trận L1

L1 =

L2 = L1.A 6−1 ; R2 =A2.A3.A4.A5

Dễ thấy

L2(1,4) =kx – a6.gx

L2(2,4) = ky – a6.gy

R2(1,4) = a4.c23 + a2.c2

R2(2,4) = a4.s23 + a2.s2

(a 4 c 23−kx+a 6 gx )2 + (a 4 s 23−ky +a 6 gy)2 = a 22

A.c23 + B.s23 = C

Do đó : q23 = atan2(B,A) + atan2 (±√A2+B2−C2 ,C)

q2 = atan2 (ky – a6.gy – a4.c23 , kx – a6.gx – a4.c23)

T36 =

Đặt L3 = A 4−1A 3−1A 2−1A 1−1T06 ; R3 = T46

Ta thấy :

h1y.c4-h1z.s4 = 0 nên q4 = atan2 (h1y , h1z) và q4 = (h1y, h1z) + π

c5 = h1z nên q5 = ± arccos(h1z)

q6 = atan2(f1y.c4 – f1x.s4 , g1y.c4 -g1x.4)

V Thiết kế quỹ đạo chuyển động bậc 3 cho từng biến khớp

- Công thức tổng quát trong trường hợp quỹ đạo bậc 3 là:

q=a3 t3+a2 t2+a1t+a0

- Ta sẽ xác định các hệ số a0, a1, a2, a3 sao cho thỏa mãn điều kiện giá trị góc, vận tốc đầu và cuối quỹ đạo

- Tương đương với việc giải hệ phương trình:

{a3θ3+a2θ2+a1θ+a0=θ0

a3t3f+a2t f2+a1t f+a0=θ f

3 a3θ2

+2 a2θ+a1=θ0'

3 a3t f2+2a2t f+a1=θ ' f

- Từ phương trình (1) và (3), dễ dàng suy ra: a0=θ0, a1=θ0' Từ đó thay ngược trở lại phương trình (2)

và (4), ta có:

{a3t3f+a2t f2=θ f−θ0' t f−θ0

3 a3t f2+2 a2t f=θ ' f−θ '0

- Đây là hệ phương trình bậc nhất 2 ẩn Giải hệ, ta có:

a2=3

t f2(θ f−θ0)−2 θ0'

θ f '

t f

t f3

(θ f−θ0)+1

t f2

(θ f '+θ0')

- Từ đó, ta có kết luận:

Xây dựng quỹ đạo cho từng biến khớp q i bằng hàm:

q i(t )=[−2t f3

(θ if−θ i 0)+1

t f2

(θ if ' +θ i 0 ' )]t3+[t3f2

(θ if−θ i 0)−1

t f(θ if '

+2θ i 0 ' )]t2+θ i 0 ' t+θ i 0

VI Mô hình động lực học cho đối tượng trên Simmechanics

VI.1 Vẽ các chi tiết của robot trên SolidWorks 2010

VI.2 Xuất sang file Matlab

Góc quay của các khớp được thiết lập trong Slider Gains.

Mô phỏng trên Matlab: