Bao cao Bài tập lớn Robotic

1 MỤC LỤC LỜI MỞ ĐẦU..................................................................................................... 3 CHƯƠNG 1: KHẢO SÁT BÀI TOÁN ĐỘNG HỌC..................................... 4 1.1. Xây dựng mô hình khảo sát bài toán động học.................................... 4 1.1.1. Mô hình khảo sát........................................................................................... 4 1.1.2. Xác định hệ trục tọa độ ................................................................................. 5 1.1.3. Xác định bộ thông số DenavitHartenbeg.................................................... 5 1.2. Bài toán động học thuận......................................................................... 6 1.2.1. Tìm vị trí điểm thao tác biểu diễn theo các tọa độ khớp, hướng của khâu thao tác ........................................................................................................ 6 1.2.2.Vận tốc điểm tác động cuối E và vận tốc góc các khâu................................ 8 1.2.3. Tính gia tốc điểm tác động cuối. Tính gia tốc khâu thao tác.................... 10 1.3. Bài toán động học ngược...................................................................... 11 1.3.1 Tính các tọa độ khớp.................................................................................... 12 1.3.2. Vận tốc dài của các khâu tương ứng với các khớp tịnh tiến và vận tốc góc ứng với các khớp quay.............................................................................. 14 1.3.3. Gia tốc dài của các khâu tương ứng với các khớp tịnh tiến và gia tốc góc ứng với các khớp quay.............................................................................. 15 CHƯƠNG 2: TĨNH HỌC ROBOT ................................................................ 17 2.1. Mô hình khảo sát bài toán tĩnh học..................................................... 17 2.2. Tính toán bài toán tĩnh học.................................................................. 18 CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG ............................. 22 3.1. Thiết kế quỹ đạo trong không gian khớp ........................................... 22 3.2. Thiết kế trong không gian thao tác ..................................................... 24 CHƯƠNG 4: ĐỘNG LỰC HỌC ROBOT .................................................... 28 4.1 Các tham số động lực học ..................................................................... 28 4.2. Ma trận Jacobi tịnh tiến của các khâu ............................................... 28 4.3. Tính ma trận jacobi quay của các khâu ............................................. 30 4.4. Tính tensor quán tính của các khâu.................................................... 31 4.5. Tính ma trận khối lượng...................................................................... 32 4.6. Tính toán thế năng, lực thế .................................................................. 33 CHƯƠNG 5: ĐIỀU KHIỂN ........................................................................... 342 5.1 Điều khiển phản hồi và điều khiển vòng kín....................................... 34 KẾT LUẬN....................................................................................................... 413 LỜI MỞ ĐẦU Với sự phát triển mạnh của nền công nghiệp Robot thì môn học Robotícs trang bị cho sinh viên những kiến thức cơ sở về tính toán và thiết kế Robot. Đây là những kiến thức cơ sở hết sức quan trọng. Trong bài tập lớn này đề cập tới các bài toán động học, tĩnh học, động lực học, thiết kế quỹ đạo chuyển động và điều khiển phục vụ cho quá trình thiết kế cũng như thực tiễn có thể gặp phải trong công việc. Bài báo cáo chọn mô hình robot 3 bậc tự do làm mô hình nghiên cứu chính. Để thực hiện bài tập lớn này, em xin cám ơn thầy giáo PGS.TS Phan Bùi Khôi đã hướng dẫn và giúp đỡ trong quá trình học tập và nghiên cứu. Hà Nội, Ngày ,Tháng , Năm Sinh viên thực hiện4 CHƯƠNG 1: KHẢO SÁT BÀI TOÁN ĐỘNG HỌC Trong chương 1 giải quyết các vấn đề sau: 1. Tính số bậc tự do của robot. 2. Vẽ các hệ trục tọa độ gắn liền với khâu theo quy tắc DH. 3. Lập bảng DH. Tính các ma trận DH. 4. Tìm vị trí điểm thao tác biểu diễn theo tọa độ khớp. Xác định hướng của khâu thao tác. 5. Tính vận tốc điểm tác động cuối E. Tính vận tốc góc các khâu. 6. Tính gia tốc điểm tác động cuối E. Tính gia tốc góc khâu thao tác. 7. Cho vị trí, vận tốc, gia tốc điểm tác động cuối, hướng, vận tốc góc, gia tốc góc khâu thao tác 8. Tính các tọa độ khớp 9. Tính vận tốc, gia tốc dài của các khâu tương ứng với các khớp tịnh tiến 10.Tính vận tốc góc, gia tốc góc của các khâu ứng với các khớp quay 1.1. Xây dựng mô hình khảo sát bài toán động học 1.1.1. Mô hình khảo sát Số bậc tự do: DoF=6.(33)+3.1=35 Robot có 2 khớp quay và 1 khớp tịnh tiến 1.1.2. Xác định hệ trục tọa độ 1.1.3. Xác định bộ thông số DenavitHartenbeg Từ mô hình và hệ trục tọa độ trên ta có bảng thông số DH như sau Joint i di ai i 1 1 d1 a1 0 2 0 d2 0 0 3 3 0 a3 0 Trong đó: Các thông số được biển diễn như trên hình vẽ Các biến khớp tương ứng là là   1 2 3 , , d6 Dựa vào bảng thông số động học trên, xây dựng được các ma trận biến đổi tọa độ thuần nhất như sau 1 1 1 1 0 1 1 1 1 1 1 cos( ) sin( ) 0 cos( ) sin( ) cos( ) 0 sin( ) 0 0 1 0 0 0 1 a a A d                     1 2 2 1 0 0 0 0 1 0 0 0 0 1 0 0 0 1 A d              3 3 3 3 2 3 3 1 3 3 cos( ) sin( ) 0 cos( ) sin( ) cos( ) 0 sin( ) 0 0 1 0 0 0 0 1 a a A                     1.2. Bài toán động học thuận 1.2.1. Tìm vị trí điểm thao tác biểu diễn theo các tọa độ khớp, hướng của khâu thao tác Ta có các ma trận biến đổi thuần nhất biểu diễn vị trí và hướng của hệ tọa độ khâu thứ i đối với hệ tọa độ cơ sở O x y z 0 0 0 0 là: 1 1 1 1 0 1 1 1 1 1 1 cos( ) sin( ) 0 cos( ) sin( ) cos( ) 0 sin( ) 0 0 1 0 0 0 1 a a A d                    7 1 1 1 1 0 0 1 1 1 1 1 2 1 2 1 2 cos( ) sin( ) 0 cos( ) sin( ) cos( ) 0 sin( ) . 0 0 1 0 0 0 1 a a A A A d d                       1 3 1 3 1 1 3 1 3 0 0 1 1 3 1 3 1 1 3 1 3 3 1 2 1 2 cos( ) sin( ) 0 cos( ) a cos( ) sin( ) cos( ) 0 sin( ) a sin( ) . 0 0 1 0 0 0 1 a a A A A d d                                      Từ ma trận trên ta xác địnhđược Ma trận biểu diễn vị trí điểm thao tác theo các tọa độ khớp 1 1 3 1 3 0 1 1 3 1 3 1 2 cos( ) a cos( ) sin( ) a sin( ) E E E E x a r y a z d d                                  Hướng của tọa độ khâu thao tác so với hệ tọa độ cơ sở được các định từ các góc RollPitchYaw, với kí hiệu      Roll Pitch Yaw , , và các phép quay tương ứng như sau: Mô tả các phép quay RPY8 cos .cos sin .cos cos .sin .sin sin .sin cos .sin .cos sin .cos cos .cos sin .sin .sin cos .sin sin .sin .cos sin cos .sin cos .cos RRPY                                                Để tính được các góc   , , thì sẽ so sánh với ma trận quay của khâu cuối: 1 3 1 3 0 3 1 3 1 3 cos( ) sin( ) 0 sin( ) cos( ) 0 0 0 1 R                         Giải các hệ phương trình tìm được các góc như sau: 1 3 1 3 0 0 sin( ) arctan( ) cos( )                     1.2.2.Vận tốc điểm tác động cuối E và vận tốc góc các khâu  Xác định vận tốc điểm tác động cuối E Vận tốc điểm tác động cuối E có được từ việc đạo hàm theo t ma trận định vị củađiểm tác động cuối E, nhận được ma trận biểu diễn vận tốc tuyệt đối trong hệ cơ sở:             3 1 3 1 3 1 1 1 0 0 3 1 3 1 3 1 1 1 2 .sin . .sin cos . .cos E E a a V r a a d                                   Xác định vận tốc góc các khâu. Các ma trận quay biểu diễn hướng của hệ tọa độ khâu i so với hệ tọa độ cơ sởO x y z 0 0 0 09         1 1 0 1 1 1 cos sin 0 sin cos 0 ; 0 0 1 R                         1 1 0 2 1 1 cos sin 0 sin cos 0 ; 0 0 1 R                         1 3 1 3 0 3 1 3 1 3 cos sin 0 sin cos 0 0 0 1 R                         Đạo hàm của các ma trận quay         1 1 1 1 0 1 1 1 1 1 .sin .cos 0 .cos .sin 0 0 0 0 R                               1 1 1 1 0 2 1 1 1 1 .sin .cos 0 .cos .sin 0 0 0 0 R                                       1 3 1 1 3 1 3 1 3 0 3 1 3 1 3 1 3 1 3 .sin .cos 0 .cos .sin 0 0 0 0 R                                          Chuyển vị của các ma trận quay         1 1 0 1 1 1 cos sin 0 sin cos 0 0 0 1 R T                10         1 1 0 2 1 1 cos sin 0 sin cos 0 0 0 1 R T                         1 3 1 3 0 3 1 3 1 3 cos sin 0 sin cos 0 0 0 1 R T                         Các ma trận sóng tương ứng 1 ~ 0 0 1 1 1 1 0 0 . 0 0 0 0 0 R RT                 1 ~ 0 0 2 2 2 1 0 0 . 0 0 0 0 0 R RT                 1 3 ~ 0 0 3 3 3 1 3 0 0 . 0 0 0 0 0 R RT                      Vận tốc của các khâu trong hệ cố định là 1 2 3 1 1 1 3 0 0 0    0 ; 0 ; 0                                       1.2.3. Tính gia tốc điểm tác động cuối. Tính gia tốc khâu thao tác  Xác định gia tốc điểm tác động cuối Gia tốc điểm tác động cuối E có được từ việc đạo hàm theo t ma trận biểu diễn vận tốc của điểm tác động cuối E trong hệ cơ sở, nhận được ma trận biểu diễn gia tốc của điểm tác động cuối E trong hệ cơ sở:11 2 2 3 1 3 1 3 3 1 3 1 3 1 1 1 1 1 1 2 0 0 2 3 1 3 1 3 3 1 3 1 3 1 1 1 1 1 1 2 sin cos sin cos cos sin cos sin E E a a a a a v a a a a d                                                                                                        Xác định gia tốc góc khâu thao tác Gia tốc góc khâu thao tác có được từ việc đạo hàm theo t ma trận biểu diễn vận tốc góc của khâu thao tác trong hệ tọa độ cơ sở, nhận được ma trận biểu diễn gia tốc góc khâu thao tác trong hệ tọa độ cơ sở: 3 1 3 0 0    E E                   1.3. Bài toán động học ngược Đã biết     , , , , , , T E E E T E E E T E E E p x y z p x y z p x y z                  Cần tìm12     1 2 3 1 2 3 1 2 3 , , , , , , T T T q q q q q q q q q q q q                  1.3.1 Tính các tọa độ khớp Ta có: 1 1 3 1 3 0 1 1 3 1 3 1 2 cos( ) a cos( ) sin( ) a sin( ) E E E E x a r y a z d d                                  Rút ra các hệ phương trình sau: 1 1 3 1 3 1 1 3 1 3 1 2 cos( ) a cos( ) x (1) sin( ) a sin( ) y (2) (3) E E E a a d d z                    Trong đó q q d q 1 1 2 2 3 3      ; ; là các biến khớp cần giải a a d y z 1 3 1 ; ; ;x ; ; E E E là các số liệu cho trước Bình phương 2 vế của phương trình (1) và (2) sau đó cộng 2 phương trình này lại ta được phương trình sau a a a a x y 1 3 1 3 1 1 3 1 1 3 2 2 2 2        2 cos cos sin sin               E E 2 2 2 2      a a a a x y 1 3 1 3 3 2 cos E E 2 2 2 2 1 3 3 1 3 cos 2 x y a a E E a a      13 Ta có 2 sin 1 cos   3 3    2 2 2 2 1 3 1 3 3 2 2 2 2 1 3 1 3 1 2 arctan 2 E E E E x y a a a a q x y a a a a                               Biến đối phương trình (1) (2) đưa về dạng sau     1 1 3 1 3 1 3 1 1 3 1 3 1 3 cos cos cos sin sin sin sin cos cos sin E E a a x a a y                      1 3 3 3 3 1 2 2 1 3 1 3 3 1 3 3 3 3 1 2 2 1 3 1 3 3 ( cos ) sin cos 2 cos (a a cos ) sin sin 2 cos E E E E x a a y a a a a a y x a a a a a                               1 3 3 3 3 1 1 3 3 3 3 cos sin arctan cos sin E E E E y a a x a q x a a y a                   Từ phương trình (3)  q z d 2 1   E Như vậy ta đã hoàn thành xác định được các tọa độ khớp như sau14     1 3 3 3 3 1 1 3 3 3 3 2 1 2 2 2 2 1 3 1 3 3 2 2 2 2 1 3 1 3 cos sin arctan cos sin 1 2 arctan 2 E E E E E E E E E y a a x a q x a a y a q z d x y a a a a q x y a a a a                                                              1.3.2. Vận tốc dài của các khâu tương ứng với các khớp tịnh tiến và vận tốc góc ứng với các khớp quay Ta có các phương trình động học sau     1 1 1 3 1 3 2 1 1 3 1 3 3 1 2 cos cos 0 cos cos 0 0 E E E f a a x f a a y f d d z                          Áp dụng công thức q J J p  q P 1 để tính vận tốc các biến khớp 1 1 1 1 2 3 2 2 2 1 2 3 3 3 3 1 2 3 q f f f p p p f f f J p p p f f f p p p                                              1 1 3 1 3 3 1 3 1 1 3 1 3 3 1 3 sin sin 0 sin cos cos 0 cos 0 1 0 a a a a a a                              15 1 1 1 1 2 3 2 2 2 1 2 3 3 3 3 1 2 3 1 0 0 0 1 0 0 0 1 p f f f p p p f f f J p p p f f f p p p                                                            1 3 1 3 1 1 3 1 2 3 1 1 1 3 3 1 3 3 1 1 1 3 3 sin cos sin sin sin cos cos sin E E q p E E E E E y x q a q q J J p z q a y a y a x a x a a                                                     1.3.3. Gia tốc dài của các khâu tương ứng với các khớp tịnh tiến và gia tốc góc ứng với các khớp quay Ta có: 1 p p q q p p q q p p J q J p J q J q J p J p q J J q J p J p                  Trong đó                 1 1 1 3 1 3 1 3 3 1 3 1 3 1 1 1 3 1 3 1 3 3 1 3 1 3 cos cos 0 cos sin sin 0 sin 0 0 0 q a a a J a a a                                           16 0 0 0 0 0 0 0 0 0 p J                    1 3 1 3 1 3 1 3 1 1 3 3 1 3 1 3 3 1 3 1 3 3 1 3 3 cos sin 0 sin sin 0 0 1 cos cos sin sin 0 sin sin P a a J a a a a a a a a                                         17 CHƯƠNG 2: TĨNH HỌC ROBOT 2.1. Mô hình khảo sát bài toán tĩnh học Cho trước các thông số: ➢ Coi các thanh là thanh đồng chất, tiết diện ngang không đáng kể, khối lượng các khâu là m1, m2, m3. ➢ Khâu 2 có chiều dài L2 Cho lực tác dụng từ robot lên môi trường F, M: 0 0 0 0 0 0 4,3 0 0 4,3 0 0 0 0 0 0 T T y z y z T T y y F F F F F F M M M M                      18 2.2. Tính toán bài toán tĩnh học  Biễu diễn các lực Pi:             0 0 0 1 2 3 0 0 1 1 1 1 0 0 2 2 2 2 0 0 3 3 3 3 0 0 , 0 0 , 0 0 . 0 0 . 0 0 . 0 0 T T T T T T g g g g g g P m g m g P m g m g P m g m g                 Biễu diễn các vector i i i i r r i ci 1; trong hệ tọa độ các khâu       1 1 1 1 1 0 1 1 1 2 2 2 2 2 1 2 2 3 3 3 3 3 2 3 3 0 , 0 0 2 0 0 , 0 0 2 0 0 , 0 0 2 c c c a r a d r d r d r a r a r                                 Tính lực và momen của khâu 2 tác dụng lên khâu 3 Hệ phương trình cân bằng dạng ma trận khảo sát trong hệ tọa độ cơ sở: 0 0 0 3,2 4,3 3 ~ ~ 0 0 0 3 0 0 3 0 2 3 3,2 4,3 3,2 3 . . c F F P M M r F r P           Trong đó: 3 1 3 0 3 0 3 3 2 3 2 3 1 3 cos( ) sin( ) 0 a r R r a                    19 0 3 3 1 3 ~ 2 3 1 3 3 1 3 3 1 3 0 0 sin( ) 0 0 cos( ) sin( ) cos( ) 0 a r a a a                          3 1 3 0 3 0 3 3 3 1 3 3 3 3 cos( ) 2 sin( ) 2 0 c c a a r R r                             3 1 3 0 3 ~ 3 1 3 2 3 1 3 3 1 3 sin( ) 0 0 2 cos( ) 0 0 2 sin( ) cos( ) 0 2 2 a a r a a                                   0 0 0 3,2 0 0 3 3 0 1 3 3 3 1 3 3 3 0 0 0 3,2 1 3 3 3 1 3 3 3 0 1 3 3 0 0 0 0 1 sin( ) ( ) sin( ) 2 1 cos( ) ( ) cos( ) 2 cos( ) y y z z z y z y F F F F m g F m g a m g F a m g M M a m g F a m g a F                                                                                              Tính lực và momen của khâu 1 tác dụng lên khâu 2 Hệ phương trình cân bằng dạng ma trận khảo sát trong hệ tọa độ cơ sở 0 0 0 2,1 3,2 2 ~ ~ 0 0 0 2 0 0 2 0 1 2 2,1 3,2 2,1 2 . . c F F P M M r F r P           Trong đó:20 0 2 2 ~ 0 2 0 2 2 1 1 2 1 2 2 2 0 2 ~ 0 2 0 2 2 2 2 2 2 2 2 0 0 2,1 0 3 2 0 1 3 3 3 0 2,1 0 0 0 0 0 0 0 0 0 0 0 2 0 0 0 0 2 0 0 0 2 0 1 sin( ) ( ) sin( 2 c c c y z z d r R r r d d L L r R r r L F F F m g m g a m g F M                                                                                       0 1 3 3 3 2 0 0 1 3 3 3 1 3 3 3 0 1 3 3 ) 1 cos( ) ( ) cos( ) 2 cos( ) y y z y a m g d F M a m g F a m g a F                                              Tính lực và momen của khâu 0 tác dụng lên khâu 1 Hệ phương trình cân bằng dạng ma trận khảo sát trong hệ tọa độ cơ sở 0 0 0 1,0 2,1 1 ~ ~ 0 0 0 1 0 0 1 0 0 1 1,0 2,1 1,0 1 . . c F F P M M r F r P          Trong đó: 1 1 0 1 0 1 1 0 1 0 1 1 1 cos sin a r R r a d                 21 0 1 1 1 1 ~ 0 1 1 1 1 1 1 1 0 sin 0 cos sin cos 0 d a r d a a a                    1 1 0 1 0 1 1 1 1 1 1 1 cos 2 sin 2 0 c c a a r R r                         1 1 0 1 ~ 1 1 1 1 1 1 1 sin 0 0 2 cos 0 0 2 sin cos 0 2 2 c a a r a a                           0 0 1,0 0 3 2 1 0 0 0 0 13 3 3 13 3 3 2 1 1 1 1 2 3 1 1 1 0 0 0 0 1,0 13 3 3 13 3 3 1 1 1 2 3 1 1 1 0 13 3 0 1 1 ( ) ( ) ( ) ( ) 2 2 1 1 ( ) ( ) ( ) ( ) 2 2 ( ) y z z y y z y z z y F F F m g m g m g s a m g F s a m g d F d F a s m g m g m g F a s m g M M c a m g F c a m g a c m g m g m g F a c m g c a F                                            0 a c F 1 1  y                              22 CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG 3.1. Thiết kế quỹ đạo trong không gian khớp Cho 2 điểm A, B bất kì trong không gian làm việc, biết tọa độ điểm thao tác ( x y z E E E , , ) và hướng của khâu thao tác. Thiết kế quỹ đạo chuyển động bất kì từ A đến B, chỉ quan tâm đến các điều kiện về vị trí,vận tốc gia tốc tại điểm đầu và điểm cuối. Chọn quỹ đạo thiết kế là đa thức bậc 3 theo thời gian có dạng: q t a bt c t d t i i i i i ( )     2 3 với i = 1,2,3. Ứng với 3 biến khớp là  1 2 3 , , d Ta được hệ phương trình quỹ đạo chuyển động : 2 3 1 1 1 1 1 2 3 2 2 2 2 2 2 3 3 3 3 3 3 ( ) ( ) ( ) q t a b t c t d t q t a b t c t d t q t a b t c t d t                  Yêu cầu thời gian robot đi từ A đến B trong t(s) và vận tốc tại 2 điểm A, B bằng 0. Từ đó ta có:   2 3 2 (0) ( ) ( ) ( ) 0 ( ) 0 ( ) ( ) 2 3 0 i i i i i i i i i i i i i i i i i i q q A a q t q B a b t c t d t q q B b q t q B a b t c t d t                            2 3 ( ) 0 3 ( ) ( ) 2 ( ) ( ) i i i i i i i i i a q A b q B q A c t q B q A d t                Cho các thông số của robot: a a d 1 3 1    0.2, 0.2, 0.223 Giả sử Robot cần dịch chuyển từ điểm A(0.2932, 0.2250, 0.3) tới điểm B(0.3, 0.1732, 0.35) trong thời gian t=5(s). Giải động học ngược ta được 1 2 3 2 ( ) 3 ( ) 0.1 ( ) 4 q A q A q A            và 1 2 3 5 ( ) 3 ( ) 0.15 ( ) 3 q B q B q B            Từđó ta tính được các hệ số của phương trình quỹ đạo cho khâu 1: 1 1 1 1 2.0944 0 0.377 0.0503 a b c d            => 2 3 q t t t t 1( ) 2.0944 0 0.377 0.0503     Tương tự ta tính được 2 3 1 2 3 2 2 3 3 ( ) 2.0944 0 0.377 0.0503 1 ( ) 0.1 0 0.006 1250 ( ) 0.7854 0 0.0314 0.0042 q t t t t q t t t t q t t t t                    Sử dụng matlab để vẽ đồ thị: Đồ thị khớp 124 Đồ thị khớp 2 Đồ thị khớp 3 3.2. Thiết kế trong không gian thao tác Thiết kế quỹ đạo cho điểm tác động cuối đi từ điểm A tới điểm B nhận AB làm đường kính. Dùng phương trình đường tròn trong không gian là mặt phẳng giữa 2 điểm A x y B x y ( , ,0), ( , ,0) 0 0 e e lấy AB làm đường kính. Phương trình đường tròn ( ) ( ) x x y y R     i i 2 2 225 Với 0 0 2 2 0 0 2 2 1 ( ) ( ) 2 e i e i e e x x x y y y R x x y y                 Phương trình viết dưới dạng tham số như sau: cos ( ) sin ( ) 0 i i x x R t y y R t z             Để thỏa mãn điều kiện về vận tốc thì tính toán hàm ( ) t là hàm bậc 3 theo thời gian: 2 3 ( ) t a a t a t a t     0 1 2 3 Thỏa mãn các điều kiện: 0 0 0 (0) ( , ,0) (0) 0 ( ) ( , ,0) ( ) 0 e e e e e s A x y v v s t B x y v t v             Từ điều kiện trên tìm được các hệ số a a a a 0 1 2 3 , , , như sau: 0 0 1 2 2 3 3 cos( ) 0 3 2 i e e x x a arc R a k a t k a t                 và k arccos arccos x x x x 0 i e i R R                26 Giả sử cần tạo quỹ đạo chuyển động từ điểm A(0.7, 0) đến B(0.7, 0) trong thời giante =5(s).Ta tính được các giá trị sau: 0 1 2 3 1.5708 0 2.3562 0.7854 a a a a             ( ) 1.5708 0 2.3563 0.7854 t t t t     2 3  0 0.7sin ( ) 0 0.7 cos ( ) 0 x t y t z             Sử dụng Matlab vẽ được đồ thị: Đồ thị x(t);v (t) x27 Đồ thị y(t);v (t) y28 CHƯƠNG 4: ĐỘNG LỰC HỌC ROBOT 4.1 Các tham số động lực học Coi các thanh là đồng chất, tiết diện ngang không đáng kể, trọng tâm của các khâu đặt tại trung điểm của nó Bảng các tham số động lực học 4.2. Ma trận Jacobi tịnh tiến của các khâu Khâu 1: Ta có 1 1 1 1 1 0 1 1 1 1 1 1 1 1 cos sin 0 cos 1 0 0 2 sin cos 0 sin ; 0 1 0 0 0 0 1 0 0 1 0 0 0 0 1 0 0 0 1 c a a a A A d                                       1 1 1 1 1 1 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 cos sin 0 cos cos 2 2 sin cos 0 sin sin 2 2 0 0 1 0 0 0 1 c c c a a a a A r A r d d                                                    29 1 1 1 1 1 1 0 0 1 1 1 1 1 sin 0 0 1 1 2 sin cos 0 2 2 1 cos 0 0 ; 0 0 0 2 0 0 0 0 0 0 T T T a a a J a J                                          Khâu 2: Tính toán tương tự khâu 1 ta có 1 1 0 0 2 2 2 2 1 1 1 2 2 cos sin 1 2 c c a r A r a d d L                     1 1 1 1 1 1 0 0 2 1 1 2 sin 0 0 sin cos 0 cos 0 0 ; 0 0 1 0 1 0 0 0 0 T T T a a a J a J                              Khâu 3:     1 1 3 1 3 0 0 3 3 3 3 1 1 3 1 3 1 2 1 cos cos 2 1 sin sin c c 2 a a r A r a a d d                              30         1 1 3 1 3 3 1 3 0 3 1 1 3 1 3 3 1 3 1 1 sin sin 0 sin 2 2 1 1 cos cos 0 cos 2 2 0 1 0 T a a a J a a a                                              1 1 3 1 3 1 1 3 1 3 0 3 3 1 3 3 1 3 1 1 sin sin cos cos 0 2 2 0 0 1 1 1 sin cos 0 2 2 T T a a a a J a a                                  4.3. Tính ma trận jacobi quay của các khâu Khâu 1: 1 1 1 1 1 1 0 0 1 1 1 1 1 1 1 1 1 cos( ) sin( ) 0 cos( ) 2 cos( ) sin( ) 0 sin( ) cos( ) 0 sin( ) sin( ) cos( ) 0 2 0 0 1 0 0 1 0 0 0 1 c c a a A R d                                            1 1 1 1 . . 0 0 1 1 1 1 1 1 1 cos( ) sin( ) 0 sin( ) cos( ) 0 sin( ) cos( ) 0 ; cos( ) sin( ) 0 0 0 1 0 0 0 T c c R R                                     . 1 ~ . . 0 0 1 1 1 1 1 . 1 0 0 0 0 0 0 0 0 0 r T T r c c R R                                     1 1 0 0 0 0 0 1 0 0 0 ; 0 0 0 1 0 0 0 0 0 T J J R R                       31 Khâu 2: Tính toán tương tự khâu 1 ta có: . 1 ~ . . 0 0 2 2 2 1 2 . 1 0 0 0 0 0 0 0 0 0 r T T r c c R R                                     2 2 0 0 0 0 0 1 0 0 0 ; 0 0 0 1 0 0 0 0 0 T J J R R                        Khâu 3: Tính toán tương tự khâu 1 ta có: . . 1 3 ~ . . . 0 0 3 3 3 1 3 3 . . 1 1 3 3 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 ; 0 0 0 1 0 1 0 0 1 r T T r c c T R R R R J J                                                                  4.4. Tính tensor quán tính của các khâu Coi các khâu của robot là các thanh đồng chất tiết diện đều và bề ngang không đáng kể. Ta có: 2 2 2 2 2 2 1 2 3 1 1 2 2 3 3 1 2 3 2 2 1 1 3 3 0 0 0 0 0 12 0 0 0 0 0 ; 0 0 ; 0 0 12 12 12 0 0 0 0 0 0 0 12 12 c c c c c c m L m a m L m a m a m a                                                            32 4.5. Tính ma trận khối lượng Áp dụng công thức tính ma trận khối lượng sau: x 1 ( ) ( ) n T T i Ti i Ti Ri ci Ri n n i M q J m J J J      Sử dụng Maple tính được ma trận M(q) như sau: 11 12 13 21 22 23 31 32 33 ( ) m m m M q m m m m m m            với 2 2 2 2 11 1 1 2 1 3 1 3 3 3 1 3 3 12 2 13 3 1 3 3 3 3 21 22 2 3 23 2 31 3 1 3 3 3 3 32 33 3 3 1 1 cos( ) 3 3 0 1 1 cos( ) 2 3 0 0 1 1 cos( ) 2 3 0 1 3 m m a m a a a m m a m a m m a a m m a m m m m m m a a m m a m m m a                                    33 4.6. Tính toán thế năng, lực thế 1 2 3 1 1 1 1 1 2 2 2 2 1 2 2 3 3 3 3 1 2 1 1 2 1 2 2 3 1 2 1 1 2 2 3 2 3 3 z d 1 z ( ) 2 z ( ) 1 d ( ) ( ) 2 0 ( ) ( ) 0 c c c m g m g m g m g d d L m g m g d d m g m g d d L m g d d q G G q G g m m q G q                                                                          34 CHƯƠNG 5: ĐIỀU KHIỂN 5.1 Điều khiển phản hồi và điều khiển vòng kín Khi xét bài toán điều khiển cho một tay máy trong công nghiệp, trước hết chúngta phải mô hình hóa tay máy đó là một cơ cấu – đối tượng được điều khiển, trong đócác cảm biến được đặt tại các khớp để giám sát trạng thái của các khớp và các cơ cấudẫn động được đặt tại các khớp để sinh lực (momen) dẫn động các khâu. Chúng ta luôn muốn điều khiển các khớp của robot bám theo đúng quỹ đạo đãthiết kế. Các phần tử dao động làm việc để nhận lệnh và sinh lực (momen). Do vậy phải sử dụng các hệ điều khiển để tính toán các lệnh phù hợp nhất cho các phần tử nàysao cho chúng thực hiện các quy luật chuyển động mong muốn. Từ các giá trị ban đầu là quy luật q t d   , quy luật vận tốc q t d   , quy luật gia tốc q t d   mong muốn, bộ điều khiển có nhiệm vụ tính toán lực F, là nguồn động lực để các hệ robot chuyển động với quy luật là q và q . Nhờ các cảm biến mà bộ điều khiển có thể đọc được giá trị này theo thời gian thực. Việc tính toán lực F bằng cách sử dụng các phương trình động lực học, với đầu vào là q t q t q t d d  , ,   d   . Từ kết quả tính toán động lực học , F được xác định theo công thức: F M q q C q q q G q     . ,     F được xác định như trong công thức trên sẽ giúp cho chuyển động theo đúng như quỹ đạo mong muốn, nếu mô hình động học là hoàn toàn đúng,35 hệ không chịu tác động của nhiễu . Nhìn chung các quy luật để xây dựng một hệ điều khiển có năng suất cao là phải sử dụng được tín hiệu phản hồi. Theo đó, tín hiệu phản hồi được sử dụng để tính toán sai lệch vị trí e và vận tốc e e q q e q q     d d ; Với bộ điều khiển phản hồi kín ta có: F q k e k e    d v q F mq bq kq    F F     0 v p     e k e k e 5.2. Thiết kế bộ điều khiển trong không gian khớp Giả sử bàn kẹp E đang ở vị trí ( , , ) x y z E E E nào đó ứng với các tọa độ khớp qi, nhiệm vụ của bộ điều khiển là phải đưa được điện áp đặt vào động cơ để bàn kẹp di chuyển đến đúng hay gần đến vị trí mong muốn rE với sai lệch cho phép, tương ứng vị trí mong muốn của bàn kẹp rE là các tọa độ khớp qi Sau đây là xét luật điều khiển đơn giản nhất đáp ứng được yêu cầu trên, đó là luật điều khiển PID cùng với bù trọng lượng. Lực điều khiển tính từ các vị trí mong muốn (ứng với tọa độ biến khớp qi) và trạng thái chuyển động của robot tại thời điểm hiện tại mà thu thập từ các cảm biến đặt tại các khớp. Sau khi tìm được phương trình vi phân chuyển động, ta thiết kế bộ điều36 khiển PID có bù trọng lượng trong Simulink như sau: Trong đó bộ điều khiển PID được thiết kế như sau:37 PHỤ LỤC Chương trình tính toán động học thuận trong Maple > > C ấu hình38 Các ma trận DH Các ma trận biến đổi tọa độ thuần nhất của khâu i so với khâu gốc Kết quả Vị trí điểm thao tác cuối Vận tốc điểm tác động cuối E39 Gia tốc điểm tác động cuối Vận tốc góc và gia tốc góc của các khâu Các ma trận cosin chỉ phương Đạo hàm các ma trận cosin chỉ phương Chuyển vị các ma trận cosin chỉ phương4041 KẾT LUẬN Qua bài tập lớn này đã thực hiện được nhưng bài toán sau  Bài toán động học  Bài toán động lực học  Bài toán tĩnh học  Bài toán thiết kế quỹ đạo chuyển động  Bài toán điều khiển Tuy nhiên trong quá trình thực hiện vẫn còn nhiều vấn đề đã bị bỏ qua hoặc sai sót do khả năng không cho phép. Rất mong có sự thông cảm của thầy cô Em xin chân thành cảm ơn.

