1 1 0 90 40 2 2 170 0 0 3 3 40 0 0
Bài toán động học thuận:
Sau khi có bảng thơng số DH, nhóm tiến hành xác định bài toán động học thuận của robot. Bài toán cho trước các giá trị a, alpha tetal để tìm vị trí điểm P cuối cùng của khâu chấp hành.
Dạng tổng quát ma trận i sang i+1: Ai =
(3.2)
Để dễ nhìn và trình bày gọn hơn, qui ước:
Cos( 1), Cos( 2), Cos( 3) lần lượt là C1, C2, C3
Sin( 1), Sin( 2), Sin( 3) lần lượt là S1, S2, S3
Ma trận chuyển đổi từ hệ toạ độ 0 sang hệ toạ độ 1
T =
(3.3)
A1=
(3.4)
Cos( i) -Sin( i)×Cos( i) Sin( i)×Sin( i) ai×Cos( i)
Sin( i) Cos( i)×Cos( i) - Cos( i)×Sin( i) ai×Sin( i)
0 Sin( i) Cos( i) di
0 0 0 1
Cos( 1) -Sin( 1)×Cos( 1) Sin( 1)×Sin( 1) ai×Cos( 1)
Sin( 1) Cos( 1)×Cos( 1) - Cos( 1)×Sin( 1) ai×Sin( 1)
0 Sin( 1) Cos( 1) d1 0 0 0 1 C1 0 S1 0 S1 0 -C1 0 0 1 0 d1 0 0 0 1
Ma trận chuyển đổi từ hệ toạ độ 1 sang hệ toạ độ 2: T =
(3.5) A2 =
(3.6) Ma trận chuyển đổi từ hệ toạ độ 2 sang hệ toạ độ 3: T =
(3.7) A3 =
(3.8) Cos( 2) -Sin( 2)×Cos( 2) Sin( 2)×Sin( 2) a2×Cos( 2)
Sin( 2) Cos( 2)×Cos( 2) - Cos( 2)×Sin( 2) a2×Sin( 2)
0 Sin( 2) Cos( 2) d2 0 0 0 1 C2 -S2 0 a2×C2 S2 C2 0 a2×S2 0 0 1 0 0 0 0 1
Cos( 3) -Sin( 3)×Cos( 3) Sin( 3)×Sin( 3) ai×Cos( 3)
Sin( 3) Cos( 3)×Cos( 3) - Cos( 3)×Sin( 3) ai×Sin( 3)
0 Sin( 3) Cos( 3) d3 0 0 0 1 C3 -S3 0 a3×C3 S3 C3 0 a3×S3 0 0 1 0 0 0 0 1
Sau khi có ma trận các hệ toạ độ, nhóm ứng dụng phần mềm Matlab để tính ma trận tổng quát như hình dưới:
Hình 3. 42 Sử dụng matlab đê tính ma trận tổng quát Ta được ma trận tổng quát có dạng: T = (3.9) Trong đó: r11 = C1×C2×C3 – C1×S2×S3 r11 r12 r13 Px r21 r22 r23 Py r31 r32 r33 Pz 0 0 0 1
r12 = -C1×C2×S3 – C1×C3×S2 r13 = S1 r21 = C2×C3×S1 – S1×S2×S3 r22 = -C2×S1×S3 – C3×S1×S2 r23 = - C1 r31 = C2×S3 + C3×S2 r32 = C2×C3 - S2×S3 r33 = 0
Px = a2×C1×C2 – a3×C1×S2×S3 + a3×C1×C2×C3
Py = a2×C2×S1 – a3×S1×S2×S3 + a3×C2×C3×S1
Pz = d1 + a2×S2 + a3×C2×S3 + a3×C3×S2
Ta dùng hàm simplify trong matlab để rút gọn biểu thức Px Py Pz trong ma trận tổng quát T :
Với:
C23 = Cos( 2 + 3) (3.10)
S23 = Sin( 2 + 3) (3.11)
px = simplify(T03 (1,4)) (T03 là kí hiệu của ma trận T trong matlab)
(matlab sẽ rút gọn phần tử tại hàng thứ 1 cột thứ 4 của ma trận T, làm tương tự cho Py và Pz)
Ta được kết quả:
Px = C1×[ a3×C23 + a2×C2] (3.12)
Py = S1×[ a3×C23 + a2×C2] (3.13)
Pz = d1 + a3×S23 + a2×S2 (3.14)
Bài toán động học nghịch
Là bài tốn đi tìm các biến khớp để thoải mãn vị trí, hướng khâu cuối ( điểm P) cho trước. Nhóm sử dụng phương pháp đại số để tính:
Từ phương trình động học thuận:
= (3.15)
Ta nhân 2 vế cho ( là ma trận nghịch đảo của ), ta có thể dùng matlab để tìm
(3.16) Vì = 1 nên ta có phương trình
(3.17)
Sử dụng hàm simplify trong matlab ta rút gọn được các biểu thức n1, n2, n3 trong ma trận vế trái:
n1 = Px*C1 + Py*S1 (3.18)
n2 = Pz – d1 (3.19)
n3 = Px*S1 – Py*C1 (3.20)
Sử dụng hàm simplify trong matlab ta rút gọn được các biểu thức m1, m2, m3 trong ma trận vế phải: m1 = a3*C23 + a2*C2 m2 = a3*S23 + a2*S2 m3 = 0 Tính 1 Ta có : n3 = m3
→ PxC1 – PyS1 = 0 → → 1 = atan(Px,Py) Tính 3 n1 = m1 => n2 = m2 Đặt PxC1 + PyS1 là nx và Pz – d1
là ny rồi tiến hành bình phương 2 vế ta được:
=>
C ộng phương
trình (nx)2 cho phương trình (ny)2 ta được:
(nx)2 + (ny)2 = (a3)2[(C23)2 + (S23)2] + (a2)2[(C2)2 + (S2)2] + 2a3a2[C23C2 + S23S2]
Trong đó:
(C23)2 + (S23)2 = Cos( 2 + 3)2 + Sin( 2 + 3)2 = 1 (C2)2 + (S2)2 = Cos( 2)2 + Sin( 2)2 = 1
=> (nx)2 +(ny)2 = (a3)2 + (a2)2 + 2a3a2[C23C2 + S23S2] PxC1 + PyS1 = a3C23 + a2C2 (1)
Pz - d1 = a3S23 + a2S2(2)
(nx)2 = (a3C23 + a2C2)2
(ny)2 = (a3S23 + a2S2)2
(nx)2 = (a3)2(C23)2 + 2 a3C23a2C2 + (a2)2(C2)2
Tại đây, ta dùng hàm simplify trong matlab để rút gọn cụm C23C2 + S23S2 ta được C3:
=> (nx)2 +(ny)2 = (a3)2 + (a2)2 + 2a3a2C3
=> C3 =
Thay nx ny vào phương trình C3 với các phần tử trong nx ny là các giá trị ta đã tính tốn ở bước trước Mà Cos( 3)2 + Sin( 3)2 = 1 => Sin( 3) = => S3 = => 3 = atan 2(S3, C3) Tính 2 Từ hệ phương trình:
Tại đây, ta dùng expand trong matlab để triển khai C23 S23
Ta khai triển biểu thức, phương trình đầu ta đặt C2, phương trình sau ta đặt S2 làm nhân tử chung
Đầu tiên ta tìm C2
Thế S2 vào phương trình C2, ta được:
=> => =>
Chuyển vế và đặt C2 làm nhân tử chung:
Rút gọn
Tương tự tìm S2:
Từ hệ phương trình (**), thế C2 vào phương trình S2 ta được:
Biến đổi tương tự như tính C2 ta được:
tan 2 =
2 = )
Phần tool gắp hàng
Để đơn giản, nhóm sử dụng bộ giác hút chân khơng mini thay cho tool gắp hàng.
Hình 3. 44 Giác hút gắn trên cánh tay Robot
Qua khảo sát, trọng lượng của một chi tiết sản phẩm không quá 100g nên khơng cần lực hút q lớn. Vì thế nhóm sử dụng động cơ bơm hút chân không loại 5V.
Thông số kỹ thuật:
-Điện áp hoạt động: 3,7V-6V -Điện áp định mức: 6V
-Nguyên lý hoạt động: hút chân không (tự mồi bơm) -Áp suất: -60KPa -100KPa
-Lưu lượng khí: 3.2L/phút
-Lưu lượng nước bơm: 0.35L/phút
Để cánh tay robot có thể gắp nhả linh hoạt, nhóm sử dụng thêm một van điện từ 2 chiều mini để phục vụ thao tác nhả sản phẩm.
Van này hoạt động khi ta cấp nguồn điện cho cuộn dây, cửa số 2 sẽ đóng cho cửa số 1 thơng với 3 để xả khí lưu lại từ cửa 1.
Hình 3. 46Van điện từ mini 2 chiều
Thông số kĩ thuật
Điện áp hoạt động: 24V DC - Loại: Van điện từ 2 chiều - Công suất: 2.4W
- Áp suất chịu được: 750 mmhg - 2 đường ống vào/ ra
- Đường kính ống: 6mm - Loại : thường mở
Nhóm sử dụng Arduino điều khiển van này nên phải thêm modul relay trung gian 5v, sơ đồ bố trí như hình dưới.
1 1 2 2 3 3
Hình 3. 47 Bố trí van điện từ
Hình 3. 48 Sơ đồ đi dây điều khiển động cơ servo
3.2.7. Khối nguồn
Do hệ thống sử dụng nhiều loại tải với mức điện áp và dòng điện hoạt động khác nhau nên việc sử dụng một bộ nguồn chung cho tồn bộ hệ thống khơng đảm bảo về độ an tồn và tính ổn định cho hệ thống. Nhóm đề tài thay thế bằng việc sử dụng những bộ nguồn độc lập và được thi công sẵn, với ưu điểm đáp ứng được mức điện áp và dòng điện một cách ổn định, giá thành hợp lý và được bán rộng rãi trên thị trường…
Nhóm sử dụng bộ nguồn 24V DC – 10A để cấp cho khối xử lý trung tâm PLC, và các thiết bị ngoại vi được liệt kê như dưới đây: