111Equation Chapter 1 Section 1
BỘ GIÁO DỤC & ĐÀO TẠO
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HỒ CHÍ MINH
KHOA ĐIỆN – ĐIỆN TỬ
BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN
-----------------⸙∆⸙----------------- BÁO CÁO MÔN HỌC
THIẾT KẾ CÁNH TAY ROBOT 3 BẬC TỰ DO
GVHD: Trần Đức Thiện SVTH:
Tống Việt Hoàng 21151234
Nguyễn Viết trường 21151378
Tp. Hồ Chí Minh tháng 10 năm 2024
CHƯƠNG 1. THIẾT KẾ ROBOT 3 BẬC TỰ DO Mục lục
Mục lục......................................................................................................................1
Danh sách hình ảnh...................................................................................................3
Danh sách Bảng.........................................................................................................5 Chương 1.
Thiết kế robot 3 bậc tự do..................................................................6 1.1
Vẽ các bộ phận robot 3 bậc tự do với Solidworks....................................6 1.2
Đặt trục robot trong Solidworks...................................................................9 Chương 2.
Động học thuận..................................................................................1 2.1
Lập bảng DH................................................................................................1 2.2
Lập bảng DH................................................................................................2 2.3
Tính toán các ma trận chuyển đổi................................................................2 Chương 3.
Động học nghịch................................................................................4 3.1
Cách tiếp cận tính toán.................................................................................4 3.2
Các lời giải bài toán động học nghịch..........................................................5 Chương 4.
Quy hoạch quỹ đạo............................................................................8 4.1
Giới thiệu.....................................................................................................8 4.2
Quy hoạch quỹ đạo ở không gian khớp và không gian làm việc..................8 4.3
Các kiểu quy hoạch quỹ đạo........................................................................9 4.3.1
Vận tốc hình thang................................................................................9 4.3.2
Đa thức (Polynomial)..........................................................................10 4.4
Quy hoạch quỹ đạo robot 3 bậc dùng đa thức bậc 3...................................11 4.4.1
Quy hoạch quỹ đạo theo đường thẳng.................................................12 4.4.2
Quy hoạch quỹ đạo theo hình tam giác................................................13 Chương 5.
Mô phỏng và đánh giá......................................................................15 5.1
Mô phỏng động học thuận..........................................................................15 5.2
Mô phỏng động học nghịch........................................................................16 5.3
Mô phỏng quy hoạch quỹ đạo....................................................................19 5.3.1
Quy hoạch quỹ đạo theo đường thẳng 5 lần........................................19 5.3.2
Quy hoạch quỹ đạo theo hình tam giác 5 lần.......................................21 5.4
Chuyển đổi Solidworks sang Matlab..........................................................25 5.4.1
Cài đặt add-on Simscape Multibody cho Matlab...............................25 5.4.2
Xuất file xml từ Solidworks................................................................25 5.4.3
Mô phỏng bằng Matlab Simulink......................................................26
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 1
CHƯƠNG 1. THIẾT KẾ ROBOT 3 BẬC TỰ DO 5.4.4
Kết quả mô phỏng...............................................................................28 5.4.5
Kết quả kiếm chứng kết hợp với động học thuận và nghịch.............30 Chương 6.
Kết luận............................................................................................42 Chương 7.
Tài liệu tham khảo............................................................................43
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 2
CHƯƠNG 1. THIẾT KẾ ROBOT 3 BẬC TỰ DO Danh sách hình ảnh
Hình 1.1: Mô tả robot................................................................................................1
Hình 1.2: Đặt trục cho robot......................................................................................2
Hình 3.1: Quy hoạch chuyển động (Motion planning)..............................................9
Hình 3.2: Không gian làm việc và không gian khớp...............................................10
Hình 3.3: Vận tốc hình thang...................................................................................11
Hình 3.4: Quy hoạch đa thức...................................................................................12
Hình 3.5: Lưu đồ giải thuật từ điểm đến điểm.........................................................14
Hình 3.6: Sơ đồ giải thuật quy hoạch quỹ đạo tam giác..........................................15
Hình 4.1: Hình dáng robot với
..............................................16
Hình 4.2: Kiểm chứng động học thuận
.................................16
Hình 4.3: Hình dáng robot với
............................................17
Hình 4.4: Kiểm chứng động học thuận với .........................17
Hình 4.5: Kết quả kiểm chứng động học nghịch ...................19
Hình 4.6: Kết quả kiểm chứng động học nghịch .................19
Hình 4.7: Kiểm chứng động học nghịch ..........................19
Hình 4.8: Quy hoạch quỹ đạo qua 2 điểm 5 lần.......................................................20
Hình 4.9: Đồ thị biểu diễn vị trí x qua hai điểm A,B 5 lần......................................22
Hình 4.10: Đồ thị biểu diễn vị trí y qua hai điểm A,B 5 lần....................................22
Hình 4.11: Quy hoạch quỹ đạo qua 3 điểm 5 lần.....................................................23
Hình 4.12: Đồ thị biểu diễn vị trí x qua 3 điểm A,B,C 5 lần...................................25
Hình 4.13: Đồ thị biểu diễn vị trí y qua 3 điểm A,B,C 5 lần...................................25
Hình 5.1: Chuyển đổi đơn vị trong Solidworks.........................................................6
Hình 5.2: Mô hình 3D trong Solidworks...................................................................6
Hình 5.3: Hình ảnh khâu đế(Base) trên Solidworks..................................................7
Hình 5.4: Bản vẽ Link 1............................................................................................7
Hình 5.5: Bản vẽ Link 2............................................................................................8
Hình 5.6: Bản vẽ Link 3............................................................................................8
Hình 5.7: Matlab add-on cần tải................................................................................9
Hình 5.8: Kích hoạt add-in Simscape Multibody Link trong Solidworks..................9
Hình 5.9: Xuất file xml từ mô hình trong Solidworks.............................................10
Hình 5.10: Matlab Simulink từ file xml...................................................................10
Hình 5.11: Mô hình mô phỏng từ Simulink.............................................................10
Hình 5.12: Thiết lập xoay hệ trục tọa độ.................................................................11
Hình 5.13: Thiết lập khối Revolute.........................................................................11
Hình 5.14: Chuyển đổi tín hiệu đặt cho các khớp....................................................12
Hình 5.15: Chuyển đổi tín hiệu ra từ các khớp để quan sát.....................................12
Hình 5.16: Mô hình Simulink mô phỏng cánh tay robot.........................................12
Hình 5.17: Bên trong khối Matlab Subsystem.........................................................12
Hình 5.18: Kết quả mô phỏng 1..............................................................................13
Hình 5.19: Kết quả mô phỏng 2..............................................................................13
Hình 5.20: Kết quả mô phỏng 3..............................................................................14
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 3
CHƯƠNG 1. THIẾT KẾ ROBOT 3 BẬC TỰ DO
Hình 5.21: Toàn bộ chương trình Matlab Simulink kiểm chứng động học thuận
nghịch......................................................................................................................14
Hình 5.22: Trong khối 3DOF ROBOT ARM..........................................................17
Hình 5.23: Cấu hình khối Simulink-PS Converter..................................................17
Hình 5.24: Cấu hình khối PS-Simulink Converter..................................................18
Hình 5.25: Kết quả kiểm chứng động học nghịch bộ nghiệm
thứ nhất....................................................................................................................18
Hình 5.26: Hình dáng robot trường hợp bộ nghiệm thứ nhất 19
Hình 5.27: Kết quả kiểm chứng động học nghịch bộ nghiệm
thứ hai.....................................................................................................................19
Hình 5.28: Hình dáng robot trường hợp bộ nghiệm thứ hai. .20
Hình 5.29: Kết quả kiểm chứng động học nghịch bộ nghiệm
thứ nhất....................................................................................................................20
Hình 5.30: Hình dáng robot trường hợp bộ nghiệm thứ nhất
................................................................................................................................21
Hình 5.31: Kết quả kiểm chứng động học nghịch bộ nghiệm
thứ hai.....................................................................................................................21
Hình 5.32: Hình dáng robot trường hợp bộ nghiệm thứ hai 22
Hình 5.33: Kết quả kiểm chứng động học nghịch bộ
nghiệm thứ nhất.......................................................................................................22
Hình 5.34: Hình dáng robot trường hợp bộ nghiệm thứ
nhất..........................................................................................................................23
Hình 5.35: Kết quả kiểm chứng động học nghịch bộ
nghiệm thứ hai.........................................................................................................23
Hình 5.36: Hình dáng robot trường hợp bộ nghiệm thứ hai
................................................................................................................................24
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 4
CHƯƠNG 1. THIẾT KẾ ROBOT 3 BẬC TỰ DO Danh sách Bảng
Bảng 1.1: Thông số và ký hiệu của robot 3 bậc tự do................................................2
Bảng 1.2: Bảng DH...................................................................................................2
Bảng 3.1: Ưu nhược điểm của các không gian sử dụng quy hoạch.........................10
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 5
CHƯƠNG 1. THIẾT KẾ ROBOT 3 BẬC TỰ DO
Chương 1. Thiết kế robot 3 bậc tự do
1.1 Vẽ các bộ phận robot 3 bậc tự do với Solidworks
Để vẽ một robot nhiều bậc tự do thì chúng ta phải vẽ từng khâu của robot đó
rồi lắp ráp lại để được một cánh tay robot hoàn chỉnh. Sau khi tải phần mềm về, ta
cần chọn đơn vị sai cho chính xác với đơn vị ta đang sử dụng trên phần mềm
Solidworks. Về việc vẽ robot ta đang cần đơn vị milimet nên ta sẽ chuyển đổi thành
mlimet. Ta có thể thay đổi đơn vị ở góc phải dưới màn hình Solidworks.
Hình 1.1: Chuyển đổi đơn vị trong Solidworks
Ta sẽ có mô hình 3D robot trong môi trường Assembly của Solidworks như sau:
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 6
CHƯƠNG 1. THIẾT KẾ ROBOT 3 BẬC TỰ DO
Hình 1.2: Mô hình 3D trong Solidworks
Ta sẽ đi vào chi tiết từng khâu như sau:
Bản vẽ khâu đế được mô tả tại Hình 1.3
Hình 1.3: Hình ảnh khâu đế(Base) trên Solidworks
Khâu 1 có thể xoay của robot và có bản vẽ như Hình 1.4
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 7
CHƯƠNG 1. THIẾT KẾ ROBOT 3 BẬC TỰ DO Hình 1.4: Bản vẽ Link 1
Tiếp theo là bản vẽ cho khâu 2, 3 như các Hình 1.5: Bản vẽ Link 2
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 8
CHƯƠNG 1. THIẾT KẾ ROBOT 3 BẬC TỰ DO Hình 1.6: Bản vẽ Link 3
1.2 Đặt trục robot trong Solidworks
Để thực hiện đặt trục robot ta thực hiện các bước như sau:
Bước 1: Vẽ cấu hình robot và xác định các đường vuông góc chugn của robot
Bước 2: Vẽ trục tọa độ thứ {0} trùng với gốc của tọa độ
Bước 3: Xác định trục Zi trùng với các khớp quay thứ i
Bước 4: Xác định trục Xi dọc theo các đường vuông góc chung giữa 2 trục Z kề nhau.
Bước 5: Xác định trục Yi theo quy tắc bàn tay phải
Bước 6: Xác định sao cho trục của hệ trục tọa độ {0} trùng với hên trục tọa độ {1}
và hệ trục tọa độ {N} sẽ chọn XN là ngẫu nhiên (thường chọn dọc theo vị trí điểm đầu cuối)
Ta thực hiện đặt trục cho robot 3 bậc trên Solidworks tại Hình 1.2 như sau:
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 9
CHƯƠNG 1. THIẾT KẾ ROBOT 3 BẬC TỰ DO
Hình 1.7: Đặt trục trên Solidworks
212Equation Chapter (Next) Section 1
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 10
CHƯƠNG 2. ĐỘNG HỌC THUẬN
Chương 2. Động học thuận 2.1 Lập bảng DH
Sau khi thiết kế mô hình Solidworks ta lấy lại kích thước các khâu như hình để tính
toán chính xác lại động học của robot:
Hình 2.8: Thông số robot 3 bậc tự do Hình 2.9: Mô tả robot
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 1
CHƯƠNG 2. ĐỘNG HỌC THUẬN
Sau khi đặt trục xong ta xác định thông số và ký hiệu được nêu ra trong Hình 2.1 Ký hiệu Mô tả Thông số Đơn vị L0
Chiều dài từ gốc tọa độ tới đế 100 mm L1 Chiều dài link thứ 1 200 mm L2 Chiều dài link thứ 2 150 mm L3 Chiều dài link thứ 3 100 mm D1
Khoảng cách giữa trục tọa độ khớp 1 và 2 21 mm D2
Khoảng cách giữa trục tọa độ khớp 3 và 4 21 mm 2.2 Lập bảng DH
Lưu ý: Do mô hình robot 3 bậc tự do được thiết kế với D1 và D2 có độ dài bằng
nhau, nên hệ trục tọa độ của link thứ 3 nằm trên cùng mặt phẳng với hệ trục tọa độ
link thứ 1. Do đó, ta có thể xem như khoảng cách giữa trục tọa độ khớp 1 và 2, 3 và 4 bằng 0.
Tiếp theo dựa vào hình 2.1 và hình 2.2, ta lập được bẳng DH như sau: Bảng 2.1: Bảng DH 1 L0 0 L1 Q1 2 0 90 0 Q2 3 L2 0 0 Q3 4(EE) L3 0 0 0
Trong đó: I là vị trí của khớp;
là khoảng cách giữa trục và được đo theo trục ; là góc giữa trụ và
được tính toán quanh trục ; là khoảng cách giữa trục và
dọc theo trục ; là góc giữa trục và quanh trục
Ta có ma trận chuyển đổi giữa hệ trục tọa độ {i-1} sang hệ trục tọa độ {i} là: 323\* MERGEFORMAT (.)
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 2
CHƯƠNG 2. ĐỘNG HỌC THUẬN Trong đó:
2.3 Tính toán các ma trận chuyển đổi
Từ công thức 23 ta tìm ma trận chuyển đổi hệ trục tọa độ {0} sang hệ tọa độ {1}: 424\* MERGEFORMAT (.)
Ma trận chuyển đổi hệ trục tọa độ {1} sang hệ trục tọa độ {2}: 525\* MERGEFORMAT (.)
Ma trận chuyển đổi hệ trục tọa độ {2} sang hệ trục tọa độ {3}: 626\* MERGEFORMAT (.)
Ma trận chuyển đổi hệ trục tọa độ {3} sang hệ trục tọa độ {4(EE)}: 727\* MERGEFORMAT (.)
Từ đây ta tìm được ma trận chuyển đổi từ trục tọa độ {0} sang trục tọa độ {4(EE)}: 828\* MERGEFORMAT (.) Trong đó:
Ta tìm được tọa độ điểm đầu cuối:
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 3
CHƯƠNG 2. ĐỘNG HỌC THUẬN 929\* MERGEFORMAT (.)
1013Equation Chapter (Next) Section 1
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 4
CHƯƠNG 3. ĐỘNG HỌC NGHỊCH
Chương 3. Động học nghịch
3.1 Cách tiếp cận tính toán
Xác định mô hình động học thuận:
 Trước tiên, cần xây dựng mô hình động học thuận của hệ thống, trong đó xác