MỤC LỤC

LỜI MỞ ĐẦU 3

CHƯƠNG 1: KHẢO SÁT BÀI TOÁN ĐỘNG HỌC 4

1.1 Xây dựng mô hình khảo sát bài toán động học 4

1.1.1 Mô hình khảo sát 4

1.1.2 Xác định hệ trục tọa độ 5

1.1.3 Xác định bộ thông số Denavit-Hartenbeg 5

1.2 Bài toán động học thuận 6

1.2.1 Tìm vị trí điểm thao tác biểu diễn theo các tọa độ khớp, hướng của khâu thao tác 6

1.2.2.Vận tốc điểm tác động cuối E và vận tốc góc các khâu 8

1.2.3 Tính gia tốc điểm tác động cuối Tính gia tốc khâu thao tác 10

1.3 Bài toán động học ngược 11

1.3.1 Tính các tọa độ khớp 12

1.3.2 Vận tốc dài của các khâu tương ứng với các khớp tịnh tiến và vận tốc góc ứng với các khớp quay 14

1.3.3 Gia tốc dài của các khâu tương ứng với các khớp tịnh tiến và gia tốc góc ứng với các khớp quay 15

CHƯƠNG 2: TĨNH HỌC ROBOT 17

2.1 Mô hình khảo sát bài toán tĩnh học 17

