Đồ án Tốt nghiệp Tính toán chuyển động chương trình và thiết kế Robot MMR

y chiếu thuận. ♦ Chọn hệ tọa độ O4x4y4z4 đặt ở vị trí thao tác, trục z4 trùng với trục của khâu 4, x4 là đường vuông góc chung của z3 và z4, y4 chọn sao cho O4x4y4z4 là hệ quy chiếu thuận. Từ hệ tọa độ đã chọn ta có bảng động học Denavit-Hartenberg như sau: Trong đó: d1= 90, a1= 45, a2= 283, a3= 263, a4= 130 (mm) Từ cơ sở lý thuyết đã nêu ở chương 1 ta xác định các ma trận Denavit-Hartenberg như sau: ♦ Ma trận mô tả vị trí và hướng của O1x1y1z1 đối với O0x0y0z0 : 0 H1 0 ⎡cos q1 ⎢ ⎢ sin q1 H1= ⎢ ⎢ 0 ⎢ − sin q1 cosá1 cos q1 cosá1 sin á1 0 sin q1 sin á1 − cos q1 sin á1 cosá1 0 a1 cos q1 ⎤ ⎥ a1 sin q1 ⎥ ⎥ d1 ⎥ 1 ⎦ - 27 -Khâu èi di ai ái 1 q1 d1 a1 ð/2 2 q2 0 a2 0 3 q3 0 a3 0 4 q4 0 a4 0 ⎣ 0 ⎥ Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR ⎡cos q1 ⎢ 0 ⎢ sin q1 H1= ⎢ ⎢ 0 ⎢  0 sin q1 0 − cos q1 1 0 0 0  a1 cos q1 ⎤ ⎥ a1 sin q1 ⎥ ⎥ d1 ⎥ 1 ⎦ (2.1) ♦ Ma trận mô tả vị trí và hướng của O2x2y2z2 đối với O1x1y1z1 : 1 H2 ⎡cos q2 ⎢ 1 ⎢ sin q2 H2= ⎢ ⎢ 0 ⎢ − sin q2 cosá 2 cos q2 cosá 2 sin á 2 0 sin q2 sin á 2 − cos q2 sin á 2 cosá 2 0 a2 cos q2 ⎤ ⎥ a2 sin q2 ⎥ ⎥ d 2 ⎥ 1 ⎦ ⎡cos q2 ⎢ 1 ⎢ sin q2 H2= ⎢ ⎢ 0 ⎢ − sin q2 cos q2 0 0 0 a2 cos q2 ⎤ ⎥ 0 a2 sin q2 ⎥ 1 0 0 1 ⎦ (2.2) ♦ Ma trận mô tả vị trí và hướng của O3x3y3z3 đối với O2x2y2z2 : 2 H3 2 ⎡cos q3 ⎢ ⎢ sin q3 H3 = ⎢ ⎢ 0 ⎢ − sin q3 cosá 3 cos q3 cosá 3 sin á 3 0 sin q3 sin á 3 − cos q3 sin á 3 cosá 3 0 a3 cos q3 ⎤ ⎥ a3 sin q3 ⎥ ⎥ d3 ⎥ 1 ⎦ 2 ⎡cos q3 ⎢ ⎢ sin q3 H3 = ⎢ ⎢ 0 ⎢ − sin q3 cos q3 0 0 0 a3 cos q3 ⎤ ⎥ 0 a3 sin q3 ⎥ 1 0 0 1 ⎦ (2.3) - 28 -⎣ 0 ⎥ ⎣ 0 ⎥ ⎣ 0 ⎥ ⎥ ⎥ ⎣ 0 ⎥ ⎣ 0 ⎥ ⎥ ⎥ Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR ♦ Ma trận mô tả vị trí và hướng của O4x4y4z4 đối với O3x3y3z3 : 3 H4 3 H4 = ⎡cos q4 ⎢ ⎢ sin q4 ⎢ ⎢ 0 ⎢ − sin q4 cosá 4 cos q4 cosá 4 sin á 4 0 sin q4 sin á 4 − cos q4 sin á 4 cosá 4 0 a4 cos q4 ⎤ ⎥ a4 sin q4 ⎥ ⎥ d 4 ⎥ 1 ⎦ 3 ⎡cos q4 ⎢ ⎢ sin q4 H4 = ⎢ ⎢ 0 ⎢ − sin q4 cos q4 0 0 0 a4 cos q4 ⎤ ⎥ 0 a4 sin q4 ⎥ 1 0 0 1 ⎦ (2.4) Từ các ma trận 0H1, 1H2, 2H3, 3H4 được xác định theo công thức (2.1), (2.2), (2.3), (2.4) ta tính được ma trận mô tả vị trí và hướng của khâu thao tác trong hệ tọa độ cố định O0x0y0z0 theo công thức: T4 =0H1. 1H2. 2H3. 3H4 (2.5) Giả sử robot cần thực hiện thao tác đối với đối tượng như hình vẽ ( hình 2.2). Ta sử dụng hệ tọa độ Od xd yd zd gắn vào đối tượng. Khi đó ma trận mô tả vị trí và hướng của Od xd yd zd trong hệ tọa độ cố định O0 xyz0 là ma trận: 0 A d Ma trận mô tả vị trí và hướng của khâu thao tác trên đối tượng đối với hệ tọa độ Od xd yd zd là ma trận: d A f . Vậy ta có 0 A f = 0 A d .d A f chính là ma trận mô vị trí và hướng của khâu thao tác trên vật đối với hệ tọa độ cố định. - 29 -⎣ 0 ⎥ ⎣ 0 ⎥ ⎥ ⎥ 0 0 Đồ án tốt ngghiệp  Tínhh toán chhuyển động chương trình và thiết kế robott MMR z0 yd Đối tượng thao tác xd x0  zd Hình 2.2 Theo cơ sở lý thuuyết đã trìnnh bày ở chhương 1 taa có : T4 = 0 A f (2.6) ⎡ A ⎣ 0 p 4 ⎤ 1 ⎥⎦  vvà  0 ⎡C ⎣ 0 r f ⎤ 1 ⎥⎦ Từ đây ta rút ra 6 phương trình gồm 10 tham số: fá (x) = 0 (2.7) x = [q1 q 2 q3 q 4 xp yp zp rotxp rotyp rotzp] (2.8) Vì robot có 4 bậc tự do ta cchỉ thực hiện điều khiển chuyển động của robbot với 4 thham số ở đây cho quyy luật của điểm tác động: xp = xp(t ), ypp = yp(t ), zp = zp(t ) vvà một 1 thham số xácc định hướng cua khââu thao tác có thể ho trước otyp = 0 . Từ đó giải 6 phương trình 6 ẩn số. 30 Trong đó T4 = ⎢ 4 A f = ⎢ d c, ch ro Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR Các phương trình động học của robot MMR như sau: f1 := 0.13 (cos (q1) cos (q2) cos (q3) - cos (q1) sin(q2) sin(q3)) cos (q4) + 0.13 (-cos(q1) cos(q2) sin(q3) - cos(q1) sin(q2) cos(q3)) sin(q4) + 0.263 cos(q1) cos(q2) cos(q3) - 0.263 cos (q1) sin(q2) sin(q3) + 0.283 cos (q1) cos (q2) + 0.045 cos (q1) - xp f2 := 0.13 (sin (q1) cos (q2) cos (q3) - sin (q1) sin (q2) sin (q3)) cos (q4) + 0.13 (-sin(q1) cos(q2) sin(q3) - sin(q1) sin(q2) cos(q3)) sin(q4) + 0.263 sin(q1) cos(q2) cos(q3) - 0.263 sin (q1) sin (q2) sin (q3) + 0.283 sin (q1) cos (q2) + 0.045 sin (q1) - yp f3 := 0.13 (sin(q2) cos(q3) + cos(q2) sin(q3)) cos(q4) + 0.13 (-sin(q2) sin(q3) + cos(q2) cos(q3)) sin(q4) + 0.09 + 0.263 sin (q2) cos (q3) + 0.263 cos (q2) sin (q3) + 0.283 sin (q2) - zp f4 := -(cos (q1) cos (q2) cos (q3) - cos (q1) sin (q2) sin (q3)) sin (q4) + (-cos (q1) cos (q2) sin (q3) - cos (q1) sin (q2) cos (q3)) cos (q4) + cos (rotyp ) sin (rotzp ) f5 := -cos (q 1 ) + sin (rotxp ) cos (rotyp ) f6 := (sin (q2) cos (q3) + cos (q2) sin (q3)) cos (q4) + (-sin (q2) sin (q3) + cos (q2) cos (q3)) sin (q4) + cos (rotxp ) sin (rotyp ) cos (rotzp ) - sin (rotxp ) sin (rotzp ) (2.9) 2.2 Bài toán vị trí 2.2.1 Bài toán thuận. ♦ Biết trước giá trị của biến khớp (q1,q2,q3,q4) ♦ Yêu cầu tìm các toạ độ của khâu cuối ( xp, yp, zp, rotxp, rotyp, rotzp). Vị trí của điểm tác động cuối lên đối tượng cần thao tác được xác định bởi toạ độ điểm P(xp, yp, zp), hướng của nó được xác định bởi các góc quay (rotxp, rotyp,rotzp). Theo hệ phương trình (2.6): - 31 - Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR Ba phương trình đầu xác định được vị trí của đối tượng: ⎧ xp = 0 A f [1,4]=T4 [1,4] ⎪ 0 ⎪ 0 (2.10) Ba phương trình sau cho ta bài toán xác định hướng của điểm tác động cuối lên đối tượng: ⎧ f 4 = T4 [1,2]- 0 A f [1,2] ⎪ 0 ⎪ f 6 = T4 [3,1]- 0 A f [3,1] (2.11) Ba phương trình f 4 , f5 , f6 đã được tính ở phần trên theo (2.9): f4 := -(cos (q1) cos (q2) cos (q3) - cos (q1) sin (q2) sin (q3)) sin (q4) + (-cos (q1) cos (q2) sin(q3) - cos (q1) sin(q2) cos (q3)) cos (q4) + cos (rotyp ) sin(rotzp ) f5 := -cos (q 1 ) + sin (rotxp ) cos (rotyp ) f6 := (sin (q2) cos (q3) + cos (q2) sin (q3)) cos (q4) + (-sin (q2) sin (q3) + cos (q2) cos (q3)) sin (q4) + cos (rotxp ) sin (rotyp ) cos (rotzp ) - sin (rotxp ) sin (rotzp ) Giải các phương trình trên ta sẽ tính được hướng của hệ tọa độ khâu thao tác đối với hệ tọa độ cố định. 2.2.2 Bài toán ngược Bài toán ngược là bài toán có ý nghĩa rất quan trọng trong thực tế. Khi biết quy luật chuyển động của khâu thao tác và ta phải tìm các giá trị của biến khớp. Việc xác định các giá trị của biến khớp cho phép ta điều khiển robot theo đúng quỹ đạo đã cho. Từ trên theo (2.7) ta đã có 6 phương trình với 10 tham số: fá (x) = 0 - 32 -⎨ yp = A f [2,4]=T4 [2,4] ⎩ zp = A f [3,4]=T4 [3,4] ⎨ f5 = T4 [2,3]- A f [2,3] ⎩ Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR x = [q1 q 2 q3 q 4 xp yp zp rotxp rotyp rotzp] Vì vậy ta phải biết trước 4 tham số hay còn gọi là biến điều khiển. Với mô hình robot MMR này ta cho biết trước [xp yp zp rotyp] . Các phương trình trên đều là các phương trình đại số phi tuyến do đó để giải các phương trình này ta dùng phương pháp lặp Newton-Raphson . 2.3 Bài toán vận tốc 2.3.1 Bài toán thuận Ta có thể viết lại phương trình (2.7) ở dạng sau: f(p,q) = 0 (2.12) Trong đó: p: là vector chứa thông số của điểm tác động cuối: p = [xp yp zp rotpx rotyp rotzp] q : Là véctơ có các thành phần là các tọa độ điều khiển: q = [q1 q2 q3 q4] Đạo hàm hai vế của phương trình (2.12) theo thời gian ta được: 6 i =1  ∂fá ∂pi  p& i = −  4 k =1 k  k  á = 1..6 (2.13) Có thể viết: - 33 -∑ ∂fá ∑ ∂q q& Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR ⎡ ∂f1 1 ⎢ ∂f 2 1 ⎢ ⎢⎣ ∂p1  ∂f1 ∂p2 ∂f 2 ∂p2 M ∂f 6 ∂p2  1 L ⎥ ⎢ ⎥ ⎢ L M ⎥ ⎢ ⎥ ⎢ M L  ⎢ ⎢  −  − − −  ∂f1 ∂q2 ∂f 2 ∂q2 M ∂f 6 ∂q2  − − −  ∂f1 ∂q3 ∂f 2 ∂q3 M ∂f 6 ∂q3  − − −  ∂f1 ⎤ q&2 3 ⎥ q& ∂q4 ⎥⎦ Đặt (2.14) ⎡ ∂f1 1 ⎢ ∂f 2 ⎢ ⎢ ⎢⎣ ∂p1 ∂f1 ∂p2 ∂f 2 ∂p2 M ∂f 6 ∂p2  L L L  ⎥ ⎥ ⎥ ⎡ ∂f1 1 − ⎢ ⎢⎣ ∂q1  − − − ∂f1 ∂q2 ∂f 2 ∂q2 M ∂f6 ∂q2  − − − ∂f1 ∂q3 ∂f 2 ∂q3 M ∂f 6 ∂q3  − − − ∂f1 ⎤ ⎥ ∂f 2 ⎥ ⎥ ⎥ ∂q4 ⎥⎦ (2.15) Thế vào được phương trình: J p .p = J q .q (2.16) Hay - 34 -⎢ ∂p ⎢ ⎢ ∂p ⎢ ⎢ M ⎢ ∂f 6 ∂f1 ⎤ ⎡ ∂f1 L ∂p6 ⎥ ⎢ ∂q1 ⎥ ⎡ p& ⎤ ∂f 2 ⎥ ⎢ ⎥ ⎢ ∂f 2 ⎥ ⎢ p& 2 ⎥ = ⎢− ∂q ∂p6 1 M ⎥ ⎢⎣ p& 6 ⎥⎦ ∂f 6 ⎥ ⎢− ∂f6 ∂p6 ⎥⎦ ⎣⎢ ∂q1 ∂q4 ⎥ ⎥ ⎡ q&1 ⎤ ∂f 2 ⎥ ⎢ ⎥ ∂q4 ⎥ ⎢ ⎥ ⎥ ⎢ q& ⎥ M ⎥⎢ ⎥ ∂f 6 ⎥ ⎣ 4 ⎦ ⎢ ∂p ⎢ J p = ⎢ ∂p1 ⎢ M ⎢ ∂f 6 ∂f1 ⎤ ∂p6 ⎥ ∂f 2 ⎥ ∂p6 ⎥ L M ⎥ ∂f6 ⎥ ∂p6 ⎥⎦ ⎢− ∂q ⎢ ⎢ ∂f 2 J q = ⎢⎢ ∂q1 ⎢ M ⎢− ∂f 6 ∂q4 ⎥ ∂q4 ⎥ M ⎥ ∂f6 ⎥ & & Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR 1 (2.17) Trong đó J −1.J q = J f (2.18) 2.2.2 Bài toán ngược Ta có thể viết (2.7) dưới dạng sau: f (p* , s) = 0 (2.19) Trong đó : p* = ⎡⎣ p1*  p2*  p3*  p4*  p5*  p6* ⎤⎦ T = [q1 q2  q3  q4 T s = [ s1  s2  s3  T = [ xp  yp  T Đạo hàm phương trình (2.19) theo thời gian ta được: 6 i =1 ∂fá * ∂pi* 4 k =1 k  k  á = 1..6  (2.20) Có thể viết: ⎡ ∂f1 1 ⎢ ∂f 2 1 ⎢ ⎢⎣ ∂p1* ∂f1 ∂p2* ∂f 2 ∂p2* M ∂f6 ∂p2*  L * ⎥ L * ∂f1 ⎤ ⎡ ∂f1 1 − ∂p6 1s ⎥ ⎣ p6 ⎦ ⎢  − − − ∂f1 ∂s2 ∂f 2 ∂s2 M ∂f6 ∂s2  − − − ∂f1 ∂s3 ∂f 2 ∂s3 M ∂f6 ∂s3  − − − ∂f1 ⎤ s&2 3 ⎥ s& ∂s4 ⎥⎦ - 35 -p = J − .J q .q = J f .q p rotxp rotzp] s4 ] zp rotyp] ∑ p& i = − ∂fá ∑ ∂s s& ⎢ ∂p* ⎢ ⎢ ∂p* ⎢ ⎢ M ⎢ ∂f 6 ⎢ − ∂s ∂p6 ⎥ ⎡ p&1* ⎤ ⎢ ∂f 2 ⎥ ⎢ * ⎥ ⎢ ∂f 2 L p& 2 = ⎢⎢ ∂ * ⎥⎢ ⎥ ⎥⎢ M ⎥ L M ⎥⎢ * ⎥ ⎢ M ∂f 6 ⎥ ⎢− ∂f 6 ⎢⎣ ∂s1 ∂p6 ⎥⎦ ∂s4 ⎥ ⎥ ⎡ s&1 ⎤ ∂f 2 ⎥ ⎢ ⎥ ∂s4 ⎥ ⎢ ⎥ ⎥ ⎢ s& ⎥ M ⎥⎢ ⎥ ∂f 6 ⎥ ⎣ 4 ⎦ & Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR Đặt: (2.21) ⎡ ∂f1 1 ⎢ ∂f 2 ⎢ ⎢ ⎢⎣ ∂p1*  * *  ∂p  ⎥ ⎥ ⎡ ∂f1 1 − ⎢ ⎢ ⎢⎣ ∂s1  − − − ∂f1 ∂s2 ∂f 2 ∂s2 M ∂f6 ∂s2  − − − ∂f1 ∂s3 ∂f 2 ∂s3 M ∂f 6 ∂s3  − − − ∂f1 ⎤ ⎥ ∂f 2 ⎥ ⎥ ⎥ ∂s4 ⎥⎦ (2.22) Thế vào ta nhận được phương trình: J *p .p& * = J s .s& (2.23) Hay * (2.24) Trong đó J *−1.J s = J inv (2.25) - 36 -⎢ ∂p* ⎢ J *p = ⎢ ∂p1* ⎢ M ⎢ ∂f 6 ∂f1 ∂f1 ⎤ ∂p2 6 * L * ⎥ ⎥ ∂f 2 ∂f 2 ⎥ ∂p2 6* ⎥ L ∂p M L M ⎥ ∂f6 6f ⎥ ∂ ∂p2 6* ⎥⎦ L ∂p ⎢ − ∂s ⎢ ⎢ ∂f 2 J s = ⎢ ∂s1 ⎢ M ⎢− ∂f 6 ∂s4 ⎥ ∂s4 ⎥ M ⎥ ∂f 6 ⎥ p& = J* −p1.J s .s& = J inv .s& p Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR 2.3Bài toán gia tốc 2.3.1 Bài toán thuận Đạo hàm phương trình (2.13) theo thời gian ta có: 6 i =1 ∂fá ∂pi  &&pi + 6 6 j =1 i=1 ∂fá2 ∂pi .∂p j  pi . p j = − 4 k =1 ∂fá ∂qk  q&&k − 4 4 l =1 k =1 ∂fá2 ∂ql .∂qk  ql .qk (2.26) á = 1..6 Hay có thể viết (2.26) ở dạng: J p .p&& = g (2.27) Với J d xác định theo công thức (2.14) g = [g1 g 2 g3 g 4 g5 g 6 ] Ở đây: gá = − 6 6 j =1 i =1 ∂fá2 ∂pi .∂p j  pi . p j − 4 k =1 ∂fá ∂qk  q&&k − 4 4 l =1 k =1 ∂fá2 ∂ql .∂qk  ql .qk (2.28) Từ hệ thức (2.27) ta nhận được: p&& = J −p1.g 2.3.2 Bài toán ngược Đạo hàm hệ phương trình (2.20) theo thời gian ta được 6 6 j =1 i=1 ∂ 2 fá * *  p& i*. p& *j + 6 i =1 ∂fá * ∂pi 4 4 l =1 k =1 ∂ 2 fá ∂sk ∂sl  s&l s&k − 4 k =1 ∂fá ∂sk  &&s k (2.29) Với á = 1..6 Có thể viết (2.29) dưới dạng: - 37 -∑ ∑∑ ∑ ∑∑ & & & & ∑∑ ∑ ∑∑ & & & & ∑∑ ∂pi ∂p j ∑ &&pi = − ∑∑ ∑ Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR J*p .p&&* = g*  (2.30) J*p được tính theo công thức (2.21) g = [ g1 g 2 g3 g 4 g5 g6 ] *  6 6 j =1 i=1  ∂ 2 fá * *  *  4 4 l =1 k =1  ∂ 2 fá ∂sk ∂sl  s&l s&k −  4 k =1 k  k  (2.31) Từ (2.30) ta có : p&&* = J* −p1.g* 2.4 Chuyển động chương trình của robot MMR 2.4.1 Robot thao tác trong quá trình đóng gói sản phẩm Tính toán cụ thể với robot thực. Tay robot di chuyển từ vị trí A(xA,yA,zA) trong không gian đến vị trí B(xB,yB,zB) để gắp sản phẩm, sau đó mang sản phẩm từ B đến C(xc, yc,zc) cho vào thùng đóng gói như hình 2.3 - 38 -gá = − ∑∑ ∂pi ∂p j p& i j* − . p& ∑∑ ∂fá ∑ ∂s &&s Đồ án tốt ngghiệp  Tínhh toán chhuyển động chương trình và thiết kế robott MMR Hình 2.3 Giả sử robot đi từ A đến B theo một đường thẳnng.Ta có phhương trìnnh đường thẳng AB có dạng sau: x − xA xB − xA  = y − y A yB − y A  = z − z A zB − z A (2.32) Cho roobot chuyển động trong thời giaan là 20s sẽ đi từ A(880,139,5966) đến B(300,2550,300). Tại vị trí A các khớp q1= 600, q2= 1200, q3=-600, q4= 450. Từ (2.332) ta có pphương trìnnh AB ( hìnnh 2.4) 39 Đồ án tốt ngghiệp  Tínhh toán chhuyển động chương trình và thiết kế robott MMR Hìnhh 2.4 x − 80 300 − 80  = y − 139 250 − 1399  = z − 0,1132 300 − 5596  = t 20  (2.33) Rút gọn lại ta được : ⎧ x = 11.t + 80 ⎪ ⎪ z = −14.8 * t + 5996 (2.34) Sau kh robot đi từ A đến B gắp sản phẩm và tiếp tục đi từ B đến C theeo một cunng tròn giả sử cùng tròòn là nửa đường trònn đường kínnh BC. Chho tọa độ điểm C(300,50,1100). Nhận thấy BC nằm trong mặt phẳng vuông góc với trục Ox (hình 2.5). 40 Ä1⎨ y = 5,555.t + 139 ⎩ hi Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR Hình 2.5 Ta viết phương trình cung BC trong mặt phẳng vuông góc với trục Ox có dạng sau : ( y − 150)2 + ( z − 200)2 = 100 2 (2.35) Hay: 2 2 + ⎝ 100 2 ⎠ ⎝ 100 2 ⎠ (2.36) Đặt y 100 2  = sin(at + b) z − 200 100 2 = cos(at + b) (2.37) Ta cho chuyển động của khâu thao tác đi từ B đến C trong thời gian 20s. Tìm được Ä2 có dạng như sau: - 41 -⎛ y − 150 ⎞ ⎛ z − 200 ⎞ ⎜ ⎟ ⎜ ⎟ = 1 Đồ án tốt ngghiệp  Tínhh toán chhuyển động chương trình và thiết kế robott MMR ⎧ ⎪ x = 300 ⎪ Ä2 ⎨ y = 100 2 sin( ⎪ ⎪ ⎪⎩  ð .t 40 ð .t 40  + +  ð 4 ð 4  ) + 1500 )+200  (2.38)) Vậy khhi robot di chuyển để thực hiện công việc đi từ A đến B sau đó từ B đến C thhì quỹ đạo của điểm ttác động cuuối có dạnng như hìnhh 2.6 Hình 2.66 2.44.2 Robot thực hiện một công việc trên bề mặt ch tiết Giả sử robot MM cần phải hàn một bề mặt theo một quỹ đạo hình elipp cho trước ( hình 2.77) 42 ⎪ z = 100 2 cos( hi MR Đồ án tốt ngghiệp  Tínhh toán chhuyển động chương trình và thiết kế robott MMR Hình 2.7 Hìình 2.8 43 Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR Phương trình quỹ đạo khâu thao tác có dạng : - 44 -  ⎪ ⎪ z = 130 ⎪ ⎧ ð 10  (2.39)⎪ x = 60sin(10 t ) + 417 ⎪ Ä3 ⎨ y = 80sin( t ) + 417 ⎪ ⎩ Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR CHƯƠNG 3 XÂY DỰNG PHẦN MỀM TÍNH TOÁN VÀ MÔ PHỎNG Để giải quyết bài toán cơ học ta có thể sử dụng nhiều phần mềm tính toán khác nhau. Trong đồ án này sử dụng Maple để tính toán và ghi két quả ra File , sau đó dùng Visual C++ để đọc kết quả và mô phỏng. 3.1 Phần mềm ứng dụng tính toán 3.1.1 Giới thiệu về Maple Maple là một phần mềm được phát triển ở trường đại học Waterloo ở Canada từ năm 1990 và phát triển tiếp tục. Đây là một phần mềm rất thích hợp dùng cho PC. Maple là một môi trường tính toán số và chữ và các ứng dụng đồ hoạ của toán học. Nó không chỉ thuần túy là môi trường tính toán mà còn là một ngôn ngữ lập trình dạng biên dịch. Maple cho phép người sử dụng có thể triển khai các ứng dụng một cách nhanh chóng. Một đặc điểm nổi bật nhất của Maple mà hầu như không có một ngôn ngữ nào hiện nay có được chính là khả năng thay thế việc tính toán biến đổi bằng tay bằng việc tính toán biến đổi bằng máy. Khả năng này cực kỳ linh hoạt và phong phú. Điều này làm cho Maple trở nên rất hấp dẫn người sử dụng đặc biệt là những người làm về kỹ thuật. Maple có thể giải quyết rất nhiều vấn đề của toán học như đại số ma trận, vector, giải hệ phương trình phi tuyến, hệ phương trình tuyến tính, tích phân, giải phương trình vi phân thường, phương trình đạo hàm riêng, giải các bài toán về trị riêng, vector riêng, các bài toán đa thức…Nói chung tất cả các lĩnh vực của toán học cổ điển cũng như hiện đại đều có thể tìm thấy trên Maple. Điều này giúp chúng ta có thể giảm thiểu thời gian tính toán, dành nhiều thời gian cho việc hoàn thiện mô hình và đánh giá kết quả. - 45 - Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR Cửa sổ làm việc chính của Maple: Giao diện của Maple cũng giống như giao diện của các chương trình ứng dụng khác trên Windows. Tuy nhiên Maple là chương trình thiên về tính toán, nên nó cũng có một số chức năng đặc thù riêng. Maple là một hệ thống mở, nó cho phép ta tạo ra những ứng dụng riêng mới dựa trên những cái có sẵn rất. Nó cung cấp rất nhiều các thư viện chuẩn. Cú pháp gọi thư viện: [> with(library_name); library_name : Là tên thư viện cần gọi. Trong Maple có rất nhiều thư viện như: ♦ Linalg: Thư viện chương trình đại số tuyến tính. ♦ Plots: Thư viện đồ họa vẽ đồ thị hai chiều và ba chiều. ♦ Plottools: Thư viện đồ họa cung cấp các đối tượng hai chiều và ba chiều như hình trụ, hình cầu, hình nón… - 46 - Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR a) Một số lệnh cơ bản Các lệnh cơ bản như cộng, trừ, nhân, chia ma trận, vector có kí hiệu như toán học thông thường dễ sử dụng. ♦ [> restart: Lệnh này thường dùng khởi đầu một chương trình Maple. Sau lệnh này các biến dùng trước nó không còn. ♦ [>multiply(A,B): Lệnh nhân hai ma trận. Trong thư viện linalg [>restart; with(linalg): > A := array( [[1,2],[3,4]] ); B := array( [[0,1],[1,0]] ); C := array( [[1,2],[4,5]] ); multiply(A, B, C); ⎡1 2⎤ ⎥ ⎡0 1⎤ ⎥ ⎡1 2⎤ ⎥ ⎡ 6 9 ⎤ ⎢16 23⎥ ♦ [>inverse(A): Lệnh tính ma trận nghịch đảo của A. > A:=array(1..2,1..2,[[1,2],[3,4]]); ⎡1 2⎤ ⎥ > inverse(A); ⎡−2 1 ⎤ ⎢ − ⎥ ⎣ 2 2 ⎦ - 47 -A =⎢ ⎣3 4⎦ B =⎢ ⎣1 0⎦ C =⎢ ⎣4 5⎦ A =⎢ ⎣3 4⎦ ⎢ 3 1 ⎥ ⎣ ⎦ Đồ án tốt ngghiệp  Tínhh toán chhuyển động chương trình và thiết kế robott MMR ♦ [>diff((f(x),x): Đạo hàm của hàm f(x) theo x. > ddiff(x*sin(coos(x)),x); sin ( cos ( x) ) - x cos ( cos ( x)) sin ( x) ♦ Subs(vvar(1)=rep((1),…,var((n)=rep(n),,expr): Lệnnh thay thế giá trị vàào một biểu thức. > suubs( x=2, x^^2+x+1 ); 7 ♦ [>fsolvve(eqns, vaars, option) Giải hệ phương trìình phi tuyến. eqns: Tập các phương trình của hệ var: Tập các biến của hệ. option: Các lựa chhọn cho viiệc giải > f := sin(x+y) - exp(x)*y = 0; g := x^2 - y = 2; fsollve({f,g},{x,y},{x=-1..1,yy=-2..0}); f := sinn(x + y) - e x y = 0 g := x 2 - y = 2 {x = -0.6687012050, y = -1.5552838698} ♦ [plot(): Dùng để vẽ đồ thị [> plot([sin(x) , x-x^3/6], x=0..2, colorr=[red,blue] style=[poiint,line]); 48 ): Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR b) Các kiểu dữ liệu cơ bản Để nắm được Maple phải nắm vững các kiểu dữ liệu của nó. Đây cũng là một ưu điểm nổi bật của Maple so với ngôn ngữ khác. Các kiểu dữ liệu này giúp cho việc lập trình như tính toán trở lên cực kỳ linh hoạt. Một số kiểu hay dùng: ♦ Kiểu tuần tự (sequences) đây là một kiểu đơn giản nhất của Maple, đó là một nhóm các biểu thức được viết cách nhau bởi dấu phẩy: > seq( i^2, i=1..5 ); 1, 4, 9, 16, 25 ♦ Kiểu liệt kê ( Lists) khác với kiểu tuần tự, các thành phần của danh sách bị bao bởi cặp dấu ngoặc vuông “[” và “]” các phần tử cách nhau bởi dấu phẩy: > L := [1,[2,3],[4,[5,6],7],8,9]; L := [1, [2, 3], [4, [5, 6], 7], 8, 9] ♦ Kiểu tập hợp( sets) khác với kiểu tuần tự, các phần tử của tập hợp bị bao bởi cặp các dậu móc nhọn “{“ và ”}” , cũng cách nhau bởi dấu phẩy. Ý nghĩa của kiểu tập hợp rất giống trong toán học: > S := {v,w,x,y,z}; S := {x, y, v , w, z} ♦ Kiểu mảng (arrays) đây là kiểu dữ liệu thông dụng như trong các ngôn ngữ lập trình khác. Cú pháp : name:=array(…) > A:=Array([[1,2,3],[4,5,6]]); ⎡1 2 3⎤ ⎥ ♦ Kiểu bảng (table) giống kiểu record trong pascal hoặc kiểu Struct trong C. - 49 -A =⎢ ⎣4 5 6⎦ Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR > S := table([(2)=45,(4)=61]); S := table ([2 = 45 , 4 = 61 ]) ♦ Kiểu xâu ký tự (String) để khai báo một xâu ký tự ta sử dụng hai dấy phẩy kép: > myname:="Do Viet Hung"; myname := "Do Viet Hung" c) Lập trình trong Maple ♦ Các loại toán tử thường sử dụng khi lập trình: ♦ Các kiểu dữ liệu đã trình bày ở trên. ♦ Lệnh rẽ nhánh if if then else end if; Hoặc sử dụng nhiều lệnh rẽ nhánh if then elif then else - 50 -Toán tử Ký hiệu Số học +, -, *, /, ^, ** Quan hệ , =, , = Logic and, or, not, xor Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR end if; Ví dụ > a := 3; b := 5; a := 3 b := 5 > if (a > b) then a else b end if; 5 ♦ Lệnh for for from by to while do end do; Ví dụ: [>for i from 6 by 2 to 100 do print(i) end do; ♦ Lệnh while while do end do; Ví dụ: [>tot := 0; for i from 11 by 2 while i < 100 do tot := tot + i end do; ♦ Xây dựng hàm thủ tục: proc Hàm và thủ tục thường hay được sử dụng để cho chương trình sáng sủa và ngắn gọn. Ý nghĩa của nó hoàn toàn giống hàm thủ tục trong bất kỳ ngôn ngữ lập trình nào. - 51 - Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR Name:=proc(agseq)::type local var1::type1, var2::type2, …; global nseq; options nseq; desription stringseq; statseq return … End proc; ♦ Thao tác với File: fopen, fclose, fprintf, fscanf, readline, readdata. Đặc điểm của File là có thể lưu bất kỳ kiểu dữ liệu với kích thước không hạn chế. Có hai loại File: dạng BINARY và dạng TEXT. Việc sử dụng các loại File này tùy thuộc vào cách thức tổ chức dữ liệu cũng như yêu cầu của từng bài toán. 3.1.2 Giải bài toán thuận và bài toán ngược Phần trên ta đã thiết lập cách xây dựng các phương trình của robot. Trong phần này ta trình bày cách giải bằng phần mềm Maple. - 52 -fopen Mở File fclose Đóng File fprintf Ghi dữ liệu từ File fscanf Đọc dữ liệu từ File readline Đọc một dòng dữ liệu từ File readdata Đọc một dòng dữ liệu từ File có cấu trúc đã định sẵn Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR ♦ Mục dữ liệu dùng lệnh đọc read để đọc File data và File Pro_matrix. File data.txt Unknowns := [q[1], q[2], q[3], q[4],rotxp,rotzp]; là các biến cần phải đi xác định trong bài toán ngược của vị trí. InitrP := [xp, yp, zp,rotxp, rotyp,rotzp]; Vector mô tả vị trí và hướng của điểm tác động cuối trong hệ tọa độ cố định O0x0y0z0. Init_1:=[1.047197551,2.094395103,-1.047197551, - .7853981635,0.538739,0.13318]: Điều kiện đầu của bài toán, hay chình la giá trị của các biến khớp của robot trước khi làm việc. #------------------ Quy luật chuyển động của khâu thao tác---------- TRAEK_P0:=[11*t+80,5.5*t+139,-14.8*t+596]; Quỹ đạo ta xây dựng trong chương 2 ## THONG SO DONG HOC DENAVIT-HATENBERG ## robot:=table([Init= [0,0,0,0,0,0], DH =[[theta[1],d[1],a[1],alpha[1]], - 53 - Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR [theta[2],d[2],a[2],alpha[2]], [theta[3],d[3],a[3],alpha[3]], [theta[4],d[4],a[4],alpha[4]]], flatform=[xp,yp,zp,rotx,roty,rotz]]); bảng động học Denavit- Hartenberg của robot. #----------Khâu1------------- d[1]:=90; a[1]:= 45; alpha[1]:= Pi/2; #-----------khâu 2----------- d[2]:=0; a[2]:=283; alpha[2]:=0; #----------khâu 3------------- d[3]:=0; a[3]:=263; alpha[3]:=0; #----------khâu 4------------- d[4]:=0; a[4]:=130; alpha[4]:=0; #--------------Các biến khớp------------------------------ theta[1]:=q[1]; theta[2]:=q[2]; theta[3]:=q[3]; theta[4]:=q[4]; #------------------------------------------------------------- dt:=0.2; Thời gian mỗi bước của robot khi di chuyển. tg:=0.0; T_:=20.0; Thời gian robot thực hiện thao tác. MaxLoop:=100; Số vòng lặp tối đa trong thuật giải Newton-Raphson. - 54 - Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR AbsErr:=0.0001; Điều kiện để hội tụ của thuật giải Newton-Raphson. ` Pro_matrix.txt Gồm các thủ tục tính ma trận Denavit-Hartenberg, Cardan, ghi File kết quả. # thủ tục tính ma trận DH Dmat:=proc(theta,d,a,alpha) local A; A:=matrix(4,4); A[1,1]:=cos(theta); A[1,2]:=-sin(theta)*cos(alpha); A[1,3]:=sin(theta)*sin(alpha); A[1,4]:=a*cos(theta); #------------------------------------------ A[2,1]:=sin(theta); A[2,2]:=cos(theta)*cos(alpha); A[2,3]:=-cos(theta)*sin(alpha); A[2,4]:=a*sin(theta); #------------------------------------------ A[3,1]:=0; A[3,2]:=sin(alpha); A[3,3]:=cos(alpha); A[3,4]:=d; #------------------------------------------ A[4,1]:=0; A[4,2]:=0; A[4,3]:=0; A[4,4]:=1; return (A); end; # thủ tục tính ma trận Cardan Cardan:=proc(x,y,z,alpha,Psi,theta) local A; A:=matrix(4,4); - 55 - Đồ án tốt nghiệp  Tính toán chuyển động chương trình và thiết kế robot MMR A[1,1]:=cos(Psi)*cos(theta); A[1,2]:=-cos(Psi)*sin(theta); A[1,3]:=sin(Psi); A[1,4]:=x; #--------------------------