định mối quan hệ giữa các biến khớp (góc, độ dài, v.v.) và vị trí/hướng của điểm cuối.
 Mô hình này thường được biểu diễn dưới dạng phương trình hình học hoặc
phương trình ma trận (ví dụ: sử dụng phương pháp Denavit-Hartenberg).
Thiết lập phương trình động học nghịch:
 Sử dụng mô hình động học thuận, thiết lập các phương trình động học
nghịch. Mục tiêu là tìm các giá trị của biến khớp sao cho điểm cuối đạt được
vị trí và hướng mong muốn.
 Các phương trình này thường không có dạng đóng và có thể dẫn đến nhiều
nghiệm khác nhau (vì có thể có nhiều cấu hình khớp tương ứng với cùng một
vị trí/hướng điểm cuối).
Giải phương trình động học nghịch:
 Phương pháp giải tích: Nếu hệ thống đơn giản, có thể giải trực tiếp phương
trình động học nghịch để tìm các nghiệm của các biến khớp.
 Phương pháp số: Đối với hệ thống phức tạp, phương pháp giải tích thường
không khả thi, do đó người ta sử dụng các phương pháp số như thuật toán
Newton-Raphson, gradient descent, hay các phương pháp tối ưu hóa khác để tìm nghiệm.
Xử lý các nghiệm:
 Bài toán động học nghịch thường có nhiều nghiệm hoặc không có nghiệm.
