Đăng ký Đăng nhập
Trang chủ Giáo dục - Đào tạo Cao đẳng - Đại học Tính toàn và mô phỏng robot motoman....

Tài liệu Tính toàn và mô phỏng robot motoman.

.PDF
44
1214
88

Mô tả:

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 của đồ án Lời nói đầu Chương I: Tổng quan về 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 bài 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 toá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 các thuật toán điều khiển và mô phỏng 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    1 YÊU CẦU ĐỐI VỚI ĐỒ ÁN - Tìm hiểu các thông tin về Robot Motoman.  - Tính toá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 toán động lực học cho ba khớp đầu Robot  - Giải bài toán động học vận tốc (tính toá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 toán được cho ba khớp đầu bằng phần mềm Matlab  2 LỜI NÓI ĐẦU Ngày nay trong sự nghiệp công nghiệp hoá, hiện đại hoá đất nước vấn đề tự động hoá  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 xuyên 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 chuyên 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 quá 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 hoà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  3 CHƯƠNG I: TỔNG QUAN VỀ ROBOT MOTOMAN 1. 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 hoá công nghiệp.  Yaskawa Motoman là một công ty chuyên 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 hoàn chỉnh như các hệ thống vận hành, thiết bị  an toà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.    2. 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 yêu cầu về chất lượng, thời gian hoà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. Ngoà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 quá 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 hoá.  ...  4 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 5 3. Thông số kỹ thuật - Số bậc tự do: 6 - Kiểu khớp: quay Mô hình Robot:   Không gian làm việc của Robot:   6 CHƯƠNG II: CÁC BÀI TOÁN ĐỘNG HỌC ROBOT A. BÀI TOÁN ĐỘNG HỌC THUẬN ROBOT MOTOMAN Áp dụng phương pháp Denavit – Hartenberg cho mô hình toá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    7 Ma trận biến đổi thuần nhất   :  − − = 0 0   0 0 1 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  1 0  0 0 s1 a1c1  0  c1 a1s1       1 0 d1   0 0 1   c3 s A3  3 0  0 0 s3 0  c3 1 0 0 0 a 3 c3  a 3s3  d3   1   c5 s A5   5 0  0 0 s5 0  c5 1 0 0 0 0 0      d5   1       s2 c A2  2  0   0              c2 s2 0 0 0  a 2s 2  0 a 2c2    1 0   0 1  c 4   A  s4 4 0  0 0  s4 0 c4 1 0 0 0  c6    A s6 6 0  0  s6 c6 0 0 0 0    0  1     0 0 0 0    1 d6   0 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 0 0 1   8 n x  c1[s 23 ( c 4c5c 6  s 4s 6 )  c 23s5s 6 ]  s1 (s 4c5c 6  c 4s 6 )   n y  s1[s23 (c4c5c6  s4s6 )  c23s5s6 ]  c1 (s4c5c6  c4s6 )   n z  c 23 (c 4c5c6  s 4s 6 )  s32c5c6     o x  c1[s 23 (c 4 c5c 6  s 4 c 6 )  c 23s 5s 6 ]  s1 (s 4 c5s 6  c 4s 6 )   oy  c1[c23 (c4c5c6  s4s6 )  s23s5s6 ]  c1 (s1c5s6  c4c6 )   o z  c 23 (c 4c5s 6  s 4c 6 )  s 23s5s 6     a x  c1 ( s 23c 4s5  c 23c5 )  s1s 4s5   a y  s1 (c23c4s5  s23s5 )  c1s4s5   a z  c 23c 4 s 5  s 23c 5     p x  c1[ s 23 (c 4s5d 6  s 4d 5 )  c 23c5d 6  (s 2  c 2 )a 3s 3  a 2s 2 )  s1 (s 4s5 d 6  c 4 d 5  d 3 )  a1c1     py  s1[c23 (c4s5d6  s4d5 )  s23c5d6  (c2  s2 )a 3s3  a 2s2 )  c1 (s4s5d6  c4d5  d3 )  a1s1     p z  c 23 (c 4s5d 6  s 4d 5 )  s 23c5d 6  (c 2  s 2 )a 3s3  a 2s 2  d1     Giao diện tính toán động học thuận thiết kế trên giao diện GUIDE Matlab:   Giao diện tính toán động học thuận  9 B. BÀI TOÁN ĐỘNG HỌC NGƯỢC ROBOT MOTOMAN   Bài toá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  toá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 toá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 toá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 toá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 0 0 1 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 1 0 0 0 0 0 1   0 0 0 1   10 =                                 ( ) ( ) ( ) 0 ( ) ( ) ( ) 0 ( ) ( ) ( ) 0 ( ) ( )     ( ) 1   (2)    Trong đó:  ( )= . + .    ( ) =    ( )= . + .    Với x, y, z là các thành phần của vecto v  Ma trận   được tính theo phương trình (1):  − − − − = − − + + 0 1 − 0 0 + + 0  (3)    Cân  bằng  các  thành  phần  cột  4  của  hai  ma  trận  phương  trình  (2)  và  (3)  ta  nhận  được  phương trình sau:  ( )= . + . + .                (4a)  ( )= . + . + .               (4b)              (4c)            (5a)            (5b)  ( ) = 0        Ta có:  − = 0  Suy ra: = ( ,   ) Kết hợp phương trình (1),(4a) và (6c) ta được:  . + − . = = . + + .   +   .        11 Viết lại phương trình (5a) và (5b) như sau:  . + . − − . = = . + +   .                 (6a)            (6b)    Bình phương hai phương trình (6a) và (6b) và cộng lại ta có phương trình sau:  ( + − ) +( − ) =( ) +( ) +2 ( + )  Sử dụng các hàm lượng giác, viết gọn lại phương trình trên như sau:  = cos[( +                    )− + ]=     − ) − [(     (7)    Do đó nhận được:  = ( + − ) +( 2 ) +( ) ]     Suy ra:  = ( , ) Tiếp tục nhân ma trận nghịch đảo của  ( ) =( ) ( −  sẽ nhận được phương trình sau:  ) ( ) ( ) . − − = 0 0 0 0 =   0 0   0 1   Cân bằng các phần tử ta có:  − + + = 0  Giải phương trình trên, ta được :  = 2( , + )    12 Ta xác định   các phương trình sau:  + = + = Vì  = − + =  và  + + − + +( =    nên ta có:  +( =   ) − + )+ +     Suy ra:  = = Vậy: = ( )( + )− − ( ( ) + + ( ( , + ( − ) − ) ) + − + + + ) + (     ). Giao diện tính toán động học ngược cho 3 khớp đầu: Giao diện tính toán động học ngược 13 C. MA TRẬN JACOBIEN ROBOT MOTOMAN   1. Xác định ma trận HJ cho 6 khớp Ta có  nx6     = c6;  ny6     = s6;  nz6     = 0;    ox6     = -s6;  oy6     = c6;  oz6     = 0;    ax6     = 0;  ay6     = 0;  az6     = 1;    px6     = 0;  py6     = 0;  pz6     = d6;    J61=-nx6 *py6 + ny6 *px6 = 0  J62= -ox6 *py6+oy6 *px6  = 0  J63= -ax6 *py6 +ay6 *px6 = 0  J64= nz6 = 0   J65= oz6  =  0  J66 = az6 =  1  nx5     = c5*c6;  ny5     = c6*s5;   nz5     = s6;         ox5     = -c5*s6;    oy5     = -s5*s6;   oz5     = c6;       14 ax5     = s5;    ay5     = -c5;   az5     = 0;    px5     = s5*d6;  py5     = -c5*d6;  pz5     = d5;    J51=-nx5 *py5 + ny5 *px5  =-(c5*c6)*(-c5*d6)+( c6*s5)* (s5*d6)  J52= -ox5 *py5+oy5 *px5 = -(-c5*s6)*( -c5*d6)+( -s5*s6)*( s5*d6)  J53= -ax5 *py5 +ay5 *px5 = -( s5)*( -c5*d6)+( -c5)*( s5*d6)  J54= nz5 = s6  J55= oz5  = c6  J56 = az5 = 0  nx4     = c4*c5*c6-s4*s6;  ny4     = c4*s6 + c5*c6*s4;  nz4     = -c6*s5;                  ox4     = - c6*s4 - c4*c5*s6;   oy4     = c4*c6 - c5*s4*s6;  oz4     = s5*s6;        ax4     = c4*s5;   ay4     = s4*s5;   az4     = c5;               px4     = c4*s5*d6 - s4*d5;  py4     = c4*d5 + s4*s5*d6;  pz4     = c5*d6;  J41=-nx4 *py4 + ny4 *px4    = -( c4*c5*c6-s4*s6)*( c4*d5 + s4*s5*d6)+( c4*s6 + c5*c6*s4)*( c4*s5*d6 - s4*d5)  J42= -ox4 *py4+oy4 *px4    = -(- c6*s4 - c4*c5*s6)*( c4*d5 + s4*s5*d6)+( c4*c6 - c5*s4*s6)*( c4*s5*d6 - s4*d5)  J43= -ax4 *py4 +ay4 *px4   15 = -( c4*s5)*( c4*d5 + s4*s5*d6)+( s4*s5)*( c4*s5*d6 - s4*d5)  J44= nz4 = -c6*s5  J45= oz4  =  s5*s6   J46 = az4 = c5         nx3     = - c3*(s4*s6 - c4*c5*c6) - c6*s3*s5;  ny3     = c3*c6*s5 - s3*(s4*s6 - c4*c5*c6);  nz3     = c4*s6 + c5*c6*s4;                       ox3     = s3*s5*s6 - c3*(c6*s4 + c4*c5*s6);  oy3     = - s3*(c6*s4 + c4*c5*s6) - c3*s5*s6;   oz3     = c4*c6 - c5*s4*s6;    ax3     = c5*s3 + c3*c4*s5;   ay3     = c4*s3*s5 - c3*c5;   az3     = s4*s5;                          px3     = c3*a3 - c3*(s4*d5 - c4*s5*d6) + c5*s3*d6;  py3     = s3*a3 - s3*(s4*d5 - c4*s5*d6) - c3*c5*d6;  pz3     = d3 + c4*d5 + s4*s5*d6;  J31=-nx3 *py3 + ny3 *px3    = -(- c3*(s4*s6 - c4*c5*c6) - c6*s3*s5)*( s3*a3 - s3*(s4*d5 - c4*s5*d6) - c3*c5*d6)  +( c3*c6*s5 - s3*(s4*s6 - c4*c5*c6))*( c3*a3 - c3*(s4*d5 - c4*s5*d6) + c5*s3*d6)  J32 = -ox3 *py3+oy3 *px23   = -( s3*s5*s6 - c3*(c6*s4 + c4*c5*s6))*( s3*a3 - s3*(s4*d5 - c4*s5*d6) - c3*c5*d6)  +(- s3*(c6*s4 + c4*c5*s6) - c3*s5*s6)*( c3*a3 - c3*(s4*d5 - c4*s5*d6) + c5*s3*d6)  J33 = -ax3 *py3 +ay3 *px3   = -( c5*s3 + c3*c4*s5)*( s3*a3 - s3*(s4*d5 - c4*s5*d6) - c3*c5*d6)  +( c4*s3*s5 - c3*c5)*( c3*a3 - c3*(s4*d5 - c4*s5*d6) + c5*s3*d6)  J34= nz3   = c4*s6 + c5*c6*s4  16 J35= oz3    = c4*c6 - c5*s4*s6  J36 = az3   = s4*s5    n x2 =s 23 (-c 4 c5c6 +s 4s 6 )-c 23s 5c6 ; n y2 =c23 (c 4 c5 c6 -s 4s 6 )+s 23s5c6 ; n z2 =s 4 c5c6 +c4s 6 ; o x2 =s 23 (c 4 c5s 6 +s 4c6 )-c 23s5s 6 ; o y2 =-c 23 (c 4 c5s 6 +s 4c6 )+s 23s5s 6 ; oz2 =-s 4c5s 6 +c 4c6 ; a x2 =-s 23c 4s 5 +c23c5 ; a y2 =c 23c 4s5 +s 23c5 ; a z2 =s 4s5 ; p x2 =-s 23 (c4s5d 6 -s 4d 5 )+c 23c5d 6 -(s 2 +c 2 )a 3s3 -a 2s 2 ; p y2 =c23 (c 4s5d 6 -s 4 d5 )+s 23c5d 6 -(c 2 -s 2 )a 3s3 +a 2s 2 ; p z2 =s 4s5d 6 +c 4d 5 +d3 ;   J 21   n x2 * p y2     n y2  * p x2     s 23 (-c 4 c5 c6 +s 4s 6 )-c 23s5c6  *  c 23 (c 4s 5d 6 -s 4 d 5 )+s 23c5d 6 -(c 2 -s 2 )a 3s 3 +a 2s 2      c 23 (c 4 c5c6 -s 4s 6 )+s 23s5 c6  *  -s 23 (c4s5 d 6 -s 4d 5 )+c23c5d 6 -(s 2 +c 2 )a 3s 3 -a 2s 2  J 22       o x2 * p y2  o y2   * p x2    s 23 (c 4 c5s 6 +s 4 c6 )-c23s 5s 6  *  c 23 (c 4s5 d 6 -s 4d 5 )+s 23c5d 6 -(c 2 -s 2 )a 3s 3 +a 2s 2      -c 23 (c 4 c5s 6 +s 4 c6 )+s 23s5s 6  * (-s 23 (c 4s5 d 6 -s 4 d 5 )+c 23c5 d 6 -(s 2 +c 2 )a 3s3 -a 2s 2 ) J 23       a x2 * p y2    a y2   * p x2  (-s 23c 4s5 +c23c5 ) *(c23 (c 4s5d 6 -s 4d 5 )+s 23c5d 6 -(c 2 -s 2 )a 3s3 +a 2s 2 )   (c 23c 4s5 +s 23c5 )*(-s 23 (c 4s 5d 6 -s 4d 5 )+c23c5d 6 -(s 2 +c 2 )a 3s3 -a 2s 2 ) 17 J 24   n z2  s 4c5c6 +c4s6   J 25   oz2  -s4c5s6 +c4c6   J 26     a z2  s 4 s5   n x1  c1[s23 (c4c5c6  s4s6 )  c23s5s6 ]  s1 (s4c5c6  c4s6 )   n y1  s1[s 23 ( c 4c5c 6  s 4s6 )  c 23s 5s 6 ]  c1 (s 4 c5c6  c 4s 6 )   n z1  c23 (c4c5c6  s 4s6 )  s32c5c6     o x1  c1[s 23 (c 4c5c6  s 4c6 )  c23s5s6 ]  s1 (s 4c5s6  c 4s6 )   o y1  c1[  c 23 (c 4 c 5c 6  s 4s 6 )  s 23s 5s 6 ]  c1 (  s1c 5s 6  c 4 c 6 )   oz1  c23 (c4c5s6  s 4c6 )  s23s5s6     a x1  c1 ( s 23c 4s 5  c 23c5 )  s1s 4s 5   a y1  s1 (c 23 c 4 s 5  s 23s 5 )  c1s 4 s 5   a z1  c 23 c 4 s 5  s 23 c 5     p x1  c1[ s 23 (c 4s5d 6  s 4d 5 )  c 23c5d 6  (s 2  c2 )a 3s3  a 2s 2 )  s1 (s 4s 5d 6  c 4d 5  d 3 )  a1c1   p y1  s1[c 23 (c 4s 5d 6  s 4 d 5 )  s 23c5d 6  (c 2  s 2 )a 3s 3  a 2s 2 )  c1 (s 4s5d 6  c 4 d 5  d 3 )  a1s1       pz1  c23 (c4s5d6  s4d5 )  s23c5d6  (c2  s 2 )a 3s3  a 2s2  d1   J11 =-n x1*p y1  + n y1*p x1   =-(c1[s23 (-c4c5c6 +s 4s6 )-c23s5s6 ]+s1 (s 4c5c6 +c4s6 ))*(c1[-s23 (c4s5d6 -s 4d5 )+c23c5d6 -(s 2 +c2 )a 3s3 -a 2s2 )+s1 (s4s5d 6 +c4 d5 +d3 )+a1c1 ) +(s1[s23 (-c4 c5c6 +s4s6 )-c23s5s6 ]-c1 (s4c5c6 +c4s6 ))*(c1[-s 23 (c4s5d 6 -s4d5 )+c23c5d6 -(s2 +c2 )a 3s3 -a 2s2 )+s1 (s4s5d6 +c4 d 5 +d3 )+a1c1 )   J12  = -o x1*p y1 +o y1  *p x1 =-(c1[s23 (c 4c5c6 +s4 c6 )-c23s5s6 ]+s1 (-s4 c5s6 +c4s6 ))*(s1[c23 (c4s5 d6 -s4 d5 )+s23c5 d6 +(c2 -s2 )a 3s3 +a 2s2 )-c1 (s4s5d 6 +c4 d5 +d3 )+a1s1 ) +(c1[-c23 (c4c5c6 +s4s6 )+s 23s5s6 ]-c1 (-s1c5s6 +c4 c6 ))*(c1[-s23 (c4s5 d 6 -s4 d5 )+c23c5 d 6 -(s2 +c2 )a 3s3 -a 2s2 )+s1 (s 4s5d 6 +c4 d5 +d3 )+a1c1 )   J13  = -a x1*p y1  +a y1  *p x1 =-(c1 (-s 23c 4s5 +c 23c5 )+s1s 4s5 )*(s1[c 23 (c4s5d 6 -s 4d5 )+s 23c5d 6 +(c 2 -s 2 )a 3s3 +a 2s 2 )-c1 (s 4s5 d 6 +c 4 d5 +d3 )+a1s1 )   +(s1 (c 23c 4s5 +s 23s5 )-c1s 4s5 )*(c1[-s 23 (c4s5d 6 -s 4d5 )+c23c5d 6 -(s 2 +c 2 )a 3s3 -a 2s 2 )+s1 (s 4s5d 6 +c4d 5 +d 3 )+a1c1 ) 18 J14 = n z1 =c23 (c4 c5c6 -s 4s6 )+s32 c5 c6 J15 = o z1 =-c 23 (c 4 c5s6 +s 4 c6 )+s 23s5s6     J16  = a z1 =c 23c 4 s 5 +s 23c5   Ta có ma trận Jacoby   nx n  y n J  z 0 0   0  nx n  y n  z 0 0   0 ox ax 0 0 oy ay 0 0 oz 0 az 0 0 nx 0 ox 0 0 ny oy 0 0 nz oz ox ax 0 0 oy ay 0 0 oz 0 az 0 0 nx 0 ox 0 0 ny oy 0 0 nz oz 0 0  0 H . J ax  ay  a z  0   J11 0   J 21 0   J 31 . a x   J 41 a y   J 51   a z   J 61 J12 J13 J14 J15 J 22 J 32 J 23 J 33 J 24 J 34 J 25 J 35 J 42 J 43 J 44 J 45 J 52 J 62 J 53 J 63 J 54 J 64 J 55 J 65 J16  J 26  J 36   J 46  J 56   J 66    2. Ma trận Jacoby cho 3 khớp đầu của Robot Ma trận Jacoby dạng điều khiển đối với 3 khớp đầu     J11 J12 J b =  J 21 J 22  J 31 J32                                                J13  J 23  J 33      19 Ta có vị trí khâu tác động cuối khớp 3 là:  p x =cos(q1 )  a1 -a 2cosq 2 -a 3sin(q 2 +q 3 ) +s1d 3   p y =sin(q1 )  a1 -a 2sinq 2 -a 3sin(q 2 +q 3 ) -cosq1d 3   p z =d1 +a 2 cosq 2 +a 3 .cos(q 2 +q 3 )   Nên   p x  J11 = q =-sinq1 a1 -a 2 cosq 2 -a 3sin(q 2 +q 3 )  +d3cosq1 1   p y =cosq1 a1 -a 2 sinq 2 -a 3sin(q 2 +q 3 )  +d 3sinq1 J 21 =  q 1   p z =0 J 31 = q1  p x  J12 = q =a 2 cosq1.sinq 2 -a 3cosq1cos(q 2 +q 3 ) 2   p y =-a 2sinq1 .cosq 2 -a 3sinq1cos(q 2 +q3 ) J 22 = q 2   p J 31 = z =-a 2sinq 2 -a 3sin(q 2 +q 3 ) q 2  p x  J13 = q =-a 3cosq1.cos(q 2 +q3 ) 3   p y =-a 3sinq1cos(q 2 +q3 ) J 23 = q3   p J 33 = z =-a 3sin(q 2 +q 3 ) q3                           20
- Xem thêm -

Tài liệu liên quan