2.2.1. Ma trận jacobi
Việc tính toán ma trận Jacobi cho tay máy là một chủ đề nghiên cứu quan trọng. Tính quan trọng của ma trận Jacobi được thể hiện ở cả phân tích và điều khiển robot.
Ma trận Jacobi được viết dưới dạng tổng quát với robot n khớp như sau: J = JJ = [J J … J ] =
𝑧 × p 𝑧 × (p − p ) ⋯ 𝑧 × (p − p ) ⋯ 𝑧 × (p − p
𝑧 𝑧 ⋯ 𝑧 ⋯ 𝑧
(2-8 ) Viết dưới dạng khai triển ta được:
J = JJ = ⎣ ⎢ ⎢ ⎢ ⎢ ⎡JJ JJ JJ JJ JJ JJ J J J J J J J J J J J J J J J J J J J J J J J J ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ (2-9)
Vi phân các phương trình Px: Py: Pz ở bài toán động học thuận lần lượt theo các biến θ (i = 1: 2: 3: 4: 5: 6) ta được các phần tử ở ma trận Jacobi như sau: Jv = J J J J J J J J J J J J J J J J J J (2-10) Các phần tử ở hàng 1 J11 = a3(S1S2S3-C2C3S1) + d4(C2S1S3 + C3S1S2) – d3C1 – a2C2S1 J12 = -C1(d4C23 + a3S23 + a2S2) J13 = -C1(d4C23 + a3S23) J14 = 0 J15 = 0 J16 = 0
Các phần tử ở hàng 2 J21 = a2C1C2 – d4(C1C2S3 + C1C3S2) – d3S1 –a3(C1S2S3 – C1C2C3) J22 = -S1(d4C23 + a3S23 + a2S2) J23 = -S1(d4C23 + a3S23) J24 = 0 J25 = 0 J26 = 0 Các phần tử ở hàng 3 J31 = 0 J32 = d4S23 – a3C23 – a2C2 J33 = d4S23 – a3C23 J34 = 0 J35 = 0 J36 = 0
3 dòng dưới của ma trận Jacobi được xây dựng bằng cách xếp các ma trận Zi (i = 1: 2: 3: 4: 5) là các ma trận được trích ra từ cột thứ 3 của ma trận chuyển vị của khớp n so với khớp thứ nhất ta được ma trận J
Jw = [Z Z Z Z Z Z ] Các phần tử ở hàng 4 J41 = 0 J42 = 0 J43 = -S1 J44 = -S1 J45 = -S23C1 J46 = -C4S1-S4(C1S2S3-C1C2C3) Các phần tử hàng 5
J51 = 0 J52 = 0 J53 = C1 J54 = C1 J55 = -S23S1 J56 = C1C4 – S4(S1S2S3 – C2C3S1) Các phần tử hàng 6 J61 = 1 J62 = 1 J63 = 0 J64 = 0 J65 = -C23 J66 = -S23S4
Xếp các hàng trên lại ta được ma trận Jacobi hoàn chỉnh. 2.2.2. Động lực
Đầu tiên chúng ta tính động năng và thế năng của tay máy và sau đó sử dụng phương trình Lagrange của chuyển động. Phương trình Lagrange dựa vào phép tính vi phân của các thành phần năng lượng với các biến thay đổi của hệ thống và thời gian.
Phương trình chuyền động Lagrange dành cho hệ thống bảo toàn được đưa ra như sau: 𝑑 𝑑𝑡 ∂𝐿 ∂𝑞˙− ∂𝐿 ∂𝑞 = 𝜏 (2-11)
Với q là vector n hệ trục tổng quát với hệ trục thành phần là q , 𝜏 là vector n của lực tồng quát với các lực thành phần là 𝜏 và hàm Lagrange là hiệu số của động năng trừ thế năng.
𝐿 = 𝐾 − 𝑃 (2-12) q là vector biến khớp bao gồm các góc khớp 𝜃 (tính theo độ hoặc radian) và độ dịch chuyển của khớp d (m). Sau đó, 𝜏 là vector có thành phần là n (Nm) tương ứng với góc khớp và f là lực (N) tương ứng với độ dịch chuyển của khớp.
Phương trình Lagranger được xây dựng từ việc viết phương trình vị trí của các khớp. Từ phương trình vị trí ấy ta sẽ tìm được các thành phần thế năng và động năng. Ta có phương trình vị trí của các khớp như sau:
Khâu 1: X1 = a2S1 Y1 = a2C1 Z1= 0 Khâu 2: X2 = X1 + d3C2S1 = a2S1 + d3C2S1 Y2 = Y1 + d3C2C1 = a2C1 + d3C2C1 Z2 = Z1 + d3S2 = d3S2 Khâu 3: X3 = X2 + d4C23S1 = a2S1 + d3C2S1 + d4C23S1 Y3 = Y2 + d4C23C1= a2C1 + d3C2C1 + d4C23C1 Z3 = Z2 + d4S23 = d3S2 + d4S23
Từ vị trí của các khâu, ta đạo hàm theo các biến theta sẽ ra được vận tốc của các khâu. Từ đó sẽ tính được động năng K của từng khâu.
𝐾 = 1 2𝑚𝑣
(2-13)
Tổng động năng: K = K1 + K2 + K3 + K4 + K5 + K6 Thế năng của các khâu:
𝑃 = 𝑚ℎ𝑔 (2-14) Tổng thế năng: P = P1 + P2 + P3 + P4 + P5 + P6
Phương trình lagranger:
L = K – P (2-15)
Cuối cùng ta tìm được momen xoắn của từng khâu T. 2.3. Tính toán thiết kế cánh tay robot 6 bậc tự do
Việc thiết kế lên mô hình cánh tay robot vô cùng phức tạp. Có rất nhiều phương án thiết kế được đưa ra nhưng trong số đó không phải phương án nào cũng phù hợp và tối ưu nhất. Ngày nay nhờ sự phát triển của các phần mềm CAD giúp việc thiết kế này trở lên đơn giản hơn rất nhiều. Người thiết kế vẽ ra trên các phần mềm CAD, qua quá trình đóng góp của các bên liên quan cuối cùng lựa chọn được mô hình tối ưu nhất.
2.3.1. Cơ cấu gắp
Có thể sử dụng tay gắp hay giác hút khối lượng từ 1,5 – 2,5 kg.
Sử dụng gắp linh kiện điện tử hay lắp ráp mạch. Tải trọng tối đa chịu đựng 0.5-0.9 kg.
2.3.2. Khâu 6
`Thực hiện vẽ mô hình trên phần mềm siemens NX 1953 sau đó gán vật liệu cho từng bộ phận của khâu, từ đó xác định được các thông số:
Khối lượng: 8,7kg
Khoảng cách từ tâm quay đến trọng tâm: 20,03 mm
Momen tối thiểu để khâu có thể quay quanh Z
Hình 2.5. Tay gắp
M = P.d = 8,7.10.20,03.10-3 = 1,74 (N.m) Động cơ: HF-KP23B (có phanh) Bảng 2.2. Thông số động cơ khớp 6 Hãng Mitsubishi Electric Mã HF-KP23B Có phanh Đường kính trục 14 mm Tổng chiều dài trục 30 mm Đường kính lỗ khoan M5 mm
Momen xoắn cực đại 1.9 N.m
Momen danh nghĩa 0.6 N.m
Momen phanh 1.3 N.m Tốc độ 3000 Vòng/phút Momen quán tính 0.5 kgcm2 Khối lượng 1.6 kg Giảm tốc: CSF-14-30-2UH-SP_A_8_M3 Bảng 2.1. Thông số giảm tốc khớp 1 Tỷ số truyền 30
Shape With key slot
Lỗ 8 mm
Mô-men xoắn định mức L10 19 N.m
Giới hạn cho mô-men xoắn trung bình 35 N.m
Giới hạn cho mô-men xoắn cực đại lặp lại 45 N.m Giới hạn cho Mô men xoắn đỉnh nhất thời 50 N.m
Mô-men xoắn ngược 10 N.m
Tốc độ đầu vào 3500 Vòng/phút
Độ chín xác tiêu chuẩn 1 arc min
Khối lượng 1.5 kg
2.3.3. Khâu 5 kết hợp khâu 6 Khối lượng: 13,9 kg. Khối lượng: 13,9 kg.
Khoảng cách từ tâm quay đến trọng tâm: 240,12427795 mm.
Momen tối thiểu để khâu có thể quay quanh X:
M = P.d = 13,9.10.240,124.10-3 = 33,37 (N.m) Động cơ: HF-KP23B (Bảng 2.2. Thông số động cơ khớp 6) Sử dụng giảm tốc: Trục vít bánh vít SR31/50
Bảng 2.3. Thông tin bộ truyền
Mômen xoắn tối đa 15.3 Nm
Mô-men xoắn dừng khẩn cấp 10.1 Nm
Tốc độ đầu vào max 3000 Vòng / phút
Khối lượng 0.18 kg
2.3.4. Khâu 4 kết hợp với khâu 5 và 6 Khối lượng: 18,4 kg. Khối lượng: 18,4 kg.
Khoảng cách từ tâm quay đến trọng tâm: 205,26232406mm.
Momen tối thiểu để khâu có thể quay quanh Z:
Hình 2.8. Bộ truyền trục vít bánh vít
M = P.d = 18,4.10.205,26.10-3 = 37,76 (N.m) Động cơ: HF-KP23B (Bảng 2.2. Thông số động cơ khớp 6) Sử dụng giảm tốc mã: CSF-25-50-2UH-SP_B2_14_5_M4
Bảng 2.4. Thông tin hộp giảm tốc khớp 4
Tỷ số truyền 50
Shape With key slot
Lỗ 14 mm
Mô-men xoắn định mức L10 39 N.m
Giới hạn cho mô-men xoắn trung bình 55 N.m
Giới hạn cho mô-men xoắn cực đại lặp lại 98 N.m Giới hạn cho Mô men xoắn đỉnh nhất thời 186 N.m
Bắt đầu mô-men xoắn 15 N.cm
Mô-men xoắn ngược 9 N.m
Tốc độ đầu vào 3500 vòng/phút
Độ chín xác tiêu chuẩn 1 arc min
Khối lượng 1.5 kg
2.3.5. Khâu 3 kết hợp với khâu 4, 5, 6 Khối lượng: 24,2kg. Khối lượng: 24,2kg.
Khoảng cách từ tâm quay đến trọng tâm: 514,87217454 mm. Momen tối thiểu để khâu có thể quay quanh Z:
Động cơ: HF-KP23B (Bảng 2.2. Thông số động cơ khớp 6) Sử dụng giảm tốc mã: CSF-25-50-2UH-SP_B2_14_5_M4
Bảng 2.5. Thông tin hộp giảm tốc khớp 3
Tỷ số truyền 50
Shape With key slot
Hole 14 mm
Mô-men xoắn định mức L10 39 N.m
Giới hạn cho mô-men xoắn trung bình 55 N.m
Giới hạn cho mô-men xoắn cực đại lặp lại 98 N.m Giới hạn cho Mô men xoắn đỉnh nhất thời 186 N.m
Bắt đầu mô-men xoắn 15 N.cm
Mô-men xoắn ngược 9 N.m
Tốc độ đầu vào 3500 vòng / phút
Độ chín xác tiêu chuẩn 1 arc min
Khối lượng 1.5 kg
2.3.6. Khâu 2 kết hợp với khâu 3, 4, 5, 6 Khối lượng: 33,1 kg. Khối lượng: 33,1 kg.
Khoảng cách từ tâm quay đến trọng tâm: 320,87385829 mm. Momen tối thiểu để khâu có thể quay quanh Z:
M = P.d = 33,1.10.320,87.10-3 = 106,2 (N.m)
Động cơ: HF-KP43B
Bảng 2.6. Thông tin động cơ khâu 2 Manufacture Mitsubishi Electric
Model HF-KP43B Có phanh Đường kính trục 14 mm Tổng chiều dài trục 30 mm Đường kính lỗ khoan M5 mm Max. torque 3.8 N.m Nominal torque 1.3 N.m Brake torque 1.3 N.m Tốc độ 3000 Vòng/phút Inertia 0.5 kgcm2 Weight 2.1 kg Sử dụng giảm tốc mã: CSF-32-50-2UH-SP_C_14_M5 Hình 2.11. Khâu 2
Bảng 2.7. Thông tin hộp giảm tốc khâu 2
Tỷ số truyền 50
Shape With key slot
Lỗ 14 mm
Mô-men xoắn định mức L10 76 Nm
Giới hạn cho mô-men xoắn trung bình 108 Nm
Giới hạn cho mô-men xoắn cực đại lặp lại 216 Nm Giới hạn cho Mô men xoắn đỉnh nhất thời 382 Nm
Bắt đầu mô-men xoắn 31 Ncm
Mô-men xoắn ngược 18 Nm
Tốc độ đầu vào 3500 Vòng/phút
Độ chín xác tiêu chuẩn 1 arc min
Khối lượng 3.2 kg
2.3.7. Khâu 1 kết hợp với khâu 2, 3, 4, 5, 6 Khối lượng: 44,21 Kg. Khối lượng: 44,21 Kg.
Khối lượng chân đế: 32Kg Động cơ: HF-KP43B(Bảng 2.6. Thông tin động cơ khâu 2)
Sử dụng giảm tốc mã: CSF-40-160- 2UH-SP_C_14_M5 có thông số kỹ thuật:
Bảng 2.8. Thông tin hộp giảm tốc khớp 1
Tỷ số truyền 160
Shape With key slot
Hole 14 mm
Mô-men xoắn định mức L10 294 N.m
Giới hạn cho mô-men xoắn trung bình 451 N.m Giới hạn cho mô-men xoắn cực đại lặp lại 647 N.m Giới hạn cho Mô men xoắn đỉnh nhất thời 1.180 N.m
Bắt đầu mô-men xoắn 24 Ncm
Mô-men xoắn ngược 46 N.m
Tốc độ đầu vào 3.000 vòng/phút
Độ chín xác tiêu chuẩn 1 arc min
Khối lượng 5 kg
2.4. Mô phỏng một số chuyển động
Sử dụng solidworks motion để mô phỏng chuyển động của cánh tay robot. Mô hình sử dụng mô phỏng được lấy từ bước thiết kế trên. Tiến hành ghép các ràng buộc để thể hiện các khớp quay của robot. Khai báo động cơ giả lập cho từng khớp. Vẽ một số quỹ đạo chuyển động rồi gán vận tốc cho robot chạy rồi từ đó xác định momen xoắn của động cơ.
2.4.1. Quỹ đạo chuyển động là đường thẳng trong không gian
Đối với trường hợp này vì quỹ đạo di chuyển là đường thẳng nên tiến hành khai báo động cơ cho khớp 2 và khớp 5 các khớp khác gần như không chuyển động. Gán vận tốc ở điểm cuối cùng của khâu 6 đạt 120 mm/s. Thực hiện mô phỏng ta có kết quả sau.
Khớp 2:
Momen xoắn lớn nhất sinh ra là 21 N.mm, momen xoắn nhỏ nhất là 0 N.mm. Tại thời điểm momen xoắn bằng 0 N.mm lúc này khớp 2 không quay mà chỉ có khớp 5 quay.
Hình 2.14. Kết quả momen khớp 2
Khớp 5:
Momen xoắn lớn nhất sinh ra là 250 N.mm, momen momen xoắn nhỏ nhất là 0 N.mm. Tại thời điểm momen xoắn bằng 0 N.mm lúc này khớp 5 không quay mà chỉ có khớp 2 quay.
2.4.2. Quỹ đạo chuyển động là đường cong trong không gian Vẽ quỹ đạo cong bất kì trong không gian, Vẽ quỹ đạo cong bất kì trong không gian,
thiết lập các ràng buộc để điểm cuối của khâu 6 di chuyển theo quỹ đạo chuyển động cho trước ấy. Gán vận tốc di chuyển ở điểm cuối đạt 120 mm/s, khai báo động cơ servo cho khớp. Đối với biên dạng này gán động cơ cho các khớp ở vị trí 1, 4 và 5. Tiến hành mô phỏng ta có kết quả sau:
Hình 2.15. Kết quả momen khớp 5
Khớp 1:
Momen xoắn lớn nhất sinh ra bằng 0,34 N.mm, momen xoắn nhỏ nhất sinh ra đạt 0,16 N.mm
Khớp 4:
Momen xoắn lớn nhất sinh ra đạt 26 N.mm, momen xoắn nhỏ nhất sinh ra đạt 0 N.mm.
Khớp 5:
Hình 2.18. Đồ thị momen xoắn khớp 4
Hình 2.19. Đồ thị momen xoắn khớp 5 Hình 2.17. Đồ thị momen xoắn khớp 1
Momen xoắn lớn nhất sinh ra đạt 50 N.mm, momen xoắn nhỏ nhất là 0 N.mm
Kết luận chương 2
Động học robot luôn là vấn đề được quan tâm đầu tiên trước khi tính toán thiết kế robot. Việc tính toán động học được sử dụng để xử lý các vấn đề về vị trí, vận tốc, quỹ đạo chuyển động... của robot.
Động học là một mục quan trọng của robot vì chỉ có nó thì ta mới có thể kiểm soát được vấn đề quỹ đạo của robot. Động học thì gồm có động học thuận và động học nghịch , cả hai động học học đó ta đều cần phải xác định ngay từ đầu. Sau khi có nó thì ta có thế tính toán được động lực học của robot và không gian làm việc của nó.
Tiếp theo, đến tính toán thiết kế cánh tay robot , ta tiến hành tính toán ngược từ khâu cuối đến khầu đầu để có thể lựa chọn được động cơ,hộp giảm tốc hay là các bộ truyền khác để đáp ứng được yêu cầu làm việc.
Chương 3. Điều khiển robot 6 bậc tự do
3.1. Xây dựng sơ đồ khối simulink
Simulink là phần mềm mở rộng của matlab nhằm mục đích mô hình hóa, mô phỏng và khảo sát các hệ thống động học cụ thể là đã hỗ trợ xây dưng mô hình và mô phỏng cánh tay robot qua các khối chức năng quen thuộc của simulink hoặc là tự người dùng tạo ra khối chức năng riêng biệt của cá nhân.
Sau khi tìm hiểu và tham khảo các nguồn đi trước thì nhóm đã có thể tự tạo ra toàn bộ các khối để phục vụ việc mô phỏng cánh tay robot. Bao gồm 3 vùng làm việc chính: tín hiệu đầu vào, hệ thống xử lý robot và tín hiệu đầu ra.
Hình 3.1. Sơ đồ khối trong simulink 3.1.1. Khối tín hiệu đầu vào
Chức năng: Giúp cho người dùng có thể điều chỉnh tham số mong muốn để robot có thể vận hành theo ý của minh. Khối tín hiệu đầu vào gồm 3 phần chính:
Bảng 3.1. Ý nghĩa các khối tín hiệu đầu vào
Khối Ý nghĩa
Sử dụng khối Subsystem để tạo ra hệ thống con trong khuôn khổ của một mô hình simulink nhằm thu gọn lại các giá trị mà ta cần cho vào. Khối chứa toàn bộ dữ liệu đầu vào mà ta mong muốn.
Sử dụng khối Fcn ta có thể khai báo một hàm của biến vào dưới dạng một biểu thức viết theo phong cách của ngôn ngữ lập trình.
Ở đây ta cho biến đầu vào là một mà trận G và biến thời gian t đi qua các chương trình quỹ đạo chuyển động mà ta đã tính toán được để tạo ra các giá trị x,y,z theo thời gian.
3.1.2. Khối hệ thống điều khiển robot
Chức năng: xử lý các tín hiệu đầu vào mà người dùng đã khai báo để có thể vận hành robot. Khối hệ thống điều khiển robot gồm 3 phần chính:
Bảng 3.2. Ý nghĩa các khối hệ thống robot
Khối Ý nghĩa
Khối động học nghịch có chức năng tiếp nhận trực tiếp các giá trị vị trí từ khối đầu vào để xử lý tính toán ra các giá trị góc theta giúp robot có thể điều chỉnh để có thể đạt được giá trị vị trí mà người dùng yêu cầu.
Khối robot là phần quan trọng nhất chứ toàn bộ dữ liệu của robot như chiều dài , khối lượng, các chi tiết của robot,…..
Khối động học thuận có chức năng kiểm tra lại những thông tin từ người dùng nhập vào theo tính toán lý thuyết để xem các số liệu đó đã chính xác hay chưa.
Ở trong khối Robot thì ta có rất nhiều các thành phần cấu tạo nên robot từ phần mềm solidworks mà nhóm đã thực hiện vẽ và gán vật liệu vào. Sau đó nhóm đã sử dụng Simscape Multibody 6.1 của matlab xuất ra từ file đã vẽ trên solidworks. Tiếp đó ra smimport vào matlab để tạo ra các thành phần robot.
Hình 3.2. Sơ đồ khối robot trong Simulink Bảng 3.3. Ý nghĩa các khối trong robot
Khối Ý nghĩa