2.2 Tính toán bài toán tĩnh học 18

CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG 22

3.1 Thiết kế quỹ đạo trong không gian khớp 22

3.2 Thiết kế trong không gian thao tác 24

CHƯƠNG 4: ĐỘNG LỰC HỌC ROBOT 28

4.1 Các tham số động lực học 28

4.2 Ma trận Jacobi tịnh tiến của các khâu 28

4.3 Tính ma trận jacobi quay của các khâu 30

4.4 Tính tensor quán tính của các khâu 31

4.5 Tính ma trận khối lượng 32

5.1 Điều khiển phản hồi và điều khiển vòng kín 34 KẾT LUẬN 41

Bài báo cáo chọn mô hình robot 3 bậc tự do làm mô hình nghiên cứu chính Để thực hiện bài tập lớn này, em xin cám ơn thầy giáo PGS.TS Phan Bùi Khôi đã hướng dẫn và giúp đỡ trong quá trình học tập và nghiên cứu

Hà Nội, Ngày ,Tháng , Năm

Sinh viên thực hiện

CHƯƠNG 1: KHẢO SÁT BÀI TOÁN ĐỘNG HỌC

Trong chương 1 giải quyết các vấn đề sau:

5 Tính vận tốc điểm tác động cuối E Tính vận tốc góc các khâu

