Luận văn Thiết kế, chế tạo và thử nghiệm robot ổn định thế khâu cuối

c tác động. Khi tốc độ quay càng cao thì các đặc tính này càng thể hiện rõ hơn. Hình 2. 8 Con quay hồi chuyển Từ đó có thể thấy nếu trục quay của con quay được nối với vật cần ổn định thế, hướng trục con quay được chọn trước và rotor quay với tốc độ đủ lớn, hướng đã chọn này được bảo toàn (bảo toàn mô men động lượng). Việc nhận biết góc cần ổn định và cấp năng lượng duy trì trạng thái ổn định không tách rời nhau, đều nằm trong hiệu ứng hồi chuyển. Con quay hồi chuyển được ứng dụng trong rất nhiều lĩnh vực như: - Thay cho la bàn từ, nơi mà nhiễu từ làm mất độ chính xác của la bàn từ; 21 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN - Thay cho la bàn từ trong không gian (định hướng kính viễn vọng) vì trong không gian (ngoài khí quyển) không có từ trường; - Ổn định các vật thể bay như máy bay trực thăng; - Làm la bàn trong lòng đất (hầm, lò) nơi từ trường bị nhiễu; - Kết hợp con quay hồi chuyển với cảm biến gia tốc, trong đó con quay hồi chuyển xác định vị trí “home” còn cảm biến gia tốc xác định vị trí thực của thiết bị so với góc “home” này, các thiết bị di động thường ứng dụng kỹ thuật này nhưng không sử dụng con quay cơ học mà thay bằng một thiết bị mô phỏng nguyên lý cơ học với kích thước cực bé (MEMS). Hình 2. 9 Chip L3G4200DH với chức năng nhận biết góc quay của thiết bị di động - Ổn định pháo của xe tăng khi tự hành (Meteor, Meteo-M1/ xe T62 Nga). Tuy nhiên con quay có nhược điểm nó cần có hai đường truyền động riêng biệt để làm hai việc “điều khiển” và “ổn định ở thế đã chọn”: - Chế độ lấy nguồn động từ con quay để giữ góc “home” khi hành quân, trạng thái này không điều khiển, nó chỉ duy trì góc phương vị cho nòng pháo ở thế đã chọn; - Chế độ truyền động “điều khiển” để xoay nòng pháo sang góc mới, bám mục tiêu. Do vật cần ổn định hướng phải nối trực tiếp với trục con quay, do vậy góc di chuyển của nó bị giới hạn bởi kết cấu cơ khí, nhất là khi kết cấu này đòi hỏi 22 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN rất cứng vững như nòng pháo. Con quay hồi chuyển đáp ứng ổn định rất nhanh nhưng để duy trì tốc độ quay cho nó cũng tốn một năng lượng dẫn động thường xuyên không nhỏ. Con quay chỉ ổn định hướng chứ không có khả năng ổn định vị trí, yêu cầu ổn định cả vị trí và hướng đồng thời sẽ cần thiết kế một kết cấu mới như trong luận văn này. Ngoài phương án sử dụng con quay hồi chuyển, để ổn định hướng cho vật thể có thể dùng các cơ cấu cơ khí đặc biệt khác. Hình 2. 10 Cơ cấu ổn định hướng cơ khí Cơ cấu cơ khí đơn giản này có chức năng ổn định hướng rất tốt trong quá trình giá của nó chuyển động quay. https://www.youtube.com/watch?v=jMCBm9bG4EY) Tuy nhiên số răng bánh răng đầu và cuối phải giống nhau, nó chỉ thích hợp khi cơ cấu có ít bậc tự do và có không gian phù hợp để bố trí các bánh răng hành tinh. Các kết cấu đòi hỏi góc di động lớn, gọn nhẹ, ít tiêu hao năng lượng hơn sẽ chuyển sang sử dụng các đồ gá ổn định hướng khác, không dựa trên hiệu ứng con quay này. Việc nhận biết góc nghiêng cần ổn định thuộc về một hệ thống cảm biến góc trong khi việc ổn định góc này cần bổ sung năng lượng dẫn động vào hệ. Dẫn động điện không liên tục với độ trễ nhỏ sẽ đáp ứng được vấn đề mở rộng góc công tác và tiết kiệm năng lượng cho hệ công tác. 2.2 Phương trình động học robot Đồ gá ổn định thế cần một số bậc tự do nhất định để tạo ra các chuyển động bù, do vậy nó có cấu hình robot. Việc sử dụng kỹ thuật robot vào thiết kế đồ gá này là việc hoàn toàn tự nhiên. Dưới đây nhắc lại một số quy tắc căn bản 23 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN dùng để mô tả động học hệ thống robot, các kiến thức này được dùng ở bước mô hình hóa cơ cấu đồ gá ổn định hướng sau đó. a. Phép quay Roll-Pitch-Yaw: Xét một vật thể nổi có 6 bậc tự do, chẳng hạn như một con thuyền hình 2.11 Hình 2. 11 Mô tả phép quay roll-pitch-yaw Hình 2. 12 Quan hệ giữa phép quay RPY với các chuyển động của bàn tay khi phương trục khớp khác nhau Đầu tiên cần hiểu khái niệm trục ban đầu và trục tức thời, xét hệ quy chiếu O1 qua một loạt các biến hình trở thành O2 và O3 khi đó các trục của hệ quy chiếu O1 gọi là hệ quy chiếu ban đầu, các trục hình thành về sau gọi là hệ quy chiếu tức thời. Phép quay roll-pitch-yaw xác định luật biến hình định nghĩa bởi: 24 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN                                                        cccss cssscccssssc sscscsccsscc cs sc cs sc cs sc .. ....... ....... 0 0 001 . 0 010 0 . 100 0 0 (2.1) Nhận thấy điểm đặc trưng của quy tắc này là thực hiện quay một hệ quy chiếu đi ba lần liên tiếp xung quanh các trục của một hệ quy chiếu ban đầu theo thứ tự z,y,x. RRPY = rot(z, ).rot(y,  ).rot(x, ) (2.2) b. Phép quay Euler: Bộ góc Euler khác với bộ góc RPY ở chỗ nó sử dụng cả các trục ban đầu và trục tức thời làm trục quay, theo tính toán bằng lý thuyết đại số tổ hợp, chú ý rằng không chọn trùng lại một quay hai lần quay liên tiếp, có tất cả 3.2.2 = 12 bộ góc Euler khác nhau, chẳng hạn: ),().,().,( 321  zrotyrotxrotREUL  (2.3) c. Quy tắc DH: Do robot liên kết thành chuỗi, việc mô tả vị trí và hướng của khâu cuối trong hệ quy chiếu gắn với khâu đầu tạo thành đặc trưng động học để thiết lập các tính toán định lượng phục vụ điều khiển. Lập phương trình này cần vận dụng một trong các quy tắc như [11]: - Quy tắc DH; - Quy tắc Craig; - Quy tắc chuyển vị xoắn; Mục đích: quy tắc DH có mục đích chỉ dẫn phương pháp xác lập hệ quy chiếu suy rộng gắn với từng khâu của robot hướng tới tính duy nhất. Nội dung: a. Phần định tính: - Sử dụng hệ quy chiếu trực tiếp tuân theo quy tắc bàn tay phải. Trục oz: 25 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN - Có phương trùng với phương trục khớp; - Có chiều dương tùy chọn. Trục ox: - Nằm theo phương đường vuông góc chung giữa hai trục z liên tiếp; - Chiều dương hướng từ giao điểm của đường vuông góc chung với trục z có STT nhỏ sang giao điểm với trục z có STT lớn. b. Phần định lượng: Để xây dựng ma trận truyền giữa hai khâu bất kỳ của robot xét sơ đồ sau: Hình 2. 13 Sơ đồ quan hệ giữa hai hệ quy chiếu Oi-1 và Oi theo DH Sau khi xác lập được các hệ quy chiếu suy rộng găn với mỗi khâu như chỉ dẫn của phần định tính, nhiệm vụ của phần định lượng là giả sử rằng cần tìm ra quan hệ chuyển trục giữa hai hệ quy chiếu Oi-1 và Oi thỏa mãn định nghĩa: ii i i OOA   1 1 . (2.4) Ma trận A i i 1 được gọi là ma trận truyền giữa hai hệ quy chiếu Oi-1 và Oi. Hãy theo dõi sự di động của hệ quy chiếu Oi-1 trên cơ sở biến hình theo các trục cơ bản trong bảng sau Bảng 2. 2 Các bước biến đổi DH Bước Rot(z, ) Tran(z,d) Tran(x,a) Rot(x, ) Trôc cña khíp i-1 trôc cña khíp i Trôc cña khíp i+1 Kh©u i-1 Kh©u i Zi-1 Zi' Xi-1 Oi-1 Xi' Oi' Oi Xi Zi C¸c th«ng sè ®éng häc cña Denavit-Hartenberg 26 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN Đặc trưng              1000 0100 00 00   cs sc             1000 100 0010 0001 d             1000 0100 0010 001 a              1000 00 00 0001   cs sc Mục đích ii xsongsongx 1 '1 ii OO  'ii OO  ii ZZ 1 Ma trận A i i 1 nhân theo thứ tự bốn ma trận đặc trưng trong bảng nói trên:                 1000 )cos()sin(0 )sin(*)sin(*)cos(cos*)cos()sin( )cos(*)sin(*)sin()cos(*)sin()cos( 1 d a a A i i    (2.5) 2.3 Tính tương đối của một chuyển động/ phép đổi giá Xét một tay robot tổng quát gồm n bậc tự do như hình 2.14. Hình 2. 14 Tay robot với vai trò đồ gá có chức năng ổn định thế của vật gá Đầu gắn với hệ quy chiếu O0 là giá của tay robot, đầu còn lại gắn bàn kẹp mang vật thể gắn với hệ quy chiếu On. Hệ quy chiếu On gắn với vật có một yêu cầu đặc biệt là dù giá của robot chuyển động như thế nào thì O6 cũng phải giữa nguyên vị trí (hoặc hướng, hoặc cả vị trí và hướng) không thay đổi. Tay robot làm được việc này nhờ có khả năng thay đổi các giá trị biến khớp của nó một cách chủ động để bù trừ các chuyển động mà giá tác động vào khâu 1 sinh ra. Giả sử quỹ đạo của giá O0 cho trước cần bám theo phương trình mô tả trong O0 bởi (2.6): f(x,y,z) = 0 (2.6) 27 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN Trong khi vị trí và hướng của O6 mô tả trong O0 được biểu diễn bởi ma trận (2.7): 𝐴6 0 = | 𝑛 𝑠 𝑎 𝑝 0 0 0 1 | (2.7) Thông thường khi đồ gá ở trạng thái (q1, .., q6) = 0 nó sẽ sao chép các chuyển động của O0 sang O6. Để ổn định thế của vật trong bàn tay, lúc này cần tính toán các chuyển động (q1, .., q6) sao cho vật đứng yên, tức là: 𝐴0 6. 6O = 𝑓(𝑥, 𝑦, 𝑧) (2.8) Trường hợp đặc biệt nếu chỉ yêu cầu ổn định hướng hoặc vị trí mà không yêu cầu ổn định đồng thời hai thông số trên thì sẽ ứng với các ma trận (2) khác nhau. Trường hợp chỉ ổn định vị trí: 𝐴0 6 = | 0 0 0 𝑝 0 0 0 1 | (2.9) Trường hợp chỉ ổn định hướng: 𝐴0 6 = | 𝑛 𝑠 𝑎 0 0 0 0 1 | (2.10) Để đạt được mục đích ổn định động học vật, việc giải phương trình (2.8) để điều khiển cơ cấu của giá đỡ là cần thiết. 2.4 Mô hình toán của đồ gá tổng quát cấu hình robot Đối với một robot tổng quát nếu cho trước quỹ đạo chuyển động của giá (2.6) luôn xây dựng được phương trình (2.8), biến đổi tương đương phương trình này sang dạng: 𝑓(𝑥, 𝑦, 𝑧) = (𝐴6 0)−1 = 𝐴0 6 (2.11) Bài toán được hiểu là giữ bàn tay cố định cả vị trí và hướng, điều khiển giá di chuyển theo quỹ đạo cho trước, đó là phép đổi giá. Các tọa độ suy rộng tìm được từ lời giải của bài toán này về bản chất trùng với lời giải của bài toán (2.8). Nó đảm bảo rằng khi khâu đầu di động với quy luật cho trước, các khớp sẽ tự khử các tác động không mong muốn để giữ vật trong bàn tay giữ nguyên một thế đã chọn. 28 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN Sau khi có quỹ đạo thành phần qi(t) việc kiểm tra đáp ứng khử các chuyển động làm di chuyển vật khỏi thế đã xác định của đồ gá được tiến hành trên phương trình động học thuận của tay máy. Cụ thể là, nếu gọi Pi(q1, , q6) là một điểm thuộc không gian khớp đã được ánh xạ từ Pi(xi, yi, zi) tương ứng thuộc f(x,y,z) = 0 của không gian công tác qua bài toán (2.8). Việc kiểm tra tương ứng với chạy truy hồi phương trình sau: 𝑃𝑖(𝑞1, . . , 𝑞6). 𝐴0 6 = 6O 𝑣ớ𝑖 𝑖 = 1 ÷ 𝑛 (2.12) Số lượng điểm kiểm tra cần đủ dày để khẳng định lời giải là đúng. Trong trường hợp cấu trúc robot là song song, ứng dụng cho các vật có khối lượng lớn, kỹ thuật tính toán sẽ khác với quá trình tính toán cho robot song song ở chỗ robot song song chỉ có hai hệ quy chiếu là hệ quy chiếu gắn với giá và hệ quy chiếu gắn với khâu cuối, nó không có các hệ quy chiếu trung gian. Kỹ thuật đổi giá được áp dụng bình thường khi mô tả robot này tuy nhiên thay vì tìm nghịch đảo của phương trình động học, phương án xây dựng phương trình theo giá mới có vẻ thuận tiện và dễ thực hiện hơn, gốc gác của vấn đề này là do robot song song có nhiều vòng kín thay vì có một vòng kín như robot chuỗi. 2.5 Phương pháp và công cụ giải bài toán động học robot 2.5.1 Chuyển đổi bài toán động học thành bài toán tối ưu Với mô hình tổng quát (2.11), cần có một phương pháp và một công cụ cho bài toán này. Theo [3], phương pháp GRG đã được vận dụng cho bài toán kiểu này hiệu quả. Về bản chất, bài toán động học ngược robot cần chuyển đổi dạng thức để ứng dụng được phương pháp GRG. Theo phép chuyển đổi thuần nhất thế của khâu chấp hành là hàm của các biến khớp, mô tả bằng ma trận tổng hợp của phép chuyển đổi: 29 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN (2.13) Trong đó: với i = 1n, là ma trận chuyển đổi giữa hệ toạ độ thứ i đến hệ i-1, xác định theo quy tắc Denavit-Hartenberg; n là số biến khớp (bậc tự do) của robot.Vị trí và hướng của khâu chấp hành được xác định từ quỹ đạo cho trước: (2.14) Trong đó: ; q1 qn các biến khớp; n, s, a là các vec tơ chỉ phương; p là véc tơ chỉ vị trí; oxyz là hệ toạ độ gốc.Ma trận chuyển đổi tổng hợp có dạng: (2.15) Các thành phần aij với i,j =13 là các cosin chỉ phương của n,s,a; a14, a24, a34 lần lượt là các thành phần chiếu lên hệ oxyz của p. Do tính chất trực giao của các vec tơ chỉ phương, cho nên chỉ có ba thành phần trong các cosin chỉ phương độc lập. Vì vậy kết hợp (3.2) và (3.3) nhận được:  1 6 12 1 6 13 1 6 23 1 6 14 1 6 24 1 6 34 ( ,.., ) ( ,.., ) ( ,.., ) ( ,.., ) ( ,.. ) ( ,.. ) x x y x y z s q q a a q q a a q q a p q q a p q q a p q q a            (2.16)     n i i in AA 1 10 A i i 1 An zzzz yyyy xxxx n pasn pasn pasn T 00 1000  ),...,,( 21 0 nn qqqfT  1000 34333231 24232221 14131211 0 aaaa aaaa aaaa An                  34 24 14 23 13 12 ap ap ap aa aa as z y x y x x 30 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN Giải hệ phương trình này nhận được giá trị các biến khớp. Khi giải có thể gặp các trường hợp sau: - Hệ phương trình (2.16) có thể phi tuyến hoặc phải xác định biến từ hàm siêu việt vì vậy kết quả không chính xác hoặc có nhiều lời giải. - Hệ (2.16) có thể vô định vì số bậc tự do thừa. - Các kết quả có thể không thoả mãn được các điều kiện ràng buộc về mặt kết cấu. Mục tiêu của điều khiển động học là đạt được độ chính xác về vị trí và hướng của khâu chấp hành. Như vậy chỉ cần xác định các giá trị của các biến khớp sao cho đảm bảo sai số vị trí và hướng là nhỏ nhất đồng thời thoả mãn các điều kiện ràng buộc về mặt kết cấu. - Gọi q = {q1, q2, ..., qn } : là véc tơ các biến khớp. - Không gian khớp D xác định miền giá trị của các biến khớp: (2.17) L =f(q): Hàm mô tả sai lệch vị trí và hướng của khâu chấp hành. Bài toán xác định giá trị các biến khớp được viết: (2.18) Trong đó: Đây là bài toán tối ưu, nghiệm của (2.17) phải là nghiệm của (2.18) vì vậy hàm mục tiêu được xác định theo (2.18) như sau, trước hết viết lại hệ phương trình (2.18) dưới dạng tương đương:           nnn bqa bqa bqa  222 111 min),...,( 21  nqqqfL ni Dqi   1 ; 31 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN (2.19) Bình phương hai vế của hệ phương trình này và cộng theo vế để có: Rõ ràng vế trái không âm nên giá trị nhỏ nhất của vế trái bằng không, tương đương với hệ phương trình (2.16) được thỏa mãn. Đặt L là hàm số ở vế trái: Trong điều khiển chỉ đòi hỏi độ chính xác hướng của khâu chấp hành bài toán có dạng: (2.20) Ràng buộc: Trong đó: - Hàm mô tả sai lệch hướng. (2.21) - Hàm mô tả sai lệch vị trí . (2.22) - U, V: Các sai lệch giới hạn xác định theo yêu cầu kỹ thuật. - Tương tự nếu đòi hỏi độ chính xác vị trí của khâu chấp hành bài toán có dạng: (2.23)                 0 0 0 0 0 0 34 24 14 23 13 12 ap ap ap aa aa as z y x y x x 0)()()()()()( 234 2 24 2 14 2 23 2 13 2 12  apapapaaaaas zyxyxx 2 34 2 24 2 14 2 23 2 13 2 12 )()()()()()( apapapaaaaasL zyxyxx  min),...,( 211  nqqqfL ni Dq ULV i    1 ; 2 2 23 2 13 2 121 )()()( aaaaasL yxx  2 34 2 24 2 142 )()()( apapapL zyx  min),...,( 212  nqqqfL 32 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN Trong đó Về bản chất các bài toán (2.21),(2.22),(2.23) là bài toán tối ưu hóa trên miền kín vì trên thực tế các khớp tịnh tiến hoặc quay của robot thường có không gian hoạt động bị giới hạn trong một phạm vi nhất định. Dấu của biến khớp thể hiện hướng di chuyển của chuyển động, trong khi các biến đều chuyển động khứ hồi nên các ràng buộc thường có dạng chung cho khớp tịnh tiến và quay: (2.24) Tập hợp ràng buộc của n biến khớp là một miền kín. Do vế phải của hàm mục tiêu luôn dương nên giá trị nhỏ nhất của mục tiêu là bằng không, hoặc có giá trị vô cùng bé đủ đáp ứng các yêu cầu kỹ thuật cụ thể. Phương án làm cho giá trị hàm mục tiêu bằng không là phương án nghiệm vật lí, ngược lại nếu giá trị mục tiêu L > 0, không tồn tại phương án nghiệm vật lí. 2.5.2 Phương pháp GRG giải bài toán tối ưu Nội dung đầy đủ của thuật toán có thể tham khảo tại [3], dưới đây là nội dung cơ bản của thuật toán. khởi tạo Chọn một điểm bắt đầu 𝑥0 ≥ 0 sao cho Ax = b. Để k = 0 (2.25) Bước chính - Hình thành B từ những cột của A tương ứng với các thành phần lớn nhất m của 𝑥𝑘. Xác định N là các cột còn lại của A, xác định 𝑥𝐵 là các phần tử của 𝑥 𝑘 tương ứng với B, và xác định 𝑥𝑁 tương tự. - Tính gradient giảm 𝑟 ≔ (−∇𝐵𝑓(𝑥)𝑇(𝐵)−1𝑁 + ∇𝑁𝑓(𝑥)𝑇)𝑇 (2.26) - Tính 𝑠𝑁 (𝑠𝑁)𝑖 = { −(𝑥𝑁)𝑖𝑟𝑖 nếu 𝑟𝑖 > 0 −𝑟𝑖 nếu 𝑟𝑖 ≤ 0 (2.27) ni Dq ULV i    1 ; 1 )()( iii upperboundqlowerbound  ),...,,( 21 nqqq 33 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN - Tính 𝑠𝐵 𝑠𝐵 = −(𝐵)−1𝑁𝑠𝑁. (2.28) - Hình thành 𝑠𝑘 từ 𝑠𝑁 và 𝑠𝐵. - Nếu 𝑠𝑘 = 0, DỪNG LẠI (𝑥𝑘 là một điểm KKT) Line search - Tính 𝜆max 𝜆max = { min𝑗:(𝑠𝑘) 𝑗 <0 −(𝑥𝑘)𝑗 (𝑠𝑘)𝑗 nếu𝑠𝑘 ≱ 0 ∞nếu𝑠𝑘 ≥ 0 (2.29) - Thực hiện thuật toánline search 𝜆𝑘 = arg min 0≤𝜆≤𝜆𝑚𝑎𝑥 𝑓(𝑥𝑘 + 𝜆𝑠𝑘). (2.30) - Đặt 𝑥𝑘+1 = 𝑥𝑘 + 𝜆𝑘𝑠 𝑘 và thay k bằng k + 1. (2.31) - Lặp lại bước chính. 2.5.3 Xác định vùng đáp ứng ổn định thế Vì tất cả các khớp loại 5 đều sẽ có giới hạn khi xét đến lý do kết cấu cơ khí, giới hạn này được mô tả bởi (2.24), vì vậy bài toán (2.18) sẽ chỉ có nghiệm trên một vùng nào đó hoàn toàn xác định về hình dáng và thể tích. Khi giá chuyển động ra ngoài vùng này các yếu tố ổn định không duy trì được nữa. Để xác định vùng này sử dụng các kỹ thuật giới thiệu ở [12]. Kết luận chương 2 Theo cấu trúc luận văn, chương 2 trình bày các vấn đề cơ sở để phân loại bài toán, theo đó bài toán ổn định thế chia ra loại một và hai yếu tố động, đây là căn cứ triển khai chương tiếp theo. Ở chương này tác giả cũng trình bày kỹ thuật đổi giá khi mô hình hóa bài toán động học trong đó giá chuyển động, bàn tay đứng yên. Trên cơ sở quy tắc DH, mô hình toán nhận được đề xuất chuyển đổi sang dạng tối ưu. Bài toán tối ưu sau đó được đề xuất giải bằng phương pháp GRG, theo nhận định [3], đây là kỹ thuật phù hợp để giải bài toán này hiệu quả. 34 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN Dữ liệu điều khiển robot có thể đạt được thông qua tính toán trên mô hình, cũng có thể đạt được thông qua sử dụng các cảm biến. Cơ sở đo lường các dữ liệu này bằng cảm biến gia tốc cũng được trình bày trong chương. Trường hợp cao hơn bài toán ổn định thế đó là khi hệ quy chiếu gắn với giá có di động cùng lúc với hệ quy chiếu của khâu cuối cũng di động để bám đối tượng đã khóa, bài toán điều khiển động học trở thành tổng quát và cần cập nhật lại thế của mục tiêu. Trường hợp này giải theo [3] bình thường, thiết bị chỉ gọi là gá ổn định thế khi mà thế của mục tiêu không thay đổi, chỉ có giá di động. Tóm lại chương này đã chỉ ra có ba vấn đề cần quan tâm là mô hình động học cơ cấu robot khi đổi giá như thế nào, các quỹ đạo khi đổi giá sẽ thay đổi theo quan hệ nào. Hai là phương pháp nào giải được kiểu bài toán động học sau khi đổi giá với cả robot chuỗi và robot song song. Ba là công cụ nào chạy trên thuật toán của phương pháp đã chọn có thể ứng dụng thuận tiện nhất cho mục đích này. 35 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN CHƯƠNG 3: THIẾT KẾ, CHẾ TẠO VÀ KIỂM NGHIỆM GÁ ỔN ĐỊNH THẾ 3.1 Xây dựng dữ liệu điều khiển thông qua tính toán trên mô hình 3.1.1 Cấu trúc chuỗi Hình 3. 1 Đồ gá cấu hình robot 3 bậc tự do Trong ví dụ này lấy: d1 = 200(mm); d2 = 250(mm) , a3 = 165(mm). Các ma trận truyền được xác định được theo quy tắc DH là: 0 1 0 0 0 0 1 0 200 0 0 1 0 0 0 0 1 O OA  1 1 1 10 1 0 0 0 0 0 1 0 0 0 0 0 1 c s s c A   (3.1) 2 2 2 21 2 0 0 0 0 0 1 0 250 0 0 0 1 c s s c A    3 3 3 3 3 32 3 0 165. 0 165. 0 0 1 0 0 0 0 1 c s c s c s A    Thế của O3 trong O mô tả bởi tọa độ lý thuyết là: 1 2 3 1 3 3 1 1 2 3 1 2 1 1 3 1 2 3 1 3 2 3 1 2 1 3 1 3 1 2 1 3 1 2 3 1 3 3 2 2 3 2 3 2 250 165 165 165 250 165 200 165 0 0 0 1 O c c c s s c s c c s c s s s s c c c c s c c s c s s c c s s c s c c c s A c s s s c c s            (3.2) Tính toán được thế của O so với chuẩn quy chiếu O3 dưới dạng lý thuyết là: 36 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN 1 3 1 2 3 1 3 3 1 2 3 2 1 3 3 2 3 1 3 1 1 3 2 1 3 2 1 3 2 3 1 3 3 2 1 31 3 3 1 2 1 2 2 1 2 5(40 33 50 40c c s ) 50( 4c c 5c 4c s s ) ( ) 200 0 0 0 1 O O s s c c c c s c s c c s c s s c s c s c c c c s s s s A A c s s s c s s                    (3.3) Giả sử sẽ di chuyển gốc O theo quỹ đạo là một đường tròn trong mặt phẳng xOz có tâm ở gốc O hiện thời và có bán kính là r = 45(mm). Chuyển phương trình biểu diễn đường tròn f(x,y) trong hệ quy chiếu O0 về biểu diễn trong hệ quy chiếu O6 theo quan hệ sau: 0 60 1 6(A ) . (x,y,z) (x,y,z) O O f f  (3.4) Giả sử tâm bàn tay O3 được định vị trong hệ quy chiếu O ở tọa độ thực ứng với bộ tọa độ suy rộng (100,150,300): 3 0.7370 0.6260 0.2549 165.0143 0.6377 0.7690 0.0449 59.0125 0.2241 0.1294 0.9659 36.9837 0 0 0 1 OA    (3.5) Từ đây tính toán để chuyển đổi các điểm keypoint thuộc đường tròn x2+z2 = 452 nhận O làm tâm về biểu diễn trong hệ quy chiếu O3 như bảng 1. Việc chuyển đổi này được thực hiện theo mô hình ở chương 2 nói trên cho 8 điểm keypoint đã chọn. Lúc này ma trận chuyển đổi ngược có dạng tọa độ thực là: 3 0.7370 0.6377 0.2241 167.5327 0.6260 0.7690 0.1294 62.7058 0.2549 0.0449 0.9659 8.988 0 1 7 0 0 OA       (3.6) Chú ý rằng các ma trận (3.5) và (3.6) là nghịch đảo của nhau. Bảng 3. 1 Điểm keypoint quy đổi khi biểu diễn trong hệ quy chiếu O và O3 Điểm Trong O Trong O3 p1 (45, 120, 0) (-57.8488 -126.8157 7.8744) p2 (31.8198, 120, 31.8198) (-60.4302 -130.9488 -26.2206) p3 (0, 120, 45) (-80.9266 -149.1627 -47.0621) 37 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN p4 (-31.8198, 120, 31.8198) (-107.3315 -170.7878 -42.4415) p5 (-45, 120, 0) (-124.1773 -183.1565 -15.0654) p6 (-31.8198, 120, -31.8198) (-121.5960 -179.0234 19.0296) p7 (0, 120, -45) (-101.0995 -160.8095 39.8712) p8 (31.8198, 120, -31.8198) (-74.6946 -139.1844 35.2505) Xét các phương trình chuyển động thành lập được dưới dạng cùng biểu diễn trong hệ quy chiếu O6 như (3.7): 6 0 1 6(x,y,z) (A ) O f  (3.7) Lời giải động học ngược cho các điểm key point tương ứng cho thấy trong bảng 2. Do cơ cấu chỉ có ba bậc tự do nên không thể đồng thời giữ cả vị trí và định hướng, trong bảng 2 trình bày phương án giữ vị trí bàn kẹp không đổi khi di chuyển khâu đầu theo quỹ đạo đường tròn ở cao độ y = 120 (mm). Bảng 3. 2 Lời giải động học ngược tại các điểm keypoint khi giữ vị trí so với mô tả ở ma trận thế 13, đồng thời di chuyển gốc O theo đường x2+z2=452, y = 120. Điểm (q1, q2, q3) rad p1 0.581116 -0.07178 0.529794 p2 0.605745 0.232343 0.551299 p3 0.68843 0.379416 0.701584 p4 0.774694 0.308229 0.941895 p5 0.851403 0.10031 1.185189 p6 0.811723 -0.13153 1.103713 p7 0.722838 -0.30612 0.853354 p8 0.635887 -0.30131 0.639653 Các đặc tính của từng bậc tự do sau khi nội suy trên mô hình đường bậc ba được biểu diễn như sau, giả sử để di chuyển giá hết đường tròn cần đến 16 giây: Biến q1 u_12= 0.000549875*t.^3+0.0050575*t.^2+0.581116 38 Số hóa bởi Trung tâm Học liệu và Công nghệ thông tin – ĐHTN u_23= -0.003404813*t.^3+0.0344955*t.^2-0.07029575*t+0.635593 u_34= -0.000820875*t.^3+0.011939625*t.^2-0.01387