tính toán động học và mô phỏng robot motoman

MỤC LỤC Trang  2  3    4  4  4  6    7  7  10  14    21  21  21  21  25  26    32  32  34  35  35  37  39  Yêu cầu đồ án Lời nói đầu Chương I: Tổng quan Robot Motoman 1. Lịch sử phát triển sơ lược   2. Robot Motoman  3. Các thông số kỹ thuật của Robot Motoman    Chương II: Các toán động học cho Robot 1. Bài toán động học thuận   2. Bài toán động học ngược  3. Ma trận Jacobien cho Robot  Chương III: Bài toán động lực học Robot 1. Hàm Lagrange và các vấn đề về động lực học Robot  2. Tính tốn các giá trị động lực học cho Robot Motoman  a. Động năng  b. Thế năng  c. Xây dựng phương trình động lực học Robot    Chương V: Xây dựng thuật tốn điều khiển mơ 1. Thiết kế bộ điều khiển theo phương pháp PD bù trọng trường   2. Thiết kế quỹ đạo bậc ba cho Robot   3. Mơ phỏng ba khớp đầu Robot  a. Phương pháp điều khiển PD bù trọng trường trên Simulink  b. Phương pháp điều khiển PD bù trọng trường có kèm thiết kế quỹ đạo  c. Xây dựng mơ phỏng bằng Toolbox SimMechanics     Tài liệu tham khảo   44    U CẦU ĐỐI VỚI ĐỒ ÁN - Tìm hiểu các thơng tin về Robot Motoman.  - Tính tốn mơ hình động học Robot gồm động học thuận cho các khớp và động  học ngược cho ba khớp đầu  - Giải bài tốn động lực học cho ba khớp đầu Robot  - Giải bài tốn động học vận tốc (tính tốn ma trận Jacoby)  - Xây dựng luật điều khiển và thiết kế quỹ đạo cho Robot  - Mơ phỏng kết quả tính tốn được cho ba khớp đầu bằng phần mềm Matlab  LỜI NĨI ĐẦU Ngày nay trong sự nghiệp cơng nghiệp hố, hiện đại hố đất nước vấn đề tự động hố  sản  xuất  có  vai  trị  đặc  biệt  quan  trọng.Vì  vậy  trong  những  năm  gần  đây  các  lĩnh  vực  thuộc tự động hóa ngày càng được quan tâm nghiên cứu và phát triển, trong đó khơng thể  khơng nói đến những ứng dụng quan trọng của kỹ thuật Robot trong cơng nghiệp.  Mục  tiêu  ứng  dụng  kỹ  thuật  Robot  trong  cơng  nghiệp  nhằm  nâng  cao  năng  suất  dây  chuyền cơng nghệ, nâng cao chất lượng và khả năng cạnh tranh của sản phầm, đồng thời  cải thiện điều kiện lao đơng. Sự cạnh tranh hàng hóa đặt ra một vấn đề thời sự là làm sao  để hệ thống tự động hóa sản xuất phải có tính linh hoạt nhắm đáp ứng với sự biến động  thường xun của thị trường hàng hóa. Robot cơng nghiệp là bộ phận cấu thành khơng  thể thiếu trong hệ thống sản xuất tự động linh hoạt đó.  Ở nước ta, từ những năm 1990 trở lại đây, Robot và kỹ thuật Robot đã được ứng dụng  vào  sản  xuất  khá  rộng  rãi.  Trong  những  Robot  được  ứng  dụng  vào  sản  xuất  thì  Robot  Motoman là một Robot đóng góp phần đáng kể trong lĩnh vực tự động hóa cơng nghiệp ở  nước  ta.  Đây  là  một  Robot  được  ứng  dụng  khá  đa  dạng  trong  nhiều  lĩnh  vực  :  hàn  tự  động, các dây chuyền sản xuất công nghiệp tự động, phun sơn   Nhận  thấy  tầm  quan  trọng  của  kỹ  thuật  Robot  nói  chung  và  ứng  dụng  của  Robot  Motoman  nói  riêng,  với  kiến  thức  học  hỏi  dược  trong  quá  trình  học  tập  tại  bộ  mơn  tự  động hóa và sự hướng dẫn nhiệt tình của cơ  giáo T.S Nguyễn Phạm Thục Anh nhóm  sinh viên chúng em đã chọn đề tài đồ án chun nghành là “ROBOT MOTOMAN” với  mục  đích  tìm  hiểu,  xây  dựng,  mơ  phỏng  mơ  hình  điều  khiển  robot  nhằm  ứng  dụng  lý  thuyết vào thực tế. Dù được sự hướng dẫn tận tình của cơ giáo nhưng do q trình tích  lũy kiến thức và kinh nghiệm cịn hạn chế nên đồ án khơng thể tránh khỏi những thiếu  sót. Chúng em mong các thầy cơ góp ý để đồ án của chúng em được hồn thiện hơn.  Chúng em xin chân thành cảm ơn!  Sinh viên thực hiện    Lương Đình Ngọc    Trần  Thành  Kiên  CHƯƠNG I: TỔNG QUAN VỀ ROBOT MOTOMAN Lịch sử phát triển sơ lược Kể  từ  khi  Robot  đầu  tiên  được  chế  tạo  với  sản  phẩm  đầu  tiên  có  tên  gọi  người  máy  cơng nghiệp là verstran của cơng ty Mỹ vào năm 1960 các sản phẩm liên quan đến tay  máy  cơng  nghiệp  bắt  đầu  được  quan  tâm  nghiên  và  nhờ  đó  kỹ  thuật  Robot  bắt  đầu  có  những bước phát triển đầu tiên.  Cùng theo bản quyền của Mỹ các nước trên thế giới đă chạy đua sản xuất robot cơng  nghiệp,các cơng ty về Robot được thành lập để nghiên cứu và phát triển ,ứng dụng Robot  trong tự động hố cơng nghiệp.  Yaskawa Motoman là một cơng ty chun cung cấp các giải pháp tự động hóa sáng tạo  cho hầu như tất cả các ngành cơng nghiệp và ứng dụng robot như: hàn, lắp ráp, sơn, pha  chế,  cắt  gọt  vật  liệu…  với  hơn  175  mơ  hình  robot  khác  biệt  và  hơn  40  giải  pháp  cơng  nghệ để giải quyết các cơng việc một cách hồn chỉnh như các hệ thống vận hành, thiết bị  an tồn.  Yaskawa Motoman là cơng ty đi đầu trong lĩnh vực phát triển các Robot phục vụ cho  các cơng nghệ gia cơng và tự động, Robot Motoman chính là sản phẩm nghiên cứu sản  xuất thành cơng của hãng.    Robot Motoman a Giới thiệu chung Robot Motoman là Robot tác động nhanh, linh hoạt, nhỏ gọn và đáng tin cậy. Đây là  một loại Robot hoạt động tốt với nhiều cài đặt. Nó cũng cung cấp rất nhiều ứng dụng, với  hiệu suất sử dụng cao, đảm bảo những u cầu về chất lượng, thời gian hồn vốn ngắn.  Motoman được thiết kế cứng và thẳng, điều này dẫn đến độ ồn làm việc thấp, thời gian  bảo trì lâu. Ngồi ra nó cịn được thiết kế nhỏ gọn, cổ tay mỏng, hiệu suất hoạt động cao  ngay cả trong những vị trí khó.  b Các ứng dụng      Robot Motoman được ứng dụng rộng rãi vào các dây chuyền sản xuất tự động, hiện  nay các lĩnh vực phổ biến nhất là:  - Các q trình hàn và nhiệt luyện.  - Cơng nghệ gia cơng lắp ráp.  - Phun sơn,vận chuyển hàng hố.    c Phân loại sản phẩm Với dịng sản phẩm ban đầu,qua nhiều năm phát triển đã có nhiều mẫu Motoman được  sản xuất dựa trên cùng một mơ hình động học hệ thống nhưng có cải tiến và khác nhau về  cơ cấu cơ điện tử và giới hạn làm việc các khâu.  Các  dịng  sản  phẩm  thuộc  Motoman  trên  thị  trường  hiện  nay  phổ  biến  là  các  mẫu  series Motoman HP , Motoman UP, Motoman SV.      Motoman UP Motoman HP20 Motoman SV3 Thông số kỹ thuật - Số bậc tự do: - Kiểu khớp: quay Mơ hình Robot:   Không gian làm việc Robot:   CHƯƠNG II: CÁC BÀI TOÁN ĐỘNG HỌC ROBOT A BÀI TỐN ĐỘNG HỌC THUẬN ROBOT MOTOMAN Áp dụng phương pháp Denavit – Hartenberg cho mơ hình tốn học của Robot Motoman,  ta đặt các hệ trục  như sau:         Từ mơ hình của Robot, ta xác định được bảng thơng số D–H như sau :  i  1  2  3  4  5  6  o θ i ( )  θ1  θ2+90  θ3  θ4  θ5  θ6    αi(o)  900  00  900  -900  900  00  ai(mm)  a1   a2   a3   0  0  0  di(mm)  d1  0  d3  0  d5  d6    Ma trận biến đổi thuần nhất   :  − − = 0   0 Dựa vào bảng thông số D-H, thay các giá trị vào ma trận ta được:   c1 s A1  0  0 s1 a1c1   c1 a1s1       d1   0   c3 s A3  0  0 s3  c3 0 a c3  a 3s3  d3     c5 s A5   0  0 s5  c5 0 0      d5   1       s2 c A2                  c2 s2 0  a 2s  a 2c2       c   A  s4 4 0  0  s4 c4 1 0  c6    A s6 6 0  0  s6 c6 0 0    0  1     0 0    d6   1     Nhân các ma trận  ,  ,  ,  ,  ,   ta được ma trận biểu diễn vị trí và hướng của  khâu tác động cuối của Robot như sau:    =         =                 =   0   n x  c1[s 23 ( c 4c5c  s 4s )  c 23s5s ]  s1 (s 4c5c  c 4s )   n y  s1[s23 (c4c5c6  s4s6 )  c23s5s6 ]  c1 (s4c5c6  c4s6 )   n z  c 23 (c 4c5c6  s 4s )  s32c5c6     o x  c1[s 23 (c c5c  s c )  c 23s 5s ]  s1 (s c5s  c 4s )   oy  c1[c23 (c4c5c6  s4s6 )  s23s5s6 ]  c1 (s1c5s6  c4c6 )   o z  c 23 (c 4c5s  s 4c )  s 23s5s     a x  c1 ( s 23c 4s5  c 23c5 )  s1s 4s5   a y  s1 (c23c4s5  s23s5 )  c1s4s5   a z  c 23c s  s 23c     p x  c1[ s 23 (c 4s5d  s 4d )  c 23c5d  (s  c )a 3s  a 2s )  s1 (s 4s5 d  c d  d )  a1c1     py  s1[c23 (c4s5d6  s4d5 )  s23c5d6  (c2  s2 )a 3s3  a 2s2 )  c1 (s4s5d6  c4d5  d3 )  a1s1     p z  c 23 (c 4s5d  s 4d )  s 23c5d  (c  s )a 3s3  a 2s  d1     Giao diện tính tốn động học thuận thiết kế giao diện GUIDE Matlab:   Giao diện tính tốn động học thuận  B BÀI TỐN ĐỘNG HỌC NGƯỢC ROBOT MOTOMAN   Bài tốn với dữ kiện ban đầu là vị trí của khâu tác động cuối, nhiệm vụ đưa ra là tìm  giá trị của các biến khớp để đảm bảo là khi chuyển động ứng với các biến khớp đó thì  khâu tác động cuối của Robot sẽ chuyển động chính xác đến vị trí này.  Trong  bài  tốn  động  học  thuận,  vị  trí  và  hướng  tay  máy  được  xác  định  từ  các  biến  khớp (góc quay ở khớp quay đã biết )  Để điều khiển Robot  di chuyển theo các vị trí mong muốn của tay máy trong khơng  gian,  cần  xác  định  các  giá  trị  biến  khớp  tương  ứng  với  vị  trí  và  hướng  của  tay  Robot  mong muốn. Đây là nội dung của bài tốn động học ngược  Do với robot motoman có 6 khớp quay tự do nhưng việc xác định vị trí trong khơng  gian là bài tốn của ba khớp đầu,cịn 3 khớp cuối là khâu tác động tuỳ từng nhiệm vụ kỹ  thuật nên trong đồ án này chúng em sẽ chỉ tính tốn động học ngược và động lực học của  ba khớp đầu để ứng dụng điều khiển vị trí của robot.  Ta có phương trình động học thuận của Robot có dạng:    ( =   ) ( ) ( ) ( ) ( = ) ( )     (*)    0 Ma trận   đã biết, tức là vị trí và hướng của khung tọa độ tay Robot đã biết, cần xác  định giá trị các biến khớp. Nhân hai vế của phương trinh (*) với ma trận nghịch đảo của   là ma trận ( )  nhận được phương trình sau:        (   ) = =               (1)  Từ đó ta có:  = 0 − 0 0 0   0   10 c Phương trình động lực học viết dạng ma trận trạng thái: Phương trình động lực học thanh nối   T=H(Q) Q +V(Q, Q )+G(Q)  M1   H11 M  = H    21  M   H 31 H12 H 22 H32   θ1 H13     V11  G1    H 23  θ  +  V21  + G    H33     V31  G  θ    Với:   H11 =J1 +m1a g1   H12 =2m (d g3a g3c 23 -d g3a c )   H13 =-2m 3d g3a g3c 23  H 21 =2m3 (d g3a g3c 23 -d g3a c )   H 22 =J  2  H 23 =2(a g3a c c 23 +a 2s +a g3s 23 +a g3c 23 )m3  H 31 =-2m3d g3a g3c 23  2  H 32 =2(a g3a c c 23 +a 2s +a g3s 23 +a g3c 23 )m   H 33 =J +2a g3m 2 V11 =(2m a g2 s c -2a1m a g2 c +2a 22s c +2a g3 s 23c 23 -2a1a c +2a a g3c 23 ) θ 2 g3 23 23 +(2a s c -2a1a g3c3 +2a a g3c 23 ) θ3 +(2m3d g3a c -2m3d g3a g3s 23 ) θ +2m 3d g3a g3s 23 30 2 V21 =-(2m a g2 s c -a1a g2 c2 +2a 22s c +2a g3 s 23c 23 -2a1a c -2a1ag3c23+2a a g3c23 ) θ1 -2a2ag3s3θ3 -(2m3d g3a g3s 23 +2d g3a g3s 23 ) θ1 θ3 V31 =2d g3a g3s 23 θ1 θ +2(-2a g3a c2s 23 +2m3a g3c 23 -4m3a g3 c 23s23 ) θ θ3 -4d g3a g3s 23 θ1 θ +(a g3s 23 -a1 +a ).2a g3c 23 θ1 -2a a g3s 23 θ 2 g3 23 23 +2m3 (-a g2 a 2s c 23 -a g2 c c23 +a c2 +a g3c 23 -2a c s ) θ   G1  G2  a g m gc2 G3  a g3gc3 m3g                                    31 CHƯƠNG IV XÂY DỰNG CÁC THUẬT TOÁN ĐIỀU KHIỂN VÀ MÔ PHỎNG Thiết kế điều khiển dùng phương pháp PD bù trọng trường Bài tốn đặt ra là xác định cấu trúc bộ điều khiển đảm bảo hệ thống ổn định tuyệt đố  xung quanh điểm cân bằng, khơng phụ thuộc vào khối lượng thanh nối và tải.   Luật điều khiển này dưa trên tiêu chuẩn ổn định Liapunov.   Mơ hình bộ điểu khiển theo phương pháp PD bù trọng trường  Tổng quan điều khiển PD Phương trình động lực học:    +G  Q T=H  Q Q+V Q,Q    Hàm điều khiển :                                                       Trong đó :   Tdk =Kp.E+Kd.E+G(Q)   E  Qd  Q    E  Qd        (1)                                                          Qd: Giá trị đặt  Mục đích;    +G  Q         (2)    Kp.E+Kd.E+G(Q)  H  Q  Q+V Q,Q                      32 Tính chất phương trình động lực học:         +   H(Q): là hàm xác định dương , hàm đối xứng         +    V  Q,Q  H (Q )  S (Q, Q )                  (3)   T      +   Q S (Q, Q ).Q    T T      +    a H (Q).b  a.H (Q).b           (4)   Mặt khác ta có:    Q T H (Q).Q  (Q T H (Q).Q   Q T H (Q).Q  Q T H (Q).Q )       QT H (Q).Q 2   d T                                                                 = Q H (Q).Q     (5) dt d E T Kp.E   Q T Kp.E         (6)   +      E  Q         dt      H (Q)  S (Q, Q )  Q   Từ (1)(2)(3)          Kp.E  Kd Q  H (Q).Q        H (Q)  S (Q, Q )  Q  Kp.E                           Kd Q  H (Q).Q 2        Nhân cả hai vế của biểu thức trên với   Q T      Q T H (Q ).Q  Q T S (Q , Q ).Q  Q T Kp.E  Q T Kd Q   (7)   Q T H (Q ).Q 2           Thay (4)(5)(6) vào (7) ta được:     d T            Q H (Q ).Q  E T Kp.E   Q T Kd Q    dt                  Từ biểu thức trên ta thấy được hàm Lyapunov:      Q T H (Q ).Q  E T Kp.E     V  Q,Q                   Q T Kd Q    Q,Q V                33 Ở chế độ làm việc xác lập các thành phàn tốc độ và gia tốc bằng khơng từ đó ta thấy  được:  Q  Qd E                                                 E  Q  Qd Với luật bù trọng lực phi tuyến với bộ điều khiển PD ,tất cả các điểm làm việc cân  bằng  sẽ  làm  việc  cân  bằng  không  chịu  sự  ảnh  hưởng  của  trọng  lực  Rôbôt.  Mức  độ  ổn  định và chất lượng của quá trình động phụ thuộc vào giá trị của Kd và Kp.    Thiết kế quỹ đạo bậc ba cho Robot Bài tốn thiết kế quỹ đạo cho khớp là xác định đường biểu diễn của vị trí khớp ( góc  quay của khớp quay hoặc độ di chuyển của khớp tịnh tiến ) theo thời gian khi di chuyển  từ vị trí ban đầu   đến vị trí cuối cùng  trong thời gian   ,với q là biến khớp tổng  qt.Quỹ đạo di chuyển của khớp giữa hai vị trí sẽ thoả mãn 4 điều kiện :  - Vị trí ban đầu và vị trí cuối cùng.  - Tốc độ tại vị trí ban đầu và tại vị trí cuối cùng.  Do đó đa thức bậc 3 sẽ thích hợp cho quỹ đạo của khớp robot :  q(t)=  + t+  +     Các điều kiện đầu và cuối:  ( )=   q( )=   ̇ ( )=  ̇   ̇ ( )=  ̇   Trong mô phỏng quỹ đạo khớp bằng simulink ta sẽ sử dụng phương pháp xây dựng quỹ  đạo bậc 3 để tạo hàm tổng quát cho quỹ đạo di chuyển khớp.  34 Mơ khớp đầu robot a Mơ hình điều khiển PD bù trọng trường Simulink   Mơ hình bộ điều khiển PD bù trọng trường  Thông số các thanh nối lần lượt như sau:  m1 = 60(kg)  ;   m2 = 30(kg) ;   m3 = 20(kg) ;  d1=0.275(m);  a1=0.15(m);  a2=0. 6(m);  d3=0.17(m);  a3=0.12(m);    Khâu khuếch đại và tỉ lệ đạo hàm được lựa chọn :  0  0 1800  200    Kp   1800     va   Kd=  250    0 400 60  Các giá trị đặt:  qd1  3.4  rad  ;  qd2  2.1 rad  ; qd3  2.7  rad  ; 35 Kết chạy mơ phỏng: Đáp ứng đầu ra với bộ điều khiển PD  36 Nhận xét : Nhờ việc chọn đúng thơng số của hai bộ điều khiển Kp và Kd do vậy các đường đăc  tính có chất lượng tốt : thời gian xác lập nhanh