6 Tính gia tốc điểm tác động cuối E Tính gia tốc góc khâu thao tác

7 Cho vị trí, vận tốc, gia tốc điểm tác động cuối, hướng, vận tốc góc, gia tốc góc khâu thao tác

8 Tính các tọa độ khớp

9 Tính vận tốc, gia tốc dài của các khâu tương ứng với các khớp tịnh tiến

10 Tính vận tốc góc, gia tốc góc của các khâu ứng với các khớp quay

1.1 Xây dựng mô hình khảo sát bài toán động học

1.1.1 Mô hình khảo sát

Số bậc tự do: DoF=6.(3-3)+3.1=3

Robot có 2 khớp quay và 1 khớp tịnh tiến

Dựa vào bảng thông số động học trên, xây dựng được các ma trận biến đổi tọa

độ thuần nhất như sau

cos( ) sin( ) 0 cos( )

sin( ) cos( ) 0 sin( )

a a A

sin( ) cos( ) 0 sin( )

a a A

1.2 Bài toán động học thuận

1.2.1 Tìm vị trí điểm thao tác biểu diễn theo các tọa độ khớp, hướng của khâu thao tác

Ta có các ma trận biến đổi thuần nhất biểu diễn vị trí và hướng của hệ tọa độ khâu thứ i đối với hệ tọa độ cơ sở O x y z là: 0 0 0 0

