I. Giới Gi ới thiệu thi ệu phần mềm EASY-ROB EASY-ROB 2.0 EASY-ROB là công cụ cụ mô phỏng robot sử dụng đồ hoạ trong không gian 3 chiều chiều (3D) và và các hình ảnh có thể hoạt động được. Một hệ thống 3D3D-CAD đơn giản cho phép tạo tạo ra các khối h ình học h ọc cơ bản như khối khối trụ, khối cầu, khối chữ chữ nhật, khối tam giác, khối khối h ình thang, ... để vẽ kết cấu của robot. Trong EASY-ROB EASY-ROB chúng ta có thể thể dùng chuột chuột để quay hoặc tịnh tiến robot đến một toạ độ tuỳ ý. EASY-ROB EASY-ROB cũng cũng có các chức chức năng phóng to, thu nhỏ đối tượng vẽ như nhiều nhiều phần mềm thiết kế khác... Chương Chương tr ình cho phép thiết thiết kế các robot đến 12 bậc tự do. Chuyển động của Robot có thể thể được điều khiển theo các biến khớp hoặc các toạ độ Đề-cát. Đề-cát. Chúng ta cũng có thể mô tả động học của robot theo kiểu DH hoặc trong trong hệ toạ độ toàn toàn cục đã có sẵn và (Universa Coordinates). Easy-Rob Easy- Rob đã sẵn các tr ình điều khiển động học thuận và ngược ngược của các cấu cấu h ình robot thông dụng, dụng, khi thiết kế ta chỉ cần khai báo kiểu động học thích hợp. Trong trường hợp robot có kết cấu đặc biệt hoặc có các khâu bị động gắn với các chuyển động của các khớp th ì cần phải giải bài toán động học ngược hoặc hoặc xác định hàm hàm toán học học mô tả sự phụ thuộc của khâu bị động đối với khớp quay, viết viết chương tr ình xác định sự phụ thuộc đó bằng ngôn ngữ C và sau đó dùng tập tin MAKE.EXE trong C để dịch thành th ành tậ liên k ết ết động er_kin.dll (Easy(Easytập tin thư thư viện vi ện liên li ên k ết ết Rob kinematic Dynamic link library), khi chạy ch ạy chương tr ình, EASY-ROB sẽ sẽ liên với tập tin nầy và và thực th ực hiện kiểu động học đã đã được khai báo trong chương tr ình điều khiển. khiển. riêng, Chương Easy-ROB có một một số các lệnh điều khiển riêng, Chương tr ình được viết theo kiểu kiểu xử lý tuần tự, tập tin dạng Text, có thể soạn thảo chương tr ình trong bất bất kỳ tr ình soạn soạn thảo nào. nào. Các công cụ cụ gắn tr ên ên khâu chấp ch ấp hành cuối cuối có thể thay đổi được. cho một robot theo một quỹ đạo Chúng ta có thể thể viết một chương tr ình chuyển chuyển động cho mong muốn, muốn, có thể kiểm tra khả năng vươn tới của cánh tay, xác định vùng vùng làm việc việc của robot . . . Robot mô phỏng có thể cầm nắm hoặc thả các đối tượng làm l àm việc. việc. Các vào một và có thể thực hiện chuyển chuyển động của robot có thể ghi vào m ột tập tin và thể thực hiện lại. Phần Phần mềm cho phép ta xem được các hệ toạ độ đã đã gắn gắn tr ên ên các khâu của của robot, xem được quỹ đạo chuyển động của điểm cuối công cụ gắn tr ên ên khâu chấp ch ấp hành cuối. cuối. Phần mềm còn còn có nhiều nhiều tiện ích khác như : cho phép ta lập lập tr ình điều khiển robot bằng bằng ph phương pháp dạy học, thiết kế các đối tượng làm làm việc việc của robot, có các cửa sổ về toạ độ và và giá trị trị góc quay của các khớp tại từng thời điểm khi robot hoạt động... ệc sử dụng phần phần mềm EASYEASY-ROB để mô phỏng robot giúp chúng ta hai Việc Vi khả khả năng nghiên nghiên cứu cứu : a. Mô phỏng ph ỏng lại một robot đã có và các đối tượng làm làm việc việc của nó. Đánh giá khả khả năng làm làm việc việc và mức độ linh hoạt của robot, xác định các thông số điều khiển, quỹ quỹ đạo chuyển động để dùng trong điều khiển thực. b. Nghiên cứu cứu thiết kế động học, các kích thước và và k ết ết cấu của robot tr ên ên máy tính để có thể chọn được phương án động học tốt nhất, đảm bảo cho robot hoàn hoàn yêu cầu. thành các nhiệm nhiệm vụ yêu cầu. II. HƯỚNG DẪN SỬ DỤNG PHẦN MỀM EASY ROB 2.0 1. Lưu đồ giải thuật của chương tr ình.
EASY ROB 2.0
Khởi Khởi động chương tr ình
Thiết Thiết kế hệ tọa độ
Thiết Thiết kế h ình dáng robot
Xây dựng dựng chương tr ình điều khiển robot
Chạy Chạy mô phỏng Hình 2.1: Lưu L ưu đồ giải thuật của EASY ROB 2. Giao di ện l àm việc của phầ n m ềm. Khởi Khởi động chương tr ình EASY ROB 2.0 là chương tr ình tự tự chạy bạn không cần phải cài đặt. Để chạy chương tr vào tập ình bạn b ạn kích vào t ập tin EASYROBW.EXE Giao diệ diện làm việc vi ệc của phần mềm gồm 3 phần chính: chính: Cửa sổ làm làm việc việc Các thanh công cụ cụ Thanh menu chính
Hình 2.2 Màn hình làm việc vi ệc của EASY ROB 2.0
EASY ROB 2.0
a. Thanh menu chính c ủa
chương tr ình
Menu file: Dùng để load, lưu, xóa các dạng file của robot
File có dạng *.Rob : (Robotfile) để mô tả riêng k ết cấu của một robot. File có dạng *.Bod : (Bodyfile) để mô tả các đối tượng làm việc của robot. File có dạng *.Tol : (Toolfile) để mô tả công cụ gắn tr ên khâu chấp hành cuối của robot. File có dạng *.Vie : (Viewfile) để xác định góc nh ìn trong không gian. File có dạng *.igp : (Igrip Partfile) lưu trử một bộ phận kết cấu. File có dạng *.Prg : (Programm) Chương tr ình điều khiển. Menu Robotics : Dùng để nhập các thông số DH, xác định vị trí của dụng cụ, xác định vị trí robot và các thông số khác.
Menu simulate: Dùng mô phỏng hoạt động của robot.
EASY ROB 2.0
Menu 3D-CAD : Cung cấp các công cụ để vẽ kết cấu robot trong không gian 3 chiều (3D) cũng như để thiết kế các công cụ, các đối tượng làm việc. Để vẽ được kết cấu của robot, dựa vào các khối h ình học đơn giản ta có thể lắp ghép chúng lại để tạo nên các hình dáng khác nhau của robot.
Menu view: Dùng để xem màn hình robot dưới các góc độ khác nhau.
Menu aux : Dùng để điều chỉnh robot trong quá tr ình thiết kế và trong quá trình lập tr ình cho robot.
EASY ROB 2.0
Menu help : Tiện ích giúp đỡ của chương tr ình b. Thanh công c ụ
nằm ngang phía tr ên
Chức năng của các nút trên thanh công cụ, tính từ trái qua phải. 1. Bật tắt chế độ chiếu sáng các đối tượng vẽ. 2. Chuyển tất cả các đối tượng sang dạng lưới. 3. Chuyển đối tượng dạng trụ / khối phức tạp. 5. Thể hiện/không thể hiện sàn. 6. Thể hiện sàn ở dạng lưới. 7. Reset vị trí robot tr ên màn hình. 8. Chuyển đổi cửa sổ khi mở Cellfile hoặc igip partfile (kết hợp với nút 7). 9. Chạy chương tr ình. 10. Tạm dừng chương tr ình. 11. Tiếp tục chạy cương tr ình. 12. Kết thúc chương tr ình. 13. Chạy chương tr ình theo từng bước. 14. Lặp lại chương tr ình sau khi k ết thúc. 15. 16. Giảm và tăng tốc độ điều khiển. 17. Đánh giá sai số và xem các giá trị động học c.
Thanh công c ụ
nằm ngang phía dưới
Chức năng của các nút tr ên thanh công cụ, tính từ trái qua phải: 1. Thấy hoặc không thấy kết cấu robot. 2. Thấy hoặc không thấy dụng cụ. 3. Thấy hoặc không thấy các đối tượng làm việc. 4. Thể hiện/không thể hiện hệ toạ độ gắn với dụng cụ . 5. Thể hiện/không thể hiện hệ toạ độ gắn tr ên các khâu của robot. 6. Thể hiện vị trí điều khiển. 7. Mô phỏng động lực học. 8. Thể hiện quĩ đạo chuyển động. 9. Sử dụng các giới hạn của khớp. 10. Soạn thảo chương tr ình và dạy học. 12. Thể hiện hoặc không thể hiện Hệ toạ độ gắn trên đối tượng hiện thời. 13. Chuyển đến đối tượng tiếp theo (khi thiết kế). 14. Xác định vị trí tuyệt đối của đối tượng hiện tại. 15. Xác định vị trí tương đối của đối tượng hiện tại. 16. Reset vị trí của đối tượng hiện tại. 17. Ghi lại vị trí của đối tượng sau khi điều chỉnh. 18. Đưa robot về vị trí dừng (Home position). EASY ROB 2.0
19. Điều khiển robot theo khớp quay. d. Thanh công c ụ
thẳng đứng
Chức năng của các nút tr ên thanh công cụ, tính từ tr ên suống dưới. 1. Dùng chuột để view, zoom và Pan. 2.3. Điều khiển hướng của khâu chấp hành cuối bằng chuột. 4. Điều khiển các khớp 1,2,3 (Dùng các phím chuột). 5. Di chuyển thân robot. (hệ toạ độ cơ sở) 6. Di chuyển các đối tượng (body) bằng chuột. 7. Di chuyển tất cả các đối tượng bằng chuột. 9. Chuyển đổi chuyển động là quay hoặc tịnh tiến 11.12. Tăng giảm tốc độ điều khiển bằng chuột 3. Thao tác chu ột
Easy-Rob cho phép dùng chuột với nhiều chức năng như : Khi nút lệnh số 1 của thanh công cụ thẳng đứng được chọn : Zoom (Phóng to, thu nhỏ) : ấn nút chuột phải, r ê chuột lên xuống theo phương thắng đứng của màn hình. Pan (thay đổi vị trí của đối tượng so với khung màn hình) : ấn đồng thời hai nút chuột phải và trái, rê chuột tr ên màn hình. Rotate (quay robot để nh ìn ở các góc độ khác nhau) : ấn chuột trái, r ê chuột.
Khi nút lệnh số 4 của thanh công cụ thẳng đứng được chọn lần thứ nhất sẽ điều khiển ba khớp đầu tiên (1, 2, 3) : Quay khớp 1: ấn nút chuột phải, r ê chuột (nếu là khớp tịnh tiến sẽ làm khâu chuyển động tịnh tiến). Quay khớp 2: ấn đồng thời 2 nút chuột phải và trái, rê chuột. Quay khớp 3: ấn nút chuột trái, r ê chuột. Khi click lên nút số 4 lần thứ hai sẽ điều khiển ba khớp tiếp theo (4, 5, 6) Để điều khiển các khớp làm giống như trên. ấn nút chuột phải, r ê chuột (nếu là khớp tịnh tiến sẽ làm khâu Quay khớp 4: chuyển động tịnh tiến). Quay khớp 5: ấn đồng thời 2 nút chuột phải và trái, rê chuột. Quay khớp 6: ấn nút chuột trái, r ê chuột. Tương tự khi nút số 4 được chọn lần thứ 3 sẽ điều khiển được ba khớp tiếp theo. 4. Xây dựng
hệ tọa độ Muốn xác định hệ toạ độ của robot trước hết phải thực hiện bằng tay các công việc sau: Vẽ sơ đồ động robot ở vị trí dừng, gắn hệ toạ độ của các khâu lên hình vẽ trên giấy, xác định các thông số DH. Các buớc tiếp theo : 1- Bật nút lệnh số 5 tr ên menu ngang, dưới. 2- Vào menu chính : FILE -> LOAD -> ROBOTFILE chọn DHTempl -> OPEN. 3- Vào menu chính : ROBOTICS -> ROBOTMOTION + KINEMATICS EASY ROB 2.0
->KINEMATICS DATA. 4- Chọn Active Join -> Ok -> Activ Joint (1) RZ (hoặc chọn TZ nếu là khớp tịnh tiến) -> Ok -> Nhập các thông số DH của khâu thứ nhất. 5- Chọn Quit -> Ok. Vào lại bước 4 -> Number Active Joint(1) -> Ok -> ấn đúp chuột vào vệt xanh hoặc đưa con trỏ vào phần nhập dữ liệu (text box) ấn 2 (Bây giờ số khâu động là 2), nhập các thông số DH cho khâu số 2 ... Làm tương tự cho đến khi đủ số khớp yêu cầu. Ta có thể kiểm tra các số liệu đã nhập bằng cách kích chuột vào menu : ROBOTICS -> ROBOTMOTION + KINEMATICS -> KINEMATICS DATA-> KINEMATIC INFOMATION để xem lại số khâu, khớp và các thông số DH. Nếu vào dữ liệu sai ta có thể hiệu chỉnh lại. Để thể hiện hệ toạ độ của robot tr ên màn hình (Hệ toạ độ màu vàng), nhớ kích chuột vào nút số 5 của thanh công cụ nằm ngang phía dưới. 5. Thi ết kế hình dáng robot Sau khi hoàn thành việc gắn hệ toạ độ của robot, bước tiếp theo là vẽ h ình dáng của nó. H ình dáng của robot có thể được mô phỏng giống như robot thực nhờ công cụ 3D CAD của EasyRob. Menu 3D-CAD cho phép tạo ra các khối h ình học cơ bản như khối trụ, khối cầu, khối chữ nhật, khối tam giác ... Sự phối hợp hợp lý về kích thước và vị trí của các khối hình học nầy cho phép thể hiện được các kết cấu khác nhau của robot. Các menu kéo xuống của Menu 3D- CAD , một số các chức năng chính như sau:
Select group : Chọn nhóm đối tượng để thiết kế : 1/Robot group, 2/Tool group hay 3/ Body group. Select body from group : Chọn các bộ phận của robot đã vẽ (theo tên đặt trước) của nhóm chọn hiện hành. Create/Import new 3D body : Tạo mới hoặc nhập một bộ phận đã có sẳn. Cần nhập các thông số cần thiết để tạo ra đối tượng mong muốn. Modify sel. Body_set Jnt_idx : Hiệu chỉnh các thuộc tính của bộ phận hiện hành. Clone : Copy bộ phận đang vẽ thành nhiều h ình. Render : Biểu hiện đối tượng ở dạng lưới, dạng hộp, . . . Color : Thay đổi màu sắc. Name:Thay đổi tên bộ phận đang vẽ. Clear : Xoá đối tượng (bộ phận) hiện hành. Position's : Thay đổi vị trí của đối tượng (bộ phận) hiện hành. 3D CAD Coorsys Visibility : Cho hiện hoặc ẩn hệ tọa độ của đối tượng
EASY ROB 2.0
Next Body in group : Chọn đối tượng vẽ tiếp theo. Dùng menu 3D CAD ta lần lượt vẽ tất cả các khâu của robot, có thể dung các màu sắc khác nhau để thể hiện h ình dáng của robot. Lưu ý trong quá trình vẽ, nếu vẽ sai phải dùng mục CLEAR để xóa đi hoặc dùng mục MODIFY CEL để hiệu chỉnh. Mỗi đối tượng vẽ phải gắn với một khâu nhất định, được khai báo trong mục SET JOINT INDEX. Có thể dùng thanh công cụ thẳng đứng phía phải để thay đổi vị trí của các đối tượng vẽ cho thích hợp.
6. L ập
tr ình điều khiển robot Để lập tr ình điều khiển robot đã mô phỏng ta dùng phương pháp lập tr ình kiểu dạy học. Sau khi đã thiết kế h ình dáng robot, công cụ gắn tr ên khâu chấp hành cuối, các đối tượng làm việc khác . . . ta có thể lập tr ình để điều khiển robot đã mô phỏng. Việc lập tr ình thực hiện theo tr ình tự sau đây : Nhấp chuột vào nút lệnh số 10 (Show program window) để kích hoạt cửa sổ lập trình.
Chọn New để đặt tên cho File chương tr ình. Chọn Append nếu muốn bổ sung một chương tr ình đã có trên đĩa. Xác định vị trí các điểm mà dụng cụ phải đi qua (dùng chuột để điều khiển các khớp, dùng menu đứng). Cứ sau mỗi lần xác định được một vị trí th ì ấn nút PTP (điều khiển điểm) hoặc LIN (điều khiển đường) hoặc VIA (điểm trung gian dẫn hướng khi điều khiển đường cong), CIRC (điều khiển theo đường cong). Làm liên tục cho tất cả các điểm để có một chương tr ình hoàn thiện. Sau khi k ết thúc việc dạy robot học, ấn nút Close trên Program Window để kết thúc. Để hiệu chỉnh và bổ sung các lệnh điều khiển khác vào chương tr ình, ấn chuột vào nút EDIT, Dùng các lệnh của EasyRob như dưới đây để hoàn thiện chương tr ình. Một số lệnh thường dùng trong lập tr ình: PROGRAMFILE : Bắt đầu chương tr ình ENDPROGRAMFILE or END : Kết thúc chương tr ình. CALL fct_name : Gọi một hàm có tên fct_name(), đã được định nghĩa trong chương trình. CALL FILE filename : Gọi một File chương tr ình có tên filename, File phải có cung cấu trúc như chương tr ình chính. FCT fct_name() : Bắt đầu Định nghĩa một hàm có tên fct_name(). ENDFCT : Kết thúc định nghĩa một function. ! Các ghi chú trong chương tr ình. TOOL X Y Z A B C [m, deg] : Định tọa độ điểm cuối của dụng so so với khâu chấp hành cuối. EASY ROB 2.0
PTP X Y Z A B C [m,deg] : Di chuy ển robot đến điểm mới (tọa độ tuyệt đối). Điều khiển điểm. PTP_REL dX dY dZ dA dB dC [m,deg] : Di chuy ển robot đến điểm mới (tọa độ tương đối). Điều khiển điểm. LIN X Y Z A B C [m,deg] : Di chuy ển robot đến điểm mới (tọa độ tuyệt đối). Điều khiển đường. LIN_REL dX dY dZ dA dB dC [m,deg] : Di chuyển robot đến điểm mới (tọa độ tương đối). Điều khiển đường. CIRC X Y Z A B C [X2 Y2 Z2] [m,deg] : Di chuy ển robot đến điểm mới (tọa độ tuyệt đối). Điều khiển đường cong. [X2 Y2 Z2] - Điểm trung gian (3 điểm để xác định một cung tr òn). CIRC_REL dX dY dZ dA dB dC [dX2 dY2 dZ2] [m,deg] : Di chuy ển robot đến điểm mới (tọa độ tương đối). Điều khiển đường cong. WAIT x [sec] : Robot dừng hoạt động trong x giây. ERC TRACK ON,OFF : Thể hiện hoặc không thể hiện quỹ đạo chuyển động. ERC LOAD TOOL filename : Gọi một Tool file (*.tol) ERC LOAD VIEW filename : Gọi một View file (*.vie) ERC LOAD ROBOT filename Loads a Robot file (*.rob) ERC LOAD BODY filename Loads a Body file (*.bod) ERC LOAD TAGS filename Loads a Tag file (*.tag) ERC GRAB BODY ’bodyname’ : Dụng cụ cầm lấy một vật thể (body) có tên Bodyname. ERC GRAB BODY_GRP : Dụng cụ cầm lấy một nhóm vật thể (Body_Grp). ERC RELEASE BODY ’bodyname’ : Dụng cụ thả (buông) một vật thể (body) có tên Bodyname. ERC RELEASE BODY_GRP Dụng cụ thả (buông) một nhóm vật thể (Body_Grp). ERC ROBOT_BASE XYZ ABC [m,deg] : Di chuyển gốc tọa độ cơ bản của robot đến vị trí mới. v.v... Ghi chú : - Đơn vị chiều dài là Mét [m], Góc là độ [deg] hoặc [%] - Đơn vị của tốc độ là [m/s] - Vị trí và hướng của hệ tọa độ gắn tr ên khâu chấp hành cuối được xác định gồm : X, Y và Z : chỉ tọa độ vị trí, A, B và C chỉ góc hướng. Hướng của khâu chấp hành cuối xác định theo các góc ABC là: Rot (A,B,C) = Rot(X,A) * Rot(Y,B) * Rot(Z,C) III. THIẾT KẾ VÀ MÔ PHỎNG MỘT SỐ ROBOT THÔNG DỤNG 1. Mô phỏng robot ELBOW ELBOW là robot có 6 khâu với cấu hình RRRRRR. Tất cả 6 khâu đều chuyển động quay.
EASY ROB 2.0
Hình 3.1 Robot Elbow Khâu 1
i
i
ai
d i
900
0
d 1
0
a2
0
0
a3
0
-900
a4
0
900
0
0
0
0
d 4
1
2
2
3
3
4
4
5
5
6
6
Hình 3.2 Bảng thông số DH của robot Elbow Trong ví dụ mô phỏng robot này chúng ta chọn các giá trị như sau: a2 = 0.4m a3 = 0.3m a4 = 0.25m d 1 = 0.45m d 4 = 0.15m
Các giá trị chọn ban đầu bằng 0 B1. Xây d ựng hệ toạ độ. i
Bật nút lệnh số 5 tr ên menu ngang, dưới. Vào menu chính : FILE -> LOAD -> ROBOTFILE chọn DHTempl -> OPEN. Vào menu chính : ROBOTICS -> ROBOTMOTION + KINEMATICS -> KINEMATICS DATAChọn Active Join -> Ok -> Activ Joint (1) RZ (khớp thứ nhất là khớp quay) -> Ok -> Nhập các thông số DH của khâu thứ nhất. Vào lại bước 4 -> Number Active Joint(1) -> Ok -> ấn đúp chuột vào vệt xanh hoặc đưa con trỏ vào phần nhập dữ liệu (text box) ấn 2 (Bây giờ số khớp động là 2), nhập các thông số DH cho khâu số 2.
EASY ROB 2.0
Làm tương tự nhập các thông số DH cho 4 khâu còn lại. Chú ý tất cả các khớp đều là khớp quay (Activ Joint (n) RZ)
Kết quả các hệ tọa độ gắn trên các khâu như h ình sau
EASY ROB 2.0
Ta có thể kiểm tra các số liệu ã nhập bằng cách kích chuột vào menu : ROBOTICS -> ROBOTMOTION + KINEMATICS -> KINEMATICS DATA-> KINEMATIC INFOMATION để xem lại số khâu, khớp và các thông số DH. Nếu vào dữ liệu sai ta có thể hiệu chỉnh lại.
B2. Thi ết
kế h ình dáng robot Thiết kế robot Vào menu 3D-CAD -> select group -> robot group-> ok (Để thiết kế robot)
Thiết kế khâu thứ nhất: Vào menu 3D-CAD -> create/import new 3D body-> cylinder ( ch ọn khối h ình trụ)
Chọn vị trí đặt của khâu
EASY ROB 2.0
Điều chỉnh kích thước của khâu
Chọn khớp mà khâu nối vào
Đặt tên cho khâu
EASY ROB 2.0
Nếu chưa vừa ý chúng ta có thể sửa chửa kích thước khâu, thay đổi vị trí của khâu, thay đổi màu hoặc có thể xóa đi. Làm tương tự để thiết kế h ình dáng các khâu còn lại.
Kết quả h ình dáng robot sau khi thiết kế.
Thiết kế dụng cụ làm việc. Vào menu 3D-CAD -> select group -> tool group-> ok (Để thiết kế dụng cụ làm việc)
Tương tự như trên ta có thể thiết kế được h ình dạng dụng cụ làm việc theo ý muốn của m ình.
EASY ROB 2.0
Thiết kế đối tượng làm việc: Vào menu 3D-CAD -> select group -> body group-> ok (Để thiết kế đối tượng làm việc làm việc)
Kết quả sau khi thiết kế xong robot.
B3. Lập trình
điều khiển robot.
Sau khi thiết kế xong robot công việc cuối cùng của chúng ta là lập tr ình điều khiển robot. Chương tr ình điều khiển robot Elbow. PROGRAMFILE ! prgfln E:\LE KHANH\EASY_ROB\proj\program_elbow.prg ! TRINH DIEN RO BOT ERC LOAD VIEW K1 WAIT 2 ERC LOAD VIEW K2 WAIT 2 ERC LOAD VIEW K3 WAIT 2
EASY ROB 2.0
ERC LOAD VIEW K4 WAIT 2 ERC LOAD VIEW K5 WAIT 2 ERC LOAD VIEW K6 WAIT 2 ! GOI CAC CHUONG TRINH CON CALL A1 CALL A2 CALL A3 CALL A4 CALL A5 CALL A6 ! CHUONG TRINH A1 FCT A1 () PTP_AX -0.0000 44.0000 -53.5000 -72.5000 PTP_AX -29.5000 39.0000 -43.0000 -83.5000 ERC LOAD VIEW W1 PTP_AX -29.5000 39.0000 -43.0000 -83.5000 PTP_AX -29.5000 29.5000 -43.0000 -78.5000 ERC LOAD TOOL KHANHPRO_TOOL_CLOSE ERC GRAB BODY A1 PTP_AX -31.0000 39.5000 -43.0000 -88.0000 PTP_AX -77.9999 55.5000 -87.5000 -59.0001 ERC LOAD VIEW W4 PTP_AX -69.0000 60.0000 -97.4999 -52.5001 ERC LOAD VIEW W2 PTP_AX -68.5000 60.0000 -98.9999 -51.5001 PTP_AX -68.5000 49.5000 -98.9999 -44.0001 ERC LOAD TOOL KHANHPRO_TOOL_OPEN ERC RELEASE BODY A1 ENDFCT ! CHUONG TRINH A2 FCT A2 () ERC LOAD VIEW W PTP_AX -68.5000 64.0000 -98.9999 -54.0001 PTP_AX -68.5000 64.0000 -98.9999 -54.0001 PTP_AX -20.5000 42.0001 -55.4999 -75.5001 PTP_AX -20.5000 42.0001 -54.4999 -78.0001 PTP_AX -20.5000 35.0001 -54.4999 -73.0001 ERC LOAD TOOL KHANHPRO_TOOL_CLOSE ERC GRAB BODY A2 PTP_AX -20.5000 46.0001 -54.4999 -81.5000 PTP_AX 77.5000 59.0001 -95.9997 -56.0001 PTP_AX 77.0000 50.5001 -99.9997 -43.5001 ERC LOAD TOOL KHANHPRO_TOOL_OPEN ERC RELEASE BODY A2 ENDFCT ! CHUONG TRINH A3 FCT A3 () PTP_AX 77.0000 PTP_AX 77.0000 PTP_AX -11.3750 PTP_AX -10.7500
EASY ROB 2.0
62.3750 62.3750 62.3750 52.8751
-99.9997 -99.9997 -99.9997 -70.1249
0.0000 0.0000 91.0000 -61.9999 91.0000 -120.9998 91.0000 -120.9998 91.0000 -120.9998 1.0000 -120.9998 90.5001 -120.9998 90.5001 -69.9998 90.5001 -69.9998
91.0001 -69.9998 181.5000 -69.9998 90.9999 -69.9998 90.9999 -103.4997 91.4999 -103.4997 91.4999 -103.4997 90.9999 -103.4997 91.9999 -100.9997
-51.2501 91.9999 -100.9997 -49.7501 3.9999 -100.9997 -49.7501 3.9999 -100.9997 -69.6250 89.6249 -143.8749
PTP_AX -10.7500 52.8751 -70.1249 -69.6250 ERC LOAD VIEW W1 PTP_AX -10.7500 41.5001 -70.1249 -61.0001 ERC LOAD TOOL KHANHPRO_TOOL_CLOSE ERC GRAB BODY A3 PTP_AX -10.7500 56.1251 -70.1249 -73.0001 PTP_AX -10.7500 56.1251 -70.1249 -72.2500 PTP_AX -83.5000 56.1251 -70.1249 -72.3750 PTP_AX -83.5000 54.0002 -91.3747 -53.0002 ERC LOAD VIEW W2 PTP_AX -83.5000 54.0002 -91.3747 -53.0002 PTP_AX -83.3750 46.5002 -91.9997 -46.6252 ERC LOAD TOOL KHANHPRO_TOOL_OPEN ERC RELEASE BODY A3 ENDFCT ! CHUONG TRINH A4 FCT A4 () ERC LOAD VIEW W PTP_AX -83.3750 65.5001 -92.4997 -62.1252 PTP_AX 2.6250 55.5002 -71.4998 -73.1252 PTP_AX 1.6250 53.0002 -63.4999 -77.6251 PTP_AX 2.5000 45.0002 -75.9998 -58.2502 ERC LOAD TOOL KHANHPRO_TOOL_CLOSE ERC GRAB BODY A4 PTP_AX 3.7500 64.8751 -96.1247 -61.2501 ERC LOAD VIEW W3 PTP_AX 68.3750 52.3752 -86.6248 -55.7502 PTP_AX 68.0000 44.1252 -86.3748 -48.5002 ERC LOAD TOOL KHANHPRO_TOOL_OPEN ERC RELEASE BODY A4 ENDFCT ! CHUONG TRINH A5 FCT A5 () ERC LOAD VIEW W PTP_AX 70.0000 61.6251 -91.6248 -56.7502 PTP_AX 13.0000 72.5001 -90.1248 -68.7501 ERC LOAD VIEW W1 PTP_AX 14.3750 56.3752 -77.7498 -62.5002 PTP_AX 14.8750 45.3752 -77.7498 -53.8752 ERC LOAD TOOL KHANHPRO_TOOL_CLOSE ERC GRAB BODY A5 PTP_AX 14.5000 65.3751 -79.4998 -76.3752 PTP_AX 80.2499 65.3751 -79.4998 -76.3752 PTP_AX 80.2499 47.6252 -73.7499 -63.2503 PTP_AX 80.2499 38.7502 -73.7499 -57.5003 ERC LOAD TOOL KHANHPRO_TOOL_OPEN ERC RELEASE BODY A5 ENDFCT
89.6249 -100.5001 89.6249 -100.5001 89.6249 -100.5001 181.1249 -101.6251 181.1249 -101.6251 89.1249 -146.3753 89.1249 -85.1254 89.1249 -81.2504
175.6249 -96.2504 175.6249 -96.2504 90.1251 -86.7504 89.1251 -86.7504 186.8750 -80.6254 91.1250 -80.6254 92.0000 -80.6254
0.7500 -80.6254 -2.2500 -80.6254 91.2500 -80.3754 90.8750 -80.3754 179.6251 179.6251 90.1250 90.3750
-80.1254 -80.1254 -82.8754 -82.8754
! CHUONG TRINH A6 FCT A6 () ERC LOAD VIEW W PTP_AX 80.2499 53.5002 -73.7499 -65.8753 183.7501 -89.1254 PTP_AX 28.6250 41.3752 -52.6249 -76.7503 88.2501 -104.8753 ERC LOAD VIEW W1
EASY ROB 2.0
PTP_AX 28.1250 41.3752 -52.6249 -73.5003 PTP_AX 28.1250 34.1252 -52.8749 -70.5003 ERC LOAD TOOL KHANHPRO_TOOL_CLOSE ERC GRAB BODY A6 PTP_AX 19.0000 54.5002 -65.4999 -73.2503 PTP_AX -72.8749 50.2502 -74.2498 -63.6254 ERC LOAD VIEW W2 PTP_AX -72.8749 50.2502 -74.2498 -63.6254 PTP_AX -72.8749 38.7502 -74.2498 -56.6254 ERC LOAD TOOL KHANHPRO_TOOL_OPEN ERC RELEASE BODY A6 PTP_AX 0 0 0000 ENDFCT PTP_AX 0 0 0000 ENDPROGRAMFILE
88.2501 -67.6253 90.7500 -62.3753 180.1250 -93.3753 90.2499 -116.2504 90.2499 -77.2504 90.2499 -77.2504
2. Mô phỏng robot SCARA SCARA là robot gồm 4 khâu có cấu h ình kiểu RRTR
Hình 3.3 Robot SCARA và các hệ tọa độ (vị trí ban đầu) Khâu
i
1 2 3 4
1
2
0
4
i
ai
d i
0
a1
0
180
a2
0
0 0
0 0
d 3
d 4
Hình 3.4 Bảng thông số DH của robot SCARA
EASY ROB 2.0
Hình 3.5 Mô hình robot scara Chương tr ình điều khiển robot SCARA PROGRAMFILE ! prgfln E:\LE KHANH\EASY_ROB\proj\HUI_PROGRAM.prg ! CHƯƠNG TR ÌNH TẠO LOGO HUI ! CHUONG TRINH TAO H1 PTP_AX -4.5000 0.0000 0.0000 PTP_AX -6.0000 -67.9999 0.0000 PTP_AX -6.0000 -67.9999 0.0000 PTP_AX -6.0000 -67.9999 0.1440 !KEP BAN TAY LAI VA NAM H1 ERC LOAD TOOL HUI_CLOSE ERC GRAB BODY H1 PTP_AX -6.0000 -67.9999 0.0687 PTP_AX 0.0000 -0.0000 0.0000 ERC ROBOT_BASE 0 -0.19 0 0 0 0 PTP_AX 0.0000 -0.0000 0.1440 ! MO BAN TAY VA NHA H1 ERC LOAD TOOL HUI_OPEN ERC RELEASE BODY H1 PTP_AX 0.0000 0.0000 0.0000 ERC ROBOT_BASE 0 0 0 0 0 0
0.0000 0.0000 -73.9999 -73.9999
-73.9999 -0.0000 -0.0000
0.0000
! CHUONG TRINH TAO H2 PTP_AX 5.5000 0.0000 0.0000 0.0000 PTP_AX 1.0000 -75.1248 0.0000 0.0000 PTP_AX 1.0000 -75.1248 0.0000 -74.8749 PTP_AX 1.0000 -75.1248 0.1440 -74.8749 !KEP BAN TAY LAI VA NAM H2 ERC LOAD TOOL HUI_CLOSE ERC GRAB BODY H2
EASY ROB 2.0
PTP_AX 1.0000 -75.1248 0.0714 PTP_AX -0.0000 0.0000 0.0000 ERC ROBOT_BASE 0 -0.09 0 0 0 0 PTP_AX -0.0000 0.0000 0.1434 ! MO BAN TAY VA NHA H2 ERC LOAD TOOL HUI_OPEN ERC RELEASE BODY H2 PTP_AX 0.0000 0.0000 0.0000 ERC ROBOT_BASE 0 0 0 0 0 0
-74.8749 0.0000 0.0000
0.0000
! CHUONG TRINH TAO H3 PTP_AX 20.0000 -0.0000 0.0000 0.0000 PTP_AX 20.0000 50.2500 0.0000 0.0000 PTP_AX 20.0000 50.2500 0.0000 -109.8749 PTP_AX 20.0000 50.2500 0.1440 -109.8749 !KEP BAN TAY LAI VA NAM H3 ERC LOAD TOOL HUI_CLOSE ERC GRAB BODY H3 PTP_AX 20.0000 50.2500 0.0720 -109.8749 PTP_AX 0.0000 0.0000 -0.0000 -0.0000 ERC ROBOT_BASE 0.07 -0.12 0 0 0 0 PTP_AX 0.0000 0.0000 0.0000 -90 PTP_AX 0.0000 0.0000 0.1462 -90 ! MO BAN TAY VA NHA H3 ERC LOAD TOOL HUI_OPEN ERC RELEASE BODY H3 PTP_AX 0.0000 0.0000 -0.0000 0.0000 ERC ROBOT_BASE 0 0 0 0 0 0 ! CHUONG TRINH TAO U1 PTP_AX 14.2500 -86.2498 -0.0000 PTP_AX 14.3125 -86.2498 -0.0000 PTP_AX 14.3125 -86.2498 0.1442 !KEP BAN TAY LAI VA NAM U1 ERC LOAD TOOL HUI_CLOSE ERC GRAB BODY U1 PTP_AX 14.3125 -86.2498 0.0437 PTP_AX 0.0000 -0.0000 -0.0000 ERC ROBOT_BASE 0 -0.04 0 0 0 0 PTP_AX 0.0000 -0.0000 0.1431 ! MO BAN TAY VA NHA U1 ERC LOAD TOOL HUI_OPEN ERC RELEASE BODY U1 PTP_AX 0.0000 0.0000 -0.0000 ERC ROBOT_BASE 0 0 0 0 0 0 ! CHUONG TRINH TAO U2 PTP_AX 20.4800 -90.1196 0.0000 PTP_AX 20.4800 -90.1196 0.0000 PTP_AX 20.4800 -90.1196 0.1438 !KEP BAN TAY LAI VA NAM U2 ERC LOAD TOOL HUI_CLOSE ERC GRAB BODY U2 PTP_AX 20.4800 -90.1196 0.0691 PTP_AX 0.0000 0.0000 0.0000 ERC ROBOT_BASE 0 0.06 0 0 0 0 PTP_AX -0.0000 0.0000 0.1438 ! MO BAN TAY VA NHA U2
EASY ROB 2.0
0.0000 -72.5626 -72.5626
-73.1226 0.0000 0.0000
0.0000
0.0000 -69.5998 -69.5998
-69.5998 -0.0000 0.0000
ERC LOAD TOOL HUI_OPEN ERC RELEASE BODY U2 PTP_AX 0.0000 0.0000 ERC ROBOT_BASE 0 0 0 0 0 0
0.0000
0.0000
!CHUONG TRINH TAO U3 PTP_AX 12.0800 60.6400 -0.0000 0.0000 PTP_AX 12.0800 60.6400 -0.0000 -106.9600 PTP_AX 12.0800 60.6400 0.1438 -106.9600 !KEP BAN TAY LAI VA NAM U3 ERC LOAD TOOL HUI_CLOSE ERC GRAB BODY U3 PTP_AX 12.0800 60.6400 0.0398 -106.9600 PTP_AX 0.0000 -0.0000 -0.0000 -0.0000 ERC ROBOT_BASE 0.165 0.018 0 0 0 0 PTP_AX 0.0000 -0.0000 -0.0000 -90 PTP_AX 0.0000 -0.0000 0.1438 -90 ! MO BAN TAY VA NHA U3 ERC LOAD TOOL HUI_OPEN ERC RELEASE BODY U3 PTP_AX 0.0000 0.0000 0.0000 -0.0000 ERC ROBOT_BASE 0 0 0 0 0 0 ! CHUONG TRINH TAO U4 PTP_AX 5.0000 68.8799 0.0000 PTP_AX 5.0000 68.8799 0.0000 PTP_AX 5.0000 68.8799 0.1445 !KEP BAN TAY LAI VA NAM U4 ERC LOAD TOOL HUI_CLOSE ERC GRAB BODY U4 PTP_AX 5.0000 68.8799 0.0565 PTP_AX -0.0000 0.0000 -0.0000 ERC ROBOT_BASE 0.1570 -0.029 0 0 0 0 PTP_AX -0.0000 0.0000 -0.0000 PTP_AX -0.0000 0.0000 0.1445 ! MO BAN TAY VA NHA U4 ERC LOAD TOOL HUI_OPEN ERC RELEASE BODY U4 PTP_AX 0.0000 0.0000 -0.0000 ERC ROBOT_BASE 0 0 0 0 0 0 ! CHUONG TRINH TAO U5 PTP_AX -1.8400 76.0796 0.0000 PTP_AX -1.8400 76.0796 0.0000 PTP_AX -1.8400 76.0796 0.1438 !KEP BAN TAY LAI VA NAM U5 ERC LOAD TOOL HUI_CLOSE ERC GRAB BODY U5 PTP_AX -1.8400 76.0796 0.0279 PTP_AX 0.0000 -0.0000 0.0000 ERC ROBOT_BASE 0.1560 0.0478 0 0 0 0 PTP_AX 0.0000 -0.0000 0.0000 PTP_AX 0.0000 -0.0000 0.1438 ! MO BAN TAY VA NHA U5 ERC LOAD TOOL HUI_OPEN ERC RELEASE BODY U5 PTP_AX 0.0000 0.0000 -0.0000 ERC ROBOT_BASE 0 0 0 0 0 0
EASY ROB 2.0
0.0000 73.9600 73.9600
73.9600 0.0000 -36 -36
-0.0000
0.0000 75.3600 75.3600
75.3600 -0.0000 35 35
0.0000
! CHUONG TRINH TAO CHU I PTP_AX 7.6000 -81.2001 0.0014 PTP_AX 7.6000 -81.2001 0.0014 PTP_AX 7.6000 -81.2001 0.1442 !KEP BAN TAY LAI VA NAM I ERC LOAD TOOL HUI_CLOSE ERC GRAB BODY I PTP_AX 7.6000 -81.2001 0.0276 PTP_AX -0.0000 -0.0000 -0.0000 ERC ROBOT_BASE 0 0.11 0 0 0 0 PTP_AX -0.0000 -0.0000 0.1421 ! MO BAN TAY VA NHA I ERC LOAD TOOL HUI_OPEN ERC RELEASE BODY I PTP_AX 0.0000 0.0000 0.0000 ERC ROBOT_BASE 0 0 0 0 0 0
0.0000 -73.3599 -73.3599
-73.3599 -0.0000 -0.0000
0.0000
ENDPROGRAMFILE
3. Mô phỏng robot STANFORD Stanford là robot 6 khâu có cấu h ình RRT.RRR( khâu thứ ba chuyển động tịnh tiến, năm khâu còn lại chuyển động quay)
Khâu 1 2 3 4 5 6
Hình3.6 Hệ tọa độ của robot stanford 0 a
i
i
1
2
0
4
5
6
-90
i
0
0
0
900
0
d 2
0 -900
0 0
0
900
0
0
0
0
0
d 3
Hinh 3.7 Bảng thông số DH của robot stanford EASY ROB 2.0
Hình 3.8 Mô hình robot stanford Chương tr ình điều khiển robot stanford PROGRAMFILE ! prgfln E:\LE KHANH\EASY_ROB\proj\STANFORD_PROGRAM.prg ERC LOAD VIEW S_1 WAIT 2 ERC LOAD VIEW S_2 WAIT 2 ERC LOAD VIEW S_3 WAIT 2 ERC LOAD VIEW S_4 WAIT 2 !CHUONG TRINH GAP HANG 1 ERC LOAD VIEW S PTP_AX -95.6249 0.0000 0.0000 0.0000 PTP_AX -95.1249 113.5000 0.0349 -1.5000 ERC LOAD VIEW S2 PTP_AX -95.2499 113.5000 0.0349 -1.6250 PTP_AX -95.8749 123.8754 0.1200 -1.6250 ERC LOAD TOOL STANFORD_TOOL_CLOSE ERC GRAB BODY HANG1 PTP_AX -96.2499 108.8754 -0.0087 -0.0000 PTP_AX -21.2499 86.3755 0.4800 1.3750 ERC LOAD VIEW S1 PTP_AX -21.1249 96.7505 0.4800 1.3750 ERC LOAD TOOL STANFORD_TOOL_OPEN ERC RELEASE BODY HANG1
0.0000 0.0000 66.8750 47.0000 66.8750 -6.1250 58.0001 -6.1250 70.7500 -6.1250 92.6249 -21.7500 81.3750 -21.7500
!CHUONG TRINH GAP HANG 2 ERC LOAD VIEW S PTP_AX -21.1249 85.5006 0.5084 1.8750 95.7499 -21.7500 PTP_AX -138.0001 106.8755 0.1331 1.1250 75.1251 -114.8750
EASY ROB 2.0
ERC LOAD VIEW S2 PTP_AX -138.0001 106.8755 0.1331 1.1250 ERC LOAD VIEW S PTP_AX -138.8752 121.6258 0.1833 2.8750 ERC LOAD TOOL STANFORD_TOOL_CLOSE ERC GRAB BODY HANG2 PTP_AX -139.3752 101.5008 0.0633 -1.0000 PTP_AX -30.6252 88.0009 0.0415 1.2500 ERC LOAD VIEW S1 PTP_AX -30.6252 88.0009 0.0415 1.2500 PTP_AX -30.6252 100.1258 0.0415 1.2500 ERC LOAD TOOL STANFORD_TOOL_OPEN ERC RELEASE BODY HANG2 !CHUONG TRINH GAP HANG 3 ERC LOAD VIEW S PTP_AX -30.6252 89.0009 0.0415 1.7500 PTP_AX -30.6252 80.8759 -0.1788 1.7500 PTP_AX 48.9997 80.8759 -0.1788 -0.8750 ERC LOAD VIEW S3 PTP_AX 55.6247 104.2509 0.0590 -0.8750 PTP_AX 55.6247 104.2509 0.0590 -0.8750 PTP_AX 55.7497 121.2512 0.1811 -1.0000 ERC LOAD TOOL STANFORD_TOOL_CLOSE ERC GRAB BODY HANG3 PTP_AX 55.7497 82.2513 -0.0479 -0.5000 ERC LOAD VIEW S PTP_AX -5.5003 84.0013 0.5193 -0.5000 ERC LOAD VIEW S1 PTP_AX -5.2815 96.6263 0.5193 -0.5000 ERC LOAD TOOL STANFORD_TOOL_OPEN ERC RELEASE BODY HANG3 ! CHUONG TRINH GAP HANG 4 ERC LOAD VIEW S PTP_AX -5.2190 80.6575 0.5193 -0.7187 PTP_AX -5.2190 80.6575 -0.0807 -0.7187 PTP_AX 94.9373 110.4075 0.1571 0.3750 ERC LOAD VIEW S3 PTP_AX 94.9373 119.5324 0.2477 0.1562 ERC LOAD TOOL STANFORD_TOOL_CLOSE ERC GRAB BODY HANG4 ERC LOAD VIEW S PTP_AX 95.0310 81.5324 -0.2546 0.2187 PTP_AX -11.8440 81.5324 -0.2546 0.1875 PTP_AX -8.9378 81.5324 0.0732 1.9062 ERC LOAD VIEW S1 PTP_AX -8.5315 99.5324 0.0732 0.4688 ERC LOAD TOOL STANFORD_TOOL_OPEN ERC RELEASE BODY HANG4 PTP_AX 0.0000 -0.0000 0.0000 -0.0000 WAIT 4 ! CHO XE CHỞ HÀNG ĐI ERC LOAD VIEW S_XE ERC LOAD BODY S1 WAIT 2 ERC LOAD BODY S2
EASY ROB 2.0
75.1251 -46.2500 55.2501 -50.0000 80.7500 -50.1250 91.0000 -74.1249 91.0000 -211.2500 76.7500 -211.2500
91.0000 -211.2500 95.6250 -209.6249 95.6250 -209.6249 77.1250 -244.4998 77.1250 -215.3750 59.5001 -217.3750 96.3749 -217.3750 96.3749 -184.7497 83.3750 -184.7497
97.3750 -184.7497 97.3750 -184.7497 72.3125 -175.1872 65.8438 -175.1872
97.8750 -175.1872 98.9687 -175.1872 98.9687 -187.7810 83.1875 -187.7810 -0.0000
-0.0000
WAIT 2 ERC LOAD BODY S3 WAIT 2 ERC LOAD BODY S4 WAIT 2 ERC LOAD BODY S5 ERC LOAD BODY S6 WAIT 2 ENDPROGRAMFILE
EASY ROB 2.0
DANH SÁCH NHÓM STT
EASY ROB 2.0
HỌ VÀ TÊN
MSSV
1
CAO QUANG MINH
0511953
2
LÊ VĂN KHÁNH
0512589
3
CAO VĂN THẮNG
0512101
4
NGUYỄN HOÀNH DƯƠNG
5
TRẦN ĐỨC CÔNG