Giả sử robot được thiết kế với một số lượng các khâu và khớp tùy ý và có thể bố trí bất kì. Hình trên biểu diễn ba khâu kế tiếp nhau với hai khớp. Các khớp này có thể
ROBOT CƠNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo
là khớp tịnh tiến hoặc khớp xoay hoặc có thể cả hai. Trong thực tế, việc thiết kế robot thường là khớp bậc 5 để dễ dàng điều khiển nhưng trong hình vẽ thường biểu diễn các khớp này có hai bậc tự do. Mỗi khớp trong mơ hình hóa có thể xoay và tịnh tiến, giả sử ta đánh số n cho khớp thứ nhất trong hình trên, i+1 cho khớp thứ gau và i+2 cho khớp thứ ba. Như vậy có thể hiểu được rằng, trước và sau ba khớp này cịn có thể có rất nhiều khớp khác tác động và gắn lên hệ thống robot. Mỗi khâu cũng sẽ được đánh số, khâu i nằm giữa khớp i và i+1, khâu i+1 nằm giữa khớp i+1 và i+2.
Để mơ hình hóa robot với phương pháp D-H, đầu tiên cần phải gắn một hệ trục tham chiếu địa phương lên mỗi khớp. Vì vậy, mỗi khớp chung ta sẽ phải gắn trục Z và trục X. Chúng ta khơng nhất thiết phải gắn trục Y vì chúng ta có thể xác định trục Y theo tam diện thuận mà hệ trục tham chiết có được. Ngồi ra vì biểu diễn D-H khơng cần thiết sử dụng trục Y. Quá trình gắn hệ trục tham chiếu như sau:
Tất cả mọi khớp đều phải có trục Z. Nếu khớp là khớp xoay, trục Z được xác định theo hướng dịch chuyển tại ra do sự quay theo quy tác bàn tay phải. Nếu là khớp tịnh tiến, trục Z sẽ dọc theo hướng di chuyển. Và cách đánh số trục của khớp i sẽ là i-
1.Như vậy, trục Z biểu diễn khớp i+1 và Zi. Với quy tắc đơn giản này sẽ giúp chúng ta nhanh chóng gắn trục Z lên mọi khớp. Như vậy khi khớp quay, góc quay quanh trục Z sẽ là mốt biến khớp (θ hoặc α hoặc β) còn đối với khớp tịnh tiến thì chiều dài của khâu dọc theo trục Z biểu diễn bới thông số d sẽ là biến khớp.
Với việc mơ hình hóa khơng nhất thiết là các khớp phải song song hay giao nhau. Trong mơ hình đưa ra, trục Z là những đường thẳng chéo nhau. Như vậy luôn tồn tại một đường thẳng vng góc chung giữa các truc Z này, đây chính là khoảng cách ngắn nhất giữa hai trục Z. Hệ trục tham chiếu địa phương phải luôn được gắn theo hướng đường vng góc chung giữa trục Zi-1 và trục Zi thì hướng của trục Xi sẽ dọc theo hướng của đoạn ai. Tương tự như vậy, nếu đường vng góc chung giửa Zi và Zi+1 là ai+1, hướng của trục Xi+1 sẽ dọc theo ai+1. Đường vng góc chung của các khớp kế tiếp nhau không nhất thiết phải giao nhau hay trùng nhau. Các gốc tọa độ các hệ trục không cần trùng nhau. Ngoại trừ những trường hợp đặc biệt, ta có thể gắn hệ trục tọa độ lên mọi khớp. Nếu hai trục Z song song nhau, có vơ số đường vng góc chung thì chúng ta sẽ lấy đường vng góc chung của khớp trước. Cịn nếu trục Z của hai khớp
ROBOT CÔNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo
Như vậy, θ biểu diễn sự quay xung quanh trục Z, d biểu diễn khoảng cách trên trục Z giữa hai đường vng góc chung, a biểu diễn chiều dài đoạn vng góc chung cịn gọi là đoạn dịch chuyển của khớp (joint offset), α biểu diễn góc giữa hai truc Z kế tiếp nhau (góc xoắn). Thường chỉ có d và θ là biến khớp.
Bước kế tiếp cần di chuyển các sự chuyển vị từ hệ trục tọa đồ này đến hệ trục tọa độ kế tiếp. Giả sử ở hệ trục toạ độ địa phương i-1 chúng ta sẽ tạo ra bốn dịch chuyển tiêu chuẩn để di chuyển hệ trục này tới hệ trục i là:
Quay xung quanh trục Zi-1 một góc θi, sau khi quay xong các trục Xi-1 và Xi song song với nhau vì ai-1 và ai đều vng góc Zi-1 và quay quanh Zi-1 một góc θi sẽ làm chúng song song.
Tịnh tiến dọc trục Zi-1 một đoạn di làm cho Xi-1 và Xi trùng nhau. Rõ ràng khi Xi-1 và Xi song song và vng góc Zi-1, như vậy, khi di chuyển dọc theo Zi-1 thì chúng trùng với nhau.
Di chuyển dọc theo trục Xi-1 một khoảng a1 để Zi-1 nằm thẳng với trục Zi. Ở điểm này, hai gốc tọa độ của hai hệ trũc tham chiếu sẽ ở cùng một điểm. Quay trục Zi-1 xung quanh trục Xi một góc αi để trục Zi-1 nằm thẳng với trục Zi. Ở điểm này, hệ trục i-1 và i sẽ hoàn toàn trung nhau. Và chúng ta sẽ tiếp túc di chuyển từ hệ trục này đến hệ trục tiếp theo.
Tương tự bốn chuyển động giữa hệ trục i và i+1 sẽ tạo nên chuyển vị tiếp theo. Bắt đầu từ hệ trục tham chiếu, ta sẽ tạo nên chuyển vị từ đế robot, sau đó, tới khớp thứ nhất, thứ hai và cuối cùng tới cơ cấu chấp hành.
Ma trận A biểu diễn bốn di chuyển bằng cách nhân bốn ma trận biểu diễn từng chuyển động, kết quả như sau:
ROBOT CÔNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo
Với mỗi hệ robot, ta có thể bắt đầu từ khớp thứ nhất tới khớp thứ hai rồi khớp thứ ba… cho tới cơ cấu chấp hành của robot. Mỗi ma trận chuyển vị là Ai+n. Như vậy, ma trận chuyển vị tổng giữa đế và cơ cấu chấp hành là
Với n là số khớp. Với robot có sáu bậc tự do thì có sáu ma trận. Để tính tốn được ma trận A chúng ta phẩn lập ra bản tham số khớp và khâu với các giá trị để biểu diễn khớp và khâu được xác định từ bản vẽ hay cấu hính robot.
2.6.3. Giải thuật gắn hệ trục tọa độ cho Robot
Bắt đầu đánh số từ 0 đến n (n là số khâu của robot) và đế robot được đánh số 0. Mỗi hệ trục tọa độ của đế được gắn song song với hệ trục tham chiếu. Gốc của hệ
trục tọa độ này được gắn trùng với gốc của khớp 1. Với giả thiết là trục khớp 1 vng góc với mặt phẳng xy.
Hệ trục tọa đọ gắn trên khâu robot được gắn tại khớp xa hơn so với đế. Vì vậy chúng ta sẽ thấy hệ trục tọa độ số 1 sẽ gắn tại khớp số 2 (khớp này nối giữa khâu 1 và khâu 2).
Gốc của hệ trục tọa độ đặt tại giao điểm của đường vng góc chung của các trục khớp với nhau. Nếu các trục khớp song song với nhau thì vị trí của gốc của hệ trục tọa độ được lựa chọn sao cho khoảng cách giữa các khâu bằng 0 hoặc nhỏ nhất nếu có một khoảng dịch chuyển giữa các khâu. Nếu các trục khớp giao nhau thì gốc toại độ sẽ đặt tại điểm giao nhau của các trục.
Trục Z trùng với trục khớp. Nếu khớp tịnh tiến thì hướng trục Z sẽ là hướng di chuyển đi xa từ khớp. Nếu khớp trụ, hướng của trục Z xác định là hướng dương theo hướng quay trục Z (theo người thiết kế) hoặc theo hướng lựa chọn sao cho góc xoắn nhỏ nhất.
Trục X sẽ song song với đường vng góc chung giữa các trục khớp của khâu. Trong trường hợp các trục khớp song song với nhau, trục X sẽ trùng với đường tâm của khấu. Nếu các trục giao nhau khơng có đường vng góc chung trục X sẽ là tích vector của hai vector Zn-1 và Zn. Trong nhiều trường hợp trục X sẽ có hướng như trục X của khâu trước.
ROBOT CÔNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo
cùng của cánh tay robot với hệ trục của tay gắp/ Trục Z của hệ trục này cùng hường với khớp cuối cùng.
Từ đó ta có thể rút ra được bốn quy tắc để gắn hệ trục tọa độ theo phương pháp D-H:
Chọn trục Z sao cho trục Z là trục của động cơ.
Chọn trục X là sao cho trục Xi phải cùng vng góc với Zi và Zi-1. Trục Y là trục còn lại của quy tác bàn tay phải.
Trục X phải cắt phương của trục Zi-1, nếu trục X khơng cắt phương của Zi-1, ta có thể dời tâm của trục tọa độ i theo phương của trục Z sao cho thỏa điều kiện trên.
2.7. Động học thuận – nghịch cho mơ hình cánh tay robot 5 bậc tự do 2.7.1. Động học thuận
Sau khi đợn giản hóa mơ hình robot và gắn hệ trục tọa độ ta thành lập được bảng D-H như sau: Khâu 1 2 3 4 5
Bảng 2 Bảng DH Robot Arm 5 DOF.
Ta có ma trận chuyển vị dạng tổng quát như sau:
Đặt:
Dựa vào bảng D-H trên ta tính được các ma trận chuyển vị sau:
ROBOT CÔNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo
Ma trận chuyển vị cuối:
ROBOT CÔNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo
2.7.2. Động học nghịch
Tính các góc dựa vào hình ảnh minh họa robot trong khơng gian như hình bên dưới:
Hình 2.15. Robot 5 bậc tự do biểu diễn trong khơng gian
ROBOT CƠNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo
Dựa vào hình vẽ, ta có: Tính góc :
Tính góc :
Xét tam giác :
Áp dụng định lý cosin trong tam giác, ta có:
11\* MER GEFO RMA T () Mà: Xét tam giác , ta có: Ta có: Với: Suy ra: 22\* MER GEFO RMA T () Ta lại có: Với:
ROBOT CÔNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo Từ (1) và (2) ta suy ra: 44\* MER GEFO RMA T () Thay phương trình (4) vào phương trình (1) ta tính được giá trị , từ đó suy ra: 55\* MER GEFO RMA T () Vậy ta tính được góc : 66\* MER GEFO RMA T () Tính góc : Ta thấy rằng: Trong đó: 77\* MER GEFO RMA T () Từ hình vẽ ta tính dược: 88\* MER GEFO RMA T () Mà: Nhóm thực hiện: Nhóm 999 Trang 32
ROBOT CÔNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo 99\* MER GEFO RMA T () Từ (2), (3), (8) và (09) ta tính được , từ giá trị , đã tính được ở trên, ta suy ra giá trị góc .
Tính :
Xét điều kiện ban đầu là , ta suy ra:
1010\* MER GEFO RMA T ()
ROBOT CÔNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo
CHƯƠNG 3. THIẾT KẾ MƠ HÌNH VÀ MÔ PHỎNG CÁNH TAY ROBOT 5 BẬC TỰ DO TRÊN EASY ROB
3.1. Phần mềm EASY ROB
EASY-ROB là công cụ mô phỏng robot sử dụng đồ hoạ trong không gian 3 chiều (3D) và các hình ảnh có thể hoạt động được. Một hệ thống 3D-CAD đơn giản 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, khối hình thang, ... để vẽ kết cấu của robot. Trong EASY-ROB chúng ta có thể dùng chuột để quay hay tịnh tiến robot đến một toạ độ tuỳ ý. EASY-ROB cũng có các chức năng phóng to, thu nhỏ đối tượng vẽ như nhiều phần mềm thiết kế khác...
Chương trình cho phép thiết kế các robot đến 12 bậc tự do. Chuyển động của Robot có thể được điều khiển theo các biến khớp hay các toạ độ Đề-cát. Chúng ta cũng có thể mơ tả động học của robot theo kiểu DH hay trong hệ toạ độ tồn cục (Universa Coordinates).
EASY-ROB đã có sẵn các trình điều khiển động học thuận và ngược của các cấu hình robot thơ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 hay 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 giải bài tốn động học ngược hay xác định hàm tốn học mơ tả sự phụ thuộc của khâu bị động đối với khớp quay, 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 tập tin thư viện liên kết động er_kin.dll (Easy- Rob kinematic Dynamic link library), khi chạy chương trình, EASY-ROB sẽ liên kết với tập tin nầy và thực hiện kiểu động học đã được khai báo trong chương trình điều khiển.
Hình 3.16. Giao diện thiết làm việc trên EASY ROB.
ROBOT CÔNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo
3.2. Mơ hình Robot được thiết kế trên EASY ROB
Hình 3.17. Mơ hình robot thiết kế trên phần mềm EASY ROB.
Code mơ phỏng chương trình:
PROGRAMFILE !prgfln F:\EASY_ROB\EASY_ROB\robot project\prg_tmp.prg PTP_AX 1.5000 -25.5000 27.0000 0.0000 -0.0000 PTP_AX -0.0000 PTP_AX -0.0000 PTP_AX
ROBOT CÔNG NGHIỆP PTP_AX -24.5000 PTP_AX -24.5000 PTP_AX -42.0000
ERC LOAD TOOL TRANG2 ERC RELEASE BODY vat1 PTP_AX -42.0000 PTP_AX -42.0000 PTP_AX -27.5000 PTP_AX -27.5000
ERC LOAD TOOL TRANG2 ERC GRAB BODY vat2 PTP_AX -27.5000 PTP_AX -27.5000 PTP_AX -18.5000
ERC LOAD TOOL TRANG2 ERC RELEASE BODY vat2 PTP_AX -18.5000 PTP_AX -40.5000 PTP_AX -40.5000
ERC LOAD TOOL TRANG2 ERC GRAB BODY vat3 PTP_AX
-40.5000
ROBOT CÔNG NGHIỆP
ERC LOAD TOOL TRANG2 ERC RELEASE BODY vat3 PTP_AX -40.5000 PTP_AX -40.5000 PTP_AX -0.5000 ENDPROGRAMFILE
Kết quả sau khi mô phỏng: vị trí vật được dịch chuyển tới vị trí đã được lập trình.
ROBOT CƠNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo
CHƯƠNG 4. THIẾT KẾ MƠ HÌNH CÁNH TAY ROBOT 5 BẬC TỰ DO TRÊN SOLIDWORKS VÀ MÔ PHỎNG TRÊN MATLAB SIMULINK
4.1. Phần mềm SolidWorks
SOLIDWORKS là phần mềm thiết kế 3D tham số chạy trên hệ điều hành Windows và có mặt từ năm 1995, được tạo bởi cơng ty SOLIDWORKS Dassault Systèmes, là một công ty thành viên của tập đồn cơng nghệ hàng đầu thế giới Dassault Systèmes, S. A. (Vélizy, Pháp). Cộng đồng người dùng SOLIDWORKS bản quyền trên thế giới hiện là gần 6 triệu người với khoảng 200.000 doanh nghiệp và tập đồn.
Hình 4.19. Phần mềm SolidWorks.
4.2. Phần mềm Matlab
MATLAB là phần mềm cung cấp mơi trường tính tốn số và lập trình, do cơng ty MathWorks thiết kế. MATLAB cho phép tính tốn số với ma trận, vẽ đồ thị hàm số hay biểu đồ thơng tin, thực hiện thuật tốn, tạo các giao diện người dùng và liên kết với những chương trình máy tính viết trên nhiều ngơn ngữ lập trình khác.
ROBOT CƠNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo
Với thư viện Toolbox, MATLAB cho phép mơ phỏng tính tốn, thực nghiệm nhiều mơ hình trong thực tế và kỹ thuật.
Hình 4.20. Phần mềm Matlab.
ROBOT CÔNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo
4.4. Mô phỏng cánh tay robot với Matlab Simulink
Trong phần này nhóm chúng em sẽ liên kết hai phần mềm Solidworks và Matlab với nhau để mô phỏng robot chuyển động.
Hình 4.22. Phần mềm MATLAB và SOLID WORKS.
4.4.1. Chuyển mơ hình robot từ Solidwork sang Matlab Simulink
Nhóm đã sử dụng cơng cụ Simscape Multibody Link để chuyển mơ hình robot từ Solidworks sang Simulink (Matlab).
Hình 4.23. Kết quả sau khi chuyển đổi.
4.4.2. Tạo giao diện điều khiển robot
Nhóm đã lựa chọn thiết kế giao diện bằng công cụ Guide trên Matlab để tạo ra giao diện điều khiển robot.
ROBOT CÔNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo
ROBOT CÔNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo
Kết quả điều khiển.
ROBOT CÔNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo
CHƯƠNG 5. THI CƠNG MƠ HÌNH ROBOT 5 BẬC TỰ DO VÀ LẬP TRÌNH ĐIỀU KHIỂN ROBOT THEO HAI CHẾ ĐỘ AUTO VÀ MANUAL
5.1. Hình ảnh thi cơng mơ hình
Hình 5.25. Các phần của robot và động cơ trước khi lắp ráp.
ROBOT CÔNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo Servo khop1; Servo khop2; Servo khop3; Servo khop4; Servo khop5; Servo khaukep; int cb = 2; int bientroX = A0 ; int bientroY = A1 ; int bientroZ = A2; int bientrokep = A3;
bool mode; float x,y,z;
float a1,a2,a3,a4,a5; float d1,d2,d3,d4,d5;
float ap1, ap2, ap3, ap4, ap5; float t1, t2, t3, t4, t5;
int t1p, t2p, t3p, t4p, t5p; float cos_t3, sin_t3, beta, alpha; float t235 = 0; float d, xd, yd,r4,z4,s; float px,py,pz; int t1d,t2d,t3d,t4d,t5d,gockep; float dophangiai = 0.5; void setup(){ lcd.init(); lcd.backlight(); lcd.setCursor(0, 0); Nhóm thực hiện: Nhóm 999 Trang 44
ROBOT CƠNG NGHIỆP GVHD: ThS. Trương Ngọc Bảo