a a

a a

Hướng của tọa độ khâu thao tác so với hệ tọa độ cơ sở được các định từ

các góc Roll-Pitch-Yaw, với kí hiệu Roll, Pitch, Yaw và các

phép quay tương ứng như sau:

cos cos sin cos cos sin sin sin sin cos sin cossin cos cos cos sin sin sin cos sin sin sin cos

Vận tốc điểm tác động cuối E có được từ việc đạo hàm theo t ma trận

định vị củađiểm tác động cuối E, nhận được ma trận biểu diễn vận tốc

tuyệt đối trong hệ cơ sở:

1.2.3 Tính gia tốc điểm tác động cuối Tính gia tốc khâu thao tác

 Xác định gia tốc điểm tác động cuối

Gia tốc điểm tác động cuối E có được từ việc đạo hàm theo t ma trận biểu diễn vận tốc của điểm tác động cuối E trong hệ cơ sở, nhận được ma trận biểu diễn gia tốc của điểm tác động cuối E trong hệ cơ sở:

 Xác định gia tốc góc khâu thao tác

Gia tốc góc khâu thao tác có được từ việc đạo hàm theo t ma trận biểu

diễn vận tốc góc của khâu thao tác trong hệ tọa độ cơ sở, nhận được ma

trận biểu diễn gia tốc góc khâu thao tác trong hệ tọa độ cơ sở:

