Đề tài Nghiên cứu thiết kế, chế tạo các robot thông minh phục vụ cho các ứng dụng quan trọng - Nhóm sản phẩm đồ gá CNC

λ λ ⎧ − + = + − +⎪ − + = + − +⎨⎪ − + = −⎩ x x x y y y z z z p u a u d a u d p u a u d a u d p u u d u d (2.12) 16 Với 3 2 y y b b λ = , 1 2 3x xb bλ λ= − Thay các kết quả của hệ (2.9) vào hệ (2.12) ta đ−ợc: 11 1 1 1 1 1 21 2 2 31 3 3 1 1 1 2 2 1 22 2 2 32 3 3 1 1 1 3 3 1 2 2 3 3 1 ( 1) ( ) ( ) ( 1) ( ) ( ) ( 1) ) x x x x y y y x z z z x a u d pp a u d a u d b u d p p a u d a u d b u d pp u d u d b λ λ λ λ λ λ λ λ λ + −⎧ − + = + − +⎪⎪⎪ −− + = + − +⎨⎪⎪ −− + = −⎪⎩ (2.13) Mặt khác, dựa vào kết cấu của bàn di động B ta có : B1 B2 3B b1 3b 2b Hình 2.2.4 2 1 2 = uuuur B B 1 2 1 2( ) ( ) Tb b b b− − =b32 2 1 3 = uuuur B B 1 3 1 3( ) ( ) Tb b b b− − =b22 2 2 3 = uuuur B B 2 3 2 3( ) ( ) Tb b b b− − =b12 với : = +i ib a A .iR 0 0 id⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ (i=1,2,3) 17 1⇒ =b 11 1 1 1 1 1 1 x y z a u d u d u d +⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ ; 2 =b 21 2 2 22 2 2 2 2 x y z a u d a u d u d +⎡ ⎤⎢ ⎥+⎢ ⎥⎢ ⎥⎣ ⎦ ; 3 =b 31 3 3 32 3 3 3 3 x y z a u d a u d u d +⎡ ⎤⎢ ⎥+⎢ ⎥⎢ ⎥⎣ ⎦ 11 21 1 1 2 2 11 21 1 1 2 2 2 1 1 22 2 2 1 1 22 2 2 3 1 2 2 1 2 2 . T x x x x y y y y z z z z a a u d u d a a u d u d u d a u d u d a u d b u u d u u d − + − − + −⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥⇒ − − − − =⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥− −⎣ ⎦ ⎣ ⎦ 11 31 1 1 3 3 11 31 1 1 3 3 2 1 1 32 3 3 1 1 32 3 3 2 1 1 3 3 1 1 3 3 . T x x x x y y y y z z z z a a u d u d a a u d u d u d a u d u d a u d b u d u d u d u d − + − − + −⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥− − − − =⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥− −⎣ ⎦ ⎣ ⎦ 21 31 2 2 3 3 21 31 2 2 3 3 2 22 32 2 2 3 3 22 32 2 2 3 3 1 2 2 3 3 2 2 3 3 . T x x x x y y y y z z z z a a u d u d a a u d u d a a u d u d a a u d u d b u d u d u d u d − + − − + −⎡ ⎤ ⎡ ⎤⎢ ⎥ ⎢ ⎥− + − − + − =⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥− −⎣ ⎦ ⎣ ⎦ Hay : 2 2 2 2 11 21 1 1 2 2 1 1 22 2 2 1 1 2 2 3 2 2 2 2 11 31 1 1 3 3 1 1 32 3 3 1 1 3 3 2 2 2 2 2 21 31 2 2 3 3 22 32 2 2 3 3 2 2 3 3 1 ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) x x y y z z x x y y z z x x y y z z a a u d u d u d a u d u d u d b a a u d u d u d a u d u d u d b a a u d u d a a u d u d u d u d b ⎧ − + − + − − + − =⎪⎪ − + − + − − + − =⎨⎪ − + − + − + − + − =⎪⎩ Kết hợp với hệ (2.13) ta có hệ 6 ph−ơng trình, 6 ẩn: (2.14) 18 Hệ ph−ơng trình (2.14) chứa 9 ẩn số 1 2 3 1 2 3 1 2 3, , , , , , , ,d d d p p pα α α . Các thành phần , ,ix iy izu u u đã xác định đ−ợc, các thành phần , ,x y zu u u xác định theo (2.9) Khi giải quyết bài toán động học thuận hay ng−ợc, ta biết tr−ớc đ−ợc 3 ẩn. Công việc còn lại chỉ phải giải hệ 6 ph−ơng trình 6 ẩn số. a. Bài toán động học thuận Bài toán động học thuận là bài toán biết độ dài các chân di (i=1,2,3), ta phải tìm vị trí của bàn máy động P và ma trận ARB. Theo phần trên ta thay các giá trị di (i=1,2,3) vào hệ (4.14), ta sẽ đ−ợc hệ 6 ph−ơng trình với 6 ẩn là : 1 2 3 1 2 3, , , , ,p p pα α α Chú ý là 3 ph−ơng trình sau của hệ (2.14) chỉ chứa di và iα nên việc giải 6 ph−ơng trình đ−ợc đơn giản lại còn giải hệ 3 ph−ơng trình với 3 ẩn là iα . Sau đó thay các giá trị của di và iα vào 3 ph−ơng trình đầu ta sẽ tính đ−ợc các giá trị của P. Các giá trị còn lại tính đ−ợc bằng cách thay trực tiếp vào các ph−ơng trình (2.9), (2.10), (2.11). b. Bài toán động học ng−ợc Bài toán động học ng−ợc là bài toán biết vị trí bàn máy động P, ta phải tìm độ dài các chân di (i=1,2,3) và các góc iα (i=1,2,3) . T−ơng tự nh− cách làm đối với bài toán động học thuận ta thay các giá trị của P vào hệ (2.14), ta sẽ đ−ợc hệ 6 ph−ơng trình với 6 ẩn là : 1 2 3 1 2 3, , , , ,d d dα α α . Các giá trị còn lại tính đ−ợc bằng cách thay trực tiếp vào các ph−ơng trình (2.9), (2.10), (2.11). 2.1.5. Ví dụ tính toán Ta tính toán cho một rô-bốt song song 3 RPS cụ thể : - Tam giác A1A2A3 và tam giác B1B2B3 là các tam giác đều. 19 - PB1 = h; OA1 = g; 2 3 2 /3η η π= = - Do kết cấu của cơ cấu ta có i i iz A B⊥ - Trục i iz OA⊥ ⇒ / 2iβ π= Khi đó các đại l−ợng trong công thức (4.6) trở thành : 1 Bb = 0 0 ⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ h ; 2 Bb = 2 3 2 0 ⎡ ⎤−⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ h h ; 3 B b = 2 3 2 0 ⎡ ⎤−⎢ ⎥⎢ ⎥⎢ ⎥−⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ h h (2.15) 1a = 0 0 ⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ g ; 2a = 2 3 2 0 ⎡ ⎤−⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ g g ; 3a = 2 3 2 0 ⎡ ⎤−⎢ ⎥⎢ ⎥⎢ ⎥−⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ g g (2.16) Do / 2iβ π= nên iγ = ( / 2 )iπ β− = 0 Khi đó các ma trận cosin chỉ h−ớng ARi trở thành: 1 A R = 1 1 1 1 1 1 1 1 1 1 1 1 cos sin cos cos sin sin sin sin cos cos cos sin 0 γ α γ α γ γ α γ α γ α α − −⎡ ⎤⎢ ⎥−⎢ ⎥⎢ ⎥⎣ ⎦ 1 A R = 1 1 1 1 sin cos 0 0 0 1 cos sin 0 α α α α −⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ (2.17) 20 2 A R = 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 1 3 1 3 1 3( cos sin )sin ( cos sin )cos sin cos 2 2 2 2 2 2 3 1 3 1 3 1( cos sin )sin ( cos sin )cos sin cos 2 2 2 2 2 2 cos sin 0 γ γ α γ γ α γ γ γ γ α γ γ α γ γ α α ⎡ ⎤+ − + −⎢ ⎥⎢ ⎥⎢ ⎥− + − − −⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ 2 A R = 2 2 2 2 2 2 1 1 3sin cos 2 2 2 3 3 1sin cos 2 2 2 cos sin 0 α α α α α α ⎡ ⎤− −⎢ ⎥⎢ ⎥⎢ ⎥− −⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ (2.18) 3 A R = 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 1 3 1 3 1 3( cos sin )sin ( cos sin ) sin cos 2 2 2 2 2 2 3 1 3 1 3 1( cos sin )sin ( cos sin )cos sin cos 2 2 2 2 2 2 cos sin 0 γ γ α γ γ α γ γ γ γ α γ γ α γ γ α α ⎡ ⎤− − + +⎢ ⎥⎢ ⎥⎢ ⎥+ − − −⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ cos 3 A R = 3 3 3 3 3 3 1 1 3sin cos 2 2 2 3 3 1sin cos 2 2 2 cos sin 0 α α α α α α ⎡ ⎤−⎢ ⎥⎢ ⎥⎢ ⎥− −⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦ (2.19) Khi đó : 3 2 y y b b λ = = -1 ; 1 2 3x xb bλ λ= − = h; Thay vào hệ (2.14) ta đ−ợc : 21 1 2 2 3 3 1 1 2 2 2 3 3 3 1 1 2 2 3 3 2 2 2 1 1 2 2 1 2 1 2 1 2 13 ( sin sin ) sin 2 33 ( sin sin ) 2 3 cos cos cos (4.20) 3 3 sin 3 sin sin sin p d d d p d d p d d d g gd gd d d d d α α α α α α α α α α α α = + − −= − = + + − − + + + 21 2 1 2 2 2 2 2 1 1 3 3 1 3 1 3 1 3 1 3 1 3 2 2 2 2 2 2 3 3 2 3 2 3 2 3 2 3 2 3 2 cos cos 3 3 3 sin 3 sin sin sin 2 cos cos 3 3 3 sin 3 sin sin sin 2 cos cos 3 d d h g gd gd d d d d d d h g gd gd d d d d d d h α α α α α α α α α α α α α α ⎧⎪⎪⎪⎪⎪⎨⎪ − =⎪⎪ − − + + + − =⎪⎪ − − + + + − =⎩ a) Bài toán động học thuận Bài toán động học thuận là bài toán biết độ dài các chân di (i=1,2,3), ta phải tìm vị trí của bàn máy động P và ma trận ARB. Theo phần trên ta thay các giá trị di (i=1,2,3) vào hệ (2.20), ta sẽ đ−ợc hệ 6 ph−ơng trình với 6 ẩn là : 1 2 3 1 2 3, , , , ,p p pα α α Chú ý là 3 ph−ơng trình sau của hệ (2.20) chỉ chứa di và iα nên việc giải 6 ph−ơng trình đ−ợc đơn giản lại còn giải hệ 3 ph−ơng trình với 3 ẩn là iα . Sau đó thay các giá trị của di và iα vào 3 ph−ơng trình đầu ta sẽ tính đ−ợc các giá trị của P Các giá trị còn lại tính đ−ợc bằng cách thay trực tiếp vào các ph−ơng trình (2.9), (2.10), (2.11). b) Bài toán động học ng−ợc Bài toán động học ng−ợc là bài toán biết vị trí bàn máy động P, ta phải tìm độ dài các chân di (i=1,2,3) và các góc iα (i=1,2,3) . T−ơng tự nh− cách làm đối với bài toán động học thuận ta thay các giá trị P vào hệ (2.20), ta sẽ đ−ợc hệ 6 ph−ơng trình với 6 ẩn là : 1 2 3 1 2 3, , , , ,d d dα α α . Các giá trị còn lại tính đ−ợc bằng cách thay trực tiếp vào các ph−ơng trình (2.9), (2.10), (2.11). 22 2.2. Robot song song 3 RSS 2.2.1. Kết cấu hình học Hình 4.2.5: Kết cấu hình học của Robot song song 3 RSS Deltarca cấu tạo gồm ba chân, bàn di động 12 và bàn cố định 1. Mỗi chân gồm có khâu trên, khâu d−ới và năm khớp, trong đó có bốn khớp cầu 4, 6, 8, 10 (S) và một khớp quay 2 (R). Khâu AC đ−ợc gọi là khâu cánh tay, khâu BC đ−ợc gọi là khâu cẳng tay. Khâu AC nối với bàn cố định bởi khớp quay bản lề 2, khâu BC nối với bàn di động bởi hai khớp cầu 8, 10 và nối với khâu AC bởi hai khớp cầu 4, 6. Khâu BC thực chất là cơ cấu hình bình hành gồm các khâu 5, 7, 9, 11 với các khớp là khớp cầu chứ không phải là khớp quay. Nh− vậy Deltarca có tất cả 11 khâu và 15 khớp, đ−ợc gọi là rô-bốt song song 3 RSS (Hình 4.2.5). 2.2.2. Bậc tự do Sử dụng công thức: 1 ( 1) j i b i F n j f fλ = = − − + −∑ Với 6, 11, 15, 6n j fbλ = = = = , 12 khớp cầu và 3 khớp quay, ta có: 6(11 15 1) (12.3 3.1) 6 3F = − − + + − = 23 Nh− vậy bàn di động sẽ chuyển động với ba bậc tự do trong không gian. 2.2.3. Hệ trục tọa độ và kí hiệu Hệ trục Oxyz gắn với tâm bàn cố định, hệ trục Puvw gắn với tâm bàn di động. Các trục x, y nằm trong bàn cố định, các trục u, v nằm trong bàn di động. Tại mọi thời điểm x và y luôn song song cùng chiều với u và v. Dùng hệ trục i i i iA x y z gắn với tâm của khớp quay iA , sao cho ix nằm theo h−ớng kéo dài của iOA và trục iy dọc theo trục các khớp quay, còn trục iz song song với z. Góc iα đo từ x đến ix là một hằng số của quá trình thiết kế rô-bốt (Hình 2.6). Góc iϕ là góc tạo bởi trục ix và i iAC . Hình 4.2.6: Đặt hệ trục tọa độ lên rô-bốt 24 2.2.4. Ph−ơng trình liên kết Để thuận tiện cho quá trình thành lập hệ ph−ơng trình liên kết, ta quy định một số điểm sau:  Chọn hệ trục cố định là hệ trục Oxyz  Ta dùng chữ in nghiêng để kí hiệu véc-tơ đại số, ví dụ ai là véc-tơ đại số biểu diễn tọa độ của điểm iA so với hệ tọa độ cố định Oxyz. Còn biP là toạ độ của điểm iB trong hệ tọa độ động Puvw.  Khi nói tới tọa độ của một điểm nào đó mà ta không nói rõ là xét trong hệ toạ độ cụ thể nào thì hiểu rằng ta đang xet tọa độ của điểm đó trong hệ tọa độ cố định Oxyz. Sau đây ta xác định tọa độ của từng điểm  Tọa độ điểm iA (Hình 4.2.7): tuỳ thuộc vào kích th−ớc bàn cố định [ ] [ ], , *cos( ), *sin( ),0 TT i iai xai yai zai ra raα α= = (2.21)  Tọa độ điểm iC [ ], , .Tci xci yci zci ai Ri ciA= = + (2.22) Trong đó: Š Ri là ma trận cosin chỉ h−ớng của hệ trục i i i iA x y z so với hệ Oxyz. Để tính Ri ta chỉ việc xoay hệ Oxyz quanh trục z một góc iα , khi đó: cos( ) sin( ) 0 ( , ) sin( ) cos( ) 0 0 0 1 i i i i i iR R z α α α α α −⎡ ⎤⎢ ⎥= = ⎢ ⎥⎢ ⎥⎣ ⎦ (2.23) Š ciA là tọa độ của điểm iC trong hệ trục i i i iA x y z (Hình 4.2.4) [ ]*cos( ),0, *sin( ) Ti iciA la laϕ ϕ= − (2.24) 25 Hình 4.2.7 Hình 4.2.8 Thay (2.23) và (2.24) vào (2.22) ta đ−ợc: [ ]cos( )*( *cos( )),sin( )*( *cos( )), *sin( ) Ti i i i ici ra la ra la laα ϕ α ϕ ϕ= + + − (2.25)  Xác định tọa độ điểm B [ ], , *Tbi xbi ybi zbi p Rp biP= = + (2.26) Trong đó: [ ], , Tp px py pz= (2.27) là véc-tơ tọa độ của điểm P trong hệ trục tọa độ Oxyz Š Rp là ma trận cosin chỉ h−ớng của hệ Puvw so với Oxyz. Vì bàn di động chuyển động song phẳng so với bàn cố định nên: 1 0 0 0 1 0 0 0 1 Rp ⎡ ⎤⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦ (2.28) Š biP là tọa độ của điểm iB trong hệ trục tọa độ Puvw: [ ] [ ], , *cos( ), *sin( ),0 TT i ibiP xbiP ybiP zbiP rb rbβ β= = (2.29) Thay (2.27), (2.28), (2.29) vào (4.26) ta đ−ợc: 26 *cos( ) *sin( ) i i px rb bi py rb pz α α +⎡ ⎤⎢ ⎥= +⎢ ⎥⎢ ⎥⎣ ⎦ (2.20) Các điểm iB và iC ràng buộc với nhau bởi điều kiện: 2 2 2 2* ( ) ( ) ( )Tlb bici bici xci xbi yci ybi zci zbi= = − + − + − (2.21) Với i=1,2,3 thì hệ (2.21) là hệ gồm ba ph−ơng trình và sáu ẩn: 1 2 3, , ; , ,px py pz ϕ ϕ ϕ . Thông th−ờng ta đã biết tr−ớc ba ẩn và chỉ còn ba ẩn ch−a biết. Nh− vậy hệ (2.21) với số ph−ơng trình là 3 và bằng số ẩn nên luôn cho ta lời giải. 2.2.5. Bài toán động học thuận Khi cho biết quy luật chuyển động của ba góc: 1 2 3, ,ϕ ϕ ϕ ta sẽ xác định đ−ợc px, py và pz từ hệ (2.21) và do vậy xác định đ−ợc quy luật chuyển động của bàn di động. 2.2.6. Bài toán động học ng−ợc Khi cho biết quy luật chuyển động của bàn di động, tức là cho biết px, py, pz ta sẽ xác định đ−ợc 1 2 3, ,ϕ ϕ ϕ từ hệ (2.21). 2.3. Robot song song phẳng 3 chân RBSS phẳng 3 chân có đặc điểm là mỗi chân gồm có 2 khâu và 3 khớp, mỗi khớp cần 1 bậc tự do, tức là khớp p5: khớp quay (R), khớp tr−ợt (ký hiệu là T - Translatrion hoặc P – Prismatic). Vì vậy có 7 ph−ơng án: RRR, RRP, RPR, PRR, RPP, PPR, PPP Hình 2.8 minh họa cơ cấu RBSS phẳng 3 chân dùng cấu trúc RRR Hình 2.9 dùng cấu trúc PRR Hình 2.10. dùng cấu trúc PRP, trong đó 3 khớp quay R vuông góc với mặt phẳng chứa 1 ph−ơng tịnh tiến của 1 khớp P trên tấm cố định, còn khớp P kia liên hệ với tấm động. 27 RBSS phẳng 3 chân th−ờng đ−ợc dùng để tạo ra các đồ gá CNC để mang phôi gia công hay mang dụng cụ gia công. A1 B1 B2 3B A3 2A D E C Hình 4.2.8. RBSS phẳng 3 chân RRR Hình 4.2.9. RBSS phẳng 3 chân PRR B1 B2 3B D E C A3 A2 A1 28 Hình 4.2.10. RBSS phẳng 3 chân PRP 2.2. Đồ gá gia công vỏ hộp động cơ xe máy Trong phần phụ lục trình bày bản thuyết minh “Công trình thiết kế chế tạo cơ cấu kẹp phôi nhanh cho máy gia công CNC” và giới thiệu một số bản vẽ thiết kế đồ gá về hệ thống các xi lanh khí nén đ−ợc dùng cho đồ gá này. A B C F D E G H I P R P Tấm động Tấm cố định 29 III. Robot song song RBSS – 322 30 3.1. Giới thiệu chung Vấn đề Robot song song (RBSS) trở nờn hấp dẫn nhiều nhà nghiờn cứu từ giữa thập kỷ 90 khi nú được ứng dụng dưới dạng thiết bị cú tờn là Hexapod để tạo ra mỏy cụng cụ CNC 5 trục. Hexapod là một mụđun RBSS được kết cấu trờn nguyờn lý cơ cấu Stewart. Cơ cấu này gồm cú 6 chõn, với độ dài thay đổi được, nối với giỏ và tấm động đều bằng cỏc khớp cầu. Bằng cỏch thay đổi độ dài của cỏc chõn cú thể điều khiển sự định vị và định hướng của tấm động theo ý muốn. Ngoài cỏc ứng dụng trong ngành chế tạo mỏy cụng cụ RBSS cũn được ỏp dụng hiệu quả trong dụng cụ y học, trong hệ thống mụ phỏng, trong thiết bị thiờn văn, trắc địa và trong kỹ thuật phũng khụng v.v. Cơ cấu RBSS cũng cú nhiều biến thể khỏc nhau và xuất hiện nhiều tờn gọi khỏc nhau, vớ dụ như Hexapod, Hexaglide [4], Hexagons [5], Hexa [2], Delta [3] v.v. Dưới đõy trỡnh bày kết quả nghiờn cứu, thiết kế, chế tạo và điều khiển một kiểu robot song song. Trong thiết kế đó tham khảo phần tạo dỏng từ mẫu chào hàng robot Flex Picker của hóng ABB. 3.2. Thiết kế kết cấu và lập trỡnh điều khiển chuyển động của Robot. Trờn hỡnh 3.1 là bản vẽ 3D của Robot đó thiết kế và trờn hỡnh 3.2 là ảnh chụp sau khi đó lắp đặt vận hành. Cỏc chõn của Robot được xếp thành 3 nhỏnh đụi và được truyền động từ 3 động cơ riờng biệt. Mỗi chõn gồm cú 2 phần: phần trờn (đựi) và phần dưới (ống chõn). Chõn nhỏnh nối với giỏ đỡ bằng khớp quay (khớp hỏng) và nối với tấm động bằng “khớp cổ chõn”. Chỗ “cổ chõn” và đầu gối” đều bố trớ một khớp quay với trục quay cú 2 đầu khớp cầu 31 Hỡnh 4.3.1. Bản vẽ 3D thiết kế RBSS-322 Hỡnh 4.3.2. Ảnh chụp RBSS - 322 32 Gọi điểm đầu mỳt của dụng cụ gắn liền với tấm động là “điểm tỏc động cuối” (end - effector) của Robot. Khi lập trỡnh điều khiển chuyển động của Robot, ta quan tõm đến sự dịch chuyển của ‘điểm tỏc động cuối” này. Ngụn ngữ lập trỡnh là Visual C++ tạo điều kiện dễ dàng kết nối với cỏc thư viện đồ hoạ mạnh và phổ biến như OpenGL. Sơ đồ giải thuật chương trỡnh điều khiển giới thiệu trờn hỡnh 4.3.3. Hỡnh 4.3.3. Sơ đồ giải thuật chương trỡnh điều khiển Sau khi đó nhập dữ liệu, chỳng kiểm tra cú nằm trong vựng làm việc khụng, rồi được gửi tới cỏc chương trỡnh con để giải bài toỏn động học ngược. Cỏc kết quả đú được xử lý và truyền tới cổng nối tiếp R232C để điều khiển hoạt động của Robot qua bộ thiết bị điều khiển. Sau khi di chuyển tới điểm đó định, tớn hiệu phản hồi về sẽ là thụng điệp gửi tiếp thụng tin về điểm di chuyển tiếp theo. Quỏ trỡnh này được thực hiện lặp lại một cỏch nhịp nhàng, khụng phải chờ đợi, cho tới khi đó tới đớch cuối cựng của hành trỡnh. Begin Nhập dữ liệu Trong vựng làm việc Giải bài toỏn Động học nghịch Truyền tớn hiệu điều khiển tới R232C Nhận Tớn hiệu từ R232C End Đỳng Sai Đỳng Sai 33 Trờn hỡnh 4.3.4 là minh hoạ kết quả tớnh toỏn về hành trỡnh di chuyển của “điểm tỏc động cuối”. Hỡnh 4.3. 4. Trich kết quả tớnh toỏn về hành trỡnh Ngoài ra kết quả tớnh toỏn cho chương trỡnh điều khiển cũng đồng thời được gửi tới chương trỡnh mụ phỏng hoạt hỡnh của Robot. Hỡnh 4.3.5: Sơ đồ chức năng bảng mạch chớnh 3.3. Sơ đồ hệ thống điều khiển Trờn hỡnh 4.3.5 là sơ đồ chức năng bảng mạch chớnh (Main board). Chương trỡnh điều khiển được nạp trong bộ nhớ trong bộ vi xử lý của 89C51 DB9 2 3 5 MAX2 32 TxD PO RxD 89C51 P1 P2 Joy- stick AD 0809 ANALOG INPUT CONTROL BUS 34 thực hiện cỏc chức năng kiểm soỏt cổng P1 để nhận lệnh điều khiển từ Joystick trong chế độ điều khiển bằng tay (manual), kiểm tra cổng nối tiếp để nhận lệnh điều khiển từ mỏy tớnh PC. Chuỗi lệnh từ mỏy tớnh điều khiển truyền tới gồm 11byte. Khi nhận xong một lệnh từ cổng nối tiếp thỡ Main board sẽ truyền trở về PC một byte trả lời. Khi thực hiện xong lệnh đó nhận thỡ nú lại tiếp tục truyền trở về PC một byte bỏo cỏo thứ 2. Trờn hỡnh 4.3.6 là sơ đồ chức năng bảng mạch bộ điều khiển động cơ (Driver board). Hỡnh 4.3.6. Sơ đồ chức năng bảng mạch bộ điều khiển động cơ Hình 4.3.7.Chu trỡnh nhận và thực hiện lệnh điều khiển động cơ Driver board 89c2051 CONTROL BUS Mạch cụng suất Kiểm soỏt dũng Mạch phối hợp DC EN SENSOR Mạch phối hợp BUS điều khiển Nhận lệnh Thực hiện lệnh Kiểm soỏt dũng ĐC Đọc Encoder SENSER vị trớ ĐK Động cơ 35 Chương trỡnh điều khiển được nạp vào bộ nhớ trong của bộ vi xử lý 89C 2051 thực hiện chức năng kiểm tra bus điều khiển (control bus). Giỏ trị xung đếm vũng quay từ encoder đưa về qua mạch phối hợp được so dỏnh với giỏ trị vũng quay từ lệnh điều khiển là tớn hiệu để động cơ dừng hay hoạt động tiếp tục. Dũng điện tiờu thụ của động cơ cũng được kiểm soỏt trong quỏ trỡnh hoạt động. Nếu giỏ trị này vượt quỏ ngưỡng đó đặt thỡ chương trỡnh sẽ cho dừng động cơ và bỏo lỗi về bảng mạch chớnh. Lỗi sẽ được hiển thị bằng đốn LED và cú thể thụng bỏo về mỏy tớnh điều khiển. Cỏc sensor chỉ bỏo vị trớ ban đầu của Robot cũng được kiểm soỏt để khi cú lệnh chỳng sẽ trở về vị trớ xuất phỏt. Chu trỡnh nhận và thực hiện lệnh của Driver board được mụ tả trờn hỡnh 3.7. Hệ thống thiết bị đó được vận hành thử nghiệm tốt và đang chuẩn bị triển khai ứng dụng trong gia cụng điờu khắc. 3.4. Ch−ơng trình phần mềm điều khiển RBSS - 322 Chương trỡnh gồm cỏc file vitridau.m, nhap.m, tinhdulieu.m, chayhopmay.m %vitridau.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear; s=serial('COM4'); set(s, 'Terminator', 'CR'); s.Timeout=7; %s so_xung1=2000; %21100 1 vong highbyte1=floor(so_xung1/256); lowbyte1=mod(so_xung1,256); so_xung2=1000; %21100 1 vong highbyte2=floor(so_xung2/256); 36 lowbyte2=mod(so_xung2,256); so_xung3=1000; %21100 1 vong highbyte3=floor(so_xung3/256); lowbyte3=mod(so_xung3,256); fopen(s) fwrite(s,254) %ma Header1 fwrite(s,253) %ma Header2 fwrite(s,104) %ma1 dc1 (+ or 43 len) (- or 45 xuong) (s or 115 dung) %(h or 104 vi tri dau) fwrite(s,lowbyte1) %so xung lowbyte dc1 fwrite(s,highbyte1) %so xung highbyte dc1 fwrite(s,104) %ma1 dc2 (+ or 43 len) (- or 45 xuong) (s or 115 %dung) (h or 104 vi tri dau) fwrite(s,lowbyte2) %so xung lowbyte dc2 fwrite(s,highbyte2) %so xung highbyte dc2 fwrite(s,104) %ma1 dc3 (+ or 43 len) (- or 45 xuong) (s or 115 %dung) (h or 104 vi tri dau) fwrite(s,lowbyte3) %so xung lowbyte dc3 fwrite(s,highbyte3) %so xung highbyte dc3 fprintf(s,'%s','s') %ma4 dc4 (+ len) (- xuong) (s dung) fwrite(s,1) fwrite(s,0) data=fscanf(s) if data=='b' fwrite(s,254) fwrite(s,253) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s') 37 fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) end fclose(s); %nhap.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear; % ch=input('\n ban nhap so lieu hay doc tu dia (n/d) : ','s'); % if (ch == 'd') % filename=input('\n Nhap ten file: ','s'); % [command,mss]=sprintf('load %s Aa Bb Cc OA OB OC TAa TBb % TCc.OtTA zTA OtTB zTB OtTC zTC',filename); % eval(command); % fprintf('\n Aa=%10.3f',Aa); % fprintf('\n Bb=%10.3f',Bb); % fprintf('\n Cc=%10.3f',Cc); % fprintf('\n OA=%10.3f',OA); % fprintf('\n OB=%10.3f',OB); % fprintf('\n OC=%10.3f',OC); % fprintf('\n TAa=%10.3f',TAa); % fprintf('\n TBb=%10.3f',TBb); % fprintf('\n TCc=%10.3f',TCc); 38 % fprintf('\n khoang cach TA den tam=%10.3f',OtTA); % fprintf('\n zTA=%10.3f',zTA); % fprintf('\n khoang cach TB den tam=%10.3f',OtTB); % fprintf('\n zTB=%10.3f',zTB); % fprintf('\n khoang cach TC den tam=%10.3f',OtTC); % fprintf('\n zTC=%10.3f',zTC); % elseif (ch == 'n') % Aa=input('Aa='); % Bb=input('Bb='); % Cc=input('Cc='); % OA=input('OA='); % OB=input('OB='); % OC=input('OC='); % TAa=input('TAa='); % TBb=input('TBb='); % TCc=input('TCc='); % OtTA=input('khoang cach TA den tam='); % zTA=input('zTA='); % OtTB=input('khoang cach TB den tam='); % zTB=input('zTB='); % OtTC=input('khoang cach TC den tam='); % zTC=input('zTC='); % filesave=input('\n Nhap ten file save: ','s'); % [command,mss]=sprintf('save %s Aa Bb Cc OA OB OC TAa TBb % TCc OtTA zTA OtTB zTB OtTC zTC',filesave); % eval(command); % end %-------------------------------------------------------------------------- 39 Aa=800 Bb=800 Cc=800 OA=40 OB=40 OC=40 TAa=365 TBb=365 TCc=365 OtTA=200 zTA=615 OtTB=200 zTB=615 OtTC=200 zTC=615 xO=input('xO='); yO=input('yO='); zO=input('zO='); xA=xO-OA yA=yO zA=zO xB=xO+OB*sin(30*pi/180) yB=yO-OB*cos(30*pi/180) zB=zO xC=xO+OC*sin(30*pi/180) yC=yO+OC*cos(30*pi/180) zC=zO 40 xTA=-OtTA yTA=0 zTA xTB=OtTB*sin(30*pi/180) yTB=-OtTB*cos(30*pi/180) zTB xTC=OtTC*sin(30*pi/180) yTC=OtTC*cos(30*pi/180) zTC %----------------- ya=0; y=ya; Aa_binh=Aa^2; TAa_binh=TAa^2; eqt1=convert(xA,yA,zA,Aa_binh); eqt2=convert(xTA,yTA,zTA,TAa_binh); syms x z x1 z1 [x,z]=solve(eqt1,eqt2,'x,z'); x1=eval(x) z1=eval(z) for i=1:length(z1) xa=x1(i); za=z1(i); if z1(i)>zA 41 if x1(i)<xTA xa=x1(i); ya; za=z1(i); break; end end end xa ya za %------------------------------------------------------ Bb_binh=Bb^2; TBb_binh=TBb^2; syms x y z x2 y2 z2 eqt3=y+x*cot(30*pi/180); eqt1=convert(xB,yB,zB,Bb_binh); eqt2=convert(xTB,yTB,zTB,TBb_binh); [x,y,z]=solve(eqt1,eqt2,eqt3,'x,y,z'); x2=eval(x) y2=eval(y) z2=eval(z) for i=1:length(z2) xb=x2(i); yb=y2(i); zb=z2(i); if z2(i)>zB if x2(i)>xTB xb=x2(i); 42 yb=y2(i); zb=z2(i); break; end end end xb yb zb %------------------------------------------------------ Cc_binh=Cc^2; TCc_binh=TCc^2; syms x y z x3 y3 z3 eqt3=y-x*cot(30*pi/180); eqt1=convert(xC,yC,zC,Cc_binh); eqt2=convert(xTC,yTC,zTC,TCc_binh); [x,y,z]=solve(eqt1,eqt2,eqt3,'x,y,z'); x3=eval(x) y3=eval(y) z3=eval(z) for i=1:length(z3) xc=x3(i); yc=y3(i); zc=z3(i); if z3(i)>zC if x3(i)>xTC xc=x3(i); 43 yc=y3(i); zc=z3(i); break; end end end xc yc zc %-------------------------------------------------------- %tinh cac goc di xuong goc1=asin((zTA-za)/TAa)*180/pi goc2=asin((zTB-zb)/TBb)*180/pi goc3=asin((zTC-zc)/TCc)*180/pi s=serial('COM2'); set(s, 'Terminator', 'CR'); s.Timeout=5; %s so_xung1=21100*goc1/360; %21100 1 vong highbyte1=floor(so_xung1/256); lowbyte1=mod(so_xung1,256); so_xung2=21100*goc2/360; %21100 1 vong highbyte2=floor(so_xung2/256); lowbyte2=mod(so_xung2,256); so_xung3=21100*goc3/360; %21100 1 vong highbyte3=floor(so_xung3/256); lowbyte3=mod(so_xung3,256); 44 if goc1>0 chieu1=45; else chieu1=43; end if goc2>0 chieu2=45; else chieu2=43; end if goc3>0 chieu3=45; else chieu3=43; end fopen(s) fwrite(s,254) %ma Header1 fwrite(s,253) %ma Header2 fwrite(s,chieu1) %ma1 dc1 (+ or 43 len) (- or 45 xuong) (s or 115 %dung) (h or 104 vi tri dau) fwrite(s,lowbyte1) %so xung lowbyte dc1 fwrite(s,highbyte1) %so xung highbyte dc1 fwrite(s,chieu2) %ma1 dc2 (+ or 43 len) (- or 45 xuong) (s or 115 dung) (h or 104 vi tri dau) fwrite(s,lowbyte2) %so xung lowbyte dc2 fwrite(s,highbyte2) %so xung highbyte dc2 fwrite(s,chieu3) %ma1 dc3 (+ or 43 len) (- or 45 xuong) (s or 115 dung) (h or 104 vi tri dau) fwrite(s,lowbyte3) %so xung lowbyte dc3 45 fwrite(s,highbyte3) %so xung highbyte dc3 fprintf(s,'%s','s') %ma4 dc4 (+ len) (- xuong) (s dung) fwrite(s,1) fwrite(s,0) data=fscanf(s,'%s') if data=='b' fwrite(s,254) fwrite(s,253) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) fprintf(s,'%s','s') fwrite(s,0) fwrite(s,0) end fclose(s); %tinhdulieu.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear; Aa=800 Bb=800 Cc=800 46 OA=40 OB=40 OC=40 TAa=365 TBb=365 TCc=365