Thiết kế điều khiển đồng bộ robot 4 bậc tự do

1 BỘ GIÁO DỤC VÀ ĐÀO TẠO ĐẠI HỌC ĐÀ NẴNG PHẠM TRƯỜNG TÙNG THIẾT KẾ ĐIỀU KHIỂN ĐỒNG BỘ ROBOT 4 BẬC TỰ DO CHUYÊN NGÀNH: SẢN XUẤT TỰ ĐỘNG MÃ SỐ: 60.52.60 TÓM TẮT LUẬN VĂN THẠC SĨ KỸ THUẬT ĐÀ NẴNG – NĂM 2011 2 Công trình ñược hoàn thành tại ĐẠI HỌC ĐÀ NẴNG Người hướng dẫn khoa học: PGS.TS. Phạm Đăng Phước Phản biện 1: Phản biện 2: Luận văn ñược bảo vệ trước Hội ñồng chấm Luận văn tốt nghiệp thạc sĩ kĩ thuật họp tại Đà Nẵng vào ngày… tháng… năm 2011 Có thể tìm hiểu luận văn tại: - Trung tâm Thông tin Học liệu – ĐH Đà Nẵng. - Trung tâm Học liệu – ĐH Đà Nẵng. 3 MỞ ĐẦU 1. Lý do chọn ñề tài: • Hiện nay, với nền sản xuất công nghiệp hiện ñại, robot công nghiệp là một trong những thành phần quan trọng. • Nhiều cơ sở ñào tạo hiện chưa có robot ñể phục vụ cho sinh viên học tập và nghiên cứu. • Cần thiết phải nghiên cứu về robot công nghiệp nhằm nắm bắt và phát triển kĩ thuật robot ñể phục vụ cho nhu cầu sản xuất, phục vụ học tập, nghiên cứu. 2. Mục ñích của ñề tài: • Đề tài tập trung nghiên cứu kĩ thuật ñiều khiển các trục khớp ñồng thời ( ñiều khiển ñồng bộ các khớp) và áp dụng vào cho robot 4 bậc tự do. 3. Phạm vi và nội dung nghiên cứu: 3.1. Phạm vi: • Nghiên cứu ñể áp dụng cho robot 4 bậc tự do, sử dụng ñộng cơ ñiện một chiều. 3.2. Nội dung nghiên cứu: • Nghiên cứu lý thuyết về robot công nghiệp. Trên cơ sở ñó, ta vận dụng lựa chọn mô hình robot phù hợp. • Xây dựng mô hình toán học ñể ñiều khiển ñồng bộ các trục của robot ñã lựa chọn. Mô hình hóa luật ñiều khiển ñó bằng máy tính và sau ñó thiết kế bộ ñiều khiển ñồng bộ chuyển ñộng các trục của robot. • Chế tạo mô hình robot 4. Phương pháp nghiên cứu: • Nghiên cứu các tài liệu liên quan nhằm tổng hợp lựa chọn các phương án từ ñó ñưa ra mô hình robot phù hợp, trên cơ 4 sở ñó tính toán ñể thiết kế bộ ñiều khiển ñồng bộ các trục cho robot. • Chế tạo mô hình ñể kiểm chứng các kết quả. 5. Ý nghĩa khoa học và thực tiễn: • Áp dụng cho các sản phẩm là robot hoặc thiết bị tự ñộng nhiều trục yêu cầu chuyển ñộng ñồng bộ trong quá trình hoạt ñộng. • Góp phần thúc ñẩy việc xây dựng các mô hình phục vụ cho công tác ñào tạo sinh viên. • Tạo ra phương pháp học tập nghiên cứu trực quan bằng mô hình cụ thể. Bước ñầu tiếp cận kĩ thuật ñiều khiển robot. 6. Cấu trúc của luận văn: Cấu trúc của luận văn gồm có bốn chương. - Chương 1: Trình bày tổng các vấn ñề quan về robot; về lịch sử phát triền robot công nghiệp; các khái niệm cơ bản của robot công nghiệp. - Chương 2: Trình bày cơ sở ñể chọn lựa cấu hình robot dựa trên các ñặc tính kĩ thuật yêu cầu. Sau khi xác ñịnh ñược mô hình robot, ta tiến hành tính toán ñộng học và ñộng lực học của mô hình robot ñó. Chương 3: Trình bày về các khái niệm về ñiều khiển ñồng bộ; thiết kế quy luật chuyển ñộng của các trục khớp. Trên cơ sở quy luật chuyển ñộng ñó, ñề ra mô hình ñiều khiển ñồng bộ các trục khớp thông qua việc xây dựng phương pháp ñiều khiển theo phương pháp trượt. Sau khi xây dựng mô hình toán học, tiến hành mô phỏng trên Matlab ñể kiểm nghiệm. - Chương 4. Chương này trình bày cơ sở thiết kế mạch ñiều khiển và chương trình ñiều khiển robot trên máy tính ñược lập trình bằng ngôn ngữ Visual Basic. 5 CHƯƠNG 1 - TỔNG QUAN 1.1. LỊCH SỬ PHÁT TRIỂN CỦA ROBOT CÔNG NGHIỆP 1.2. CÁC KHÁI NIỆM VÀ ĐỊNH NGHĨA VỀ ROBOT CÔNG NGHIỆP 1.3. KẾT CẤU CƠ BẢN CỦA MỘT ROBOT CÔNG NGHIỆP 1.3.1. Kết cấu chung 1.4. PHÂN LOẠI ROBOT CÔNG NGHIỆP 1.4.1. Phân loại theo kết cấu 1.4.2. Phân loại theo hệ thống truyền ñộng 1.4.3. Phân loại theo ứng dụng 1.4.4. Phân loại theo cách thức và ñặc trưng của phương pháp ñiều khiển CHƯƠNG 2 - LỰA CHỌN MÔ HÌNH ROBOT – TÍNH TOÁN ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC 2.1. LỰA CHỌN MÔ HÌNH ĐỘNG HỌC CỦA ROBOT 2.1.1. Yêu cầu kĩ thuật của robot 2.1.1.1.Sức nâng của tay máy Sức nâng của robot từ 0.1kg ñến khoảng 2kg. 2.1.1.2.Số bậc tự do của phần công tác Ta chỉ quan tâm nghiên cứu ñối với mô hình ñộng học robot có 4 bậc tự do. 2.1.1.3.Trường công tác của robot Hình chiếu bằng của trường công tác phải là một hình tròn ñể ñảm bảo việc robot có thể nhặt ñược tất cả các vật ở vị trí xung quanh gốc cố ñịnh của nó. 6 2.1.2. Lựa chọn mô hình ñộng học của robot Trong các loại kết cấu của robot mà ta nghiên cứu ở trên, robot của ta chỉ có thể có một trong các kết cấu kiểu tọa ñộ trụ, tọa ñộ cầu, SCARA, và kiểu tay người. Sau khi phân tích các mô hình ñã nêu trên, ta chọn mô hình kết cấu robot kiểu tay người ñể thực hiện. θ 1 θ 2 θ 3 θ 4 Hình 2.1. Mô hình ñộng học của robot PDU01 2.2. TÍNH TOÁN ĐỘNG HỌC CHO MÔ HÌNH ROBOT PDU01 2.2.1. Thiết lập phương trình ñộng học thuận về vị trí cho robot PDU01 2.2.1.1.Chọn hệ tọa ñộ cơ sở, gắn các hệ toạ ñộ trung gian lên các khâu a4 a3 a2 d1 Z 4 X 4 Y 4 O 4 Z 3 X 3 Y 3 O 3 Z 2 X 2 Y 2 O 2 O 1 Y 1 X 1 Z 1 O 0 Z 0 Y 0 X 0 θ 4 θ 3 θ 1 θ 2 Hình 2.3 . Hệ tọa ñộ trung gian trên các khâu 2.2.1.2.Lập bảng thông số DH Bảng 2.1. Bảng thông số DH của robot PDU01 Khâu θ i α i a i d i 1 θ 1 * 90 0 d 1 2 θ 2 * 0 a 2 0 3 θ 3 * 0 a 3 0 4 θ 4 * 0 a 4 0 7 2.2.1.3.Dựa vào các thông số của bảng DH, ta thiết lập các ma trận A i Như vậy ta có các ma trận A i 1 1 1 1 1 1 0 0 0 - 0 0 1 0 d 0 0 0 1 C S S C A     =       ; 2 2 2 2 2 2 2 2 2 - 0 a 0 a 0 0 1 0 0 0 0 1 C S C S C S A     =       3 3 3 3 3 3 3 3 3 - 0 a 0 a 0 0 1 0 0 0 0 1 C S C S C S A     =       ; 4 4 4 4 4 4 4 4 4 - 0 a 0 a 0 0 1 0 0 0 0 1 C S C S C S A     =       2.2.1.4.Tính các ma trận biến ñổi thuần nhất T 4 4 4 4 3 4 4 4 4 4 - 0 a 0 a 0 0 1 0 0 0 0 1 C S C S C S T     =       ; 34 34 4 34 3 3 34 34 4 34 3 3 2 4 - 0 a 0 a 0 0 1 0 0 0 0 1 C S C a C S C S a S T +     +   =       234 234 4 234 3 23 2 2 1 234 234 4 234 3 23 2 2 4 - 0 a 0 a 0 0 1 0 0 0 0 1 C S C a C a C S C S a S a S T + +     + + =       1 234 1 234 1 4 1 234 3 1 23 2 1 2 1 234 1 234 1 4 1 234 3 1 23 2 1 2 4 234 234 4 234 3 23 2 2 1 C S a a S 0 a 0 0 0 1 C C S C C a C C a C C S C S S C S C a S C a S C T C S a S a S d − + +     − − + + =   + + +       Vậy ta có hệ phương trình ñộng học thuận về vị trí của robot PDU01 như sau: 8 Bảng 2.2. Hệ phương trình ñộng học thuận của robot PDU01 n x = C 1 C 234 o x = - C 1 S 234 a x = S 1 p x = 4 1 234 3 1 23 2 1 2 a C C a C C a C C+ + n y = S 1 C 234 o y = -S 1 S 234 a y = -C 1 p y = 4 1 234 3 1 23 2 1 2 a S C a S C a S C+ + n z = S 234 o z = C 234 a z = 0 p z = 4 234 3 23 2 2 1 a S a S a S d+ + + 2.2.2. Phương trình ñộng học ngược về vị trí cho robot PDU01 2.2.2.1.Điều kiện ñể hệ phương trình có nghiệm 0 y y x x y x y x z z x y z o p a o a p o on o n n a  = − =    = − = −    =   2.2.2.2.Tìm nghiệm của hệ phương trình ñộng học Thực hiện việc giải phương trình ñộng học ngược bằng cách nhân lần lượt các ma trận nghịch ñảo của ma trận A i với ma trận T 4 ta giải ñược kết quả sau: 1 arctan 2( , ) x y a a θ = − θ 234 = - arctan2(C 1 o x +S 1 o y ,o z ) 2 2 2 2 3 2 2 3 3 2 2 3 2 =arctan2( 1 , ) x y P P a a C a a θ + − − ± − + ( ) ( ) ( ) ( ) ( ) ( ) 3 3 2 3 23 2 2 3 3 3 23 2 2 3 3 2 3 23 2 2 3 3 3 23 2 2 2 2 2 2 2 3 3 2 3 3 3 3 2 3 3 ( ) ( ) ( ) ( ) =arctan2( , ) a C a a S a S a S a C a C a C a a C a C a S a S a S a C a a S a C a a S θ + + − + + + + + + + + + 4 234 2 3 = θ θ θ θ − − 2.3. LỰA CHỌN PHƯƠNG ÁN TRUYỀN ĐỘNG CHO ROBOT 2.3.1.Phương án truyền ñộng cho khớp thứ nhất 9 Hình 2.4. Phương án truyền ñộng cho khớp thứ nhất 2.3.2.Phương án truyền ñộng cho khớp thứ hai Hình 2.5. Phương án truyền ñộng cho khớp thứ hai 2.3.3.Phương án truyền ñộng cho khớp thứ ba Hình 2.6. Phương án truyền ñộng cho khớp thứ ba 2.3.4.Phương án truyền ñộng cho khớp thứ tư Hình 2.7. Phương án truyền ñộng cho khớp thứ tư 10 2.4. ĐỘNG LỰC HỌC CỦA ROBOT PDU01 2. 4.1. Mô hình ñộng lực học của robot PDU01 d1 a4 a2 a3 l 3 l 1 Y θ 1 m dc2 m dc3 m dt m d c 4 M Z X O A B C D θ 2 Hình 2.8. Mô hình tính toán ñộng lực học của robot PDU01 2.4.2. Tính ñộng năng và thế năng cho từng chất ñiểm 2.4.2.1. Tính ñộng năng và thế năng của chất ñiểm A và B 2 2 1 2 3 1 1 ( ) 2 AB dc dc K m m l θ • = + 2 1 3 2 ( ) AB dc dc P m h m h g= + 2.4.2.2. Động năng và thế năng của chất ñiểm D 2 2 2 2 2 2 2 2 4 2 2 3 3 2 3 2 3 3 3 2 2 3 2 2 1 2 2 2 2 3 3 2 3 1 2 3 3 2 2 3 1 1 1 ( ) ( ) 2 ( )cos ( ) cos 2 2 ( ) cos ( ) 2 ( )cos cos( ) D D D dc K m v m a a l a a l a a l a a l θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ • • • • • • • • •  = = + − + + + − + +    + − + + − +   [ ] 4 1 2 2 3 3 2 3 sin ( )sin( ) D D D dc P m z g m d a a l g θ θ θ = = + + − + 2.4.2.3.Tính ñộng năng và thế năng cho chất ñiểm M 2 2 2 2 2 2 2 2 3 2 3 4 2 3 4 2 3 3 2 2 3 2 4 3 4 2 2 3 4 3 4 4 2 3 2 3 4 2 2 2 2 2 2 2 2 2 2 2 1 3 2 3 1 4 2 3 4 1 2 3 1 ( ) ( ) 2 cos ( ) 2 2 cos( ) ( ) 2 cos ( )( ) cos cos ( ) cos ( ) 2 cos M K M a a a a a a a a a a a a a a θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ • • • • • • • • • • • • • • • • • • • • •    = + + + + + + + + + + + + + + + + + + + + + + 2 2 2 3 1 2 2 2 4 2 2 3 4 1 3 4 2 3 2 3 4 1 cos( ) 2 cos cos( ) 2 cos( )cos( )a a a a θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ • • •    + + + + + + + + [ ] 1 2 2 3 2 3 4 2 3 4 sin sin( ) sin( ) M M P Mz g M d a a a g θ θ θ θ θ θ = = + + + + + + 11 2.4.3. Tính lực tổng quát tác dụng vào từng khớp 2.4.3.1. Hàm Lagrange và lực tổng quát ( ) j j i j j i i L L F t θ θ • ∂ ∂ ∂ = − ∂ ∂ ∂ ∑ ∑ 2.4.3.2. Tính các thành phần L j 2.4.3.4. Tính các lực tổng quát Đặt:  Tính lực tổng quát tác dụng lên khớp thứ nhất: Đặt: { 2 2 3 11 5 2 3 6 2 7 2 3 2 3 4 2 3 8 2 3 4 9 2 3 9 2 2 3 2 3 4 10 2 3 4 10 2 2 3 4 11 2 3 4 2 sin(2 ) sin2 2 sin(2 2 )( ) sin(2 2 2 )( ) 2 sin(2 ) 2 cos sin( ) 2 sin(2 ) 2 cos sin( )( ) 2 sin(2 2 C M M M M M M M M M θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ • • • • • • • • • • • = + − − + + − + + + + − + − + − + + − + + + − + + } 2 3 4 11 2 3 2 3 4 )( ) 2 cos( )sin( )M θ θ θ θ θ θ θ θ • • • + − + + + [ ] 12 2 2 3 2 3 13 3 2 3 sin 2 sin(2 2 ) sin(2 2 ) C M M C M θ θ θ θ θ = − + + = − + Khi ñó ta có: 1 1 2 3 1 11 11 12 13 F J C C C θ θ θ θ •• • • • = + + +  Tính lực tổng quát tác dụng lên khớp thứ hai: Đặt: 2 2 2 1 2 3 1 2 4 2 3 4 3 3 2 2 2 5 4 2 3 3 6 2 7 3 8 4 9 2 3 10 2 4 11 3 4 12 4 3 3 ( ) ; ; ( ) ; ( ); ; ; ; ; ; ; ( ) dc dc dc dc dc dc M m m l M m a M m a l M m a a l M Ma M Ma M Ma M Ma a M Ma a M Ma a M m a l = + = = − = − = = = = = = = − − ( ) ( ) ( ) ( ) 2 2 2 11 1 2 6 2 3 7 2 3 8 2 3 4 5 9 2 2 3 10 11 2 3 2 3 4 cos 2 cos ( ) cos ( ) 2 cos cos( ) 2 2 cos( )cos( ) J M M M M M M M M M M θ θ θ θ θ θ θ θ θ θ θ θ θ θ = + + + + + + + + − − + + + + + + 12 ( ) { } { } { } 22 2 3 9 5 3 6 7 8 10 3 4 11 4 23 3 5 3 7 8 9 3 10 3 4 11 4 24 8 10 3 4 11 4 ( ) 2 cos 2 cos( ) 2 cos ( ) cos cos cos( ) 2 cos ( ) cos( ) cos J M M M M M M M M M J M M M M M M M J M M M θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ = + + − + + + + + + = − + + + + + + = + + + } 21 2 2 3 2 3 5 2 3 6 2 7 2 3 8 2 3 4 9 2 3 10 2 3 4 11 2 3 4 1 1 1 1 sin 2 sin(2 2 ) sin(2 ) sin 2 2 2 2 1 sin(2 2 ) sin(2 2 2 ) sin(2 ) 2 sin(2 ) sin(2 2 ) C M M M M M M M M M θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ •  = + + − + +   + + + + + + + + + + + + + { } 3 22 5 3 9 3 3 10 3 4 3 10 3 4 4 11 4 4 2 sin 2 sin 2 sin( ) 2 sin( ) 2 sinC M M M M M θ θ θ θ θ θ θ θ θ θ θ θ • • • • • = − − + − + − { } 3 23 5 3 9 3 3 10 3 4 3 10 3 4 4 10 3 4 4 11 4 4 sin sin sin( ) sin( ) sin( ) 2 sin C M M M M M M θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ • • • • • • = − − + − + − + − { } 24 10 3 4 11 4 4 sin( ) sinC M M θ θ θ θ • = − + − 6 5 7 8 2 2 2 2 3 2 3 4 2 2 3 4 os os( ) os( ) M M M M G M c g c g c g a a a a θ θ θ θ θ θ     = + + − + + + + +         Khi ñó ta ñược: 2 3 4 1 2 3 4 2 22 23 24 21 22 23 24 2 F J J J C C C C G θ θ θ θ θ θ θ •• •• •• • • • • = + + + + + + +  Tính lực tổng quát tác dụng lên khớp thứ ba: Đặt: { } 32 3 5 3 7 8 9 3 10 3 4 11 4 cos cos cos( ) 2 cosJ M M M M M M M θ θ θ θ θ = − + + + + + + { } 33 3 7 8 11 4 2 cosJ M M M M θ = + + + { } 34 8 11 4 4 cosJ M M θ θ •• = + 31 3 2 3 5 2 2 3 7 2 3 8 2 3 4 9 2 2 3 10 2 2 3 4 1 sin(2 2 ) cos sin( ) sin(2 2 ) 2 1 sin(2 2 2 ) cos sin( ) cos sin( ) 2 C M M M M M M θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ  = + − + + +   + + + + + + + + 13 } 11 2 3 4 1 sin(2 2 )M θ θ θ θ • + + + { } 32 5 3 9 3 10 3 4 2 sin sin sin( )C M M M θ θ θ θ θ • = − + + + 33 11 4 4 2 sinC M θ θ • = − { } 34 11 4 4 11 4 2 sin 2 sinC M M θ θ θ θ • • = − − 7 8 3 12 2 3 2 3 4 3 4 os( ) os( ) M M G M c g c g a a θ θ θ θ θ   = − + + + +     Khi ñó ta ñược: 2 3 4 1 2 3 4 3 32 33 34 31 32 33 34 3 F J J J C C C C G θ θ θ θ θ θ θ •• •• •• • • • • = + + + + + + +  Tính lực tổng quát tác dụng lên khớp thứ tư: Đặt: { } 42 8 10 3 4 11 4 cos( ) cosJ M M M θ θ θ = + + + { } 43 8 11 4 cosJ M M θ = + 44 8 J M= 41 8 2 3 4 10 2 2 3 4 11 2 3 2 3 4 1 1 sin(2 2 2 ) cos sin( ) cos( )sin( ) 2 C M M M θ θ θ θ θ θ θ θ θ θ θ θ θ •   = + + + + + + + + +     { } 42 10 3 4 2 11 4 2 11 4 3 sin( ) sin 2 sinC M M M θ θ θ θ θ θ θ • • • = + + + 43 11 4 3 sinC M θ θ • = + 8 4 2 3 4 4 os( ) M G c g a θ θ θ = + + Khi ñó ta ñược: 2 3 4 1 2 3 4 42 43 44 41 42 43 4 F J J J C C C G θ θ θ θ θ θ •• •• •• • • • = + + + + + + 14 2.5. TÍNH TOÁN BỘ TRUYỀN ĐỘNG CƠ KHÍ CHO ROBOT 2.5.1.Các thông số kĩ thuật Vận tốc n trục = 6 vòng/phút. Tỉ số truyền n ñộngcơ /n trục = 3. Tốc ñộ ñộng cơ cần thiết là n ñc = 18vòng/phút ( sử dụng ñộng cơ có hộp giảm tốc gắn liền). Môment xoắn cực ñại trên trục khớp robot là T = 5N.m = 5000N.mm. Hiệu suất truyền trên bộ truyền ñai răng là η 1 = 0.95. Hiệu suất truyền trên ổ lăn là η 2 = 0.99. 2.5.2.Tính toán bộ truyền ñai cho khớp thứ nhất Tính công suất ñộng cơ: Trên cơ sở ñó, ta chọn ñộng cơ model EYQF-33300-641của hãng Colman với các thông số như sau: - Tốc ñộ n = 71.67 RPM; công suất P = 10.7W. - Puli nhỏ của hệ truyền ñai ñược gắn trực tiếp lên trục ra của ñộng cơ, do ñó vận tốc cực ñại của puli nhỏ n = 18 RPM Dựa vào công suất và tốc ñộ của bộ truyền ta chọn dây ñai loại L. Với tỉ số truyền n = 3. Với loại puli nhỏ gắn liền trục ñộng cơ ñã chọn có z 1 = 12. Do ñó ta chọn bánh ñai lớn có z 2 = 36. Khoảng cách giữa trục ñộng cơ và trục gắn bánh ñai lớn là 150mm. Ta sử dụng chương trình tính toán bộ truyền ñai trên ta ñược: Bánh ñai nhỏ: • z 1 = 12. • Đường kính vòng chia d 1 = 19.09mm Bánh ñai lớn: 6 6 1 2 5000 18 10 10 0.01 10 9.55 9.55 0.95 0.99 Tn x P KW W x x ηη − − = = = = 15 • Z 2 = 36 • Đường kính vòng chia d 2 = 57.29mm Dây ñai chọn loại dây L, chiều dài dây ñai là 420mm. Khoảng cách thực giữa hai trục là148.77mm. 2.5.3.Tính toán bộ truyền ñai cho khớp thứ hai Kết quả tương tự như ở trục khớp thứ nhất. 2.5.4.Tính toán bộ truyền ñai cho khớp thứ ba Tính bộ truyền ñai từ ñộng cơ ñến bánh ñai trung gian: Kết quả tương tự bộ truyền ñai ở trục khớp thứ nhất và thứ hai Tính bộ truyền ñai từ bánh ñai trung gian ñến bánh ñai gắn trên trục khớp thứ 3 Bánh ñai thứ nhất có: - z 1 = 36. - Đường kính vòng chia 57.26mm Bánh ñai thứ hai có các thông số giống như bánh ñai thứ nhất. Sử dụng dây ñai loại L có chiều dài dây ñai là 725mm. Khoảng cách thực giữa hai trục là 272mm. 2.5.5.Tính toán bộ truyền ñai cho khớp thứ tư Ta ñược các kết quả giống như ở bộ truyền ñai cho khớp thứ nhất và khớp thứ 2. CHƯƠNG 3 - XÂY DỰNG MÔ HÌNH TOÁN HỌC ĐIỀU KHIỂN ĐỒNG BỘ CÁC TRỤC 3.1. KHÁI NIỆM ĐIỀU KHIỂN ĐỒNG BỘ CÁC TRỤC 3.2. THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CỦA CÁC KHỚP 3.3. MÔ HÌNH TOÁN HỌC ĐIỀU KHIỂN ĐỒNG BỘ CÁC TRỤC CỦA ROBOT 3.3.1. Phân tích bài toán Các yêu cầu mà bài toán ñặt ra ñối với robot của ta là: 16  Khớp thứ nhất di chuyển một góc δθ 1 = θ 1t - θ 1s  Khớp thứ nhất di chuyển một góc δθ 2 = θ 2t - θ 2s  Khớp thứ nhất di chuyển một góc δθ 3 = θ 3t - θ 3s  Khớp thứ nhất di chuyển một góc δθ 4 = θ 4t - θ 4  Yêu cầu kĩ thuật là vận tốc của các biến khớp trong quá trình chuyển ñộng phải không ñược lớn hơn vận tốc giới hạn ( v ≤ v gh ) 3.3.2. Thiết kế quỹ ñạo cho các biến khớp 3.3.2.1.Thiết kế quỹ ñạo cho các biến khớp Quỹ ñạo ña thức bậc 3 của biến khớp thư i có dạng: 2 3 0 0 0 ( ) ( ) ( ) ( ) i i i i i i i i q t a b t t c t t d t t= + − + − + − Với các ràng buộc ban ñầu: 0 0 0 0 ( ) ; ( ) ; ( ) ; ( ) i i i i i f if f i i i if q t q q t q q t q q t q • • • • = = = = Với δt i = t if – t i0 Từ ñó tính toán ta ñược: 2 1 uctri 3 i c i c q d • = − ( ) ( ) 2 0 0 1 uctri 0 0 3 (2 ) 6 3( ) if i i i if c i if i i if i q q q q t q t q q q q t δ δ δ • • • • •   − − +     ⇒ =   − − +     Trong cả hai trường hợp trục khớp chuyển ñộng theo chiều dương và âm thì ta ñều có : ( ) ( ) 2 0 0 ax 0 0 3 (2 ) 6 3( ) if i ii if m i if i i if i q q q q t v t q q q q t δ δ δ • • • •   − − +     = − − + Theo ñiều kiện kĩ thuật thì vận tốc này phải không ñược lớn hơn vận tốc giới hạn ñặt ra. Như vậy : ( ) ( ) 0 0 0 0 0 0 2 3 3 (2 ) ( ) 2 ; ; ; if i i i if i i if if i i i i i i i i i q q q q t q q t q q a q b q c d t t δ δ δ δ • • • • • − − + + − − ⇒ = = = = 17 ( ) ( ) 2 0 0 ax 0 0 3 (2 ) 6 3( ) if i ii if im gh i if i i if i q q q q t v v t q q q q t δ δ δ • • • •   − − +     = ≤ − − + Trong trường hợp thực tế người ta dùng ñể ñiều khiển robot, vận tốc khởi ñầu và kết thúc ñều bằng không, do ñó ta có: Như vậy, với 4 biến khớp, ta có: 0 3 2 if i i gh q q t v δ − ≥ Để các biến khớp ñạt ñến giá trị cho trước cùng một thời ñiểm ta phải chọn sao cho: 1 2 3 4 t t t t t δ δ δ δ δ = = = = Để các trục khớp ñạt ñược giá trị cho trước trong thời gian nhanh nhất, ta chọn: 0 1,4 1 2 3 4 3 ax( , , , ) ax( ) 2 jf j j gh q q t m t t t t m v δ δ δ δ δ = − = = ( ) 0 1,4 3 ax 2 jf j j gh m q q t v δ = − ⇔ = ( ) ( ) ( ) ( ) 0 0 2 3 0 0 0 2 3 0 0 1,4 1,4 3 2 ( ) ( ) ( ) 3 ax 3 ax 2 2 if i if i i i i i jf j jf j j j gh gh q q q q q t q t t t t m q q m q q v v = = − − = + − − −     − −                 Phương trình trên chính là quy luật chuyển ñộng của các biến khớp mà ta dùng ñể ñiều khiển ñồng bộ các trục của robot mà ta cần. ( ) ( ) 2 0 ax 0 3 2 if i im gh i if i q q v v t q q δ − = ≤ − 0 3 2 if i i gh q q t v δ − ⇔ ≥ 18 3.4. XÂY DỰNG LUẬT ĐIỀU KHIỂN CÁC KHỚP 3.4.1. Hệ phương trình trạng thái chuyển ñộng của mỗi khớp ñộng Xét sơ ñồ ñộng của ñộng cơ ñiện một chiều với tín hiệu vào là ñiện áp U a (t) ñặt vào phần ứng, tín hiệu ra là góc quay θ m của trục ñộng cơ; ñộng cơ kiểu kích từ ñộc lập. J m M m (t) θ m (t) L a (t) R a (t) U a (t) e b (t) U f (t) R f (t) L f (t) Hình 3.9.Sơ ñồ ñộng của ñộng cơ ñiện một chiều Trong thực tế, các trục khớp của ta ñược truyền ñộng từ ñộng cơ thông qua hệ thống bánh ñai và dây ñai răng. M m (t) θ m (t) θ L (t) M L (t) J m J L (t) Hình 3.10. Sơ ñồ truyền ñộng Tỉ số truyền là n. Bỏ qua ma sát. θ L là góc quay của trục khớp. Tính toán ta ñược ( ) ( ) ( ) ( ) a a a m m m a b a a a L J t R J t L dJ t u t K K K K dt θ θ θ ••• •• •   = + + + +     19 Đặt: Ta có phương trình trạng thái của hệ thống 1 2 2 2 1 ( ) ( ) ( ) ( ) ( ) ( ) a a a a a b a a a a a a dx x dt K K R J t L K K dx dJ t u t x x dt L J t L J t K K dt L J t  =       = − + −       và 1 m y x θ • = = 3.4.1.1. Hệ phương trình trạng thái chuyển ñộng cho khớp thứ nhất 11 12 11 1112 1 12 11 11 11 11 ( ) a a a a b a a a a dx x dt K R J L J K Kdx u t x x dt L J L J L J •  =      +    = − −       và 1 1 11 m y x θ • = = 3.4.1.2. Hệ phương trình trạng thái chuyển ñộng cho khớp thứ hai 21 22 22 22 22 2 22 21 22 22 22 ( ) a a a a b a a a a dx x dt K R J L J K K dx u t x x dt L J L J L J •  =      +    = − −       và 2 2 21 m y x θ • = = 1 1 2 2 3 m m m y x dx x dt dx x dt θ θ θ • •• •••   = =   = =    = =   20 3.4.1.3. Hệ phương trình trạng thái chuyển ñộng cho khớp thứ ba 31 32 33 32 33 3 32 31 33 33 33 ( ) a a a a b a a a a dx x dt dx K R J L J K K u t x x dt L J L J L J •  =      +    = − −      và 3 3 31 m y x θ • = = 3.4.1.4. Hệ phương trình trạng thái chuyển ñộng cho khớp thứ tư 41 42 42 4 42 41 44 44 ( ) a a a b a a a a dx x dt K R K K dx u t x x dt L J L L J  =     = − −   và 4 4 41 m y x θ • = = 3.4.2. Thiết kế bộ ñiều khiển dùng ñiều khiển vận tốc của các trục khớp Ta có mô hình ñối tượng cần ñiều khiển như sau: 1 2 2 2 1 ( ) ( ) ( ) ( ) ( ) ( ) a a a a a b a a a a a a dx x dt K K R J t L K Kdx dJ t u t x x dt L J t L J t K K dt L J t  =       = − + −       và 1 m y x θ • = = e(t) = y d (t) – y(t) là sai lệch giữa tín hiệu ñặt và tín hiệu ñiều khiển. Do mô hình của ta là bậc 2 nên ta sử dụng hàm trượt có dạng: Với k 1 > 0 Từ hàm trượt s ta có: 1 de s k e dt = + .  ; 4 4 4 4 4 4 4 4 4 - 0 a 0 a 0 0 1 0 0 0 0 1 C S C S C S A     =       2.2.1 .4. Tính các ma trận biến ñổi thuần nhất T 4 4 4 4 3 4 4 4 4 4 -. 34 34 4 34 3 3 34 34 4 34 3 3 2 4 - 0 a 0 a 0 0 1 0 0 0 0 1 C S C a C S C S a S T +     +   =       2 34 2 34 4 2 34 3 23 2 2 1 2 34 2 34 4 234