Báo cáo bài tập lớn học phần Robotics

1 BÁO CÁO BÀI TẬP LỚN Học phần Robotics Sinh viên thực hiện: Nguyễn Văn Huy MSSV: 20131782 2 MỤC LỤC ĐỀ BÀI ................................................................................................................................ 3 BÀI LÀM ............................................................................................................................ 4 1. BÀI TOÁN ĐỘNG HỌC ROBOT ....................................................................... 4 1.1 Vẽ hệ trục tọa độ theo quy tắc D-H và lập bảng thông số D-H .................... 4 1.2 Tính các ma trận D-H: ..................................................................................... 4 1.3 Xác định vị trí điểm tác động cuối khâu thao tác, hướng của khâu thao tác: ............................................................................................................................ 5 1.4 Vận tốc, gia tốc điểm tác động cuối. ................................................................ 6 1.5 Vận tốc góc, gia tốc góc khâu cuối. ................................................................. 6 1.6 Bài toán động học ngược ................................................................................ 10 2. TĨNH HỌC ROBOT. .......................................................................................... 11 2.1 Lực dẫn động khâu 3: .................................................................................... 12 2.2 Lực dẫn động khâu 2: .................................................................................... 14 2.3 Lực dẫn động khâu 1: .................................................................................... 15 3. ĐỘNG LỰC HỌC ROBOT ................................................................................ 16 3.1. Động năng .................................................................................................... 17 3.1.1 Ma trận Jacobi tịnh tiến trọng tâm các khâu .......................................... 17 3.1.2 Ma trận Ten-xơ quán tính ......................................................................... 18 3.1.3 Ma trận khối lượng..................................................................................... 18 3.2. Thế năng ....................................................................................................... 18 3.3. Lực suy rộng ................................................................................................ 18 3.4. Tính toán bằng phần mềm MAPLE .......................................................... 19 4. THIẾT KẾ QUỸ ĐẠO ........................................................................................ 26 5. ĐIỀU KHIỂN ROBOT ....................................................................................... 28 3 ĐỀ BÀI Chọn một mô hình robot nghiên cứu theo nội dung dưới đây: 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 các khâu theo quy tắc Denavit – Hartenberg (D- H). 3. Lập bảng D-H. Tính các ma trận D-H: i-1Ai, i=1,2 4. Tìm vị trí điểm thao tác biểu diễn theo các 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 vận tốc điểm tác động cuối. 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 tương ứng với các khớp quay. 11. Khảo sát bài toán tĩnh học robot theo một trong các câu: 13, 14, 15. 12. Thiết kế quỹ đạo chuyển động như câu 16. 13. Khảo sát động lực học theo nội dung một trong các câu 17, 18. 14. Thiết kế mô hình điều khiển robot theo một trong các câu 19, 20, 21. 15. Lập trình tính toán, vẽ đồ thị bằng phần mềm MAPLE, MATLAB, mô phỏng điều khiển robot bằng MATLAB, SIMULINK. 4 BÀI LÀM Robot có 3 bậc tự do. Có thông số như sau: L2=0.6 ; a3 = 0.4; d1 = 0.6 (m) m1 = 16; m2 = 12; m3 = 6 (kg) 1. BÀI TOÁN ĐỘNG HỌC ROBOT 1.1 Vẽ hệ trục tọa độ theo quy tắc D-H và lập bảng thông số D-H Bảng thông số D-H của tay máy robot: Joint θi di ai αi 1 θ1 d1 0 2  2 2  d2 0 2  3 θ3 0 a3 0 1.2 Tính các ma trận D-H: 5 1 0 0 0 0 1 i i i i i i i i i i i i i ii i i i i c c s s s a c s c c s c a s A s c d                            1 1 1 10 1 1 0 0 0 0 0 1 0 0 0 0 1 c s s c A d                 1 2 2 0 0 1 0 1 0 0 0 0 1 0 0 0 0 1 A d             3 3 3 3 3 3 3 32 3 0 0 0 0 1 0 0 0 0 1 c s a c s c a s A                   1 3 1 3 1 3 1 3 2 1 1 3 1 3 1 3 1 3 2 10 3 3 3 3 3 10 0 0 0 1 s s s c c a s s d s c s c c s a c s d c A c s a c d                                    1.3 Xác định vị trí điểm tác động cuối khâu thao tác, hướng của khâu thao tác: Vị trí điểm tác động khâu thao tác E: 3 1 3 2 1 0 3 3 1 3 2 1 3 3 1 E a s s d s r r a c s d c a c d                    Hướng khâu thao tác: 1 3 1 3 1 0 3 1 3 1 3 1 3 3 0 E s s s c c R R c s c c s c s                         6 So sánh với ma trận Cardan CD c c c s s R s s c c s s s s c c s c c s c s s c s s s c c c                                             ta có hệ phương trình: 1 2 1 3 1 3 0 0 0 f c c f s c f c s s c                    và tìm được bộ 3 góc Cardan. 1.4 Vận tốc, gia tốc điểm tác động cuối. Vận tốc điểm tác động cuối: 0 3 .E E TEv r r J q   trong đó JTE là ma trận Jacobi tịnh tiến của khâu cuối E 3 1 3 2 1 1 3 1 3 3 1 3 2 1 1 3 1 3 3 30 0 TE a c s d c s a s c J a s s d s c a c c a s                          ; 1 2 3 q d              3 1 3 1 2 1 1 1 2 3 1 3 3 3 1 3 1 2 1 1 1 2 3 1 3 3 3 3 3 E a c s d c s d a s c v a s s d s c d a c c a s                                   Gia tốc điểm cuối E: E E E TE TEa v r J q J q    Với 11 12 13 21 22 23 31 32 33 TE J J J J J J J J J J          thì 11 12 13 21 22 23 31 32 33 TE J J J J J J J J J J            3 1 3 1 2 1 1 1 2 3 1 3 3 1 1 3 1 3 1 3 1 3 3 3 1 3 1 2 1 1 1 2 3 1 3 3 1 1 3 1 3 1 3 1 3 3 3 3 3 s 0 0 TE a s d s c d a c c c a c c a s s J a c s d c s d a s c s a c c a c s a c                                                      Và 1 2 3 q d              thay vào ta được gia tốc aE: 1.5 Vận tốc góc, gia tốc góc khâu cuối. 7 - Vận tốc góc khâu cuối: 1 3 1 3 1 0 3 1 3 1 3 1 3 3 0 E s s s c c R R c s c c s c s                         ; 1 3 1 3 3 1 3 1 3 3 1 1 0 T E s s c s c R s c c c s c s                        1 3 1 1 3 3 1 3 1 1 3 3 1 1 1 3 1 1 3 3 1 3 1 1 3 3 1 1 3 3 3 1 0 E c s s c c c s s s R s s c c s c c s c s c                                              Vận tốc góc khâu cuối biểu diễn trong hệ tọa độ khâu cuối: 1 3 1 1 3 3 1 3 1 1 3 3 1 1 1 3 1 3 3 0 1 3 1 1 3 3 1 3 1 1 3 3 1 1 1 3 1 3 3 3 3 3 1 1 1 . 0 0 T E E E c s s c c c s s s s s c s c R R s s c c s c c s c s c c c s s c c s                                                                       1 1 3 1 3 1 1 3 1 3 1 3 1 3 1 1 3 1 3 1 0 0 ; . 0 T E E RE s c c s c s J q s c                                                   Ma trận Jacobi quay khâu cuối: 3 3 0 0 0 0 1 0 0 RE c J s             Gia tốc RE REJ q J q    Trong đó: 3 3 3 3 0 0 0 0 0 0 0 RE s J c                 3 3 1 3 1 3 1 3 1 3 3 3 2 3 2 3 1 3 1 3 3 3 3 0 0 0 0 0 0 0 0 0 0 0 0 0 1 s c s c c d s d c s                                                                           Cho quy luật chuyển động của các khâu như sau: Ta vẽ có đồ thị biểu diễn vị trí, vận tốc, gia tốc của điểm thao tác E: 8 Vị trí: Vận tốc: 9 Gia tốc: Và đồ thị vận tốc góc, gia tốc góc của khâu 3: Vận tốc góc: 10 Gia tốc góc: 1.6 Bài toán động học ngược Cho      2 0.5 0.3 0.1 0.8 ; 0.2 / ; 0 / 0.6 0.2 0.1 E E E E E E E E E E E E x x x r y m v y m s a y m s z z z                                                                 Ta có phương trình 3 1 3 2 1 3 1 3 2 1 3 3 1 E E E E x a s s d s y r a c s d c z a c d                               3 1 3 2 1 3 1 3 2 1 3 3 1 E E E x a s s d s y a c s d c z a c d                 (*) Sử dụng sự hỗ trợ của phần mềm MAPLE: > > > 11 Ta được 4 bộ nghiệm 1 2 3 q q q          như sau: -0.5585993153 2.582993338 -.5585993153 2.582993338 0.5433981132 ; -1.343398113 ; 1.343398113 ; -.5433981132 -1.570796327 -1.570796327 1.570796327 1.570796327                                        Với q2 = d2 ≤ l2 = 0.6(m) ta chỉ chọn được 1 bộ nghiệm duy nhất thỏa mãn là: 1 2 3 -0.5585993153 0.5433981132 -1.570796327 q d                      Đạo hàm 2 vế của phương trình (*) ta được: 3 1 3 2 1 1 3 1 3 1 3 1 3 2 1 1 3 1 3 2 3 3 30 0 E E E a c s d c s a s c x a s s d s c a c c d y a s z                                           q EJ q v  1q Eq J v  ta tính được: 1 2 3 -.1573033707 0.3285993428 ( / ) 0.5 q d m s                     Tiếp tục đạo hàm 2 vế của phương trình: q EJ q v ta được:     1 1 q q E q E q q E q q E q J q J q v J q v J q q J v J q q J a J q              Với sự hỗ trợ của phần mềm MAPLE ta tính được: 1 2 2 3 -0.0197058310 0.1995750313 ( / ) 0.2500000000 q d m s                     2. TĨNH HỌC ROBOT. 12 Bổ sung các tham số động học, động lực học: q1 = 120o; q2 = 0.4 m; q3 = 45o; Fx = 80N; Fz = 60N; Mx = 20Nm. Lấy g = 9.8 m/s2 2.1 Lực dẫn động khâu 3: 0 3,2 32 32 32 0 3,2 32 32 32 , , , , T x y z T x y z F F F F M M M M            Lực tác dụng từ RB lên đối tượng công nghệ F, M:         0 4,3 0 4,3 ,0, ,0, ,0,0 ,0,0 T T x z x z T T x z x F F F F F F M M M M       Áp dụng công thức đệ quy: 13 0 0 0 3,2 4,3 3 0 0 0 3 0 0 3 0 3,2 4,3 2 3,2 3 3. .c F F P M M r F r P        Trong đó:       0 03 3 30,0, 0,0, 0,0, 6 T T T g g P m g g         3 3 0 3 0 3 32 3 2 3 2,0,0 ; . T r a r R r   với 1 3 1 3 1 0 3 1 3 1 3 1 3 3 0 E s s s c c R R c s c c s c s                         3 1 3 3 3 3 1 3 0 3 0 3 2 3 1 3 2 3 3 3 1 3 3 1 3 3 1 33 3 0 0 0 a s s a c a c s r a c s r a c a s s a c s a s sa c                                       3 3 0 3 0 3 33 3 3 3 3,0,0 ; . 2 T c c c a r r R r         với 1 3 1 3 1 0 3 1 3 1 3 1 3 3 0 E s s s c c R R c s c c s c s                         3 1 3 3 3 3 1 3 0 3 0 33 1 3 3 3 3 1 3 3 3 3 3 3 1 3 3 1 3 0 2 2 2 0 2 2 2 0 2 2 2 c c a s s a c a c s a c s a c a s s r r a c a c s a s s                                                         Thay tất cả vào hệ phương trình ban đầu ta được:   0 3,2 3 3 3 1 3 3 1 3 0 3,2 3 3 1 3 1 3 3 3 1 3 Fx 0 m g+Fz 1 m g F 2 1 -m gs 2F 2 2 F Z x Z x x F a c s a c s M M a s s s c F a c s                                                Thay số vào ta được: 14     0 3,2 0 3,2 80 0 118.8 8.94 2+20 16 2-8.94 6 -8 2 F N M Nm                              2.2 Lực dẫn động khâu 2: 0 2,1 21 21 21 0 2,1 21 21 21 , , , , T x y z T x y z F F F F M M M M            Lực tác dụng từ khâu 3 lên khâu 2:     0 3,2 0 3,2 80 0 118.8 8.94 2+20 16 2-8.94 6 -8 2 F N M Nm                              Áp dụng công thức đệ quy: 0 0 0 2,1 3,2 2 0 0 0 2 0 0 2 0 2,1 3,2 1 2,1 2 2. .c F F P M M r F r P        Trong đó:       0 03 2 20,0, 0,0, 0,0, 12 T T T g g P m g g         2 2 0 2 0 2 21 2 1 2 10, ,0 ; . T r q r R r  với 1 1 0 2 1 1 0 0 1 0 0 s c R c s               22 0 2 0 32 2 1 2 2 2 3 0 0 22 3 0 0 2 2 0 3 0 2 2 dd d d r r d d                                         15  2 2 0 2 0 2 22 2 2 2 20, ,0 ; . 2 T c c c l r r R r        22 0 2 0 22 2 2 2 2 2 3 0 0 44 3 0 0 4 4 0 3 0 4 4 c c ll l l r r l l                                          Thay tất cả vào hệ phương trình ban đầu ta được:     0 2,1 0 2,1 80 0 236.4 3.003 52.067 4.686 F N M Nm                          2.3 Lực dẫn động khâu 1: 0 1,0 10 10 10 0 10 10 10 10 , , , , T x y z T x y z F F F F M M M M            Lực tác dụng từ khâu 3 lên khâu 2:     0 3,2 0 3,2 80 0 236.4 3.003 52.067 4.686 F N M Nm                          Áp dụng công thức đệ quy: 0 0 0 1,0 2,1 1 0 0 0 1 0 0 1 0 1,0 2,1 0 1,0 1 1. .c F F P M M r F r P        Trong đó:       0 01 1 10,0, 0,0, 0,0, 16 T T T g g P m g g       16   1 1 0 1 0 1 10 1 0 1 00, ,0 ; . T r d r R r  với 1 1 0 1 1 1 0 0 0 1 0 c s R s c              1 0 1 0 3 1 2 1 1 0 0 0 0 0 0 0 0 0 d r r d d                         1 1 0 1 0 1 11 1 1 1 10, ,0 ; . 2 T c c c d r r R r        1 0 1 0 1 1 1 1 1 0 0 20 0 0 0 2 0 0 0 2 c c d d r r d                                  Thay tất cả vào hệ phương trình ban đầu ta được:     0 1,0 0 1,0 80 0 392.2 3.003 100.067 6.686 F N M Nm                          3. ĐỘNG LỰC HỌC ROBOT Phương trình động lực học Lagrange loại II: , 1..i i i i i d T T Q U i n dt q q q                Trong đó: o T – Động năng của cả hệ robot o Π – Thế năng của cả hệ robot o qi – tọa độ suy rộng thứ i o Qi – Lực suy rộng của các lực không thế ứng với tọa độ suy rộng qi o Ui – Lực/Momen điều khiển ứng với tọa độ suy rộng qi. o n – Số bậc tự do của robot. Phương trình động lực học dạng ma trận: ( ) ( , ) ( )M q q C q q q G q Q U    Trong đó: 17 o   1 ( ) n T T r Ti i Ti Ri ci Ri i n n M q J m J J J            o   1 , 1 ( , ) ( , ) ; ( , ) , ; n j j k ln k l C q q q c q q q c q q q k l j q q        o ( ) ( ) 1 ; ( )j j j j G q g q n g q q        o aQ Bq Dsignq Q   trong đó: Bq là ma sát nhớt, Dsignq là ma sát khô, aQ là lực suy rộng của các lực không thế 3.1. Động năng   3 1 1 1 ( ) 2 2 T T T T r Ti i Ti Ri ci Ri i T q M q q q J m J J J q            3.1.1 Ma trận Jacobi tịnh tiến trọng tâm các khâu Ma trận Jacobi tịnh tiến các trọng tâm các khâu: 0 0 0 0.ici i ci ci Ti r A A A r J q       Ta tính được: 0 0 0 1 0 0 0 0 0 0 JT          ; 2 1 2 1 1 2 1 2 1 1 1 0 2 1 2 0 2 0 0 0 l c d c s JT l s d s c                          ; 3 1 3 2 1 1 3 1 3 3 1 3 2 1 1 3 1 3 3 3 1 1 2 2 1 1 3 2 2 1 0 0 2 a c s d c s a s c JT a s s d s c a c c a s                                  Ma trận Jacobi quay các khâu: 0 0 0 0T i i i iR R JRi q          Ta tính được: 18 0 0 0 1 0 0 0 1 0 0 JR          ; 0 0 0 2 0 0 0 1 0 0 JR          ; 1 1 0 0 1 0 0 1 0 0 c JR s            . 3.1.2 Ma trận Ten-xơ quán tính 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 1 1 2 1 1 1 1 0 0 12 0 0 0 0 0 12 c m d m d                  ; 2 2 2 2 2 2 2 2 0 0 12 0 0 0 0 0 12 c m l m l                  ; 2 3 3 2 3 3 3 3 0 0 0 0 0 12 0 0 12 c m a m a                   3.1.3 Ma trận khối lượng Với sự hỗ trợ của phần mềm MAPLE ta tính được M:  Động năng T: 3.2. Thế năng 3 1 . .i ci i g m z    với  1 2 3m m m m và 1 2 1 3 3 3 1 1 1 ; ; 2 2 c c c d z z d z a c d     Π= 3.3. Lực suy rộng Với lực không thế tác dụng lên RB là    0 ; 0 0 T T F Fx Fz M Mx  19 Lực suy rộng   3 1 T T Ti i Ri i i Q J F J M    trong đó F1 = F2 = 0; M1 = M2 = 0; F3 = F; M3 = M ta tính được Q: 3.4. Tính toán bằng phần mềm MAPLE Chương trình MAPLE tính toán động lực học thuận: > Ma tran Denavit Hartembeg: DH(theta, d, a, alpha) Nhan 2 ma tran: nhan2(A,B) Nhan 3 ma tran: nhan3(A,B,C) ma tran Jacobi 3dof : J(A,q1,q2,q3) ma tran Jacobi 2dof: J(A,q1,q2) Rut gon: rutgon(A) tao ma tran song: w2ws(w) van toc goc tu ma tran song: ws2w(ws) > #--------Nhập các biến khớp của robot:-------# > #--------các ma trận D-H---------# 20 > #------Nhập tọa độ trọng tâm các khâu của rb-----# > #-----Ma trận Jacobi tịnh tiến trọng tâm các khâu------# 21 > tao ma tran omega tu ma tran DH: w(A) > 22 > ma tran Jacobi quay tu ma tran omega: jr(w) > > #---- Tính ma trận khối lượng:----# #---ma trận quán tính của các khâu đối với hệ tọa độ qua trọng tâm của khâu đó: 23 > > #--=>ĐỘNG NĂNG--- 24 > #----Tính ma trận C----- #----hàm tính ma trận C----# > 25 > > #------Tính thế năng PI, ma trận G---- > > > #-----Tính lực suy rộng Q---- > #----lực dẫn động U----- > 26 4. THIẾT KẾ QUỸ ĐẠO Cho 2 điểm A(0.2, 0, 0.5) và B( 0.6, 0.8, 0.7) trong không gian thao tác của robot. Quỹ đạo của điểm tác động cuối của robot theo đường thẳng từ A đến B trong khoảng thời gian te(s). Phương trình đường thẳng từ A(xA,yA,zA) đến B(xB, yB, zB) : A A A B A B A B A B A A B B A B A B A B A A B B A B A B A x x y y z z x x y y z z y y y x y x y x x x x x z z z x z x z x x x x x                        Ta thiết lập quan hệ x=x(t) là đa thức bậc 3: 2 3 0 1 2 3( )x t a a t a t a t    Với điều kiện vận tốc đầu và cuối bằng 0 ta được: 0 (0) 0 (0) 0 ( ) ( ) 0 A x x e B e x e xB x x t v v x t x t t v t v             => 0 1 2 2 3 3 0 3( ) 2( ) A B A e B A e a x a x x a t x x a t            Cho te = 5s. Thay tọa độ A và B vào ta được 27 0 1 2 2 3 3 0.2 0 3(0.6 0.2) 0.048 5 2(0.6 0.2) 0.0064 5 a a a a                 2 30,2 0.048 0.0064x t t    2 2 3 3 3 2 0.96 0.128 0.5 0.24 0.3 0, 2 0.048 0. 2 0064 t t z t t x t t y              Đồ thị biểu diễn vị trí điểm tác động cuối của robot theo 3 phương: Đồ thị trong không gian: 28 5. ĐIỀU KHIỂN ROBOT Điều khiển hệ thống theo luật điều khiển PD. Phương trình động lực học lấy từ phần 3-động lực học: ( ) ( , ) ( )M q q C q q q G q Q U    Kv, Kp là ma trận đường chéo: 1 1 2 2 2 2 0 0 0 0 0 0 ; 0 0 0 0 0 0 v p v p v p k k Kv k Kp k k k                 Được xác định từ phương trình: 0V pE K E K E   Xây dựng mô phỏng trên MATLAB-SIMULINK như sau: 29 Khối controller-PD: Khối control-nonlinear: 30 Khối robot system: