a- Mô tả và phân tích mô phỏng động học vẽ vùng hoạt động Robot Kuka
Để vẽ (xác định) ra đƣợc vùng hoạt động nhƣ trên hình 3.7 điểm P sẽ chuyển động theo trình tự sau:
Chuyển động chuẩn bị, theo trình tự: vị trí Oo vị trí Oo’ vị trí 1.
Chuyển động vẽ dạng vùng hoạt động, theo trình tự: vị trí 1 vị trí 2 vị trí 3
vị trí 4 vị trí 5 vị trí 6 vị trí 7 vị trí 1.
Chuyển động kết thúc về thế chuẩn ban đầu: vị trí 1 vị trí Oo’ vị trí Oo. Nhƣ vậy từ lúc bắt đầu cho đến khi kết thúc, chuyển động của điểm P nhƣ sau:
Từ vị trí Oo vị trí Oo’ vị trí 1 vị trí 2 vị trí 3 vị trí 4 vị trí 5 vị trí 6 vị trí 7 vị trí 1 vị trí Oo’trở về vị trí Oo và kết thúc.
Quay khâu 1 (quay cả hình chiếu đứng) quanh trục OZ một góc 228o
tạo ra vùng hoạt động của Kuka KR6-ARC (hình chiếu bằng).
Luận văn Cao học Trường ĐHBK Hà Nội
Học viên: Nguyễn Bá Ân Page 50
Sau đây là các bƣớc xác định vị trí của robot trong mặt phẳng OYZ ở các giai đoạn chuyển động:
Chuyển động chuẩn bị:
Xác định toạ độ các vị trí chuẩn (thế) ban đầu của robot:
+ Điểm P (vị trí Oo): yOo = (260 + 670) = 930, zOo = 35 + 680 + 435= 1150 + Tâm khớp nối khâu 3 và 2 ở vị trí O00: y00 = 260, z00 = 435 + 680 = 1115. + Tâm khớp nối khâu 1 và 2 ở vị trí O1: y01 = 260, z 01 = 435.
Xác định thế của robot ở vị trí Oo’: từ vị trí chuẩn Oo, quay đồng thời khâu 3 và 4 quanh tâm O1 một góc -90o
(ngƣợc chiều kim đồng hồ) đến vị trí Oo’, bán kính RO’o = (6702 + 352)1/2 = 670,9. Khi đó, toạ độ các vị trí ở thế Oo’ sẽ là:
+ Điểm P ở vị trí Oo’: y0’o = (260 - 35) = 225 ; z0’o = 435 + 680+ 670 = 1785. + Tâm khớp nối khâu 3 và 2 ở vị trí O00: yO0 = 260; z O0 = 1115
+ Toạ độ tâm khớp nối khâu 1 và 2 ở vị trí O1: y01 = 260, z 01 = 435 (nhƣ cũ). Quỹ tích điểm P thuộc phƣơng trình đƣờng tròn trong mặt phẳng OYZ có dạng: (y0’ - 260)2 + (z0’ - 435)2 = 670,92 (3.1) Vẽ cung tròn (5.1) từ vị trí Oo đến Oo’ nhƣ sau:
+ Thay toạ độ các điểm trên cung tròn (y0’, z0’)bằng toạ độ các biến chạy (yj, zj), j = 1÷ n. Ở cung tròn này ta có:
zO’o ≤ zj ≤ zOo hay zj[zO’o,zOo ], j = 1÷ n (3.2) yOo ≤ yj ≤ yO’o hay yj[yO’o,yOo ], j = 1÷ n (3.3)
(y0’o = (260 - 35) = 225 ; z0’o = 435 + 680+ 670 = 1785.) (yO0 = 260; z O0 = 1115 )
+ Chia đoạn [zO’o,zOo ] ra (n-1) phần bằng nhau, giá trị mỗi đoạn chia là h:
h = (zO’o- zOo)/(n-1) = (1785 - 1115 )/(n-1) = 670/(n-1) (3.4) + Khi zj tăng từ zOo đến zO’o thì zj đƣợc tính:
zj = zOo - (j-1).h = 1115 - (j-1).670/(n-1), j = 1÷ n (3.5) + Và yj (j = 1÷ n) giảm từ yOo đến yO’o, khi đó, từ (3.1) ta có:
Luận văn Cao học Trường ĐHBK Hà Nội
Học viên: Nguyễn Bá Ân Page 51
+ Thay (3.5) vào (3.6) tính đƣợc yj theo giá trị đoạn chia (3.4). Chú ý, n càng lớn thì giá trị đoạn chia h càng nhỏ, đƣờng tròn càng trơn, mịn.
+ Tính giá trị bộ toạ độ suy rộng 2j , 3j, j = 1÷ n, theo (yj, zj) ở (3.5), (3.6) và dựa vào kết quả bài toán ngƣợc.
+ Vẽ và xoá liên tiếp vị trí Pj của robot ứng với các giá trị 2j , 3j, j = 1÷ n (cho robot chuyển động đến vị trí Oo’).
Xác định thế của robot ở vị trí 1: từ vị trí Oo’, quay đồng thời khâu 2, quanh tâm O1
một góc -60o
khâu 3 và 4 quanh tâm Oo1 một góc -1500 (ngƣợc chiều kim đồng hồ) đến vị trí 1, bán kính R1 = (6702 + 352 +6802)1/2 = 955,26. Khi đó, toạ độ các vị trí ở thế 1 sẽ là:
+ Điểm P ở vị trí 1: y1 = -588,89 + 260 = - 328,89, z1 = 0. + Tâm khớp nối khâu 2 với 3 và 4 ở OO01 ≡ OO02 ≡ OO07: yO01 = - 328,89; z O01 = 435+ 340 = 775.
+ Tâm khớp quay nối khâu 1 với khâu 2 ở vị trí O1: yO1 = 260, z O1 = 435.
Quỹ tích điểm P thuộc phƣơng trình đƣờng tròn trong mặt phẳng OYZ có dạng: (yO1 - 260)2 + (zO1 - 435)2 = 955,262 (3.7) Vẽ cung tròn (3.7) từ vị trí Oo’ đến 1 nhƣ sau:
+ Thay toạ độ các điểm trên cung tròn (y01’, z01’)bằng toạ độ các biến chạy (yj, zj), j = 1÷ n. Ở cung tròn này ta có:
z1 ≤ zj ≤ zO’o hay zj[z1, zO’o], j = 1÷ n (3.8) yO’o ≤ yj ≤ y1 hay yj[yO’o, y1], j = 1÷ n (3.9) + Chia đoạn [z1, zO’o] ra (n-1) phần bằng nhau, giá trị mỗi đoạn chia là h:
h = (zO’o - z1)/(n-1) = (1785 - 0)/(n-1) = 1785/(n-1) (3.10) + Khi zj giảm từ zO’o đến z1 thì zj đƣợc tính:
zj = zO’o - (j-1).h = 1785 - (j-1).1785/(n-1), j = 1÷ n (3.11) + Và yj (j = 1÷ n) giảm từ yO’o đến y1, khi đó, từ (3.7) ta có:
yj = [(955,262 – (zj -435)2]1/2 - 260, j = 1÷ n (3.12) + Thay (3.11) vào (3.12) tính đƣợc yj theo giá trị đoạn chia (3.10).
Luận văn Cao học Trường ĐHBK Hà Nội
Học viên: Nguyễn Bá Ân Page 52
+ Tính giá trị toạ độ suy rộng 2j, j = 1÷ n, theo (yj, zj) ở (3.11), (3.12) và dựa vào kết quả bài toán ngƣợc ở Chƣơng 2.
+ Vẽ và xoá liên tiếp vị trí Pj của robot ứng với các giá trị 2j, j = 1÷ n (cho robot chuyển động đến vị trí 1).
Chuyển động vẽ dạng vùng hoạt động cực đại:
Xác định thế robot ở vị trí 2: từ vị trí 1, quay đồng thời khâu 3 và 4 quanh tâm Oo1 một góc 150o
(cùng chiều kim đồng hồ) đến vị trí 2, bán kính R2 = (6702 + 352)1/2 = 670,9. Khi đó, toạ độ các vị trí ở thế 2 sẽ là:
+ Điểm P ở vị trí 2: y2 = -601,92 ; z2 = 932,63. (yO01 = - 328,89; z O01 = 435+ 340 = 775).
+ Tâm khớp nối khâu 2 với 3 và 4 ở vị trí OO02 ≡OO01:yO02 = - 328,89, z O02= 775. + Tâm khớp quay nối khâu 1 với khâu 2 ở vị trí O1: yO1 = 260, z O1 = 435.
Quỹ tích điểm P thuộc phƣơng trình đƣờng tròn trong mặt phẳng OYZ có dạng: (yO1 - 260)2 + (zO1 - 435)2 = 670,92 (3.13) Xác định thế của rô bốt ở vị trí 3: từ vị trí 2, quay đồng thời khâu 2; 3 và 4 quanh tâm Oo1 một góc 150o
(cùng chiều kim đồng hồ) đến vị trí 3, bán kính
R2 = (6702 + 352 + 6802)1/2 = 995,26 . Khi đó, toạ độ các vị trí ở thế 3 sẽ là: + Điểm P ở vị trí 3: y3 = 1619; z3 = 400.
+Tâm khớp nối khâu 2 với 3 và 4 ở vị trí OO03 ≡OO05: yO03 =940 , z O03= 435. +Tâm khớp quay nối khâu 1 với khâu 2 ở vị trí O1: yO1 = 260, z O1 = 435.
Quỹ tích điểm P thuộc phƣơng trình đƣờng tròn trong mặt phẳng OYZ có dạng: (yO1 - 260)2 + (zO1 - 435)2 = 995,262 (3.14) Để vẽ cung tròn từ vị trí 2 đến 3, trong hệ toạ độ OXYZ, chia ra 2 cung (mỗi cung thuộc các phần tƣ đƣờng tròn (5.14)): 2 Oo’’; Oo’’ 3. Sau đây xét sự tăng/giảm của các toạ độ chạy (yj, zj) ứng với từng cung:
Cung (1) 1 2, yj giảm từ y1 y2 (-328,89 -601,92 ); zj tăng từ z1 z2 (0
932,63):
+ Chia đoạn [z1, z2] ra (n-1) phần bằng nhau, giá trị mỗi đoạn chia là h:
Luận văn Cao học Trường ĐHBK Hà Nội
Học viên: Nguyễn Bá Ân Page 53
+ Khi zj tăng từ z1 đến z2 thì zj đƣợc tính:
zj = z2 + (j-1).h = 932,63 + (j-1).932,63/(n-1), j = 1÷ n (3.16) + Và yj (j = 1÷ n) giảm từ y1 đến y2, ta có:
yj = [(995,262 – (zj -435)2]1/2 - 260, j = 1÷ n (3.17) + Thay (3.16) vào (3.17) tính đƣợc yj theo giá trị đoạn chia (3.15).
+ Tính giá trị toạ độ suy rộng 2j, j = 1÷ n, theo (yj, zj) ở (3.16), (3.17) và dựa vào kết quả bài toán ngƣợc ở Chƣơng 2.
+ Vẽ và xoá liên tiếp vị trí Pj của robot ứng với các giá trị 2j, j = 1÷ n (cho robot chuyển động đến vị trí Oo’’).
Cung (2) 2 Oo’’, yj tăng từ y2 yO’’o (-601,92 260 ); zj tăng từ z2 zO’’o (932,63 1785):
+ Chia đoạn [z2, zO’’o] ra (n-1) phần bằng nhau, giá trị mỗi đoạn chia là h:
h = (zO’’o – z2)/(n-1) = (1785 – 932,63)/(n-1) = 852,37/(n-1) (3.18) + Khi zj tăng từ z2 đến zO’’o thì zj đƣợc tính:
zj = z2 + (j-1).h = 932,63 + (j-1).852,37/(n-1), j = 1÷ n (3.19) + Và yj (j = 1÷ n) tăng từ y2 đến yO’’o, ta có:
yj = [(995,262 – (zj -435)2]1/2 - 260, j = 1÷ n (3.20) + Thay (3.19) vào (3.20) tính đƣợc yj theo giá trị đoạn chia (3.18).
+ Tính giá trị toạ độ suy rộng 2j, j = 1÷ n, theo (yj, zj) ở (3.19), (3.20) và dựa vào kết quả bài toán ngƣợc.
+ Vẽ và xoá liên tiếp vị trí Pj của robot ứng với các giá trị 2j, j = 1÷ n (cho robot chuyển động đến vị trí Oo’’).
Cung (3) Oo’’ 3, yj tăng từ yO’’o y3 (260 1610); zj giảm từ zO’’o z3 (1785
400):
(y0’’o = 260; z0’’o = 435 + 680+ 670 = 1785.)
(yO0 = 260; z O0 = 1115; y2 = -601,92; z2 = 932,63). (y1 = -588,89 + 260 = - 328,89; z1 = 0).
Luận văn Cao học Trường ĐHBK Hà Nội
Học viên: Nguyễn Bá Ân Page 54
h = (zO’’o – z3)/(n-1) = (1785 - 400)/(n-1) = 1385/(n-1) (3.21) + Khi zj giảm từ zO’’o đến z3 thì zj đƣợc tính:
zj = zO’’o - (j-1).h = 1785 - (j-1).1385/(n-1), j = 1÷ n (3.22) + Và yj (j = 1÷ n) giảm từ yO’’o đến y3, ta có:
yj = [(995,262 – (zj -435)2]1/2 - 260, j = 1÷ n (3.23)
+ Thay (3.22) vào (3.23) tính đƣợc yj theo giá trị đoạn chia (3.21).
+ Tính giá trị toạ độ suy rộng 2j, j = 1÷ n, theo (yj, zj) ở (3.22), (3.23) và dựa vào kết quả bài toán ngƣợc.
+ Vẽ và xoá liên tiếp vị trí Pj của robot ứng với các giá trị 2j, j = 1÷ n (cho robot chuyển động đến vị trí Oo’’’).
Vẽ cung tròn từ vị trí 3 đến 4, chia ra 2 cung (mỗi cung thuộc các phần tƣ đƣờng tròn (5.14): cung 3 3* và cung 3* 4. Sau đây xét sự tăng/giảm của các toạ độ chạy (yj, zj) ứng với từng cung:
Cung (1) 3 3*, yj giảm từ y3 y3* (từ 1610 740,8); zj giảm từ z3 z3* (từ 400 -636):
(y3 = 1610; z3 = 400); y3* = 740,8; z3* = - 636).
+ Chia đoạn [z3, z3*] ra (n-1) phần bằng nhau, giá trị mỗi đoạn chia h:
h = (z3 – z3*)/(n-1) = (400- (-636))/(n-1) = 1036/(n-1) (3.24)
+ Khi zj giảm từ z3 đến z3* thì zj đƣợc tính:
zj = z3 - (j-1).h = 400 - (j-1).1036/(n-1), j = 1÷ n (3.25)
+ Và yj (j = 1÷ n) giảm từ y3 đến y3*, theo (3.20) ta có:
yj = [(995,262 – (zj - 435)2]1/2 - 260, j = 1÷ n (3.26)
+ Thay (3.25) vào (3.26) tính đƣợc yj theo giá trị đoạn chia (3.24).
+ Tính giá trị toạ độ suy rộng 3j, j = 1÷ n, theo (yj, zj) ở (3.25), (3.26) và dựa vào kết quả bài toán ngƣợc.
+ Vẽ và xoá liên tiếp vị trí Pj của robot ứng với các giá trị 3j, j = 1÷ n (cho robot
Luận văn Cao học Trường ĐHBK Hà Nội
Học viên: Nguyễn Bá Ân Page 55
Cung (2) 3* 4, yj giảm từ y3* y4 (từ 740,8 178,89 ); zj tăng từ z3* z4 (từ - 636 130): (y3* = 740,8; z3* = - 636). (y004 = 848,89; z004 = 95). (y4 = 848,89- 670 = 178,89; z4 =95+35 = 130). (y4 = 178,89; z4 = 130). (y3 = 1610; z3 = 400).
+ Chia đoạn [z3*, z4] ra (n-1) phần bằng nhau, giá trị mỗi đoạn chia là h:
h = (z4- z3* )/(n-1) = (130- (-636))/(n-1) = 766/(n-1) (3.27)
+ Khi zj tăng từ z3* đến z4 thì zj đƣợc tính:
zj = z3* + (j-1).h = -636 + (j-1).766/(n-1), j = 1÷ n (3.28) + Và yj (j = 1÷ n) tăng từ y3* đến y4, theo (5.13) ta có:
yj = [(670,92 – (zj -260)2]1/2 - 435, j = 1÷ n (3.29)
+ Thay (3.28) vào (3.29) tính đƣợc yj theo giá trị đoạn chia (3.27).
+ Tính giá trị toạ độ suy rộng 3j, j = 1÷ n, theo (yj, zj) ở (3.28), (3.29) và dựa vào kết quả bài toán ngƣợc.
+ Vẽ và xoá liên tiếp vị trí Pj của robot ứng với các giá trị 3j, j = 1÷ n (cho robot
chuyển động đến vị trí 3).
Xác định thế của robot ở vị trí 4: từ vị trí 3, quay đồng thời các khâu 2 quanh tâm O1 một góc 60o
và khâu 3, 4 quanh tâm O04 một góc 150o đến vị trí 4. Đoạn cong 3-4 đƣợc tổng hợp từ hai chuyển động quay tròn đều (với tốc độ góc khác nhau) và đồng thời: khâu 2 khi chuyển động quanh O1, mang theo khâu 3 và 4, đồng thời khi đó, khâu 3 và 4 lại có chuyển động quanh tâm O04; nhƣ vậy điều kiện để xác định dạng đoạn cong 3-4 là tổng thời gian chuyển động để thực hiện các góc quay +150o
và 60o là như nhau; gọi 1; 2
là tốc độ góc của khâu 2; 3 và 4, t- thời gian chuyển động tƣơng ứng, ta có: 1 = +150o/ t;
2 = 60o/ t, (t > 0), từ đây rút ra:
Luận văn Cao học Trường ĐHBK Hà Nội
Học viên: Nguyễn Bá Ân Page 56
Trong (3.29) cần chú ý khi t = 0, về ý nghĩa vật lý ta hiểu rằng các khâu khi đó chƣa chuyển động. Do vậy, chỉ xét với t > 0. Ngoài ra, dấu “-“ của 2 thể hiện chiều quay của khâu 3 và 4.
Toạ độ các vị trí ở thế 4 sẽ là:
+ Điểm P ở vị trí 4: y4 = 178,89, z4 = 130.
+ Tâm khớp quay nối khâu 2 với 3 và 4 ở vị trí O04: yO04 = 848,89, z O04 = 95. + Tâm khớp nối khâu 1 với khâu 2, 3 và 4 ở vị trí O1: yO1 = 260, z O1 = 435. Quỹ tích điểm P khi khâu 2 chuyển động quanh O1 biểu diễn bởi phƣơng trình: (y -260)2 + (z - 435)2 = 995,262 (3.31) Quỹ tích điểm P khi khâu 3 và 4 chuyển động quanh O04 (yO04 = 848,89, z O04 = 95) đƣợc biểu diễn bởi phƣơng trình:
(y – 848,89)2 + (z - 95)2 = 670,92 (3.32) Trong đó yO04, zO04 là toạ độ tâm vòng tròn (3.32), tâm này lại chạy trên vòng tròn (3.31); ngoài ra phƣơng trình (3.31) và (3.32) phải luôn thoả mãn (3.30).
Để vẽ đƣờng cong từ vị trí 3 đến 4, cho rô bốt tổng hợp hai chuyển động tròn đồng thời theo điều kiện (3.30). Sau đây xét sự tăng/giảm của các toạ độ chạy(yj, zj) ứng với từng đƣờng tròn:
Đường tròn (3.31), yj giảm từ y3 y4 (từ 1610 178,89 ); zj giảm từ z3 z4 (từ 400130 ):
(y4 = 178,89 ; z4 = 130; y3 = 1610; z3 = 400).
+ Chia đoạn [z3, z4] ra (n-1) phần bằng nhau, giá trị mỗi đoạn chia là h:
h = (z3– z4)/(n-1) = (400-130 )/(n-1) = 270/(n-1) (3.33)
+ Khi zj tăng từ z3 đến z4 thì zj đƣợc tính:
zj = z3 + (j-1).h = 400+ (j-1).270/(n-1), j = 1÷ n (3.34) + Và yj (j = 1÷ n) giảm từ y3 đến y4, theo (3.28) ta có:
yj = [(995,262 – (zj - 435)2]1/2 – 260 , j = 1÷ n (3.35) + Thay (3.34) vào (3.35) tính đƣợc yj theo giá trị đoạn chia (3.33).
+ Tính giá trị toạ độ suy rộng 2j, j = 1÷ n, theo (yj, zj) ở (3.34), (3.35) và dựa vào kết quả bài toán ngƣợc.
Luận văn Cao học Trường ĐHBK Hà Nội
Học viên: Nguyễn Bá Ân Page 57
+ Vẽ và xoá liên tiếp vị trí Pj của robot ứng với các giá trị 2j, j = 1÷ n (cho robot
chuyển động đến vị trí 4).
Đường tròn (5.32), yj giảm từ y3 y4 (từ1610178,89 ); zj giảm từ z3 z4 (từ400 130 ):
(y4 = 178,89; z4 = 130; y3 = 1610; z3 = 400).
Để tránh nhầm lẫn, trong (3.31) và (3.32), đặt zj = zj* và yj = yj* rồi thay chúng vào (3.32), lúc này viết lại dạng của (3.32) nhƣ sau:
(yj - yj*)2 + (zj - zj*)2 = 670,92 (3.36)
+ Chia đoạn [z3, z4] ra (n-1) phần bằng nhau, giá trị mỗi đoạn chia là h:
h = (z3– z4)/(n-1) = (400-130 )/(n-1) = 270/(n-1) (3.37) + Khi zjgiảm từ z3 đến z4 thì zj đƣợc tính:
zj = z3 - (j-1).h = 400 - (j-1).270/(n-1), j = 1÷ n (3.38) + Và yj (j = 1÷ n) giảm từ y3 đến y4, theo (3.36) ta có:
yj = [(670,92 - (zj - zj*)2]1/2 + yj*, j = 1÷ n (3.39) + Thay (3.38) vào (3.39) tính đƣợc yj theo giá trị đoạn chia (3.37).
+ Tính giá trị toạ độ suy rộng 3j, j = 1÷ n, theo (yj, zj) ở (3.38), (3.39) và dựa vào kết quả bài toán ngƣợc.
+ Vẽ và xoá liên tiếp vị trí Pj của robot ứng với các giá trị 3j, j = 1÷ n (cho robot
chuyển động đến vị trí 4).
Chú ý: bộ toạ độ (yj*, zj*) tính theo (3.38) và (3.39) và quá trình tính và vẽ đƣờng cong từ vị trí 3 đến 4 phải thoả mãn điều kiện (3.30).
Xác định thế của robot ở vị trí 5: từ vị trí 4, quay khâu 2, 3 và 4 quanh tâm O1 một góc -60o đến vị trí 5, bán kính R5 = (6702 + 352)1/2 = 670,9
Toạ độ các vị trí ở thế 5:
+ Điểm P ở vị trí 5: y5 =680-670+260= 270 , z5 =35+435 = 470
+ Tâm khớp nối khâu 2 với 3 và 4 ở vị trí O03 ≡ O05: yO03 =940 , z O03= 435 + Tâm quay O1 nhƣ cũ: yO1 = 260, z O1 = 435.
Luận văn Cao học Trường ĐHBK Hà Nội
Học viên: Nguyễn Bá Ân Page 58
(y - 260)2 + (z - 435)2 = 670,92 (3.40) Vẽ (3.40), từ vị trí 4 đến 5, chia ra 2 cung: cung 4 4* và cung 4* 5:
Cung (1) 4 4*, zj tăng từ z4 (130) đến z4* (435), yj tăng từ y4 (178,89) đến y4* (530), j =1÷ n:
(y4 = 178,89; z4 = 130; y4* = 530, z4* = 435).
+ Chia đoạn [z4, z4*] ra (n-1) phần bằng nhau, giá trị mỗi đoạn chia là h:
h = (z4* - z4)/(n-1) = (435 – 130)/(n-1) = 305/(n-1) (3.41) + Khi zj tăng từ z4 đến z4* thì zj đƣợc tính:
zj = z4 + (j-1).h = 130+ (j-1).305/(n-1), j = 1÷ n (3.42) + Và yj giảm từ y4 đến y4* , khi đó, từ (3.40) ta có:
yj = [(670,92 - (zj -435)2]1/2 - 260, j = 1÷ n (3.43) + Thay (3.42) vào (3.43) tính đƣợc yj theo giá trị đoạn chia (3.41).
+ Tính giá trị toạ độ suy rộng 2j, j = 1÷ n, theo (yj, zj) ở (3.42), (3.43) và dựa vào kết quả bài toán ngƣợc.
+ Vẽ và xoá liên tiếp vị trí Pj của robot ứng với các giá trị 2j, j = 1÷ n (cho robot chuyển động đến vị trí 4*).
Cung (1) 4* 5, zj tăng từ z4* (435) đến z5 =470; yj giảm từ y4* (530) đến y5 = 270, j =1÷ n:
(y5 = 680-670+260= 270; z5 =35+435 = 470).
+ Chia đoạn [z4*, z5] ra (n-1) phần bằng nhau, giá trị mỗi đoạn chia là h:
h = (z5 - z4*)/(n-1) = (470 - 435)/(n-1) = 35/(n-1) (3.44) + Khi zj tăng từ z4* đến z5 thì zj đƣợc tính:
zj = z4* + (j-1).h = 435 + (j-1).35/(n-1), j = 1÷ n (3.45) + Và yj tăng từ y4* đến y5, khi đó, từ (3.40) ta có:
yj = [(670,92 - (zj -435)2]1/2 - 260, j = 1÷ n (3.46) + Thay (3.45) vào (3.46) tính đƣợc yj theo giá trị đoạn chia (3.44).
+ Tính giá trị toạ độ suy rộng 2j, j = 1÷ n, theo (yj, zj) ở (3.45), (3.46) và dựa vào kết quả bài toán ngƣợc.
Luận văn Cao học Trường ĐHBK Hà Nội
Học viên: Nguyễn Bá Ân Page 59
+ Vẽ và xoá liên tiếp vị trí Pj của robot ứng với các giá trị 2j, j = 1÷ n (cho robot chuyển động đến vị trí 5).
Xác định thế của rô bốt ở vị trí 6: từ vị trí 5, quay khâu 2, 3 và 4 quanh tâm O1 một góc -30o (ngƣợc chiều kim đồng hồ) đến vị trí 6, bán kính R6=O06; P= 670,9.
Toạ độ các vị trí ở thế 6:
+ Điểm P ở vị trí 6: y6 = ( 848,89 – 670) = 178,89; z6 = 35+340+ 435 = 810. + Tâm khớp nối khâu 2 với 3 và 4 ở vị trí O06: yO06 = 848,89; z O06 = 775 . + Toạ độ tâm quay O1 nhƣ cũ: yO1 = 260; z O1 = 435.
Quỹ tích điểm P khi quay khâu 3 và 4 quanh tâm O06 biểu diễn bởi phƣơng trình: (y - 260)2 + (z - 435)2 = 670,92 (3.47) Vẽ (3.47) từ vị trí 5 đến 6:
(y5 =680-670+260= 270; z5 =35+435 = 470).
(y6 = (848,89 – 670) = 178,89; z6 = 35+340+ 435 = 810).
+ zj tăng từ z5 (470) đến z6 (810), yj giảm từ y5 (270) đến y6 (178,89), j =1÷ n. + Chia đoạn [z5, z6] ra (n-1) phần bằng nhau, giá trị mỗi đoạn chia là h:
h = (z6 – z5)/(n-1) = (810 – 470)/(n-1) = 340/(n-1) (3.48) + Khi zj tăng từ z5 đến z6 thì zj đƣợc tính:
zj = z5 + (j-1).h = 470+ (j-1).340/(n-1), j = 1÷ n (3.49) + Và yj giảm từ y5 đến y6 , khi đó, từ (3.47) ta có:
yj = [(670,92 - (zj -435)2]1/2 - 260, j = 1÷ n (3.50) + Thay (3.49) vào (3.50) tính đƣợc yj theo giá trị đoạn chia (3.48).
+ Tính giá trị toạ độ suy rộng 2j, j = 1÷ n, theo (yj, zj) ở (3.49), (3.50) và dựa vào kết quả bài toán ngƣợc
+ Vẽ và xoá liên tiếp vị trí Pj của robot ứng với các giá trị 2j, j = 1÷ n (cho robot chuyển động đến vị trí 6).
Xác định thế của rô bốt ở vị trí 7 từ vị trí 6 quay khâu 2 quanh tâm O1 một góc -