3

1 3

0 0

- a a d1; 3; 1; x ;E yE; zE là các số liệu cho trước

Bình phương 2 vế của phương trình (1) và (2) sau đó cộng 2 phương trình này lại ta được phương trình sau

cos cos cos sin sin

sin sin cos cos sin

E E

2 cos (a a cos ) sin sin

1 2

CHƯƠNG 2: TĨNH HỌC ROBOT 2.1 Mô hình khảo sát bài toán tĩnh học

Cho trước các thông số:

➢ Coi các thanh là thanh đồng chất, tiết diện ngang không đáng kể, khối lượng các khâu là m1, m2, m3

➢ Khâu 2 có chiều dài L2

Cho lực tác dụng từ robot lên môi trường F, M:

2.2 Tính toán bài toán tĩnh học

 Biễu diễn các lực Pi:

 Tính lực và momen của khâu 2 tác dụng lên khâu 3

Hệ phương trình cân bằng dạng ma trận khảo sát trong hệ tọa độ cơ sở:

sin( )2

 Tính lực và momen của khâu 1 tác dụng lên khâu 2

Hệ phương trình cân bằng dạng ma trận khảo sát trong hệ tọa độ cơ sở

2cos( )

 Tính lực và momen của khâu 0 tác dụng lên khâu 1

Hệ phương trình cân bằng dạng ma trận khảo sát trong hệ tọa độ cơ sở

CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG

3.1 Thiết kế quỹ đạo trong không gian khớp

Cho 2 điểm A, B bất kì trong không gian làm việc, biết tọa độ điểm thao tác

(x E,y E,z E) và hướng của khâu thao tác Thiết kế quỹ đạo chuyển động bất kì từ

A đến B, chỉ quan tâm đến các điều kiện về vị trí,vận tốc gia tốc tại điểm đầu và điểm cuối

Chọn quỹ đạo thiết kế là đa thức bậc 3 theo thời gian có dạng:

2 3

( )

q t  a b tc t d t với i = 1,2,3 Ứng với 3 biến khớp là1,d2,3

Ta được hệ phương trình quỹ đạo chuyển động :

(0) ( )( ) ( )

( ) 0

q B q A c

t

q B q A d

Giả sử Robot cần dịch chuyển từ điểm A(-0.2932, 0.2250, 0.3) tới điểm B(0.3, - 0.1732, 0.35) trong thời gian t=5(s) Giải động học ngược ta được

1

2

3

2( )

3( ) 0.1

3( ) 0.15

1250( ) 0.7854 0 0.0314 0.0042

Đồ thị khớp 2

Đồ thị khớp 3

