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 CHƯƠNG 1: KHẢO SÁT BÀI TOÁN ĐỘNG HỌC 1.1 Xây dựng mơ hình khảo sát toán động học 1.1.1 Mơ hình khảo sát 1.1.2 Xác định hệ trục tọa độ 1.1.3 Xác định thông số Denavit-Hartenbeg 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 tọa độ khớp, hướng khâu thao tác 1.2.2.Vận tốc điểm tác động cuối E vận tốc góc khâu 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 tọa độ khớp 12 1.3.2 Vận tốc dài khâu tương ứng với khớp tịnh tiến vận tốc góc ứng với khớp quay 14 1.3.3 Gia tốc dài khâu tương ứng với khớp tịnh tiến gia tốc góc ứng với khớp quay 15 CHƯƠNG 2: TĨNH HỌC ROBOT 17 2.1 Mơ hình khảo sát tốn tĩnh học 17 2.2 Tính tốn 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 không gian khớp 22 3.2 Thiết kế 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 khâu 28 4.3 Tính ma trận jacobi quay khâu 30 4.4 Tính tensor quán tính khâu 31 4.5 Tính ma trận khối lượng 32 4.6 Tính tốn năng, lực 33 CHƯƠNG 5: ĐIỀU KHIỂN 34 5.1 Điều khiển phản hồi điều khiển vòng kín 34 KẾT LUẬN 41 LỜI MỞ ĐẦU Với phát triển mạnh cơng nghiệp Robot mơn học Robotícs trang bị cho sinh viên kiến thức sở tính tốn thiết kế Robot Đây kiến thức sở quan trọng Trong tập lớn đề cập tới toán động học, tĩnh học, động lực học, thiết kế quỹ đạo chuyển động điều khiển phục vụ cho q trình thiết kế thực tiễn gặp phải công việc Bài báo cáo chọn mô hình robot bậc tự làm mơ hình nghiên cứu Để thực 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 giúp đỡ trình học tập nghiên cứu Hà Nội, Ngày ,Tháng , Năm Sinh viên thực CHƯƠNG 1: KHẢO SÁT BÀI TOÁN ĐỘNG HỌC Trong chương giải vấn đề sau: Tính số bậc tự robot Vẽ hệ trục tọa độ gắn liền với khâu theo quy tắc DH Lập bảng DH Tính ma trận DH Tìm vị trí điểm thao tác biểu diễn theo tọa độ khớp Xác định hướng khâu thao tác Tính vận tốc điểm tác động cuối E Tính vận tốc góc khâu 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 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 Tính tọa độ khớp Tính vận tốc, gia tốc dài khâu tương ứng với khớp tịnh tiến 10.Tính vận tốc góc, gia tốc góc khâu ứng với khớp quay 1.1 Xây dựng mơ hình khảo sát tố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ó khớp quay khớp tịnh tiến 1.1.2 Xác định hệ trục tọa độ 1.1.3 Xác định thông số Denavit-Hartenbeg Từ mơ hình hệ trục tọa độ ta có bảng thơng số D-H sau Joint i di i 1 d1 a1 d2 0 3 a3 0 Trong đó: - Các thơng số biển diễn hình vẽ - Các biến khớp tương ứng là 1 , d2 ,3 Dựa vào bảng thông số động học trên, xây dựng ma trận biến đổi tọa độ sau cos(1 )  sin(1 )  sin( ) cos( ) 1 A1    0   1 0 A2   0  0 a1 cos(1 )  a1 sin(1 )   d1   0  d2   0 1 0 cos(3 )  sin(3 )  sin( ) cos( ) 3 A3    0   0 a3 cos(3 )  a1 sin(3 )     1.2 Bài tốn động học thuận 1.2.1 Tìm vị trí điểm thao tác biểu diễn theo tọa độ khớp, hướng khâu thao tác Ta có ma trận biến đổi biểu diễn vị trí hướng hệ tọa độ khâu thứ i hệ tọa độ sở O0 x0 y0 z0 là: cos(1 )  sin(1 )  sin( ) cos( ) 1 A1    0   0 a1 cos(1 )  a1 sin(1 )   d1   cos(1 )  sin(1 )  sin( ) cos( ) 1 0 A2  A1 A2    0   0 a1 cos(1 )  a1 sin(1 )  d1  d    cos(1  3 )  sin(1  3 )  sin(   ) cos(   ) 3 0 A3  A1 A2   0  0  a1 cos(1 )  a cos(1  3 )  a1 sin(1 )  a sin(1  3 )   d1  d   Từ ma trận ta xác địnhđược Ma trận biểu diễn vị trí điểm thao tác theo tọa độ khớp  xE   a1 cos(1 )  a cos(1  3 )  rE   yE    a1 sin(1 )  a sin(1  3 )   zE    d1  d Hướng tọa độ khâu thao tác so với hệ tọa độ sở định từ góc Roll-Pitch-Yaw, với kí hiệu  Roll ,  Pitch,   Yaw phép quay tương ứng sau: Mô tả phép quay RPY RRPY 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 Để tính góc  , , so sánh với ma trận quay khâu cuối: cos(1  3 )  sin(1  3 )  R3   sin(1  3 ) cos(1  3 )   0  Giải hệ phương trình tìm góc sau:    0   0   sin(1  3 )   arctan( ) cos(    )  1.2.2.Vận tốc điểm tác động cuối E vận tốc gó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ó 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 ma trận biểu diễn vận tốc tuyệt đối hệ sở:      a3 1  3 sin 1  3   a1.1.sin 1     0  VE  rE  a3 1  3 cos 1  3   a1.1.cos 1       d2    Xác định vận tốc góc khâu Các ma trận quay biểu diễn hướng hệ tọa độ khâu i so với hệ tọa độ sở O0 x0 y0 z0 cos 1   sin 1     R1   sin 1  cos 1   ;  0  cos 1   sin 1     R2   sin 1  cos 1   ;  0  cos 1  3   sin 1  3     R3   sin 1  3  cos 1  3    0  Đạo hàm ma trận quay  1.sin 1  1.cos 1     R1   1.cos 1  1.sin 1    0    1.sin 1  1.cos 1     R2   1.cos 1  1.sin 1    0              cos           sin       1  3 1.sin 1  3  R3   1  3 cos 1  3     3 3 Chuyển vị ma trận quay  cos 1  sin 1     T R1    sin 1  cos 1    0  0  0  0   cos 1  sin 1     R2T    sin 1  cos 1    0   cos 1  3  sin 1  3     R3T    sin 1  3  cos 1  3    0  Các ma trận sóng tương ứng 0  1  R1 RT  1 0  1 0  0 0  0   1 0  1   0 0  ~ ~   R2 RT ~   R3 RT    1  3   1  3 0 0  0   Vận tốc khâu hệ cố định 0 0   1    ; 2    ; 3    1  1  1  3  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ó từ việc đạo hàm theo t ma trận biểu diễn vận tốc điểm tác động cuối E hệ sở, nhận ma trận biểu diễn gia tốc điểm tác động cuối E hệ sở: 10 Đồ thị y(t); v y (t) 27 CHƯƠNG 4: ĐỘNG LỰC HỌC ROBOT 4.1 Các tham số động lực học Coi đồng chất, tiết diện ngang không đáng kể, trọng tâm khâu đặt trung điểm Bảng tham số động lực học 4.2 Ma trận Jacobi tịnh tiến khâu Khâu 1: Ta có cos 1  sin  A1       cos 1   Ac1   sin 1      sin 1 a1 cos 1  1  cos 1 a1 sin 1  ; Ac1  0 d1  0   0  0  sin 1 cos 1 0 0 0 0 0 a1        a1   a1  cos 1  cos 1   2    a1 a  0 sin 1  rc1  A1 rc1   sin         d1   d1     28      a1 sin 1 0    a1 sin 1     J T   a1 cos 1 0  ; J T 1T   2     0        a1 cos 1   0 0   Khâu 2: Tính tốn tương tự khâu ta có    a1 cos 1    rc  A2 rc   a1 sin 1     d1  d  L2     JT  a1 sin 1 0  a1 sin 1 a1 cos 1 0   a1 cos 1 0 ; JT 2T   0    0 0 Khâu 3:   a cos   a3 cos 1  3      0 rc  A3 rc  a1 sin 1  a3 sin 1  3       d  d     29 0 JT 1    a1 sin 1  a3 sin 1  3   a3 sin 1  3     1   a1 cos 1  a3 cos 1  3  a3 cos 1  3     2       J T 3T 1    a1 sin 1  a3 sin 1  3  a1 cos 1  a3 cos 1  3  0    0 1   1  a sin    a cos       3  3 3 2   4.3 Tính ma trận jacobi quay khâu Khâu 1:  cos(1 )  sin(1 )  Ac1   sin(1 ) cos(1 )  0   0 0 a1  cos(1 )  cos(1 )  sin(1 )   a1 sin(1 )   Rc1   sin(1 ) cos(1 )        0  d1    cos(1 ) sin(1 )    sin(1 )  cos(1 )   RcT1    sin(1 ) cos(1 ) 0 ; Rc1   cos(1 )  sin(1 )  1   0  0    0     ~     r1  RcT1 RcT1  1 0   1r    .  0 0      1   0 0 0   J R1  0 0 ; J RT1  0 0  1 0 0 0 30 Khâu 2: Tính tốn tương tự khâu ta có:     0 0    ~   2r  RcT2 RcT2   0   2 r      0 0      1    J R2 0 0  0 0 ; J 1 0  0   0 T R  0  0 0  Khâu 3: Tính tốn tương tự khâu ta có:       0      ~   r T T r  3  Rc Rc     0   3      0 0           0 0  0   J R  0 0  ; J RT3  0 0  1  0  4.4 Tính tensor quán tính khâu Coi khâu robot đồng chất tiết diện bề ngang khơng đáng kể Ta có:  m2 L2      0  0 0 0  0  12       m3a32 m1a12 m2 L2   c3 c1 c2    c1  0 ; c   0  ; c  0      12 12 12       0 m1a12  m3a32    0 0     12  12    31 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: n M (q)  [ ( J T Ti mi JTi  J T Ri i ci J Ri )]nxn i 1 Sử dụng Maple tính ma trận M(q) sau:  m11 m12 M (q)   m21 m22  m31 m32 m13  m23  m33  với 1  2 m  m a  m a  cos(  ) a a m  m a  m3a32 11 1 3 3  3  m 12   1 m13  cos(3 )a1a3m3  m3a32  m21   m22  m2  m3 m   23  1 m31  cos(3 )a1a3m3  m3a3  m32   m33  m3a3  32 4.6 Tính tốn năng, lực          m1 gz c1  m1 gd1  m2 gz c  m2 g (d1  d  L2 )  m3 gz c  m3 g (d1  d )   m1 gd1  m2 g (d1  d  L2 )  m3 g (d1  d )       q    G1           G (q )   G2       g (m2  m3 )  q G            q  3 33 CHƯƠNG 5: ĐIỀU KHIỂN 5.1 Điều khiển phản hồi điều khiển vòng kín Khi xét toán điều khiển cho tay máy cơng nghiệp, trước hết chúngta phải mơ hình hóa tay máy cấu – đối tượng điều khiển, đócác cảm biến đặt khớp để giám sát trạng thái khớp cấudẫn động đặt khớp để sinh lực (momen) dẫn động khâu Chúng ta muốn điều khiển khớp robot bám theo quỹ đạo đãthiết kế Các phần tử dao động làm việc để nhận lệnh sinh lực (momen) Do phải sử dụng hệ điều khiển để tính toán lệnh phù hợp cho phần tử nàysao cho chúng thực quy luật chuyển động mong muốn Từ giá trị ban đầu quy luật qd  t  , quy luật vận tốc qd  t  , quy luật gia tốc q d  t  mong muốn, điều khiển có nhiệm vụ tính tốn lực F, nguồn động lực để hệ robot chuyển động với quy luật q q Nhờ cảm biến mà điều khiển đọc giá trị theo thời gian thực Việc tính tốn lực F cách sử dụng phương trình động lực học, với đầu vào qd  t  , qd  t  , q d  t  Từ kết tính tốn động lực học , F xác định theo công thức: F  M  q  q  C  q, q  q  G  q  F xác định công thức giúp cho chuyển động theo quỹ đạo mong muốn, mơ hình động học hồn tồn đúng, 34 hệ khơng chịu tác động nhiễu Nhìn luật để xây dựng hệ điều khiển có suất cao phải sử dụng tín hiệu phản hồi Theo đó, tín hiệu phản hồi sử dụng để tính tốn sai lệch vị trí e vận tốc e e  qd  q; e  qd  q Với điều khiển phản hồi kín ta có: F '  qd  kv e  kq e F  m q  bq  kq F  F'    e  kv e  k p e  5.2 Thiết kế điều khiển không gian khớp Giả sử bàn kẹp E vị trí ( xE , yE , zE ) ứng với tọa độ khớp qi, nhiệm vụ điều khiển phải đưa điện áp đặt vào động để bàn kẹp di chuyển đến 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 bàn kẹp rE tọa độ khớp qi Sau xét luật điều khiển đơn giản đáp ứng yêu cầu trên, luật điều khiển PID với bù trọng lượng Lực điều khiển tính từ vị trí mong muốn (ứng với tọa độ biến khớp qi) trạng thái chuyển động robot thời điểm mà thu thập từ cảm biến đặt khớp Sau tìm phương trình vi phân chuyển động, ta thiết kế điều 35 khiển PID có bù trọng lượng Simulink sau: Trong điều khiển PID thiết kế sau: 36 PHỤ LỤC Chương trình tính tốn động học thuận Maple > > Cấu hình 37 Các ma trận DH Các ma trận biến đổi tọa độ khâu i so với khâu gốc Kết Vị trí điểm thao tác cuối Vận tốc điểm tác động cuối E 38 Gia tốc điểm tác động cuối Vận tốc góc gia tốc góc khâu Các ma trận cosin phương Đạo hàm ma trận cosin phương Chuyển vị ma trận cosin phương 39 40 KẾT LUẬN Qua tập lớn thực 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 q trình thực nhiều vấn đề bị bỏ qua sai sót khả khơng cho phép Rất mong có thông cảm thầy cô Em xin chân thành cảm ơn 41