Cần phải xác định nghiệm nào là phù hợp với ứng dụng cụ thể.
 Các tiêu chí như tối ưu hóa năng lượng, tránh va chạm, hoặc tối ưu hóa về độ
linh hoạt có thể được áp dụng để lựa chọn nghiệm.
Kiểm tra và điều chỉnh:
 Sau khi tìm được nghiệm, cần kiểm tra tính khả thi của chúng trong thực tế,
đặc biệt là khi hệ thống có các ràng buộc vật lý hoặc cơ học.
 Nếu cần, thực hiện điều chỉnh để đảm bảo hệ thống hoạt động chính xác.
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 5
CHƯƠNG 3. ĐỘNG HỌC NGHỊCH
3.2 Các lời giải bài toán động học nghịch
Ta có tọa độ điểm đầu cuối: 11311\* MERGEFORMAT (.) Ta được: 12312\* MERGEFORMAT (.) 13313\* MERGEFORMAT (.) 14314\* MERGEFORMAT (.) Tính :
Nhân một lượng vào pt (2.2) và một lượng vào pt (2.3) ta được: 1515\* MERGEFORMAT (2.) 1616\* MERGEFORMAT (2.)
Tiến hành lấy phương trình (2.5) trừ cho phương trình (2.6) để ta tìm được góc 1717\* MERGEFORMAT (2.) 1818\* MERGEFORMAT (2.) Tính :
Đầu tiên nhân 312 với lượng và 313 với lượng , ta có: 1919\* MERGEFORMAT (2.) 2020\* MERGEFORMAT (2.)
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 6
CHƯƠNG 3. ĐỘNG HỌC NGHỊCH
Cộng (2.9) và (2.10) trở thành 2121\* MERGEFORMAT (2.)
Kết hợp (2.11) và 314 ta được 2222\* MERGEFORMAT (2.) 23323\* MERGEFORMAT (.) Đặt ;
với A, B là hằng số. Từ đó 22 và 323 trở thành 2424\* MERGEFORMAT (2.) 25325\* MERGEFORMAT (.)
Ta lấy tổng bình phương từ 24 và 325 2626\* MERGEFORMAT (2.)
Rút gọn biểu thức 26 ta được 2727\* MERGEFORMAT (2.) Đặt ; ;
ta được kết quả thu gọn về phương trình 2828\* MERGEFORMAT (2.) Chia 28 cho ta được biểu thức 2929\* MERGEFORMAT (2.) Ta đặt
ta ra được công thức mới 3030\* MERGEFORMAT (2.) Rút gọn 30 ta được
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 7
CHƯƠNG 3. ĐỘNG HỌC NGHỊCH 3131\* MERGEFORMAT (2.) Từ đó ta tìm được 3232\* MERGEFORMAT (2.) Do ta đã đặt do đó ta tìm ra được 3333\* MERGEFORMAT (2.)
Chia phương trình 32 và 31 ta cô lập được góc 3434\* MERGEFORMAT (2.)
Từ đó ta rút ra được hai nghiệm của góc 35335\* MERGEFORMAT (.) 36336\ * MERGEFORMAT (.) Tính :
Đầu tiên, lấy phương trình 24 chia cho phương trình 325 ta được: 3737\* MERGEFORMAT (2.)
Tương ứng với mỗi nghiệm góc ta sẽ có một nghiệm góc 3838\* MERGEFORMAT (2.) 3939\* MERGEFORMAT (2.)
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 8
CHƯƠNG 3. ĐỘNG HỌC NGHỊCH
Sau khi tính toán bằng phương pháp đại số ta tìm được 2 bộ nghiệm cho robot 3 bậc tự do
Bộ nghiệm thứ nhất:
Bộ nghiệm thứ hai:
4014Equation Chapter (Next) Section 1
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 9