CÁNH TAY ROBOT 4 BẬC ĐIỀU KHIỂN PID GIAO DIỆN ĐIỀU KHIỂN MATLAB

BỘ GIÁO DỤC ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐIỆN – ĐIỆN TỬ BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG BÁO CÁO CUỐI KỲ KỸ THUẬT ROBOT NGÀNH KỸ THUẬT ĐIỀU KHIỂN TỰ ĐỘNG HÓA ĐỀ TÀI THIẾT KẾ VÀ MÔ PHỎNG CÁNH TAY ROBOT 4 BẬC BẰNG SOLIDWORKS VÀ MATLAB GVHD PhD Nguyễn Văn Thái SVTH Nguyễn Ngọc Thiện MSSV 18151245 Phạm Văn Bảo MSSV 18151155 Nguyễn Sơn Nam MSSV 18151204 Tp Hồ Chí Minh 62022 MỤC LỤC 1 THIẾT KẾ ROBOT 1 1 1 LINK 0 (BASE) 1 1 2, LINK 1 3 1 3 LINK 2 4 1 4 LINK 3 5 1 5 LINK 4 (.

BỘ GIÁO DỤC & ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐIỆN – ĐIỆN TỬ BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG - BÁO CÁO CUỐI KỲ KỸ THUẬT ROBOT NGÀNH KỸ THUẬT ĐIỀU KHIỂN & TỰ ĐỘNG HĨA ĐỀ TÀI: THIẾT KẾ VÀ MƠ PHỎNG CÁNH TAY ROBOT BẬC BẰNG SOLIDWORKS VÀ MATLAB GVHD: PhD Nguyễn Văn Thái SVTH: Nguyễn Ngọc Thiện MSSV: 18151245 Phạm Văn Bảo MSSV: 18151155 Nguyễn Sơn Nam MSSV: 18151204 Tp Hồ Chí Minh - 6/2022 MỤC LỤC THIẾT KẾ ROBOT 1.1 LINK (BASE) 1.2, LINK 1.3 LINK 1.4 LINK 1.5 LINK (Tool) FORWARD KINEMATIC INVERSE KINEMATIC 11 3.1 Tính theta1 12 3.2 Tính theta3 12 3.3 Tính theta2 13 3.4 Tính theta4 14 LIÊN KẾT SOLIDWORKS VÀ MATLAB SIMULINK - THIẾT KẾ GIAO DIỆN ĐIỀU KHIỂN - LẬP TRÌNH MATLAB 16 4.1 Mơ hình simscape multibody 16 4.2 Giao diện giới thiệu giao diện điều khiển 17 QUY HOẠCH QUỸ ĐẠO HÌNH TRỊN 18 5.1 Thiết kế mơ hình hệ thống Matlab Simulink 19 5.2 Kết mô 20 THIẾT KẾ PHƯƠNG TRÌNH ĐỘNG LỰC HỌC VÀ KHẢO SÁT BỘ ĐIỀU KHIỂN COMPUTED TORQUE CONTROL 21 6.1 THÀNH LẬP PHƯƠNG TRÌNH ĐỘNG LỰC HỌC 23 6.2 THIẾT KẾ BỘ COMPUTED TORQUE CONTROL TRÊN MATLAB SIMULINK 32 6.3 Kết mô 34 6.4 Kết luận hệ thống 35 THIẾT KẾ ROBOT 1.1 LINK (BASE) 1.2 LINK 1.3 LINK 1.4 LINK 1.5 LINK (Tool) FORWARD KINEMATIC Bước 1: Vẽ cấu trúc Robot đặt hệ trục tọa độ Bước 2: Lập bảng Denavit Hartenberg Bước 3: Tính tốn ma trận chuyển đổi đồng hệ trục tọa độ liên tiếp Bước 4: Tính tốn ma trận chuyển đổi đồng hệ tọa độ điểm đầu cuối hệ trục tọa độ gốc Bước 5: Xác định thành phần vị trí hướng điểm đầu cuối 𝐿1 : khoảng cách 𝑋1 𝑋2 dọc theo trục 𝑍1 𝐿2 : khoảng cách 𝑍1 𝑍2 dọc theo trục 𝑋2 𝐿3 : khoảng cách 𝑍2 𝑍3 dọc theo trục 𝑋3 𝐿4 : khoảng cách 𝑍3 𝑍4 dọc theo trục 𝑋4 Bảng D-H Axis (i) a i  i di  z zx 011 90 𝐿1 *𝜃1 x xz 010 z z x 2 𝐿2 0 *𝜃2 xx z 21 z z x 3 𝐿3 0 *𝜃3 x x z z z x 4 𝐿4 0 *𝜃4 x x z Đối với hệ trục tọa độ thứ {i} ta có: a khoảng cách i    Z i tới Z i + đo dọc theo trục X i + i  góc từ i    Z i tới Z i + đo dọc theo trục X i +    d khoảng cách X i − tới X i dọc theo trục Z i − i     góc từ X i − tới X i đo theo trục Z i − i Ma trận chuyển đổi tổng quát tối ưu hệ i sang hệ i+1 s s a c  c − s c i i i i i i  i i − 1T =  si ci ci −ci si si    i s c d   i i i   0   Ma trận chuyển đổi từ hệ sang 2: cos d ( )   sin d (1) = T1     sin d ( ) 0  − cos d ( )   L  0 1 Ma trận chuyển đổi từ hệ sang 2:  cos d ( ) − sin d ( ) L cos d ( )  2 2    sin d ( ) cos d ( ) L2 sin d ( )  T 12 =   0     0   Ma trận chuyển đổi từ hệ sang 3: cos d ( ) − sin d ( ) 3   sin d (3 ) cos d (3 ) T 32 =  0   0  L cos d ( )  3  L sin d ( )  3     Ma trận chuyển đổi từ hệ sang 4: THIẾT LẬP PHƯƠNG TRÌNH ĐỘNG LỰC HỌC VÀ KHẢO SÁT BỘ ĐIỀU KHIỂN COMPUTED TORQUE CONTROL I Khái niệm: Computed Torque Control điều khiển tính tốn moment để điều khiển khớp cánh tay robot bám theo quỹ đạo đặt cho trước Áp dụng điều khiển hồi tiếp tuyến tính hóa (tuyến tính hóa động lực học, loại bỏ thành phần phi tuyến hệ thống) điều khiển PD để đáp ứng ổn định tốt độ vọt lố POT (%), sai số xác lập sai số xác lập II Mục tiêu: Tính tốn để robot đạt theo quỹ đạo chuyển động mong muốn III Các bước thiết kế Computed Torque Control Bước 1: Thành lập phương trình động lực học có dạng: ( ) ( )  = M ( ) + C  ,  + G ( ) + F  , 21 ( ) ( ) Hay  = M ( )  + V  , + G ( ) + F  , với V = C  where M ( ) is the n x n mass matrix of the manipulator, V ( ,  ) is an n x vector of centrifugal and Coriolis terms, and is an n x vector of gravity terms Bước 2: Xác định thành phần M, C, G phương trình động lực học   P G=  M=    T M      1 1  C = M−   2  T M      n  Bước 3: Viết quy luật điều khiển tuyến tính hóa cho điều khiển moment  =  '+   = M ( ) ( ) ( )  = V  , + G ( ) + F  ,  , ',  vecto n x  ma trận n x n Bước 4: Viết quy luật điều khiển cho luật servo (bộ điều khiển bám)  ' =  + K E+ K E d v p • E =  − ; E =  − d d • Kv Kp ma trận n x n, thường chọn ma trận đường chéo với hệ số không đổi đường chéo • E E vecto n x sai số vị trí vận tốc 22 Bước 5: Chọn thông số Kp (Nm/rad) Kp (Nm.s/rad) phù hợp với hệ thống Bước 6: Quy hoạch quỹ đạo khảo sát hệ thống 6.1 THÀNH LẬP PHƯƠNG TRÌNH ĐỘNG LỰC HỌC (Đặt vị trí trọng tâm cuối khâu, bỏ qua moment quán tính ma sát hệ) Bước 1: Tính động K khâu Vị trí trọng tâm khâu 1: . x   x     1 0  1 P =  y  =     y  = 0   v = x + y + z =  K = m v =  1    1   1 1 11    z   L1    0   1 z    Vị trí trọng tâm khâu 2:      x  − L s c  − L c s   x   L c c     2 2 2  2  2  P =  y  =  L s c    y  =  L c c  − L s s    v = L2 (c  +  )  2  2   2  2 2  2 2  z  L + L s         2 z   L c   2     1 2 2  K = m v = m L (c  +  ) 2 2 2 2 23 Vị trí trọng tâm khâu 3:        − s  ( L c + L c ) −  L s ( +  ) + L s   c  2  1  x   1 23 2  23   x   c (L c + L c )         23 2        P =  y  =  s ( L c + L c )    y  = c  ( L c + L c ) −  L s ( +  ) + L s   s      23 2     1 23 2  23 2 2     z  L + L s + L s         2 23   z      L c  + L c  +       2 23       2 2    2 2  v = 1 L c + L c + L  + L  +   + 2L L c   +   2 23 2 3 3 2       .2 2    2   K = m  L c + L c + L  + L   +   + L L c    +    3 2 23 2 3 3 2         ( ) ( ) Vị trí trọng tâm khâu cuối:  x   c (L c + L c + L c ) 234     23 2 0P =  y  =  s ( L c + L c + L c )    23 2 234    z  L + L s + L s + L s    2 23 234        −s  L c + L c + L c −  L s ( +  ) + L s  + L s ( +  +  )  c  234  23 2 234    x   1 23 2              y  =  c  L c + L c + L c −  L s ( +  ) + L s  + L s ( +  +  )  s  234  23 2 234      1 23 2       z    L c ( +  ) + L c  + L c ( +  +  )     23 2 234   2 2  v2 =  L c +L c +L c + L  + L2 ( +  )2 + L2 ( +  +  )2 23 2 234 2 3 4   +2 L L c  ( +  ) + L L c ( +  )   +  +   + L L c  ( +  +  ) 33 2 3 4  4 34 2   .2  2 2 2   L c +L c +L c + L  + L ( +  ) + L ( +  +  ) 1  23 2 234 2 3 4 K = m   4      +2 L2 L3c3  ( + 3 ) + L3 L4c4 ( + 3 )   + 3 +   + L2 L4c34  ( + 3 +  )      ( ) ( ) ( ) ( ) Bước 2: Tính khâu Thế khâu 1: 24 0 P1 = −m1  0 −1 g   = m1gL  L1  Thế khâu 2:  L cc   212  P2 = −m  0 −1 g  L 2s1c2  = m 2g L1 + L s L + L s   2 ( ) Thế khâu 3:  c (L c + L c )  2   23 P3 = −m3  0 −1 g  s1(L3c23 + L2c2 )  = m3g L1 + L2s + L3s 23 L + L s + L s  2 23   ( ) Thế khâu cuối:  c (L c + L c + L c  2 234 )   23 P4 = −m  0 −1 g  s1(L3c23 + L 2c2 + L 4c234 )  = m 4g L1 + L 2s + L3s 23 + L 4s 234 L + L s + L s + L s  2 23 234   ( Bước 3: Thiết lập hàm Lagrange Phương trình Lagrange sau: 25 ) L =  K −  P = (K1 + K + K + K ) − (P1 + P2 + P3 + P4 ) .2 2   2 2 2 2  = m 2L (c2 1 + 2 ) + m3 1 L 2c2 + L3c23 + L 2 + L3  2 + 3  + 2L2L3c3 2  2 + 3        2       .2  2 2  L c + L c + L c  23 2 234 + L 2 + L3 (2 + 3 ) + L (2 + 3 + 4 )   + m4      +2L L c  ( +  ) + 2L L c ( +  )   +  +   + 2L L c  ( +  +  )  3 2 3 4  4 34 2        − m1gL 1+ m 2g L1 + L s + m3g L1 + L 2s + L3s 23 + m 4g L1 + L 2s + L3s 23 + L 4s 234   2 2  2 = 1  m 2L 2c2 + m3 L 2c2 + L3c23 + m L2c2 + L3c23 + L 4c234    2 2 2 + 2  m 2L + m3 L + L3 + 2L 2L3c3 + m L + L3 + L + 2L 2L3c3 + 2L3L 4c4 + 2L 2L 4c34    2 + 3  m3L3 + m L3 + L + 2L3L 4c4    2 2 + 4 m 4L + 2 3  m3 L3 + L 2L3c3 + m L3 + L + L 2L3c3 + 2L3L 4c4 + L 2L 4c34    2 + 3 4 m L4 + L3L4c4 + 2 4 m4 L4 + L3L4c4 + L2L4c34 − m1gL 1− m 2g L1 + L s − m3g L1 + L 2s + L3s 23 − m 4g L1 + L 2s + L3s 23 + L 4s 234 ( ) ( ) ( ( ) ( ) ) ( ( ( ( ) ( ) ( ( ( ) ( ) ) ( ) ) ( ) ) 26 ( ) ) ) Bước 4: Thiết lập phương trình Lagrange Ta có phương trình: = d L L − dt  i  i Tính tốn thành phần phương trình: d L 2 =   m L2 c + m L c + L c +m L c +L c +L c   2 2 23 2 23 234  dt  −2    m L2 c s + m L c + L c L s +L s +m L c +L c +L c  2 2 2 23 2 23 2 23 234  −2    m L c + L c L s +m L c +L c +L c L s +L s  2 23 23 2 23 234 23 234  −2   m L c + L c + L c L s 4 2 23 234 234 ( ) ( ( ( ) )( ) ) ( ( ( )( )( L2s2 + L3s23 + L4s234 ) ) ) ( ) ( ) d L =   m L2 + m L2 + L2 + L L c + m L2 + L2 + L2 + L L c + L L c + L L c   2 3 33 4 33 4 34  dt  +  m L2 + L L c +  m L2 + L L c + L L c − 2   L L m s + L L m s + L L m s  3 33 4 4 4 34  3 3 4 34  −2   m L L s + L L s −  m L L s − L L m s  − m L L s + L L s  4 4 34 4 34 33 4 4 34 d L =  m L2 + L L c +   m L2 + m L2 + L2 + L L c  +  m L2 + L L c 3 33  3 4 4  4 4 dt  −2 m L L s   − m L L s   − m L L s  4 4 33 4 4 ) ( ) ( ( ) ( ( ( ) ) ( ) ) ( ) ( d L =  m L2 + L L c + L L c +  m L2 + L L c +  m L2 4 4 34 4 4 4 dt  −m L L s   − m L L s   − m L L s + L L s   4 4 4 34 4 4 34 ( ) L =0 1 27 ) .2 L  = −   m L2 c s + m L s + L s L c +L c +m L s +L s +L s L c +L c +L c  2 2 2 23 2 23 2 23 234 2 23 234   −m gL c − m g L c + L c −m g L c +L c +L c 2 2 23 2 23 234 ( )( ( ) ) ( ( )( ) ) L  = −  m L c + L c L s +m L c +L c +L c L s +L s  2 23 23 2 23 234 23 234   − m L L s +m L L s +m L L s −  m L L s + m L L s + m L L s 33 33 4 34 3 33 33 4 34 −   m L L s − m gL c − m g L c + L c 4 34 3 23 23 234 ( ) ( ( )( ) ) ( ( ) ) L = − m L s L c +L c +L c 4 234 2 23 234  2 − m L L s + L L s −  m L L s −   m 2L L s + L L s 4 4 34 4 4 4 34 −  m L L s + L L s −   m L L s − m gL c 4 4 34 4 4 4 234 ( ) ( ) ( ( ) ) Như xác định được:  =   m L2 c + m ( L c + L c 2 +m L c +L c +L c 1 2 2 23 2 23 234  −2   m L2 c s + m L c + L c L s +L s +m L c +L c +L c  2 2 2 23 2 23 2 23 234  −2   m L c + L c L s +m L c +L c +L c L s +L s  2 23 23 2 23 234 23 234  −2  m L c + L c + L c L s 4 2 23 234 234 ) ( ( ( )( ) ) ( ( ( )( )( L2s2 + L3s23 + L4s234 ) ) ) (2  =   m L2 + m L2 + L2 + L L c )  2 ) ( 33 ) + m4 ( L22 + L32 + L24 + 2L2 L3c3 + 2L3L4c4 + 2L2 L4c34 ) ) ( +  m L2 + L L c +  m L2 + L L c + L L c − 2   L L m s + L L m s + L L m s  3 33 4 4 4 34  3 3 4 34  −2   m L L s + L L s −  m L L s − L L m s  − m L L s + L L s  4 4 34 4 34 33 4 4 34  +   m L2 c s + m L s + L s L c +L c +m L s +L s +L s L c +L c +L c  2 2 2 23 2 23 2 23 234 2 23 234  + m gL c + m g L c + L c +m g L c +L c +L c 2 2 23 2 23 234 ( ) ( ( ( )( ) ) ( ( )( ) 28 ) ) 3) ( ) ( ) ( L2 + L2 + L L c  +  m L2 + L L c 3 3 3 4 4  4 4 −2m L L s   − m L L s   − m L L s  4 4 33 4 4  +  m L c + L c L s +m L c +L c +L c L s +L s  2 23 23 2 23 234 23 234  + m L L s +m L L s +m L L s +  m L L s + m L L s + m L L s 33 33 4 34 3 33 33 4 34 +   m L L s + m gL c + m g L c + L c 4 34 3 23 23 234  =  m L2 + L L c +   m L2 + m ( ) ( )( ( ) ( ( ) ) ( ) ( L2 + L L c + L L c +  m L2 + L L c +  m L2 4 4 4 34 4 4 4 −m L L s   − m L L s   − m L L s + L L s   4 4 4 34 4 4 34 + m L s L c +L c +L c 4 234 2 23 234 2 + m L L s + L L s +  m L L s +   m 2L L s + L L s 4 4 34 4 4 4 34 +  m L L s + L L s +   m L L s + m gL c 4 4 34 4 4 4 234  = m ( ( ( ) ) ) ( ) ( ) 29 ) ) Bước 5: Phương trình động lực học dạng ma trận trạng thái    M11 M12  1  M    M 22  = M ( )  + V ( , ) + G ( )    =  21    M 31 M 32       M 41 M 42 M 13 M 23 M 33 M 43     M    V   G  14        M    V  G  24   2  + +  M    V   G  3 34        M    V  G   4  4 44       4 Trong đó:  M = m L2 c + m L c + L c + m L c + L c + L c  11 2 2 23 2 23 234 M = M = M =  12 13 14 M =  21  2 2 2  M 22 = m2 L2 + m3 L2 + L3 + L2 L3c3 + m4 L2 + L3 + L4 + L2 L3c3 + L3L4c4 + L2 L4c34    M 23 = m3 L3 + L2 L3c3   M = m L2 + L L c + L L c 4 4 34  24 M =  31   M 32 = m3 L3 + L2 L3c3   M = m L2 + m L2 + L2 + L L c 3 4 4  33   M 34 = m4 L4 + L3 L4c4   M 41 =   M = m L2 + L L c + L L c 4 4 34  42   M 43 = m4 L4 + L3 L4c4   M = m L2  44 4 ( ( ( ( ( ( ( ( ( ) ( ) ) ( ) ) ) ) ) ) ) 30 )   L s +L s +m L c +L c +L c L s +L s +L s V = −2    m L2 c s + m L c + L c 2 2 2 2 23 2 23 2 23 234 2 23 234  1  −2 1 3  m3 L2c2 + L3c23 L3s23 + m4 L2c2 + L3c23 + L4c234 L3s23 + L4 s234   −2   m L c + L c + L c L s 4 2 23 234 234   V = −2    L L m s + L L m s + L L m s  3 3 3 43 4 34     −2   m4 L3 L4 s4 + L2 L4 s34 − 3 4 m4 L2 L4 s34 − L2 L3m3s3 3 − m4 L3 L4 s4 + L2 L4 s34 4   +   m L2 c s + m L s + L s  L c +L c +m L s +L s +L s L c +L c +L c   2 2 2 23 2 23 2 23 234 2 23 234    V = −2m L L s   − m L L s   − m L L s  4 4 33 4 4   +  m L c + L c  L s +m L c +L c +L c L s +L s   2 23 23 2 23 234 23 234    +  m L L s +m L L s +m L L s +  m L L s + m L L s + m L L s 33 33 4 34 3 33 33 4 34   +   m L L s  4 34  V = − m L L s   − m L L s   − m L L s + L L s   4 4 4 34 4 4 34    + 1 m4 L4 s234 L2c2 + L3c23 + L4c234   +  m L L s + L L s +  m L L s +   m 2L L s + L L s 4 4 34 4 4 4 34   +   m L L s + L L s +  m L L s  4 4 34 4 4 ( ( )( ) ) ( ( ( )( )( ) ) ) ( ) ( ( ( )( ) ) ( ( )( )( ( ) ) ) ) ( ) ( ( ) ) G =  +m g L c +L c +L c G = m gL c + m g L c + L c 2 2 23 2 23 234   G3 = m3 gL3c23 + m4 g L3c23 + L4c234  G4 = m4 gL4c234 ( ( ) ) ( 31 ) ) ( ( ( ) ) 6.2 THIẾT KẾ BỘ COMPUTED TORQUE CONTROL TRÊN MATLAB SIMULINK ❖ Bộ Computed Torque Control ❖ Subsystem khối Robot Manipulator 32 ❖ Trajectory generation 33 6.3 KẾT QUẢ MÔ PHỎNG 34 6.4 KẾT LUẬN HỆ THỐNG • Hệ điều khiển ổn định • Tín hiệu ngõ bám theo tín hiệu đặt • Độ vọt lố thấp • Sai số xác lập nhỏ • Thời gian xác lập nhỏ 35 ... 34 4 4 4 −m L L s   − m L L s   − m L L s + L L s   4 4 4 34 4 4 34 + m L s L c +L c +L c 4 2 34 2 23 2 34 2 + m L L s + L L s +  m L L s +   m 2L L s + L L s 4 4 34 4 4 4 34. .. L L m s + L L m s + L L m s  3 3 3 43 4 34     −2   m4 L3 L4 s4 + L2 L4 s 34 − 3 ? ?4 m4 L2 L4 s 34 − L2 L3m3s3 3 − m4 L3 L4 s4 + L2 L4 s 34 ? ?4   +   m L2 c s + m L s + L s ... L L s 33 33 4 34 3 33 33 4 34   +   m L L s  4 34  V = − m L L s   − m L L s   − m L L s + L L s   4 4 4 34 4 4 34    + 1 m4 L4 s2 34 L2c2 + L3c23 + L4c2 34   +