3.2 Thiết kế trong không gian thao tác

Thiết kế quỹ đạo cho điểm tác động cuối đi từ điểm A tới điểm B nhận AB làm đường kính Dùng phương trình đường tròn trong không gian là mặt phẳng giữa

2 điểmA x y( ,0 0, 0), ( ,B x y e e, 0)lấy AB làm đường kính

(xx i) (y y i)  R

Với

0 0

2

21

2

e i

e i

x x x

y y y

k a

t k a

Giả sử cần tạo quỹ đạo chuyển động từ điểm A(0.7, 0) đến B(-0.7, 0) trong thời giant e=5(s).Ta tính được các giá trị sau:

Đồ thị y(t); v (t)y

CHƯƠNG 4: ĐỘNG LỰC HỌC ROBOT 4.1 Các tham số động lực học

Coi các thanh là đồng chất, tiết diện ngang không đáng kể, trọng tâm của

các khâu đặt tại trung điểm của nó

2 1 sin sin

cos( ) sin( ) 0 sin( ) cos( ) 0

sin( ) cos( ) 0 ; cos( ) sin( ) 0

0 0

1

Khâu 2:

Tính toán tương tự khâu 1 ta có:

1

0 0

1

4.4 Tính tensor quán tính của các khâu

Coi các khâu của robot là các thanh đồng chất tiết diện đều và bề ngang không đáng kể

4.5 Tính ma trận khối lượng

Áp dụng công thức tính ma trận khối lượng sau:

x 1

n

Ti i Ti Ri ci Ri n n i

2 3

q G

CHƯƠNG 5: ĐIỀU KHIỂN 5.1 Điều khiển phản hồi và điều khiển vòng kín

Khi xét bài toán điều khiển cho một tay máy trong công nghiệp, trước hết chúngta phải mô hình hóa tay máy đó là một cơ cấu – đối tượng được điều khiển, trong đócác cảm biến được đặt tại các khớp để giám sát trạng thái của các khớp và các cơ cấudẫn động được đặt tại các khớp để sinh lực (momen) dẫn động các khâu

Chúng ta luôn muốn điều khiển các khớp của robot bám theo đúng quỹ đạo đãthiết kế Các phần tử dao động làm việc để nhận lệnh và sinh lực (momen) Do vậy phải sử dụng các hệ điều khiển để tính toán các lệnh phù hợp nhất cho các phần tử nàysao cho chúng thực hiện các quy luật chuyển động mong muốn

Từ các giá trị ban đầu là quy luật q d  t , quy luật vận tốc q d  t , quy

luật gia tốc q d  t mong muốn, bộ điều khiển có nhiệm vụ tính toán lực

F, là nguồn động lực để các hệ robot chuyển động với quy luật là qvà

q Nhờ các cảm biến mà bộ điều khiển có thể đọc được giá trị này theo

hệ không chịu tác động của nhiễu Nhìn chung các quy luật để xây dựng một hệ điều khiển có năng suất cao là phải sử dụng được tín hiệu phản hồi Theo đó, tín hiệu phản hồi được sử dụng để tính toán sai lệch vị trí e

Sau đây là xét luật điều khiển đơn giản nhất đáp ứng được yêu cầu trên,

đó là luật điều khiển PID cùng với bù trọng lượng Lực điều khiển tính

từ các vị trí mong muốn (ứng với tọa độ biến khớp qi) và trạng thái chuyển động của robot tại thời điểm hiện tại mà thu thập từ các cảm biến

khiển PID có bù trọng lượng trong Simulink như sau:

Trong đó bộ điều khiển PID được thiết kế như sau:

PHỤ LỤC Chương trình tính toán động học thuận trong Maple

Gia tốc điểm tác động cuối

Vận tốc góc và gia tốc góc của các khâu

Các ma trận cosin chỉ phương

Đạo hàm các ma trận cosin chỉ phương

Chuyển vị các ma trận cosin chỉ phương

 Bài toán thiết kế quỹ đạo chuyển động

 Bài toán điều khiển

Tuy nhiên trong quá trình thực hiện vẫn còn nhiều vấn đề đã bị bỏ qua hoặc sai sót do khả năng không cho phép Rất mong có sự thông cảm của thầy cô

Em xin chân thành cảm ơn