BÀI TẬP LỚN Kỹ thuật Robot: Robot Vertical Articulated TV800

1 TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI BÀI TẬP LỚN MÔN HỌC Kỹ thuật Robot Robot Vertical Articulated TV800 Ngành Kỹ thuật điều khiển và tự động hóa Chuyên ngành Tự động hóa công nghiệp Giảng viên hướng dẫn TS Nguyễn Phạm Thục Anh Bộ môn Tự động hóa công nghiệp Viện Nhóm sinh viên Điện Nhóm 9 HÀ NỘI, 62018 2 MỤC LỤC CHƯƠNG 1 YÊU CẦU BÀI TOÁN VÀ ROBOT TV800 3 1 1 Yêu cầu bài toán 3 1 2 Robot Vertical Articulated TV800 3 1 3 Ứng dụng trong công nghiệp 4 1 4 Thông số kĩ thuật 4 CHƯƠNG 2 ĐỘNG HỌC THUẬN V.

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI BÀI TẬP LỚN MÔN HỌC Kỹ thuật Robot: Robot Vertical Articulated TV800 Ngành Kỹ thuật điều khiển tự động hóa Chuyên ngành Tự động hóa cơng nghiệp Giảng viên hướng dẫn: TS Nguyễn Phạm Thục Anh Bộ mơn: Tự động hóa cơng nghiệp Viện: Điện Nhóm sinh viên: Nhóm HÀ NỘI, 6/2018 MỤC LỤC CHƯƠNG YÊU CẦU BÀI TOÁN VÀ ROBOT TV800 1.1 Yêu cầu toán 1.2 Robot Vertical Articulated TV800 1.3 Ứng dụng công nghiệp 1.4 Thông số kĩ thuật CHƯƠNG ĐỘNG HỌC THUẬN VỊ TRÍ CHƯƠNG TÍNH TỒN MA TRẬN JACOBY VÀ VIẾT CHƯƠNG TRÌNH TRÊN MATLAB 13 3.1 Các bước tính tốn ma trận Jacoby theo 𝑱𝑯 13 3.2 Tính toán ma trận robot TV800 14 CHƯƠNG ĐỘNG HỌC NGƯỢC VỊ TRÍ 24 4.1 Tổng quan động học ngược vị trí 24 4.2 24 CHƯƠNG THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CỦA CÁC KHỚP THEO QUỸ ĐẠO DẠNG BẬC 27 5.1 Thiết kế quỹ đạo PTP (Point to Point) qua điểm đơn 27 5.2 Thiết kế quỹ đạo PTP qua điểm trung gian 29 CHƯƠNG XÂY DỰNG MƠ HÌNH ĐỘNG LỰC HỌC CHO ĐỐI TƯỢNG TRÊN TOOLBOX SIMSCAPE MULTIBODY/MATLAB 32 6.1 Thiết kế mơ hình 3D cho cánh tay robot 32 6.2 Liên kết với Matlab 32 6.3 Mô chuyển động 33 CHƯƠNG YÊU CẦU BÀI TỐN VÀ ROBOT TV800 1.1 u cầu tốn Giới thiệu Robot nhóm nghiên cứu, ứng dụng cơng nghiệp, kết cấu khí, thơng số kỹ thuật u cầu có hình ảnh clip hoạt động Tính tốn động học thuận vị trí Robot Xây dựng chương trình phần mềm MATLAB để nhập liệu, hiển thị kết Tính tốn ma trận Jacoby (thơng qua JH) viêt chương trình MATLAB Tính tốn động học đảo vị trí Robot Thiết kế quỹ đạo chuyển động cho khớp Robot theo quỹ đạo dạng bậc Thiết kế điều khiển chuyển động cho Robot theo thuật tốn PID Xây dựng mơ hình động lực học cho đối tượng ToolBox Simscape/ MATLAB 1.2 Robot Vertical Articulated TV800 Robot chọn báo cáo robot TV800 hãng Toshiba Robot TV800 Robot tác động nhanh, linh hoạt, nhỏ gọn đáng tin cậy Đây loại Robot hoạt động với nhiều cài đặt Nó cung cấp nhiều ứng dụng, với hiệu suất sử dụng cao, đảm bảo yêu cầu chất lượng, thời gian hoàn vốn ngắn TV800 robot tác động tốc độ cao với trục sử dụng hệ thống phát vị trí tuyệt đối, với động AC mạnh mẽ trung tâm, mang lại hiệu suất cao độ tin cậy cho việc xử lý thực phẩm, chọn, đóng gói ứng dụng xử lý vật liệu tốc độ cao khác TV800 có tổng chiều dài cánh tay 800mm, tầm với 892mm tốc độ tối đa tổng hợp 8,06 mét / giây Robot có thời gian chu kỳ tối đa từ 0,4 đến 0,5 giây, độ lặp lại ± 0,02mm trọng tải tối đa kg Hình Robot TV800 ngồi thực tế TV800 thiết kế cứng thẳng, điều dẫn đến tiếng ồn làm việc thấp, thời gian bảo trì lâu Ngồi cịn thiết kế nhỏ gọn, cổ tay mỏng, hiệu suất hoạt động cao vị trí khó 1.3 Ứng dụng công nghiệp Robot TV800 ứng dụng cho dây chuyền sản xuất tự động, lĩnh vực phổ biến là: • Xếp/dỡ máy lắp ráp • Công nghệ gia công lắp ráp • Phun sơn 1.4 Thông số kĩ thuật - Số bậc tự do: - Kiểu khớp: khớp quay - Vùng khơng gian làm việc: Hình Khơng gian làm việc robot TV800 Ta có bảng thơng số kĩ thuật robot: SPECIFICATIONS Trục Vùng hoạt Tốc độ max Momen Số trục động (o/sec) quán tính Tải tối đa[kg] Sai số ±0.02 892 cho phép J1 ±170 237 - Tầm với J2 +150/−100 240 - [mm] J3 +167/−127 288 - Khối J4 ±190 350.5 0.3kg · m lượng[kg] 45 J5 ±120 484 0.3kg · m J6 ±360 576 0.05kg · m Bảng thông số kĩ thuật robot TV800 CHƯƠNG ĐỘNG HỌC THUẬN VỊ TRÍ Hình Mơ hình robot Đây bước sở cho việc thiết kế sơ robot, từ giải tốn điều khiển robot theo quỹ đạo Từ ta có đủ thông số để điều khiển robot theo quỹ đạo cho trước với lực cho trước ta thu quỹ đạo chuyển động định Dưới phần tính tốn động học cho Robot TV800 Bài tốn động học thuận: Trục Giới hạn chuyển động Tốc độ ±170° 237°/s -100 ∼ +150° 240°/s -127 ∼ +167° 288°/s ±190° 350.5°/s ±120° 484°/s ±360° 576°/s Bộ thông số D-H Ta xây dựng mối quan hệ động học thông qua thông số D-H: Theo Denavit & Hartenberg, hai ông đề xuất dùng ma trận 4x4 để mô tả quan hệ khâu liên tiếp cấu không gian Trước hết, xác định thông số trục quay khớp động i+1 i: - 𝑎𝑖 độ dài đường vng góc chung trục khớp động i+1 i - 𝛼𝑖 góc chéo trục khớp động i+1 i, - 𝑑𝑖 khoảng cách đo dọc trục khớp động i kể từ đường vng góc chung trục khớp động i+1 trục khớp động i tới đường vng góc chung trục khớp động i trục khớp động i-1 - 𝜃𝑖 góc đường vng góc nói Biến khớp: - Nếu khớp động i khớp quay 𝜃𝑖 biến khớp - Nếu khớp động i tịnh tiến 𝑑𝑖 biến khớp Thiết lập hệ tọa độ Thiết lập hệ tọa độ hình Khâu 𝜃𝑖 (°) 𝑑𝑖 (mm) 𝑎𝑖 (mm) 𝛼𝑖 (°) Khớp 𝜃1 𝑑1 =116 𝑎1 =0 -90 R −90 + 𝜃2 𝑑2 =0 𝑎2 =380 R 𝜃3 𝑑3 =0 𝑎3 =100 -90 R 𝜃4 𝑑4 =420 𝑎4 =0 90 R −90 + 𝜃5 𝑑5 =0 𝑎5 =0 -90 R 𝜃6 𝑑6 =80 𝑎6 =0 R Các ma trận A xác định ma trận biến đổi tọa độ tổng quát : C i  S Ai =   i    − S C i i C C i i S i S S i i −C S i i C i 0 aC  i  aS  i  d    C1  S A = 1 0  0 −S   C 0  −1 d1   0 1 C3  S A = 3 0  0 − S a3C  3 C a3 S  3 −1 0   0  C4  S A = 4    −C 0  S5   −C A = 5    0  −S 0  −1 0  0 1 C6  S A = 6    −S 0  −C 0   d6   0 1 0 C  S2   −C A = 2    C S 0 S a2 S  2 a2C  2    0  0  d4   1 Từ ma trận biến đổi trục, ta xác định hàm truyền RTH robot ( ma trận chuyển đổi trục trục robot)  nx n T6 =  y  nz  0 ox oy oz ax ay az px  p y  = A1 A2 A3 A4 A5 A6 pz   1 Trong : 𝑠1 = 𝑠𝑖𝑛( 𝜃1 ); 𝑐1 = 𝑐𝑜𝑠( 𝜃1 ) 𝑠2 = 𝑠𝑖𝑛( 𝜃2 ); 𝑐2 = 𝑐𝑜𝑠( 𝜃2 ) 𝑠3 = 𝑠𝑖𝑛( 𝜃3 ); 𝑐3 = 𝑐𝑜𝑠( 𝜃3 ) 𝑠4 = 𝑠𝑖𝑛( 𝜃4 ); 𝑐4 = 𝑐𝑜𝑠( 𝜃4 ) 𝑠5 = 𝑠𝑖𝑛( 𝜃5 ); 𝑐5 = 𝑐𝑜𝑠( 𝜃5 ) 10 𝐽36 = 𝐽41 = − (𝑠5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) + 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) ∗ (𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) − 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) + 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) + 𝑐6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − 𝑠6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) 𝐽42 = (𝑐4 ∗ 𝑠6 + 𝑐6 ∗ 𝑠4 ∗ 𝑠5) ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − 𝑠6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − (𝑐4 ∗ 𝑐6 − 𝑠4 ∗ 𝑠5 ∗ 𝑠6) ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) + 𝑐6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − 𝑐5 ∗ 𝑠4 ∗ (𝑠5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) + 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) 𝐽43 = (𝑐4 ∗ 𝑠6 + 𝑐6 ∗ 𝑠4 ∗ 𝑠5) ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − 𝑠6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − (𝑐4 ∗ 𝑐6 − 𝑠4 ∗ 𝑠5 ∗ 𝑠6) ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) + 𝑐6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − 𝑐5 ∗ 𝑠4 ∗ (𝑠5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) + 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) 𝐽44 = 𝑠5 ∗ (𝑠5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) + 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) + 𝑐5 ∗ 𝑐6 ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) − 𝑠6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) + 𝑐5 ∗ 𝑠6 ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) + 𝑐6 ∗ (𝑐4 ∗ 𝑠1 − 𝑠4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2))) 𝐽45 = (𝑐6^2 + 𝑠6^2) ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 ∗ 𝑠4 − 𝑐4 ∗ 𝑠1 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2 ∗ 𝑠4) 𝐽46 = 𝑠5 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑐3 − 𝑐1 ∗ 𝑠2 ∗ 𝑠3) + 𝑐5 ∗ (𝑠1 ∗ 𝑠4 + 𝑐4 ∗ (𝑐1 ∗ 𝑐2 ∗ 𝑠3 + 𝑐1 ∗ 𝑐3 ∗ 𝑠2)) 21 𝐽51 = − (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) + 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − 𝑐6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) + 𝑠6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − (𝑠5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) − 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) ∗ (𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) − 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) 𝐽52 = (𝑐4 ∗ 𝑠6 + 𝑐6 ∗ 𝑠4 ∗ 𝑠5) ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) + 𝑠6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − (𝑐4 ∗ 𝑐6 − 𝑠4 ∗ 𝑠5 ∗ 𝑠6) ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − 𝑐6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − 𝑐5 ∗ 𝑠4 ∗ (𝑠5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) − 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) 𝐽53 = (𝑐4 ∗ 𝑠6 + 𝑐6 ∗ 𝑠4 ∗ 𝑠5) ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) + 𝑠6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − (𝑐4 ∗ 𝑐6 − 𝑠4 ∗ 𝑠5 ∗ 𝑠6) ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − 𝑐6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − 𝑐5 ∗ 𝑠4 ∗ (𝑠5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) − 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) 𝐽54 = 𝑠5 ∗ (𝑠5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) − 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) + 𝑐5 ∗ 𝑐6 ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) + 𝑠6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) + 𝑐5 ∗ 𝑠6 ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) + 𝑠5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) − 𝑐6 ∗ (𝑐1 ∗ 𝑐4 + 𝑠4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2))) 𝐽55 = (𝑐6^2 + 𝑠6^2) ∗ (𝑐1 ∗ 𝑐4 + 𝑐2 ∗ 𝑠1 ∗ 𝑠3 ∗ 𝑠4 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2 ∗ 𝑠4) 𝐽56 = 𝑠5 ∗ (𝑐2 ∗ 𝑐3 ∗ 𝑠1 − 𝑠1 ∗ 𝑠2 ∗ 𝑠3) − 𝑐5 ∗ (𝑐1 ∗ 𝑠4 − 𝑐4 ∗ (𝑐2 ∗ 𝑠1 ∗ 𝑠3 + 𝑐3 ∗ 𝑠1 ∗ 𝑠2)) 𝐽61 = (𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) − 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))^2 + (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) + 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))^2 + (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3))^2 22 𝐽62 = (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) + 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ (𝑐4 ∗ 𝑐6 − 𝑠4 ∗ 𝑠5 ∗ 𝑠6) − (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ (𝑐4 ∗ 𝑠6 + 𝑐6 ∗ 𝑠4 ∗ 𝑠5) + 𝑐5 ∗ 𝑠4 ∗ (𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) − 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) 𝐽63 = (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) + 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ (𝑐4 ∗ 𝑐6 − 𝑠4 ∗ 𝑠5 ∗ 𝑠6) − (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) ∗ (𝑐4 ∗ 𝑠6 + 𝑐6 ∗ 𝑠4 ∗ 𝑠5) + 𝑐5 ∗ 𝑠4 ∗ (𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) − 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) 𝐽64 = − 𝑠5 ∗ (𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) − 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − 𝑐5 ∗ 𝑐6 ∗ (𝑐6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − 𝑠4 ∗ 𝑠6 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) − 𝑐5 ∗ 𝑠6 ∗ (𝑠6 ∗ (𝑐5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) + 𝑐4 ∗ 𝑠5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) + 𝑐6 ∗ 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3)) 𝐽65 = 𝑠4 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3) ∗ (𝑐6^2 + 𝑠6^2) 𝐽66 = 𝑐4 ∗ 𝑐5 ∗ (𝑐2 ∗ 𝑐3 − 𝑠2 ∗ 𝑠3) − 𝑠5 ∗ (𝑐2 ∗ 𝑠3 + 𝑐3 ∗ 𝑠2) 23 CHƯƠNG ĐỘNG HỌC NGƯỢC VỊ TRÍ 4.1 Tổng quan động học ngược vị trí Động học ngược vị trí để xác định giá trị biến khớp tương ứng với vị trí mong muốn tay không gian Tức xác định ma trận biến khớp Q = [q1, q2,…, qn]T từ vị trí khâu tác động cuối biết, giả sử 𝑇𝑛0 có giá trị: 𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑝𝑥 𝑛 𝑜𝑦 𝑎𝑦 𝑝𝑦 ] 𝑇𝑛0 = [ 𝑦 𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑝𝑧 0 Ta có phương pháp giải vị trí động học ngược vị trí: • Phép đảo vị trí: áp dụng cho robot có hai bậc tự do, • Phép đảo hướng: áp dụng cho phép quay cố định, • Phương pháp phân ly biến: áp dụng cho robot có số bậc tự >3 ➔ Trong toán mày, ta sử dụng phương pháp phân ly biến để tính tốn động học ngược vị trí cho robot 4.2 Tính tốn động học ngược vị trí cho TV800 Các bước thực phương pháp phân ly biến thể Hình 4.1 Hình 4.1 Phương pháp phân ly biến Ta có: 𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑝𝑥 𝑛 𝑜𝑦 𝑎𝑦 𝑝𝑦 ] = 𝐴1 (𝑞1 )𝐴2 (𝑞2 )𝐴3 (𝑞3 )𝐴4 (𝑞4 )𝐴5 (𝑞5 )𝐴6 (𝑞6 ) (∗) 𝑇60 = [ 𝑦 𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑝𝑧 0 ❖ Nhân hai vế phương trình với A1-1(q1) ta được: 𝐴1−1 (𝑞1 )𝑇60 = 𝐴1−1 (𝑞1 )𝐴1 (𝑞1 )𝐴2 (𝑞2 )𝐴3 (𝑞3 )𝐴4 (𝑞4 )𝐴5 (𝑞5 )𝐴6 (𝑞6 ) 24 𝐴1−1 (𝑞1 )𝑇50 = 𝐴2 (𝑞2 )𝐴3 (𝑞3 )𝐴4 (𝑞4 )𝐴5 (𝑞5 )𝐴6 (𝑞6 ) = 𝑇61 Sử dụng Matlab để thực tính toán ta thu kết quả: VT1=𝐴1−1 (𝑞1 )𝑇60 = 𝑛𝑥 ∗ 𝐶1 + 𝑛𝑦 ∗ 𝑆1 𝑜𝑥 ∗ 𝐶1 + 𝑜𝑦 ∗ 𝑆1 𝑎𝑥 ∗ 𝐶1 + 𝑎𝑦 ∗ 𝑆1 𝑝𝑥 ∗ 𝐶1 + 𝑝𝑦 ∗ 𝑆1 𝑛𝑥 ∗ S1 − nyC1 𝑜𝑥 ∗ 𝑆1 − 𝑜𝑦 ∗ 𝐶1 𝑎𝑥 ∗ 𝑆1 − 𝑎𝑦 ∗ 𝐶1 𝑝𝑥 ∗ 𝑆1 − 𝑝𝑦 ∗ 𝐶1 [ ] −𝑛𝑧 −𝑜𝑧 −𝑎𝑧 𝑑1 − 𝑝𝑧 0 1 VP1=𝐴2 (𝑞2 )𝐴3 (𝑞3 )𝐴4 (𝑞4 )𝐴5 (𝑞5 )𝐴6 (𝑞6 ) = 𝑇6 𝐶23(−4)5(−6) 𝑆23(−4)5(−6) −𝑎3 ∗ 𝐶23 − 𝑎2 ∗ 𝐶2 𝑆23(−4)5(−6) −𝐶23(−4)5(−6) −𝑎3 ∗ 𝑆23 − 𝑎2 ∗ 𝑆2 ] =[ 0 −1 −𝑑4 − 𝑑6 0 Với 𝐶23(−4)5(−6) = 𝑐𝑜𝑠(𝑞2 + 𝑞3 − 𝑞4 + 𝑞5 − 𝑞6 ) Cân phần tử hàng cột hai vế VT1 VP1 ta thu được: 𝑎𝑥 ∗ 𝐶1 + 𝑎𝑦 ∗ 𝑆1 = Áp dụng trường hợp động học giải tích ta có: 𝜃1 = 𝑞1 = 𝐴𝑇𝐴𝑁2(−𝑎𝑥, 𝑎𝑦) 𝜃1 = 𝑞1 = 𝐴𝑇𝐴𝑁2(𝑎𝑥, −𝑎𝑦) Cân phần tử cột hàng ta thu được: 𝑎3 ∗ 𝐶23 + 𝑎2 ∗ 𝐶2 = 𝑚 (1) { 𝑎3 ∗ 𝑆23 + 𝑎2 ∗ 𝑆2 = 𝑛 (2) Với { 𝑚 = −(𝑝𝑥 ∗ 𝐶1 + 𝑝𝑦 ∗ 𝑆1) 𝑛 = −(𝑝𝑥 ∗ 𝑆1 − 𝑝𝑦 ∗ 𝐶1) Bình phương hai vế (1) (2) cộng lại ta được: 𝑎32 + 𝑎22 + 2𝑎2 𝑎3 (𝐶23 ∗ 𝐶2 + 𝑆23 ∗ 𝑆2) = 𝑚2 + 𝑛2 𝐶3 = 𝑚2 + 𝑛2 − 𝑎32 − 𝑎22 2𝑎2 𝑎3 ➔ 𝜃3 = 𝐴𝑇𝐴𝑁2 (±√1 − ( 𝑚2 +𝑛2 −𝑎32 −𝑎22 2𝑎2 𝑎3 ) , 𝑚2 +𝑛2 −𝑎32 −𝑎22 2𝑎2 𝑎3 ) Có 𝜃3 ta thay lại vào (1) (2) thu được: (𝑎3 ∗ 𝐶3 + 𝑎2)𝐶2 − 𝑎3 ∗ 𝑆3 ∗ 𝑆2 = 𝑚 { (𝑎3 ∗ 𝐶3 + 𝑎2)𝑆2 + 𝑎3 ∗ 𝑆3 ∗ 𝐶2 = 𝑛 Áp dụng trường hợp động học giải tích ta có: 𝜃2 = 𝐴𝑇𝐴𝑁2((𝑎3 ∗ 𝐶3 + 𝑎2) ∗ 𝑛 − 𝑎3 ∗ 𝑆3 ∗ 𝑚, (𝑎3 ∗ 𝐶3 + 𝑎2) ∗ 𝑚 + 𝑎3 ∗ 𝑆3 ∗ 𝑛) ❖ Nhân hai vế (*) với A1-1(q1)* A2-1(q2)* A3-1(q3)* A4-1(q4) ta được: 25 VT4= A1-1(q1)* A2-1(q2)* A3-1(q3)* A4-1(q4)*𝑇60 𝑋 = [𝑋 𝑋 𝑋 −𝑜𝑥𝐶1(−2)(−3)4 − 𝑜𝑦𝑆1(−2)(−3)4 −𝑎𝑥𝐶1(−2)(−3)4 − 𝑎𝑦𝑆1(−2)(−3)4 𝑜𝑦𝐶1(−2)(−3)4 − 𝑜𝑥𝑆1(−2)(−3)4 𝑎𝑦𝐶1(−2)(−3)4 − 𝑎𝑥𝑆1(−2)(−3)4 −𝑜𝑧 −𝑎𝑧 0 VP4=(𝑞5 )𝐴6 (𝑞6 ) = 𝑇64 𝑋 −𝑆5(−6) 𝑋 𝐶5(−6) =[ 𝑋 −1 𝑋 0 𝑋 𝑋] 𝑋 𝑋 𝑋 𝑋 ] 𝑋 𝑋 Cân phần tử hàng cột 3: 𝑎𝑥𝐶1(−2)(−3)4 + 𝑎𝑦𝑆1(−2)(−3)4 = Áp dụng trường hợp động học giải tích ta có: 𝜃4 = 𝐴𝑇𝐴𝑁2(−𝑎𝑥, 𝑎𝑦) − 𝜃1 + 𝜃2 + 𝜃3 𝜃4 = 𝐴𝑇𝐴𝑁2(𝑎𝑥, −𝑎𝑦) − 𝜃1 + 𝜃2 + 𝜃3 Cân phần tử cột hàng ta được: ℎ = 𝑜𝑥𝐶1(−2)(−3)4 + 𝑜𝑦𝑆1(−2)(−3)4 𝑆5(−6) = ℎ { với { 𝐶5(−6) = 𝑘 𝑘 = 𝑜𝑦𝐶1(−2)(−3)4 − 𝑜𝑥𝑆1(−2)(−3)4 ➔ 𝜃5 − 𝜃6 = 𝐴𝑇𝐴𝑁2(ℎ, 𝑘) (**) ❖ Nhân hai vế (*) với A1-1(q1)* A2-1(q2)* A3-1(q3)* A4-1(q4)* A5-1(q5) ta được: 𝑎𝑥 ∗ 𝐶 (−1)23(−4)5 − 𝑎𝑦 ∗ 𝑆(−1)23(−4)5 𝑎𝑦 ∗ 𝐶 (−1)23(−4)5 + 𝑎𝑥 ∗ 𝑆(−1)23(−4)5 𝑎𝑧 𝑋 𝑋 𝑋 𝑋 𝑋 𝑋 ] VP5= [ 𝑋 𝑋 𝑋 𝑋 𝑋 𝑋 ➔ 𝑎𝑦 ∗ 𝐶 (−1)23(−4)5 + 𝑎𝑥 ∗ 𝑆(−1)23(−4)5 = ➔ 𝜃5 = 𝐴𝑇𝐴𝑁2(−𝑎𝑦, 𝑎𝑥) + 𝜃1 − 𝜃2 − 𝜃3 + 𝜃4 𝜃5 = 𝐴𝑇𝐴𝑁2(𝑎𝑦, −𝑎𝑥) + 𝜃1 − 𝜃2 − 𝜃3 + 𝜃4 thay 𝜃5 vừa tính vào (**) 𝜃6 = 𝜃5 − 𝐴𝑇𝐴𝑁2(ℎ, 𝑘) 𝑋 𝑉𝑇5 = [𝑋 𝑋 𝑋 𝑋 𝑋 𝑋 𝑋 𝑋 𝑋] 𝑋 𝑋 26 CHƯƠNG THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CỦA CÁC KHỚP THEO QUỸ ĐẠO DẠNG BẬC 5.1 Thiết kế quỹ đạo PTP (Point to Point) qua điểm đơn Thiết kế quỹ đạo chuyển động từ A đến B, quan tâm đến điều kiện vị trí, vận tốc gia tốc điểm đầu điểm cuối Quỹ đạo đường cong có dạng đa thức Trên lí thuyết, đa thức bậc cao quỹ đạo di chuyển xác, nhiên tốn điều khiển robot, quỹ đạo có dạng đa thức bậc 3, bậc dùng nhiều cả: • Đa thức bậc 3: x(t) = 𝑎0 + 𝑎1 𝑡 + 𝑎2 𝑡 + 𝑎3 𝑡 • Đa thức bậc 5: x(t) = 𝑎0 + 𝑎1 𝑡 + 𝑎2 𝑡 + 𝑎3 𝑡 + 𝑎4 𝑡 + 𝑎5 𝑡 Đề yêu cầu thiết kế quỹ đạo dạng đa thức bậc cho robot Qũy đạo vị trí khớp viết sau: θ(t) = a0 +a1t + a2t2 + a3t3 Đạo hàm vị trí cho quỹ đạo vận tốc khớp tương ứng: θ̇(t) = a1 + 2a2t +3a3t2 Giả sử khớp quay thứ I yêu cầu quay từ vị trí ban đầu θ0 đến vị trí cuối θf khoảng thời gian cho trước tf, ta có điều kiện đầu cuối vị trí khớp quay là: θ(0) = θ0 θ(tf) = θf Cùng với điều kiện ràng buộc vận tốc quay: θ̇(0) = θ̇0 θ̇(tf) = θ̇f Thay t = 0, t = tf vào phương trình trên, giải hệ ẩn phương trình, từ tính giá trị 𝑎0 , 𝑎1 , 𝑎2 , 𝑎3 sau: a = θ0 a1 = θ̇0 a2 = (θf − θ0 ) − θ̇0 − θ̇f t f tf tf −2 a3 = (θf − θ0 ) + (θ̇f − θ̇0 ) t f t f Giả sử yêu cầu đặt sau: khớp chuyển động từ góc ban đầu θ0 = 45° đến góc cuối θf = 60° thời gian tf =3 giây Từ kiện trên, thay vào phương trình tính tốn, ta được: 27 a0 = θ0 = 45 a1 = θ̇0 = a2 = (θf − θ0 ) − θ̇0 − θ̇f = t f tf tf −2 a3 = (θf − θ0 ) + (θ̇f − θ̇0 ) = −1.11 t f t f Thay ngược lại vào phương trình quỹ đạo vị trí, tốc độ, gia tốc khớp: Phương trình quỹ đạo vị trí: θ(t) = 45 + 5t2 – 1.11t3 Hình Quỹ đạo vị trí Phương trình quỹ đạo tốc độ : θ̇(t) = 10t – 3.33t2 Hình Quỹ đạo tốc độ 28 Phương trình quỹ đạo gia tốc: θ̈(t) = 10 – 6.66t Hình Qũy đạo gia tốc 5.2 Thiết kế quỹ đạo PTP qua điểm trung gian Việc thiết kế quỹ đạo PTP qua điểm trung gian gồm phương pháp chính: • Qũy đạo đa thức bậc qua điểm trung gian θv • Qũy đạo 2-1-2 qua số điểm trung gian Ở tập thực phương pháp thiết kế qũy đạo đa thức bậc qua điểm trung gian θv Biết trước dạng đường bậc đầu là: θ(t) = a10 + a11t + a12t2 +a13t3 Đường bậc sau có dạng: θ(t) = a20 + a21t + a22t2 + a23t3 Gỉa sử đường cong xuất phát từ t = dừng thời điểm t = tfi (i=1,2) Phương trình cân vị trí điểm đầu, cuối trung gian θ0 = a10 θv = a10 + a11 t f1 + a12 t f1 + a13 t f1 θv = a20 θf = a20 + a21 t f2 + a22 t f2 + a23 t f2 29 Phương trình cân tốc độ điểm đầu cuối: θ̇0 = a11 θ̇f = a21 + 2a22 t f2 + 3a23 t f2 Phương trình cân tốc độ điểm trung gian: θ̇v = a11 + 2a12 t f1 + 3a13 t f1 = a21 Phương trình cân gia tốc điểm trung gian: θ̈v = 2a12 + 6a13 t1 = a22 Chọn tf1 = tf2 =tf Giải hệ ẩn phương trình trên, thu giá trị sau: a10 = θ0 a11 = a12 = a13 = 12θv −3θf −9θ0 4𝑡𝑓2 −8θv +3θf +5θ0 4t3 f a20 = θv 3(θf − θ0 ) a21 = 4t f −12θv + 6(θf + θ0 ) a22 = 4t f 8θv − 5θf − 3θ0 a23 = 4t f Giả sử yêu cầu đặt sau: khớp chuyển động từ góc ban đầu θ0 = 30° đến góc cuối θf = 60° qua điểm trung gian θv = 45° thời gian tf1 = tf2 = tf = giây Thay thông số vào phương trình trên, thu được: a10 = 30; a11 = 0; a12 = 2.5; a13 = -0.28 a20 = 45; a21 = 7.5; a22 = 0; a23 = -0.28 Từ thông số trên, thay ngược lại vào dạng đường bậc đầu: θ(t) = 30 + 2.5t2 - 0.28t3 Dạng đường bậc cuối: θ(t) = 45 + 7.5t - 0.28t3 5.3 Vẽ đồ thị minh họa Từ phương trình quỹ đạo trên, sử dụng Matlab để vẽ đồ thị minh họa quỹ đạo chuyển động khớp Dạng đường bậc đầu: θ(t) = 30 + 2.5t2 - 0.28t3 30 Hình Dạng đường bậc đầu Dạng đường bậc cuối: θ(t) = 45 + 7.5t - 0.28t3 Hình Dạng đường bậc đầu 31 CHƯƠNG XÂY DỰNG MƠ HÌNH ĐỘNG LỰC HỌC CHO ĐỐI TƯỢNG TRÊN TOOLBOX SIMSCAPE MULTIBODY/MATLAB 6.1 Thiết kế mơ hình 3D cho cánh tay robot Để tiến hành mô robot sau thực bước tính tốn phần trên, ta xây dựng mơ hình 3D cánh tay robot phần mềm SolidWorks 2021 sau xuất file thực mơ toolbox Simscape Matlab Ở chúng em sử dụng file mơ hình robot Yaskawa Motorman GP12 tải từ trang https://grabcad.com/library/yaskawa-motoman-gp12-1 Đây cánh tay robot trục tự có cấu tạo giống với Robot TV800 Tiến hành chọn cụm chi tiết cửa sổ phía bên trái hình, chọn Open Part in Position 6.2 Liên kết với Matlab Truy cập đường dẫn: https://www.mathworks.com/campaigns/offerings/download_smlink.html Sau đó, điền thơng tin tải file tương ứng với phiên matlab hệ điều hành sử dụng Đặt tất file folder để tiện truy cập Hình Kết sau liên kết với Matlab 32 Hình 10 Mơ hình thiết kế Hình 11 Bộ điều khiển PID Trong đó: • SV tín hiệu đặt • u tín hiệu điều khiển, • q giá trị biến khớp, • q_dot vận tốc khớp • Các hệ số Kp, Ki, Kd chỉnh định tay 6.3 Mô chuyển động Sau đặt giá trị SV cho tất khớp 𝜋 𝜋 𝑆𝑉 = −60𝑜 (− , 𝑡ạ𝑖 𝑡 = − 5𝑠) SV= −30𝑜 (− , 𝑡ạ𝑖 𝑡 = − 10𝑠), ta thu đáp ứng sau: 33 Hình 13 Vị trí ban đầu Hình 14 Vị trí SV = -60° 34 Hình 15 Vị trí SV=-30° Hình 16 So sánh SV với tín hiệu 35 ... Simscape/ MATLAB 1.2 Robot Vertical Articulated TV800 Robot chọn báo cáo robot TV800 hãng Toshiba Robot TV800 Robot tác động nhanh, linh hoạt, nhỏ gọn đáng tin cậy Đây loại Robot hoạt động với... LỤC CHƯƠNG YÊU CẦU BÀI TOÁN VÀ ROBOT TV800 1.1 Yêu cầu toán 1.2 Robot Vertical Articulated TV800 1.3 Ứng dụng công nghiệp 1.4 Thông số kĩ thuật CHƯƠNG... kĩ thuật robot TV800 CHƯƠNG ĐỘNG HỌC THUẬN VỊ TRÍ Hình Mơ hình robot Đây bước sở cho việc thiết kế sơ robot, từ giải toán điều khiển robot theo quỹ đạo Từ ta có đủ thơng số để điều khiển robot