Luận văn Giải bài toán ngược động học, động lực học và điều khiển trượt robot dư dẫn động dựa trên thuật toán hiệu chỉnh gia lượng vector tọa độ suy rộng

mong muốn là vào khoảng 1 giây và mặc dù có các sai số về cấu trúc và có các nhiễu trên các khớp nhưng rôbốt vẫn bám theo quỹ đạo mong muốn. 102 3.5 Kết luận chương 3 Dựa trên thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng đã trình bày ở chương 1, trong chương này trình bày việc vận dụng phương pháp điều khiển trượt vào bài toán điều khiển chuyển động rôbốt dư dẫn động trong không gian khớp. Chúng tôi chọn phương án điều khiển rôbốt dư dẫn động trong không gian khớp vì phương án này có thời gian điều khiển nhanh hơn so với phương án điều khiển trong không gian thao tác do không phải giải bài toán động học ngược trong quá trình điều khiển, do đó phù hợp hơn với điều khiển thời gian thực. Kết quả mới của chương này là trong phần giải bài toán động học ngược đã sử dụng thuật toán hiệu chỉnh véc tơ tọa độ suy rộng. Kết quả giải bài toán động học ngược là đầu vào của bài toán điều khiển chuyển động trong không gian khớp. Ngoài ra các ví dụ trình bày trong chương này là các ví dụ mới. Chương 4 BÀI TOÁN NGƯỢC ĐỘNG HỌC, ĐỘNG LỰC HỌC VÀ ĐIỀU KHIỂN TRƯỢT RÔBỐT ĐO BKHN-MCX-04 Để phục vụ cho luận án một rôbốt đo đã được thiết kế chế tạo tại Bộ môn Cơ khí chính xác và Quang học Trường Đại học Bách khoa Hà nội, chủ trì thiết kế, chế tạo là TS. Nguyễn Văn Vinh. Rôbốt đo này được quy định gọi là rôbốt đo BKHN-MCX-04. Chúng tôi có tham gia một phần thiết kế chế tạo và tham gia thực hiện các thí nghiệm tại Trường Đại học Bách khoa Hà nội. Trên cơ sở rôbốt chế tạo, trong chương này áp dụng các thuật toán đã trình bày trong các chương 1, 2 và 3 để tính toán phân tích động học ngược, động lực học ngược và điều khiển chuyển động rôbốt đo BKHN-MCX-04. 4.1 Tổng quan về phương pháp đo độ chính xác hình học của chi tiết máy Trong lĩnh vực đo lường, kiểm tra thì đo lường và kiểm tra tự động đang rất phát triển. Đưa những thành tựu của các ngành điện tử và tin học áp dụng vào việc ghép nối giữa máy tính và máy công tác đã cho phép chuyển thẳng số liệu đo vào máy tính để xử lý và đưa ra kết quả của phép đo một cách nhanh chóng. Các thiết bị đo tọa độ như: Máy đo tọa độ Đề các, tọa độ độc cực…đã hoàn thiện các khả năng đo lường vốn có của chúng, đặc biệt trong các phép đo gián tiếp có khối lượng xử lý thông tin lớn. Với khả năng xác định kích thước, hình dáng, vị trí một cách dễ dàng, với độ chính xác cao trong một khoảng thời gian ngắn và khả năng tiếp cận đối tượng phong phú, với những phần mềm xử lý thích hợp, có thể đo những bề mặt phức tạp. Máy đo tọa độ đã góp phần quan trọng trong việc đảm bảo nâng cao chất lượng sản phẩm và đem lại hiệu quả cao. Tuy nhiên, trong các trường hợp sau đây thì máy đo tọa độ Đề các không thể đáp ứng được: - Những vật đo có kích thước lớn, nặng, không thể di chuyển đến bàn đo của máy đo tọa độ Đề các. - Khi cần đo những chi tiết đã lắp ráp vào máy ở các công trình mà không thể nào tháo ra được. - Đo ở các vị trí có sự hạn chế về không gian như đo bên trong khoang máy, đo trong những chi tiết dạng ống v.v… Các rôbốt đo đã khắc phục được những hạn chế nêu trên. Với kết cấu gọn nhẹ, tính cơ động cao, dễ mang vác, tầm với rộng. Thay vào việc phải di chuyển chi tiết trên bàn đo của máy đo tọa độ Đề các, rôbốt đo có thể tiếp cận đối tượng đo trong mọi điều kiện của không gian đo. Rôbốt đo theo tọa độ cực có thể đo được các chi tiết có hình dạng cơ bản như: hình tròn, hình trụ, mặt phẳng, đường thẳng v.v…trong phạm vi kích thước 104 200x200x200mm của các chi tiết. Khi thực hiện phép đo, trước hết cần đặt đầu đo của rôbốt vào vị trí ban đầu rồi cho chạy chương trình, các khớp sẽ được điều khiển và di chuyển đầu đo đến vị trí cần đo và đồng thời có tín hiệu phản hồi khi đầu đo chạm vào bề mặt chi tiết cần đo. 4.2 Kết cấu của rô bốt đo BKHN-MCX-04 Kết cấu của rôbốt đo BKHN-MCX-04 được minh họa bởi mô hình trên hình 4.1. Rôbốt được thiết kế với sáu khâu động và các khớp quay, tạo thành một hệ có cấu trúc động học mạch hở được biểu diễn trên hình 4.2. Rôbốt có sáu động cơ servo dẫn động độc lập. Động cơ thứ nhất dẫn động khâu 1 quay xung quanh trục cố định theo phương thẳng đứng. Trục quay của các động cơ dẫn động các khâu 2, 3 và 4 được bố trí song song với nhau và cùng vuông góc với trục quay động cơ dẫn động khâu 5. Các thông số thiết kế (độ dài) của các khâu của rôbốt được cho trong bảng 4.1. Hình 4.1. Mô hình rôbốt đo BKHN-MCX-04 Trong khi bốn động cơ đầu tiên có nhiệm vụ đưa điểm O5 (tâm trục của khớp quay giữa khâu 5 và khâu 6) dịch chuyển theo một quĩ đạo không gian được định trước theo yêu cầu về phép đo, động cơ thứ 5 làm thay đổi hướng của Quỹ đạo định đối tượng 1 3 5 4 6 2 Quỹ đạo định trước đối tượng cần đo 105 khâu 5 cho phù hợp với hướng của bề mặt vật cần đo. Động cơ thứ 6 có nhiệm vụ dẫn động khâu 6 (que thăm dò) để mũi dò (có vị trí tại điểm O6) tiếp xúc với bề mặt vật cần đo. Với phương án thiết kết như vậy, rôbốt đo BKHN-MCX-04 có khả năng tạo ra các chuyển động của mũi dò một cách linh hoạt, phù hợp với các phép đo các bề mặt theo các phương khác nhau. Hình 4.2. Sơ đồ động học rôbốt đo BKHN-MCX-04 Bảng 4.1. Thông số hình học của rôbốt Khâu i Khoảng cách Oi-1O i (m) 1 0.14 2 0.15 3 0.20 4 0.0 5 0.163 6 0.080 x0 y0 z0 x1 x2 x3 x4 x5 x6 O1 z4 z5 O2 E≡O5 O6 q1 q2 q3 q4 q5 q6 O y1 O3≡O4 106 4.3 Tính toán động học ngược Hình 4.2 mô tả các hệ tọa độ động gắn với các khâu dựa trên quy tắc chọn hệ trục tọa độ của Denavit-Hartenberg. Các điểm gốc Oi (i = 1,2..., 5) của các hệ trục tọa độ đều nằm trong cùng một mặt phẳng thẳng đứng. Nếu xét tổng thể toàn bộ hệ với 6 dẫn động, vị trí và hướng của khâu 6 (khâu thao tác) được xác định trên hệ tọa độ cố định theo 5 tham số: ba tọa độ Đề-các của điểm gốc O5 và hai góc quay xác định hướng (thí dụ các góc q5, q6). Như vậy số dư dẫn động của rôbốt là 1. Do đặc điểm kết cấu của rôbốt được phân tích ở trên, việc điều khiển các biến khớp qi (i = 1,2..., 5) để điểm O5 chuyển động trên một quĩ đạo định trước (để thuận tiện ta ký hiệu O5≡E) có vai trò quan trọng đối với các thao tác đo. Chú ý rằng sự thay đổi trị số của góc quay q5 sẽ không có ảnh hưởng đến vị trí của điểm E nhưng sẽ làm thay đổi hướng của khâu 5 và tạo ra sự thay đổi hướng của khâu cuối 6. Bởi vậy, nhiệm vụ phân tích động học ngược của rôbốt được trình bày trong mục này tập trung vào việc tính toán các trị số của các biến khớp qi và các đạo hàm )5,,2,1i(q,q ii K&&& = dựa trên một quĩ đạo định trước của điểm E. Bảng 4.2. Bảng tham số động học DH Khâu i θi di ai αi 1 q1 d1 0 π/2 2 q2 0 a2 0 3 q3 0 a3 0 4 q4 0 0 -π/2 5 q5 d5 0 π/2 Từ hình 4.2 ta có thể thiết lập được bảng các tham số động học DH như trong bảng 4.2, trong đó các hằng số 54532321211 OOd,OOa,OOa,OOd ==== Các giá trị li (i=1,2,...,5) được cho trong bảng 4.1. Từ bảng tham số DH ta được các ma trận biến đổi hệ trục ⎥⎥ ⎥⎥ ⎦ ⎤ ⎢⎢ ⎢⎢ ⎣ ⎡ − = ⎥⎥ ⎥⎥ ⎦ ⎤ ⎢⎢ ⎢⎢ ⎣ ⎡ − = ⎥⎥ ⎥⎥ ⎦ ⎤ ⎢⎢ ⎢⎢ ⎣ ⎡ −= 1000 0100 Sa0CS Ca0SC , 1000 0100 Sa0CS Ca0SC , 1000 d010 0C0S 0S0C 3333 3333 3 2222 2222 2 1 11 11 1 HHH 107 ⎥⎥ ⎥⎥ ⎦ ⎤ ⎢⎢ ⎢⎢ ⎣ ⎡ − −= ⎥⎥ ⎥⎥ ⎦ ⎤ ⎢⎢ ⎢⎢ ⎣ ⎡ − − = 1000 d010 0C0S 0S0C , 1000 0010 0C0S 0S0C 5 55 55 5 44 44 4 HH trong đó ta sử dụng ký hiệu rút gọn )qsin(S ii = , )qcos(C ii = . Ma trận chuyển tọa độ một điểm từ hệ tọa độ cố định { }000 zyOx sang hệ tọa độ { }5555 zyxO có dạng D5 = H1 H2 H3 H4 H5 ⎥⎥ ⎥⎥ ⎦ ⎤ ⎢⎢ ⎢⎢ ⎣ ⎡ +++ ++−+−−+ ++−+−− = 1000 SaSaCddSSCCS CSaCSaSSdCSSCCSSSCCCS CCaCCaSCdCSCCSSCSSCCC 233222345123452345234 231321223415234515123415152341 231321223415234515123415152341 trong đó ta sử dụng các ký hiệu rút gọn: )qqqsin(S kjiijk ++= và )qqqcos(C kjiijk ++= . Từ các phần tử của ma trận D5 ta xác định được các tọa độ của điểm E trên hệ tọa độ cố định 1234523322E 234152313212E 234152313212E dCdSaSaz SSdCSaCSay SCdCCaCCax +++= −+= −+= (4.1) Phương trình (4.1) biểu diễn quan hệ ( )=x f q (4.2) trong đó [ ]TEEE z,y,x=x và [ ]T5432q q,q,q,q,q=q . Từ (4.1) ta xác định được ma trận Jacobi J(q) biểu diễn quan hệ ( )=x J q q&& như sau ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ = ⎥⎥ ⎥⎥ ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢⎢ ⎢⎢ ⎢ ⎣ ⎡ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ =∂ ∂= 3534333231 2524232221 1514131211 5 E 4 E 3 E 2 E 1 E 5 E 4 E 3 E 2 E 1 E 5 E 4 E 3 E 2 E 1 E JJJJJ JJJJJ JJJJJ q z q z q z q z q z q y q y q y q y q y q x q x q x q x q x q x)(qJ (4.3) Trong đó các phần tử của ma trận J(q) có dạng J11 = -d5 S1 S234 - a3 S1 C23 - a2 S1 C2 108 J12 = -d5 C1 C234 - a3 C1 S23 - a2 C1 S2 J13 = -d5 C1 C234 - a3 C1 S23 J14 = -d5 C1 C234 J15 = 0 J21 = -d5 C1 S234 + a3 C1C23 + a2 C1C2 J22 = -d5 S1C234 + a3 S1 S23 + a2 S1 S2 J23 = -d5 S1C234 + a3 S1 S23 J24 = -d5 S1C234 J25 = 0 J31 = 0 J32 = -d5 S234 + a3 C23 + a2 C2 J33 = -d5 S234 + a3 C23 J34 = -d5 S234 J35 = 0 Với ma trận J được xác định theo hệ thức (4.3) và một quĩ đạo định trước của điểm E, ta có thể xây dựng một chương trình tính toán các giá trị biến khớp qi và các đạo hàm )5,,2,1i(q,q ii K&&& = dựa trên thuật giải đã được trình bày trong chương 1. Để minh họa cho việc tính toán động học ngược bằng tính toán mô phỏng số, ta xét một thí dụ ứng với điểm E có quĩ đạo là một đường xoắn ốc bám trên một mặt cầu. Quy luật chuyển động có dạng ⎟⎠ ⎞⎜⎝ ⎛+= ⎟⎠ ⎞⎜⎝ ⎛π= ⎟⎠ ⎞⎜⎝ ⎛π+= 3 tsin1.012.0z 3 tsin)t2cos(1.0y 3 tsin)t2sin(1.03x E E E (4.4) trong đó các tọa độ được tính theo đơn vị mét. Quá trình tính toán trị số các biến khớp được thực hiện trên chương trình tính MATLAB trong khoảng thời gian tính )s( 15 t0 π≤≤ với bước thời gian h = 0.001 (s) và sai số tính toán ε = 10-6 (rad). Các kết quả tính toán được biểu diễn trên các hình 4.4, 4.5 và 4.6, trong đó các đồ thị góc quay qi được biểu diễn trên hình 4.4, hình 4.5 là đồ thị vận tốc góc iq& và hình 4.6 chứa các đồ thị gia tốc góc 109 iq&& . Như đã phân tích ở trên, tham số góc quay q5 là hằng số và không có ảnh hưởng đến quĩ đạo chuyển động của điểm E. Các kết quả này có thể sử dụng trong việc điều khiển động học và là dữ liệu cho phân tích động lực học ngược. Hình 4.3. Quỹ đạo định trước của điểm E (đường xoắn ốc) Hình 4.4. Đồ thị biến khớp q Quĩ đạo điểm ỹ đạo iể E 110 Hình 4.5. Đồ thị vận tốc góc q& Hình 4.6. Đồ thị gia tốc góc q&& 111 4.4 Tính toán động lực học ngược Như đã đề cập tới trong các chương trước, mục tiêu của bài toán phân tích động lực học ngược là xác định trị số của các mômen động cơ ứng với một quy luật chuyển động cho trước (về quĩ đạo và vận tốc) của điểm E. Mặc dù khâu thứ 6 (que đo) dùng để thao tác đo và được điều khiển độc lập với phần còn lại của hệ, khâu này vẫn có ảnh hưởng khi tính toán động lực học. Bởi vậy tất cả 6 khâu của rôbốt phải được tính đến trong các phương trình vi phân chuyển động. Trị số mômen động cơ [ ]T654321 ,,,,, ττττττ=τ được xác định từ các phương trình động lực mô tả chuyển động của rôbốt đo. Trong trường hợp bỏ qua ma sát tại các khớp và các lực cản khác, đồng thời coi lực tác động của vật cần đo lên khâu 6 tại vị trí tiếp xúc là nhỏ không đáng kể, phương trình động lực được biểu diễn dưới dạng tổng quát ( ) ( , ) ( )+ + =M q q C q q q g q τ&& & & (4.5) Để thiết lập các phương trình này ta sử dụng các ký hiệu: im khối lượng của khâu i [ ]TCCCi iii z,y,x=r vị trí khối tâm khâu i trên hệ cố định { }000 zyOx [ ]T)i(C)i(C)i(C)i(i iii z,y,x=r vị trí khối tâm khâu i trên hệ động { }iii zyOx i i=v r& vận tốc khối tâm khâu i iω véctơ vận tốc góc khâu i biểu diễn trên hệ cố định ( )iiω véctơ vận tốc góc khâu i biểu diễn trên hệ động q r qJ ∂ ∂= iTi )( ma trận Jacobi tịnh tiến của khâu i q ωqJ &∂ ∂= iRi )( ma trận Jacobi quay của khâu i iA ma trận côsin chỉ hướng của khâu i ( )iiI ma trận quán tính khối của khâu i tại khối tâm của khâu và tính trên hệ tọa độ động Ii ma trận quán tính khối của khâu i tại khối tâm của khâu và tính trên hệ tọa độ cố định Các số hạng trong phương trình (4.5) được xác định theo các bước sau đây: 112 1. Xác định các ma trận côsin chỉ hướng Ai Các ma trận biến đổi tọa độ của các khâu đối với hệ tọa độ cố định , 1000 d010 0C0S 0S0C 1 11 11 11 ⎥⎥ ⎥⎥ ⎦ ⎤ ⎢⎢ ⎢⎢ ⎣ ⎡ −== HD ⎥⎥ ⎥⎥ ⎦ ⎤ ⎢⎢ ⎢⎢ ⎣ ⎡ + −− − == 1000 dSa0CS CSaCSSCS CCaSSCCC 12222 21212121 21212121 212 HDD ⎥⎥ ⎥⎥ ⎦ ⎤ ⎢⎢ ⎢⎢ ⎣ ⎡ ++ +−− +− == 1000 dSaSa0CS CSaCSaCSSCS CCaCCaSSCCC 1233222323 23132121231231 23132121231231 323 HDD ⎥⎥ ⎥⎥ ⎦ ⎤ ⎢⎢ ⎢⎢ ⎣ ⎡ ++ +− +−− == 1000 dSaSaC0S CSaCSaSSCCS CCaCCaSCSCC 123322234234 2313212234112341 2313212234112341 434 HDD ⎥⎥ ⎥⎥ ⎦ ⎤ ⎢⎢ ⎢⎢ ⎣ ⎡ +++ −++−−+ −++−− = 1000 CddSaSaSCCCS SSdCSaCSaCSSCCSSSCCS SCdCCaCCaCSCCSSCSSCC 234512332223452345234 23415231321223451512341512341 23415231321223451512341512341 5D Ma trận côsin chỉ hướng Ai được xác định từ 9 thành phần của ma trận 3x3 ở phía trên bên trái của các ma trận biến đổi tọa độ Di. Véctơ tọa độ rOi của điểm gốc Oi của hệ tọa độ động nằm trong véctơ 3x1 phía trên bên phải của Di. ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ = ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ −= 1 O11 11 1 d 0 0 r, 010 C0S S0C 1 A 113 ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ + = ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ −− − = 122 212 212 O 22 12121 12121 2 dSa CSa CCa r, 0CS CSSCS SSCCC 2 A ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ ++ + + = ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ −− − = 233221 2313212 2313212 O 2323 1231231 1231231 3 SaSad CSaCSa CCaCCa r, 0CS CSSCS SSCCC 3 A ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ ++ + + = ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ − −− = 233221 2313212 2313212 O 234234 234112341 234112341 4 SaSad CSaCSa CCaCCa r, C0S SSCCS SCSCC 4 A ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ +++ ++− ++− = ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ +−−+ +−− = 2332223451 231321223415 231321223415 O 23452345234 234515123415152341 234515123415152341 5 SaSaCdd CSaCSaSSd CCaCCaSCd SSCCS CSSCCSSSCCCS CSCCSSCSSCCC 5 r A ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ +++++ ++−−+ ++−−+− = ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ +−+ +−−−−+− +−−+−− = 122233234566234656 23132341566234151662125166 23132341566234151662125166 O 2345234623465234623465 23451512341562341615623415623416156 23451512341562341651623415623416516 6 dSaSaC)dSa(SCCa CSaCSCCaS)SdSSa(CSaSCCa CCaCCCCaS)CdCSa(CCaSSCa SSCCSSCCSSCC CSSCCCSCSSSCCSSCSCCSSSCSC CSCCSCCCSSCCSSSCCCCSCSSSC 6 r A 2. Xác định tọa độ khối tâm của các khâu theo công thức tổng quát )6,,2,1i(,)i(iiOii K=+= rArr (4.6) trong đó véctơ Oir và ma trận iA được xác định từ bước 1. Véctơ ( )i ir được cho trước từ các thông số động lực học của rôbốt như trên hình 4.7 và trong bảng 4.3. 114 Hình 4.7. Vị trí trọng tâm các khâu Bảng 4.3. Vị trí khối tâm khâu i của rôbốt trên hệ động Vị trí trọng tâm Khâu i )i( Cix )i( Ciy )i( Ciz 1 0 -(d1-l1) 0 2 -(a2-l2) 0 0 3 -(a3-l3) 0 0 4 0 0 l4 5 0 -(d5-l5) 0 6 -(a6-l6) 0 0 3. Tính toán ma trận Jacobi tịnh tiến của các khâu dựa trên các đạo hàm riêng theo biến q, thí dụ như ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ =∂ ∂= 000000 000000 000000 1 1T q rJ x1 x2 x3 x4 x6 O1 z4 O2 O5 O6 O C1 C2 C3 C4 C6 y1 O3≡O4 l1 l2 l3 l4 l5 l6 C5 115 ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ − −− =∂ ∂= 0000Cl0 0000SSlCCl 0000CSlCSl 22 212212 122222 2 2T q rJ ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ + −−−+ −−−−− =∂ ∂= 000ClCaCl0 000SSlSSaSSlCCaCCl 000SClSCaSClCSaCSl 23322233 231321223132122313 2313212231321212313 3 3T q rJ 4. Tính toán vận tốc góc của các khâu Trước hết ta thiết lập biểu thức toán tử sóng của các véctơ vận tốc góc ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ − == 01q 000 q00 ~ 1 1 1 T 1 )1( 1 & & &AAω ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ − − − == 0qSqC qS0q qCq0 ~ 1212 122 122 2 T 2 )2( 2 && && && &AAω ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ − −+ −− == 0qSqC qS0qq qCqq0 ~ 123123 12332 12332 3 T 3 )3( 3 && &&& &&& &AAω ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ ++ − −−−− == 0qSqqq qS0qC qqqqC0 ~ 1234432 12341234 4321234 4 T 4 )4( 4 &&&& && &&&& &AAω ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ ++−−− +++−+++ +++−− = == 0)qqq(SqSCqCq )qqq(SqSC0)qqq(CqSS qCq)qqq(CqSS0 ~ 43251234512345 432512345432512345 12345432512345 5 T 5 )5( 5 &&&&&& &&&&&&&& &&&&&& &AAω Từ đó ta suy ra vận tốc góc của các khâu ⎥ ⎥⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ = 0 q 0 1 )1( 1 &ω , ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ = 2 12 12 )2( 2 q qC qS & & & ω , ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ + = 32 123 123 )3( 3 qq qC qS && & & ω , ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ −−−= 1234 432 1234 )4( 4 qC )qqq( qS & &&& & ω 116 ⎥ ⎥⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ +++ + ++− = )qqq(CqSS qCq )qqq(SqSC 432512345 12345 432512345 )5( 5 &&&& && &&&& ω ⎥ ⎥⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ ++++ +++++− +++−+ = )qqq(CqSSq qC)qqq(SSqCCqSSC qS)qqq(CSqCSqSCC 4325123456 564326512346123465 564326512346123465 )6( 6 &&&&& &&&&&& &&&&&& ω 5. Tính toán các ma trận Jacobi RiJ Trước hết ta tính toán các ma trận Jacobi quay )i(RiJ tương ứng với ( )i iω như sau ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ =∂ ∂= 000000 000001 000000 )1( 1)1( 1R q ωJ & , ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ =∂ ∂= 000010 00000C 00000S 2 2)2( 2)2( 2R q ωJ & , ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ =∂ ∂= 000110 00000C 00000S 23 23)3( 3)3( 3R q ωJ & , , 00000C 001110 00000S 234 234)4( 4)4( 4R ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ −−−=∂ ∂= q ωJ & , 00CCCSS 01000C 00SSSCS 5555234 234 5555234)5( 5)5( 5R ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ −−− =∂ ∂= q ωJ & ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ − −−−+ =∂ ∂= 10CCCSS 0CSSSSSSSCSCC 0SCSCSCSCCSCS 5555234 6656565652342346 6656565652342346)6( 6)6( 6R q ωJ & Ma trận Jacobi quay RiJ tương ứng với iω được xác định từ ( )i RiJ theo hệ thức 117 ( ) )i(Rii)i(ii)i(iiiRi JAqωAqωAqωJ =∂∂=∂∂=∂∂= &&& (4.7) 6. Xác định các phần tử của ma trận khối lượng ( )M q của rôbốt từ hệ thức tổng quát sau [ ]∑ = += 6 1i Rii T RiTii T Tim)( JIJJJqM (4.8) Để đơn giản hóa các công thức tính nhưng không mất tính tổng quát, ta coi các trục của hệ tọa độ động là các trục quán tính chính, khi đó ma trận ( )i iI bao gồm các phần tử là hằng số và được biểu diễn dưới dạng ma trận đường chéo ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ = zi yi xi )i( 1 I10 0I0 00I I (4.9) Chú ý rằng ma trận quán tính khối Ii của khâu i tại điểm khối tâm tính trên hệ tọa độ cố định được xác định bởi hệ thức Ti )i( iii AIAI = (4.10) Thế các hệ thức (4.7) và (4.9) vào phương trình (4.8) ta nhận được [ ] [ ]∑∑ == +=+= 6 1i )i( Ri )i( i T)i( RiTii T Ti 6 1i )i( Rii T i )i( ii T i T)i( RiTii T Ti mm)( JIJJJJAAIAAJJJqM (4.11) Thế các biểu thức nhận được của TiJ (bước 3), )i( RiJ (bước 5), các giá trị cho trước của mi và ma trận ( )iiI vào phương trình (4.11) ta nhận được biểu thức của 6×6 phần tử của ma trận khối lượng ( )M q . Do công thức của ( )M q được viết dưới dạng tích và tổng của các ma trận nên việc tính toán có thể thực hiện khá thuận lợi nhờ một chương trình tính có chức năng xử lý các biểu thức bằng chữ như chương trình tính toán đa năng MAPLE. 7. Xác định các phần tử của ma trận ( , )C q q& Như đã trình bày trong chương II, các thành phần cij của ma trận ( , )C q q& được xác định từ các phần tử của ma trận ( )M q theo hệ thức sau k i jk j ik k ij ij qq m q m q m 2 1C &∑ ⎟⎟⎠ ⎞ ⎜⎜⎝ ⎛ ∂ ∂−∂ ∂+∂ ∂= (4.12) trong đó 1,2,...,6i = và 1,2,...,6.j = 118 Tuy nhiên, biểu thức của các phần tử của ( )M q và ( , )C q q& là những biểu thức giải tích khá dài và phức tạp nên không được trình bày chi tiết thêm ở đây. 8. Xác định các phần tử của véc tơ g(q) từ biểu thức thế năng của hệ Thế năng của hệ có thể xác định nhờ công thức ∑ = =Π 6 1i i T im rg (4.13) trong đó véctơ ri được xác định từ bước 2, véc tơ gia tốc trọng trường tính trên hệ cố định [ ]0,0, Tg= −g với 29.81 m/sg ≈ . Sử dụng hệ thức (4.13), thế năng của hệ được viết dưới dạng )dCdSaSaSCClCSl(gm )dSaSaCl(gm )dSaSaCl(gm )dSaSl(gm)dSl(gmglm 1234522233234566234666 12223323455 12223323444 1222333122211 ++++++ ++++ ++++ +++++=Π Từ đó, ta suy ra các thành phần của véctơ g(q) theo công thức T )( ⎟⎟⎠ ⎞⎜⎜⎝ ⎛ ∂ Π∂= q qg (4.14) dưới dạng sau )SSCCC(glmg SSCglmg )SdCCClSSl(gmSglmSglmg )CdCaCCClSSl(gm )CaSl(gm)CaSl(gmCglmg )SdCaCaCCClSSl(gm )CaCaSl(gm)CaCaSl(gm )CaCl(gmCglmg 0g 234652346666 23456665 234523456623466623455234444 2345233234566234666 233234552332344423333 234522233234566234666 22233234552223323444 2223332222 1 −−= −= +−−−−= +−−− −−+−= +−−−− −−−−−− ++= = Cuối cùng, ta tính toán được các trị số mômen của động cơ của rôbốt sau khi tất cả các số hạng của vế phải của phương trình (4.5) đã được xác định. Để minh họa cho việc tính toán động lực học ngược bằng tính toán số, ta xét một thí dụ ứng với quĩ đạo chuyển động của điểm E theo dạng cung tròn phẳng. Bảng 4.4 cung cấp các trị số của các thông số động lực học của rôbốt phục vụ cho tính toán số. 119 Bảng 4.4. Các thông số động lực học của rôbốt đo (đối chiếu với hình 4.7) Khâu i mi (kg) Ixi (kgm2) Iyi (kgm2) Izi (kgm2) li (m) 1 2.0 4.0×10-3 3.0×10-3 1.0×10-3 0.10 2 0.9 0.2×10-3 3.0×10-3 3.0×10-3 0.06 3 1.2 0.5×10-3 3.5×10-3 4.0×10-3 0.10 4 1.1 0.6×10-3 2.5×10-3 3.5×10-3 0.04 5 0.5 0.7×10-3 0.2×10-3 0.3×10-3 0.03 6 0.05 0.3×10-4 0.2×10-4 0.1×10-4 0.02 Quy luật chuyển động trên mặt phẳng { }00zOx của điểm E (điểm O5) được mô tả bởi các phương trình tọa độ (4.15) và minh họa trên hình 4.8. )m(t 4 sin12.014.0z 0y )m(t 4 cos112.02.0x E E E π+= = ⎟⎠ ⎞⎜⎝ ⎛ π−+= (4.15) trong đó thời gian chuyển động hết một nửa đường tròn là 4 (s). Vị trí ban đầu (tại thời điểm t = 0) của rôbốt được xác định bởi các trị số biến khớp ).rad(0.2q,1206.2q,5511.3q,0472.1q 50403020 ==== Do chuyển động của điểm E hoàn toàn trên mặt phẳng, biến khớp của khâu 1 được ấn định q1 = 0. Hình 4.8. Quĩ đạo điểm E (nửa đường tròn trên mặt phẳng { }00zOx ) 120 Hình 4.9. Vị trí các khâu và các khớp tại một số vị trí chuyển động Hình 4.10. Trị số của biến khớp 2, 3 và 4 O1 O E ≡ O5 O4 O2 121 Hình 4.11. Vận tốc góc của động cơ dẫn động khâu 2, 3 và 4 Hình 4.12. Gia tốc góc của động cơ dẫn động khâu 2, 3 và 4 122 Hình 4.13. Mômen dẫn động cần thiết các khâu của rôbốt Kết quả tính toán động học ngược được trình bày trên các hình 4.9 - 4.12. Như đã phân tích tại mục 4.2, biến khớp q5 giữ giá trị hằng số trong suốt quá trình chuyển động của điểm E. Với các kết quả tính toán động học ngược đối với các biến khớp như trên và với khớp 6 chuyển động độc lập theo quy luật ,0q,0q,2/q 666 ==π= &&& ta có thể tính toán được mômen dẫn động cần thiết tại các khớp 1, 2,...,6 (dẫn động các khâu 1, 2,...,6 tương ứng) từ phương trình vi phân chuyển động đã thiết lập được. Kết quả tính toán số được biểu diễn trên hình 4.13. 4.5 Điều khiển trượt rôbốt BKHN-MCX-04 Để tiến hành điều khiển rôbốt chuyển động theo một quy luật định trước theo yêu cầu của bài toán công nghệ, trước tiên ta phải giải bài toán động học ngược để xác định giá trị của các véc tơ qqq &&& ,, dựa trên thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng đã được trình bày trong chương 1, sau đó giải bài toán động lực học ngược để xác định phương trình chuyển động của rôbốt và giá trị lực/mômen theo các giá trị của qqq &&& ,, qua đó xác định công suất cần thiết của động cơ điện. 123 Theo nguyên lý hoạt động của rôbốt BKHN-MCX-04, để đo một chi tiết nào đó ta phải xác định trước quỹ đạo cần thiết để điều khiển điểm O5 chuyển động theo quỹ đạo ấy nhằm đưa que đo tiếp xúc với chi tiết cần đo. Giả sử chi tiết cần đo có dạng hình cầu như hình dưới đây Hình 4.14 Quỹ đạo định trước của điểm E (đường xoắn ốc) Để đo vật thể dạng hình cầu này ta có thể tiến hành điều khiển điểm O5 chuyển động theo 1 đường xoắn ốc trên bề mặt của hình cầu. Phương trình của đường xoắn ốc có dạng ⎟⎠ ⎞⎜⎝ ⎛+= ⎟⎠ ⎞⎜⎝ ⎛π= ⎟⎠ ⎞⎜⎝ ⎛π+= 3 tsin1.012.0z 3 tsin)t2cos(1.0y 3 tsin)t2sin(1.03x E E E Bài toán động học ngược và động lực học ngược cho trường hợp này đã được giải quyết trong phần 4.3 và 4.4, ta sẽ sử dụng các kết quả này cho phần điều khiển. Như trong phần điều khiển chuyển động của rôbốt đã trình bày ở chương 3 và theo lý thuyết điều khiển dạng trượt được trình bày trong [63], để ổn định chuyển động của điểm O5 ta chọn hàm Lyapunov có dạng Quĩ đạo điểm E 124 sqMs )( 2 1V T= Trong đó M(q) là ma trận khối lượng của rôbốt, s là sai số suy rộng trong điều khiển dạng trượt, với s được xác định bởi )t()t( Λees += & trong biểu thức trên thì e(t) và )t(e& là độ sai lệch vị trí và sai lệch vận tốc của các khớp động e(t) = q(t) – qd(t) )t()t()t( dqqe &&& −= ),...,,(diag 621 λλλ=Λ Sử dụng chương trình điều khiển trượt được xây dựng theo thuật toán tính toán giải bài toán điều khiển rôbốt ở chương 3 và phần simulink trong Matlab, ta thu được kết quả điều khiển chuyển động rôbốt BKHN-MCX-04 theo các thông số của bộ điều khiển đã được chọn là: Ks= diag([20,20,20,20,1,0.1]); Kpd = diag([0.2,0.2,0.2,0.2,0.02,0.01]); λ= diag([50,50,50,50,30,20]); Nhiễu d(t) = random(6,6)*0.01; Hình 4.15 Đồ thị tọa độ xE(t) trong không gian thao tác 125 Hình 4.16 Đồ thị tọa độ yE(t) trong không gian thao tác. Hình 4.17 Đồ thị tọa độ zE(t) trong không gian thao tác. Qua kết quả trên ta thấy rằng điểm cần điều khiển O5 của rôbốt BKHN- MCX-04 bám khá tốt theo quỹ đạo mong muốn, chỉ sau một khỏang thời gian 0.25 giây điểm O5 đã bám được quỹ đạo mong muốn. 126 4.6 Thí nghiệm Qua các phần lý thuyết đã trình bày ta thấy các bài toán ngược động lực học và điều khiển rôbốt dư dẫn động đều liên quan đến việc bài bài toán động học ngược. Kết quả của bài toán động lực học và điều khiển hoàn toàn phụ thuộc vào độ chính xác của bài toán động học ngược, vì vậy trong phần thí nghiệm của mình chúng tôi chọn việc thí nghiệm nhằm kiểm tra độ chính xác của việc giải bài toán động học ngược. Mô hình thí nghiệm có dạng như hình ảnh dưới đây: Hình 4.18 Mô hình thí nghiệm rôbốt đo BKHN-MCX-04 4.6.1 Cấu tạo của hệ thống thí nghiệm Bộ thí nghiệm gồm có các phần như sau: Rôbốt BKHN-MCX-04 Camera thu nhận hình ảnh hoạt động của rôbốt Kính lọc màu Máy tính 127 4.6.2 Nguyên lý hoạt động của hệ thống thí nghiệm Trên rôbốt, tại các khớp động ta gắn các đèn led phát nguồn ánh sáng màu đỏ, phía trước camera ta gắn kính lọc màu có nhiệm vụ chỉ cho ánh sáng màu đỏ đi qua và tác động vào camera. Trong quá trình làm việc có thể có các nhiễu về màu sắc từ hệ thống tác động lên camera, để loại các nhiễu sắc này ta dùng phần mềm lọc nhiễu được viết bằng ngôn ngữ Matlab. Sau khi lọc đi các nhiễu ta còn lại các đốm sáng phát ra từ các đèn led - vị trí của các khớp mà ta quan tâm. Phần mềm có nhiệm vụ xác định vị trí tâm của các đốm sáng. Khi xác định được các tâm của đốm sáng, thì đoạn thẳng nối các tâm chính là các khâu của rôbốt ở thời điểm khảo sát. Khi rôbốt chuyển động, các đèn led gắn liền với các khớp động cũng sẽ thay đổi vị trí của nó. Sự thay đổi vị trí của các khớp động này sẽ được camera thu nhận, chụp lại ảnh và lưu vào bộ nhớ của máy tính. Phần mềm xử lý ảnh sẽ xác định tâm của các khớp và lưu vào tệp “toadotam.txt” của các khớp. Mỗi ảnh sẽ có 2n số tọa độ tâm, trong đó có n giá trị tọa độ theo trục x và n giá trị tọa độ theo trục y của các tâm ( với n là số các đèn led chiếu sáng, đây chính là số các khớp động). Khi đã có tọa độ của các tâm sáng, phần mềm sẽ tính toán ra góc giữa các khâu qua các đoạn thẳng nối liền các đốm sáng và lưu vào tệp “Goc.txt” là bộ góc giữa các khâu. Để điều khiển chuyển động của rôbốt ta xây dựng một phần mềm điều khiển viết bằng ngôn ngữ Visual Basic. Thông qua giao diện của chương trình ta nhập vào các giá trị góc quay của các khớp động mà ta tính toán được qua bài toán động học ngược. Chương trình điều khiển sẽ điều khiển các khớp rôbốt quay với các góc tương ứng. Còn phần mềm xử lý ảnh sẽ ghi và lưu lại các giá trị góc quay thực và tọa độ của các tâm khớp cũng như vẽ được quỹ đạo chuyển động của khâu thao tác. 4.6.3 Hệ thống điều khiển Rôbốt đo gồm 5 khớp điều khiển chuyển động và 1 khớp điều khiển việc đo lường. Mối một khớp được quản lý bởi 1 bộ vi xử lý. Mối bộ vi xử lý có chức năng điều khiển khép kín, vừa có nhiệm vụ điều khiển động cơ vừa có nhiệm vụ đo lường. Các bộ vi xử lý sẽ được nối chung bus và được nối với máy tính qua cổng RS232. Mỗi một bộ vi xử lí sẽ có một địa chỉ xác định để ta có thể dễ dàng điều khiển. Tay máy hoạt động theo nguyên tắc từ các bản vẽ CAD khi thiết kế bề mặt các chi tiết máy ta xác định quỹ đạo điều khiển theo các điểm cần đo. Trên quỹ đạo điều khiển đo, ta sẽ xác lập chương trình điều khiển cho các khớp để dịch chuyển các khâu của rôbốt theo các điểm trong không gian làm việc của thiết bị. Muốn xác định vị trí của một điểm nào đó trong kh«ng gian, ta ph¸t lÖnh điều khiển cho tõng khíp tõ m¸y tính. Lệnh điều khiển sẽ được truyền 128 xuống bộ vi xử lí. Khớp nào được điều khiển thì bộ vi xử lí của khớp đó sẽ nhận được lệnh và bắt đầu điều khiển động cơ. Vị trí của các khớp sẽ được xác định nhờ hệ thống đo góc bởi các encoder. Nhờ đó ta sẽ xác định được vị trí của đầu đo. Trong suốt quá trình điều khiển, vị trí của đầu đo luôn được cập nhật trong máy tính. Khi đầu đo đến đúng vị trí mà ta mong muốn thì vị trí của đầu đo cũng như vị trí của các khớp sẽ được lưu lại trong máy tính. Xuất phát từ nguyên tắc hoạt động của rôbốt đo, ta đưa ra các yêu cầu sau : - Xây dựng hệ vi xử lí thực hiện 2 chức năng điều khiển động cơ đồng thời xử lí tín hiệu phản hồi từ hệ đo. - Xây dựng hệ truyền động điện thực hiện chức năng điều khiển động cơ một chiều (có đảo chiều). - Xây dựng hệ xử lí tín hiệu phản hồi từ hệ đo, đồng thời nâng cao độ phân giải của hệ thống đo. - Truyền thông đa vi xử lí để kết nối giữa hệ điều khiển của từng khớp với máy tính. Từ yêu cầu trên ta có sơ đồ điều khiển cho toàn hệ thống như trên hình 4. 19 và sơ đồ điều khiển của từng khớp như trên hình 4.20 Hình 4.19 Sơ đồ điều khiển toàn bộ rôbốt 129 Hình 4.20 sơ đồ điều khiển từng khớp của BKHN-MCX-04 Trong các hệ thống điều khiển ở trên ta dùng vi điều khiển thuộc họ MCS-51 của Intel, cụ thể là chip AT89C2051 của hãng Atmel. Sơ đồ nguyên lý của mạch điều khiển trục có dạng như trên hình 4.21. 130 Hình 4.21 Sơ đồ nguyên lý của mạch điều khiển 1 trục C 4 10 5 R ST 9 XT A L2 18 XT A L1 19 GND 20 PS EN 29 AL E /P R O G 30 E A/ V PP 31 VCC 40 P 1. 0/ T2 1 P 1. 1/ T2 -E X 2 P 1. 2 3 P 1. 3 4 P 1. 4 5 P 1. 5 6 P 1. 6 7 P 1. 7 8 P2 .0 /A 8 21 P2 .1 /A 9 22 P2 .2 /A 10 23 P2 .3 /A 11 24 P2 .4 /A 12 25 P2 .5 /A 13 26 P2 .6 /A 14 27 P2 .7 /A 15 28 P 3. 0/ R XD 10 P3 .1 /T XD 11 P3 .2 /IN T0 12 P3 .3 /IN T1 13 P 3. 4/ T0 14 P 3. 5/ T1 15 P3 .6 /W R 16 P 3. 7/ R D 17 P 0. 0/ AD 0 39 P 0. 1/ AD 1 38 P 0. 2/ AD 2 37 P 0. 3/ AD 3 36 P 0. 4/ AD 4 35 P 0. 5/ AD 5 34 P 0. 6/ AD 6 33 P 0. 7/ AD 7 32 U 1 A T8 9C 52 1A 1 5 1A 2 7 2A 1 10 2A 2 12 1Y 1 2 1Y 2 3 2Y 1 13 2Y 2 14 1E N 6 2E N 11 1E 1 2E 15 VC C 1 9 VC C 2 4 U 4 L2 98 D 11 D IO D E IN T0 -T H C D 12 D IO D E D 7 F- H IG H D 8 D 13 D IO D E V C C VC C D 14 D IO D E C LK A 14 C LK B 1 Q A 12 Q B 9 Q C 8 Q D 11 VCC 5 GND 10 R 01 2 R 02 3 R 91 6 R 92 7 74 LS 90 VC C V C C C LK A 14 C LK B 1 Q A 12 Q B 9 Q C 8 Q D 11 VCC 5 GND 10 R 01 2 R 02 3 R 91 6 R 92 7 74 LS 90 R XD -T XD LE D -F H D 10 LE D -F L 12 V 12 V 12 V 12 V 12 V 12 V S W 1 R ES ET VC C 11 10U 5E 74 H C 04 C 3 C VC C R 1 10 0 V C C 1 2 D C F- LO W F- LO W R 2 10 K 1 2 3 714 U 7A 74 H C 00 1234 J4 E N -T H U O C 4 5 6 U 8B 74 H C 00 9 10 8 U 12 C 74 H C 00 1 2 12 13 11 U 12 D 74 H C 00 VC C EN -T H C EN -T H C Y 1 C R Y ST AL VC C D 1 R IG H T D 2 D 3 D 4 D 5 D 6 LE FT PW M 12 V C 1 12345E N -Q U AY F- H IG H F- H IG H F- H IG H F- H IG H F- LO W F- LO W VC C 2 3 14 1 7 U 9A 74 H C 12 6 5 6 4 U 10 B 74 H C 12 6 9 8 10 74 H C 12 6 C 2 12 11 13 74 H C 12 6 12 V V C C 131 4.6.4 Kết quả thí nghiệm Dưới đây là kết quả của thí nghiệm mà chúng tôi đã tiến hành trên rôbốt BKHN-MCX-04 Giả sử quỹ đạo mong muốn của bàn kẹp của rôbốt BKHN-MCX-04 có dạng là nửa vòng tròn phẳng với phương trình của nó là )m(t 4 sin12.014.0z 0y )m(t 4 cos112.02.0x E E E π+= = ⎟⎠ ⎞⎜⎝ ⎛ π−+= Giá trị tọa độ điểm cuối E và các tọa độ suy rộng của các khớp động đã được xác định từ thực nghiệm và được lưu lại theo bảng dưới đây. Bảng 4.5 Các kết quả đo thực nghiệm TT t (s) xE (m) yE (m) 2 (rad)q 3 (rad)q 4 (rad)q 1 0 0.2000 0.1405 1.0472 3.5511 2.1206 2 0.2 0.2015 0.1578 1.0957 3.5814 2.1564 3 0.4 0.2059 0.1776 1.1396 3.6379 2.1711 4 0.6 0.2131 0.1935 1.1767 3.7157 2.1622 5 0.8 0.2229 0.2110 1.2045 3.8077 2.1294 6 1.0 0.2351 0.2239 1.2212 3.9070 2.0750 7 1.2 0.2495 0.2376 1.2261 4.0085 2.0030 8 1.4 0.2655 0.2459 1.2195 4.1089 1.9177 9 1.6 0.2829 0.2546 1.2025 4.2068 1.8234 10 1.8 0.3012 0.2575 1.1763 4.3013 1.7236 11 2.0 0.3200 0.2605 1.1425 4.3922 1.6213 12 2.2 0.3388 0.2575 1.1023 4.4791 1.5192 13 2.4 0.3571 0.2546 1.0569 4.5616 1.4194 14 2.6 0.3745 0.2459 1.0076 4.6389 1.3242 15 2.8 0.3905 0.2376 0.9557 4.7099 1.2355 16 3.0 0.4049 0.2239 0.9027 4.7733 1.1549 17 3.2 0.4171 0.2110 0.8501 4.8272 1.0843 18 3.4 0.4269 0.1935 0.7997 4.8696 1.0248 19 3.6 0.4341 0.1776 0.7532 4.8987 0.9776 20 3.8 0.4385 0.1578 0.7122 4.9125 0.9432 21 4.0 0.4400 0.1400 0.6783 4.9095 0.9218 132 Hình 4.22 Quỹ đạo thực nghiệm của điểm cuối E 133 Qua kết quả thí nghiệm ta thấy độ sai lệch lớn nhất giữa kết quả tính toán và kết quả thực nghiệm là 0.1%. Điều đó chứng tỏ thuật toán mà chúng tôi đã sử dụng để giải bài toán động học ngược là hoàn toàn phù hợp. 4.7 Kết luận chương 4 Trên cơ sở rôbốt đo tự chế tạo, trong chương này đã tiến hành xác định các tham số động học, động lực học của rôbốt. Sau đó đã tiến hành tính toán động học ngược, động lực học ngược và điều khiển rôbốt bám theo quỹ đạo cần đo. Đây là một loại rôbốt có cấu trúc động học không gian nên việc thiết lập các phương trình vi phân chuyển động khá phức tạp. Do đó việc tính toán động lực học ngược và điều khiển chuyển động là không đơn giản nếu ta sử dụng các công cụ cũ. Nhờ thuật toán “hiệu chỉnh gia lượng véc tơ tọa độ suy rộng”, bài toán này được giải một cách khá nhanh chóng. Các kết quả của chương này một mặt chứng minh tính chất đúng đắn của các thuật toán trong các chương trước, mặt khác nêu ra một khả năng chế tạo và sử dụng rôbốt trong đo lường chính xác các chi tiết và cụm các chi tiết máy. 134 KẾT LUẬN CHUNG Để thiết kế chế tạo các rôbốt phục vụ cho việc đẩy mạnh công nghiệp hóa và hiện đại hóa nền công nghiệp thì việc nghiên cứu động học, động lực học và điều khiển các rôbốt là một công việc cần thiết và hết sức quan trọng. Trong luận án của mình, tác giả đã nghiên cứu các bài toán động học, động lực học và điều khiển chuyển động của rôbốt dư dẫn động. Đây là các dạng bài toán cơ bản nhất của việc phân tích và tổng hợp các rôbốt. Có thể tóm tắt lại một số kết quả chính mà chúng tôi đã đạt được như sau: Những kết quả chính của luận án 1. Dựa trên có sở của công thức khai triển Taylor, đã xây dựng thuật toán “hiệu chỉnh gia lượng véc tơ tọa độ suy rộng” để tính toán bằng số bài toán động học ngược rôbốt dư dẫn động. So với các thuật toán số đã có thuật toán này tính toán có độ chính xác cao hơn. 2. Đã xây dựng một thuật toán giải bài toán động lực học ngược rôbốt dư dẫn động trong không gian thao tác. Từ đó xây dựng một thuật toán giải quyết bài toán điều khiển rôbốt dư dẫn động trong không gian trạng thái theo phương pháp điều khiển dạng trượt. 3. Dựa trên các phần mềm đa năng MATLAB và MAPLE đã xây dựng các chương trình tính toán động học ngược, động lực học ngược và điều khiển trượt rôbốt dư dẫn động dựa trên thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng. Sử dụng các chương trình tính toán này đã tính toán nhiều thí dụ minh họa nhằm chứng minh khả năng của thuật toán và sự chính xác (sai số rất bé) của thuật toán. 4. Với sự giúp đỡ của TS. Nguyễn Văn Vinh, đã tham gia chế tạo rôbốt đo BKHN-MCX-04. Đã xác định được các tham số động học, động lực học của rôbốt tự chế tạo này. Sử dụng phần mềm xây dựng trên môi trường MATLAB và MAPLE đã tiến hành tính toán động học ngược, động lực học ngược và điều khiển trượt rôbốt đo BKHN-MCX-04. Đã tiến hành thí nghiệm nhỏ trên mô hình này. Các bài toán động học ngược, động lực học ngược và điều khiển chuyển động rôbốt đều có liên quan đến bài toán xác định các tọa độ suy rộng cho nên khi chúng ta đã xây dựng thuật toán “hiệu chỉnh gia lượng véc tơ tọa độ suy rộng” là chúng ta có một công cụ hữu ích để giải quyết các bài toán cơ bản về động học ngược, động lực học ngược và điều khiển rôbốt. Vì vậy có thể nói thuật toán “hiệu chỉnh gia lượng véc tơ tọa độ suy rộng” là kết quả cơ bản nhất của luận án này. 135 Một số vấn đề có thể tiếp tục mở rộng nghiên cứu - Do có bậc tự do còn dư mà khi có một khớp động nào đó bị kẹt thì rôbốt vẫn có khả năng thực hiện các yêu cầu của bài toán công nghệ vì vậy ta có thể đầu tư nghiên cứu về hiện tượng kẹt khớp trong quá trình làm việc của Rôbốt dư dẫn động. - Nghiên cứu ứng dụng các phương pháp điều khiển khác nhau để lựa chọn giải pháp điều khiển tối ưu. - Nghiên cứu triển khai ứng dụng trong thực tế sản xuất các kết quả đã nghiên cứu được. 136 DANH MỤC CÔNG TRÌNH CỦA TÁC GIẢ 1. Nguyễn Văn Khang, Lê Đức Đạt, Trần Hoàng Nam: “Về một phương pháp số giải bài toán động học ngược robot dạng chuỗi”. Tuyển tập Hội nghị Cơ học toàn quốc lần thứ 8, Tập 1, Tr. 250-259. NXB Bách khoa, Hà Nội 2007. 2. Nguyễn Văn Khang, Nguyễn Quang Hoàng, Lê Đức Đạt, Trần Hoàng Nam : “Về một thuật toán điều khiển trượt robot dư dẫn động”. Tạp chí Tin học và Điều khiển học, Tập 24 (2008), No.3, Tr.269-280. 3. Nguyễn Quang Hoàng, Nguyễn Văn Khang, Trần Hoàng Nam: “Bài toán động học ngược rôbốt dư dẫn động có chú ý đến sự cố kẹt khớp”. Tuyển tập Hội nghị Cơ học toàn quốc, Tập 2, Tr. 282-290, NXB Khoa học tự nhiên và Công nghệ, Hà Nội 2009. 4. Nguyễn Văn Khang, Nguyễn Quang Hoàng, Trần Hoàng Nam: “Về bài toán động lực học ngược rôbốt dư dẫn động”. Tuyển tập Hội nghị Khoa học Công nghệ Cơ khí chế tạo toàn quốc lần thứ hai, Phân ban “Tự động hóa và Cơ điện tử”, Tr 41-48, Hà Nội 2009. 5. Nguyen Van Khang, Do Tuan Anh, Nguyen Phong Dien, Tran Hoang Nam : “In fluence of trajectories on the joint torques of kinematically redundant manipulators”. Vietnam Journal of Mechanics, vol. 29 (2007), No.2, pp. 65-72. 6. Nguyen Van Khang, Nguyen Phong Dien, Nguyen Van Vinh, Tran Hoang Nam : “Inverse kinematic and dynamic analysis of redundant measuring manipulator BKHN-MCX-04”. Vietnam Journal of Mechanics, vol. 32 (2010), No.1, pp. 15-26. 137 TÀI LIỆU THAM KHẢO Tài liệu tiếng Việt 1. Đỗ Tuấn Anh: Bài toán ngược động lực học rôbốt dư dẫn động. Đồ án tốt nghiệp đại học - Đại học Bách Khoa Hà Nội 2006. 2. R. H. Bishop: Cơ điện tử, Tập 1 ( Phạm Anh Tuấn và các đồng nghiệp biên dịch). NXB Đại học Quốc gia, Hà Nội 2006. 3. Phạm Thượng Cát: “Điều khiển rôbốt n bậc tự do có nhiều tham số bất định trong không gian Đề Các”. Tuyển tập Hội nghị Cơ học toàn quốc năm 2009, Tập 1, Tr. 195-204. 4. Phan Nguyên Di, Nguyễn Văn Khang, Đỗ Sanh: Ổn định chuyển động trong kỹ thuật. Nhà xuất bàn Khoa học và Kỹ thuật, Hà nội 1986. 5. Nguyễn Văn Đạo : Ổn định của các hệ động lực. Nhà xuất bản Đại học Quốc gia Hà nội, Hà nội 1998. 6. Lê Đức Đạt: Về điều khiển phi tuyến chuyển động của Robot dư dẫn động trong không gian thao tác. Đồ án tốt nghiệp đại học, Đại học Bách Khoa Hà Nội 2008. 7. B. Heimann, W. Gerth, K. Popp : Cơ điện tử. Nhà xuất bản Khoa học và kỹ thuật, Hà nội 2008 (Người dịch: Nguyễn Văn Khang, Nguyễn Phong Điền, Nguyễn Quang Hoàng, Hoàng Minh Sơn). 8. Nguyễn Công Hiền, Nguyễn Hoàng Mai: “Điều khiển trượt thích nghi cho chuyển động tay máy”. Tuyển tập Hội nghị khoa học toàn quốc lần thứ 6 về Tự động hoá, Hà Nội 2005. 9. Đào Văn Hiệp: Kỹ thuật Rôbốt (in lần thứ 3). NXB Khoa học và Kỹ thuật, Hà Nội 2006. 10. Nguyễn Trần Hiệp, Phạm Thượng Cát: “Điều khiển rôbốt theo nguyên lý trượt sử dụng mạng nơron”. Tạp chí Tin học và Điều khiẻn học, Tập 24, Số 3, Tr.236-246. 11. Nguyễn Thế Hoàn, Phạm Phu: Cơ sở phương trình vi phân và lý thuyết ổn định, Nhà xuất bản Giáo dục, 2003. 12. Nguyễn Quang Hoàng, Nguyễn Văn Khang: “Về một thuật toán điều khiển trượt rôbốt công nghiệp”. Tuyển tập Hội nghị Cơ học toàn quốc lần thứ 8, Tập 1, Tr. 191- 199, Hà Nội 2007. 138 13. Nguyễn Quang Hoàng, Nguyễn Văn Khang, Trần Hoàng Nam: “Bài toán động học ngược rôbốt dư dẫn động có chú ý đến sự cố kẹt khớp”. Tuyển tập Hội nghị Cơ học toàn quốc, Tập 2, Tr. 282-290, Hà Nội 2009. 14. F. Holzweissig, H. Dresig : Giáo trình động lực học máy. Nhà xuất bản Khoa học kỹ thuật, Hà nội 2001, (Người dịch: Nguyễn Văn Khang, Vũ Liêm Chính, Phan Nguyên Di). 15. Lê Minh Huy: Bài toán ngược và điều khiển rôbốt dư dẫn động. Đồ án tốt nghiệp đại học, Đại học Bách Khoa Hà nội 2009. 16. Nguyễn Văn Khang: Cơ học kỹ thuật, Nhà xuất bản Giáo dục, Hà Nội. 2009. 17. Nguyễn Văn Khang: Động lực học hệ nhiều vật. Nhà xuất bản Khoa học và Kỹ thuật, Hà Nội 2007. 18. Nguyễn Văn Khang, Lê Đức Đạt, Trần Hoàng Nam: “Về một phương pháp số giải bài toán động học ngược robot dạng chuỗi”. Tuyển tập Hội nghị Cơ học toàn quốc lần thứ 8, Tập 1, Tr. 250-259. NXB Bách khoa, Hà Nội 2007. 19. Nguyễn Văn Khang, Nguyễn Quang Hoàng, Lê Đức Đạt, Trần Hoàng Nam : “Về một thuật toán điều khiển trượt robot dư dẫn động”. Tạp chí Tin học và Điều khiển học, Tập 24 (2008), No.3, Tr.269-280. 20. Nguyễn Văn Khang, Nguyễn Quang Hoàng, Trần Hoàng Nam: “Về bài toán động lực học ngược rôbốt dư dẫn động”. Tuyển tập Hội nghị khoa học công nghệ cơ khí chế tạo toàn quốc lần thứ hai, Hà Nội 2009. 21. Lại Khắc Lãi, Nguyễn Việt Anh: Lập trình Matlab và ứng dụng. Nhà xuất bản Khoa học và Kỹ thuật, Hà nội 2006. 22. Tạ Duy Liêm: Rôbốt và hệ thống công nghệ rôbốt hóa. NXB Khoa học và Kỹ thuật, Hà Nội 2004. 23. Vũ Đình Nam: Ứng dụng của điều khiển trượt trong Robot công nghiệp. Đồ án tốt nghiệp đại học - Đại học Bách Khoa Hà Nội 2008. 24. Đinh Văn Phong: Phương pháp số trong Cơ học (in lần thứ 2). NXB Khoa học và Kỹ thuật, Hà Nội 2006 25. Nguyễn Thiện Phúc: Rôbốt công nghiệp. Nhà xuất bản Khoa học và Kỹ thuật, 2004. 26. Nguyễn Doãn Phước: Lý thuyết điều khiển nâng cao. Nhà xuất bản Khoa học và Kỹ thuật, 2005. 139 27. Nguyễn Doãn Phước: Lý thuyết điều khiển phi tuyến. Nhà xuất bản Khoa học và Kỹ thuật, Hà Nội 2006. 28. Phạm Đăng Phước: Rôbốt công nghiệp. Nhà xuất bản Xây dựng, Hà Nội 2007. 29. Nguyễn Phương, Nguyễn Thị Hương Giang: Cơ sở tự động hóa trong ngành cơ khí, Nhà xuất bản Khoa học và Kỹ thuật, Hà nội 2005. 30. Nguyễn Ngọc Thuần, Hồ Quỳnh : Ứng dụng ma trận trong kỹ thuật. Nhà xuất bản Khoa học và kỹ thuật, Hà nội 1978. 31. Nguyễn Phùng Quang: Matlab/Simulink dành cho kỹ sư điều khiển tự động. Nhà xuất bản Khoa học và Kỹ thuật, 2004. 32. Lê Hoài Quốc, Chung Tấn Lâm: Nhập môn rôbốt công nghiệp. Nhà xuất bản Khoa học và Kỹ thuật, 2002. 33. Trần Thế San (biên dịch): Cơ sở nghiên cứu và sáng tạo robot. Nhà xuất bản Thống kê, Hà Nội 2003. 34. Đỗ Sanh : Cơ học giải tích. Nhà xuất bản Bách khoa Hà Nội, Hà nội 2008. 35. Đỗ Sanh: Cơ học kỹ thuật (2 tập). NXB Giáo dục, Hà Nội 2008. 36. Đỗ Sanh, Đinh Văn Phong, Nguyễn Thanh Thủy, Nguyễn Nhật Quang, Phan Mạnh Dẫn, Đỗ ĐăngKhoa: “Công nghệ tin học và một số bài toán Cơ kỹ thuật”. Kỷ yếu hội nghị cơ điện tử toàn quốc lần thứ 2, Tr. 182- 189. Hà Nội 2004. 37. Nguyễn Hoài Sơn, Đỗ Thanh Việt, Bùi Xuân Lâm : Ứng dụng Matlab trong tính toán kỹ thuật. Nhà xuất bản Đại học Quốc gia Tp. Hồ Chí Minh 2002. 38. Nguyễn Mạnh Tiến: Điều khiển rôbốt công nghiệp. Nhà xuất bản Khoa học và Kỹ thuật, Hà Nội 2007. 39. Trịnh Quang Vinh, Nguyễn Đăng Bình, Phạm Thành Long: Rôbốt công nghiệp, Nhà xuất bản Khoa học và kỹ thuật, Hà Nội 2008. 40. J. Wittenburg: Động lực học hệ vật rắn, (Người dịch Nguyễn Đông Anh). Nhà xuất bản Xây dựng, Hà Nội 2000. Tài liệu tiếng Anh 41. T.L. Boulion, P.L. Odell : Generalized Inverse matrices . Wiley – Interscience, New York 1971. 140 42. H. Bruyninckx , O. Khatib : “Gauss’ Principle and the Dynamics of Redundant and Constrained Manipulators”. Proceedings of the 2000 IEEE International Conference on Robotics & Automation, San Francisco, CA , April 2000, pp. 2563-2568. 43. G.S. Chirikjian and J.W. Burdick: “The Kinematics of Hyper-Redundant Robot Locomotion”. IEEE Transactions on Robotics and Automation, Vol. 11, No. 6, Dec. 1995, pp. 781-793. 44. J. J. Craig: Introduction to Robotics: Mechanics and Control. Pearson Prentice Hall, New Jersey 2005. 45. G.H. Golub and C.F Van Loan : Matrix computations (Third Edition). The Johns Hopkins University Press, 1996. 46. Nguyen Quang Hoang, Nguyen Van Khang : “On kinematic inverse and control of redundant manipulators under consideration of jammed joint”. Proceeding of the ISRM 2009 47. P. Hsu, John Hauser, and Shankar Sastry: “Dynamic Control of Redundant Manipulators”. Journal of Robotic Systems, 1989. pp 157- 168 48. R. Kelly, V. Statibanez : Control of Robot Manipulator in Joint Space. Springer, London 2005. 49. Nguyen Van Khang, Do Thanh Trung : “A contribution to the dynamic simulation of robot manipulator with the software ROBODYN” Vietnam Journal of mechanics, vol. 26 (2004), No. 4, pp. 215-225. 50. Nguyen Van Khang, Do Tuan Anh, Nguyen Phong Dien, Tran Hoang Nam : “In fluence of trajectories on the joint torques of kinematically redundant manipulators”. Vietnam Journal of Mechanics, vol. 29 (2007), N2, pp. 65-72. 51. Nguyen Van Khang, Nguyen Phong Dien, Nguyen Van Vinh, Tran Hoang Nam : “Inverse kinematic and dynamic analysis of redundant measuring manipulator BKHN-MCX-04”. Submitted to Vietnam Journal of Mechanics. 52. T.R. Kurfess (Editor): Robotics and Automation Handbook. CRC Press, 2005. 53. D.C. Lay: Linear Algebra and Its Applications. Pearson Addison Wesley, 2008. 141 54. F.L. Lewis, D.M. Dawson, C.T. Abdallah: Robot Manipulator Control: Theory and Practice Second Edition. Marcel Dekker, Inc. Second Edition, 2004. 55. R.H. Murray, Zexiang Li, S.S. Sastry, A. Makenratical: Introduction to Robotic Manipulation. CRS Press, Boca Rator 1994. 56. R.K. Mittal, I.J. Nagrath: Robotics and Control. Tata Mcgraw-Hill Publishing Company Limited, New Delhi, 2003. 57. Y. Nakamura : Advanced Robotics/ Redudancy and Optimination. Addison – Wesley Publishing Company, Reading 1991. 58. S.B. Niku : Introduction to Robotics. Analysis, Systems, Appliacation. Prentice Hall, Inc. 2000. 59. R.V. Patel, F. Shadpey : Control of redundant robot manipulator, Springer, Berlin 2005. 60. R. P. Paul: Robot Manipulators/Mathamatics, Programming, and Control. MIT Press, Cambridge Mass. 1981. 61. W. Perruquetti and J.P.B Barbot: Sliding Mode Control in Engineering. Marcel D Dekker Inc., New York 2002. 62. C. R. Rao, S. K. Mitra : Generalized Inverse and Applications. John Wiley & Sons Inc, 1971. 63. J.–J.E. Slotine and W. Li: Applied Nonlinear Control. Prentice Hall, Englewood Cliffs, New Jersey 1991. 64. R.J. Schilling : Fundamentals of robotics, Analysis and Control. Prentice hall, Englewood Cliffs, New Jersey, 1990. 65. B. Sicilano: Kinematic Control of Redundant Robot Manipulators: A Tutorial. Journal of Intelligent and Robotics Systems, 1990. 66. L. Sciavicco and B. Siciliano: “A Solution Algorithm to the Inverse Kinematic Problem for Redundant Manipulators”. IEEE Journal of Robotics and Automation, 1988, pp. 1046-1058 67. L. Sciavicco, B. Siciliano: Modelling and Control of Robot Manipulators (2nd Edition), Springer-Verlag, London 2000. 68. G. Schreiber, C. Ott, G. Hirzinger: “Interactive Redundant Robotics: Control of the Inverted Pendulum With Null space Motion”. Proceedings of the 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems, Maui, Hawaii, USA, Oct. 29 – Nov. 03, 2001, pp. 158-164. 142 69. M.W. Spong, S. Hutchinson, M. Vidyasagar: Robot Modeling and Control. John Wiley and Sons Inc., New York 2006. 70. A. Visioli: Practical PID Control. Springer-Verlag, London 2006. 71. Q-G. Wang, Zhen Ye, Wen-Jian Cai, Chang-Chieh Hang: PID Control for Multivariable Processes. Springer-Verlag, Berlin Heidelberg 2008. 72. N. Yagiz: “Robust control of a spatial robot using sliding modes”. Mathematical & Computational Applications, Vol. 7, 2002, No. 3, pp. 219-228.