M6 ta Robot trén Solidwork Canh tay robot 3 bac ty do là một hệ thong co khi được thiết kế để mô phỏng hoạt động của cánh tay con người, với khả năng di chuyên và điều chỉnh vị trí trong
Cách tiếp cận tính toan cc ccccccccccccescssessescesessesesessvsessesessesecevsensevevevssesevevsnsetevevenss 11
Trong động học robot, bài toán động học nghịch là việc xác định giá trị các biến khớp khi đã biết vị trí và hướng của bộ phận cuối Bài toán này thường phức tạp hơn bài toán động học thuận do tính phi tuyến và đa nghiệm Có hai phương pháp chính để giải bài toán động học nghịch: phương pháp hình học và phương pháp đại số Nhóm nghiên cứu đã áp dụng phương pháp đại số, sử dụng các phép biến đổi đại số, công thức lượng giác, và phép đồng nhất hệ số để tính toán các biến khớp q1, q2, q3 dưới dạng tổng quát.
Các lời giải bài toán động học nghịch 2012211212112 111121112211 He 11 CHƯƠNG 3: XUẤT BẢN VẼ 2D CHO CAC KHAU CUA ROBOT VA CHUYEN ĐỌÓI SOLIDWORKS SANG SIMSCAPE HH HH HH HH 15
- Ta viét lai ma tran “T„„ dưới dạng như sau:
- Nhân cả 2 về của đẳng thức với ma trận nghịch đảo ta được:
- _ Từ đó ta có các phương trỉnh tương đương như sau: ° 13213\*
- _ Xét phương trình (2.5) ta được:
- Binh phuong 2 vé phuong trinh (2.4) va (2.6) sau d6 cong vé theo vé cua 2 phuong trình trên ta được:
- Bién d6i phuong trình (2.4) va (2.6) bằng các phương trình lượng giác ta được:
- Tu (2.7) va (2.8) ta co hé phuong trinh:
- Từ đó, ta tính được l nghiệm
Kết luận: bằng phương pháp đại số ta thu được 4 bộ nghiệm của các biến khớp q, q; ¢ lần lượt như sau:
CHƯƠNG 3: XUẤT BẢN VẼ 2D CHO CÁC KHẨU CUA ROBOT VA CHUYEN
Xuất bản vẽ 2D 2c: 222 2221112111102211112211122111.1211121112011211 1e de 15
Để xuất bản vẽ 2D cho một khâu bất ky ta tạo một file Drawing trên phân mềm
SolidWorks, sau đó chọn khô giấy có kích thước phù hợp Tiến hành thiết lập đơn vị đo là millimeter tai g6c phai man hình
Để thiết lập đơn vị đo trong phần mềm SolidWorks, bạn cần tạo bản vẽ 2D bằng cách chọn Drawing — Model View — Browser Tiếp theo, hãy chọn file 3D của bộ phận mong muốn và đưa các hình chiếu ra ngoài Lưu ý chọn tỷ lệ bản vẽ cho phù hợp.
Khâu dé là phần đầu tiên trong chuỗi liên kết của robot, đóng vai trò là bộ phận cố định từ đó các khớp và cánh tay được hình thành Nó được xem như điểm gốc hay "gốc tọa độ" của robot, nơi thực hiện tất cả các tính toán liên quan đến vị trí và chuyển động.
Khâu đề là bộ phận quan trọng của robot, bao gồm động cơ, hệ thống điều khiển và các thành phần thiết yếu để đảm bảo robot di chuyển ổn định Từ khâu đề, các phần khác của robot được kết nối qua các khớp nối, tạo ra chuyển động tổng thể cho robot trong quá trình hoạt động.
Hình 4 Bản vẽ 2D khâu để s% Link l:
Link 1 là liên kết kết nối giữa khâu để và link 2 Cả hai liên kết này đều có dạng khớp xoay, cho phép thực hiện chuyển động quay theo phương song song với mặt phẳng của đề.
Link 2 là link liên két gitra link 1 va End Effector, ca 2 liên kết này đều là khớp xoay s%* End Effector:
End effector là bộ phận cuối cùng của cánh tay robot, có vai trò quan trọng trong việc tương tác với môi trường bên ngoài để thực hiện các nhiệm vụ cụ thể Nó có thể là công cụ, dụng cụ hoặc thiết bị mà robot sử dụng để thao tác, gắp, di chuyển hoặc xử lý các vật thể một cách hiệu quả.
Hinh 7 Ban vé 2D cua End Effector
Để mô phỏng robot bằng Simscape trong MATLAB Simulink, cần thực hiện 4 bước cơ bản Bước đầu tiên là cài đặt Simscape Multibody Link cho MATLAB.
Truy cập theo đường link sau: https://www.mathworks.com/campaigns/offerings/download_smlink_confirmation.html? elqsid46655391846&potential_use=student
Để cài đặt Tái Simscape cho phiên bản MATLAB, hãy chạy MATLAB với quyền quản trị viên Trong cửa sổ Command Window, nhập lệnh install_addon(‘smlink-r2022b-win64.zip’) Sau khi cài đặt hoàn tất, tiếp tục gõ lệnh smlink_linksw để liên kết với phần mềm SolidWorks Bước tiếp theo là thêm Simscape cho SolidWorks.
Vao Tools— Add-Ins — Simscape Multibody Link
Hinh 8 Click chon Simscape tai cira s6 Add-Ins s - Bước 3: Xuất File XML cho mô hình robot 3 bậc
Vao Tools—Simscape Multibody Link cho phép xuất file Simscape Multibody Đầu tiên, bạn cần chọn thư mục lưu file và xác nhận Sau đó, để xuất file, hãy nhập lệnh smimport(*#”) vào cửa sổ Command của MATLAB, trong đó * là tên file XML đã lưu trước đó Kết quả là bạn sẽ có một file Simulink mô phỏng cho hệ cánh tay robot 3 bậc mà bạn đã thiết kế trên SolidWorks.
Hinh 9 File simulink m6 phong hệ cánh tay robot 3 bậc ¢ Thiết lập các thông số mô phỏng
To access the Model Settings dialog, press Ctrl + E In the Solver selection section, choose 'Fixed step' and set the fixed step size to 0.01 Next, in the Block Parameters under Mechanism Configuration, adjust the acceleration values in the field accordingly.
Hình 10 Chỉnh sửa giá trị gia toc trong trường
Trong khối Revolute, ta chọn vào mục Actuation để chỉnh sửa Torque va Motion Tai muc Sensing ta tich chon Position
Để chỉnh sửa các khối Revolute trong mô hình, cần nhập giá trị góc quay để thu được mô phỏng Simscape hoàn chỉnh Sau đó, tiến hành chạy mô phỏng nhằm kiểm chứng kết quả đạt được.
Hình 12 File mô phỏng Simscape đề kiêm chứng các góc quay
CHUONG 4194Equation Section 4: QUY HOACH QUY DAO CHO ROBOT
4.1 Giới thiệu về quy hoạch quỹ đạo
Quy hoạch quỹ đạo là một phần quan trọng trong các vấn đề như dẫn đường và quy hoạch chuyển động Hệ thống cấp bậc cho quy hoạch chuyển động bao gồm: Thiết kế mục tiêu (task planning), đưa ra đường dẫn khả thi từ điểm đầu đến điểm cuối (path planning), lập lịch trình di chuyển theo thời gian với các ràng buộc về vị trí, vận tốc và gia tốc (trajectory planning), và cuối cùng là sử dụng hệ thống điều khiển để thực hiện kế hoạch một cách chính xác (trajectory following).
Hình 13 Quy hoạch chuyên động 4.2 Quy hoạch quỹ đạo ở không gian khớp và không gian làm việc
Quy hoạch quỹ đạo trong không gian làm việc liên quan đến việc xác định các điểm tham chiếu và nội suy theo năm trên trục tọa độ Descartes, nhằm xác định vị trí và hướng của một vị trí cụ thể trên cánh tay robot, thường là ở khâu cuối (end effector).
Quy hoạch quỹ đạo trong không gian khớp liên quan đến việc xác định các điểm tham chiếu và thực hiện nội suy trực tiếp tại các vị trí khớp Hai quỹ đạo này hoạt động trong các không gian khác nhau, tạo ra sự khác biệt trong cách thức di chuyển và điều khiển.
- Đối với mỗi không gian khác nhau sẽ có những ưu nhược điểm riêng, ưu nhược điểm của mỗi không gian ® Không gian làm việc:
-_ Ưu điểm: + Chuyên động có thể dự đoán
+ Có khả năng tránh chướng ngại vật và va chạm tốt hơn
- Nhược điểm: + Thực hiện chậm hơn
+ Chuyên động của cơ cầu truyền động không trơn tru và khó xác định hơn ® Không gian khớp:
- Ưuđiểm: + Tốc độ thực hiện nhanh hơn
+ Chuyển động của cơ cầu truyền động trơn tru và dễ xác định hơn
-_ Nhược điểm: Các điểm trung gian không được đảm bảo tuân theo các giới hạn chung hoặc va chạm
4.3 Phương pháp quy hoạch quỹ đạo bằng hàm đa thức
Phương pháp quy hoạch quỹ đạo bằng hàm đa thức sử dụng đa thức bậc 3 và bậc 5, trong đó nhóm nghiên cứu lựa chọn đa thức bậc 3 để thực hiện quy hoạch quỹ đạo cho cánh tay robot 3 bậc.
Các điêu kiện ràng buộc: ¢ Tai diém bat dau: 20420\
* MERGEFORMAT (.) Đa thức bậc ba có dạng tông quát như sau: ° 22422\
Dao ham cua da thức bậc ba: ° 23423\
Từ các điêu kiện ràng buộc ta có: ° 24424\* MERGEFORMAT (.) ° 25425\* MERGEFORMAT (.) ° 26426\* MERGEFORMAT (.) ° 27427\* MERGEFORMAT (.)
Ta có thể viết lại các phương trình dưới dạng ma trận như sau:
Khi đó ta có: 29429\* MERGEFORMAT (.)
Các hệ số của đa thức bậc ba có thê được tính theo công thức nhự sau:
Sau khi tìm được các hệ số của đa thức bậc ba ta có thê quy hoạch quỹ đạo cho robot theo ham bac ba
4.4 Quy hoạch quỹ đạo theo đường thắng
Chúng ta cần lập kế hoạch quỹ đạo cho robot di chuyển từ điểm A đến điểm B trong thời gian t và trở về từ điểm B đến điểm A cũng trong thời gian t, dựa trên hàm đa thức bậc ba Toàn bộ chu kỳ này sẽ được lặp lại 5 lần trước khi robot dừng lại tại điểm A.
Giới thiệu về quy hoạch quỹ đạo - 5s tt E1 11112121111 2 211 ng ng run 21
Quy hoạch quỹ đạo là một yếu tố quan trọng trong các vấn đề như dẫn đường và quy hoạch chuyển động Hệ thống cấp bậc trong quy hoạch chuyển động bao gồm: Thiết kế mục tiêu trong giai đoạn lập kế hoạch nhiệm vụ, xác định đường dẫn khả thi từ điểm khởi đầu đến điểm kết thúc trong giai đoạn lập kế hoạch đường đi, lên lịch trình di chuyển theo thời gian với các ràng buộc về vị trí, vận tốc và gia tốc trong giai đoạn lập kế hoạch quỹ đạo, và cuối cùng là sử dụng hệ thống điều khiển để thực hiện kế hoạch một cách chính xác trong giai đoạn theo dõi quỹ đạo.
Quy hoạch quỹ đạo ở không gian khớp và không gian làm viỆc
Quy hoạch quỹ đạo trong không gian làm việc liên quan đến việc xác định các điểm tham chiếu và nội suy năm trên trục tọa độ Descartes, nhằm xác định vị trí và hướng của một điểm cụ thể trên cánh tay robot, thường là khâu cuối (end effector).
Quy hoạch quỹ đạo trong không gian khớp bao gồm việc xác định các điểm tham chiếu và thực hiện nội suy trực tiếp tại các vị trí khớp Hai quỹ đạo này hoạt động trong các không gian khác nhau.
- Đối với mỗi không gian khác nhau sẽ có những ưu nhược điểm riêng, ưu nhược điểm của mỗi không gian ® Không gian làm việc:
-_ Ưu điểm: + Chuyên động có thể dự đoán
+ Có khả năng tránh chướng ngại vật và va chạm tốt hơn
- Nhược điểm: + Thực hiện chậm hơn
+ Chuyên động của cơ cầu truyền động không trơn tru và khó xác định hơn ® Không gian khớp:
- Ưuđiểm: + Tốc độ thực hiện nhanh hơn
+ Chuyển động của cơ cầu truyền động trơn tru và dễ xác định hơn
-_ Nhược điểm: Các điểm trung gian không được đảm bảo tuân theo các giới hạn chung hoặc va chạm
4.3 Phương pháp quy hoạch quỹ đạo bằng hàm đa thức
Phương pháp quy hoạch quỹ đạo bằng hàm đa thức sử dụng đa thức bậc 3 và bậc 5, trong đó nhóm nghiên cứu chọn đa thức bậc 3 để quy hoạch quỹ đạo cho cánh tay robot 3 bậc.
Các điêu kiện ràng buộc: ¢ Tai diém bat dau: 20420\
* MERGEFORMAT (.) Đa thức bậc ba có dạng tông quát như sau: ° 22422\
Dao ham cua da thức bậc ba: ° 23423\
Từ các điêu kiện ràng buộc ta có: ° 24424\* MERGEFORMAT (.) ° 25425\* MERGEFORMAT (.) ° 26426\* MERGEFORMAT (.) ° 27427\* MERGEFORMAT (.)
Ta có thể viết lại các phương trình dưới dạng ma trận như sau:
Khi đó ta có: 29429\* MERGEFORMAT (.)
Các hệ số của đa thức bậc ba có thê được tính theo công thức nhự sau:
Sau khi tìm được các hệ số của đa thức bậc ba ta có thê quy hoạch quỹ đạo cho robot theo ham bac ba
4.4 Quy hoạch quỹ đạo theo đường thắng
Chúng ta cần lập kế hoạch quỹ đạo cho robot di chuyển từ điểm A đến điểm B trong thời gian t, sau đó quay trở lại từ điểm B về điểm A cũng trong thời gian t, theo hàm đa thức bậc ba Toàn bộ chu kỳ này sẽ được lặp lại 5 lần trước khi robot dừng lại tại điểm A.
Lưu đồ giải thuật giúp xác định thời gian hiện tại trong chu kỳ bằng cách sử dụng phép chia lấy dư Với t là thời gian thực tế và Ty là tổng thời gian chạy của 5 chu kỳ, ta có thể tính toán thời gian thuộc khoảng quy hoạch quỹ đạo nào Thời gian I chu kỳ được xác định là 2t, từ đó ta tiến hành quy hoạch quỹ đạo tương ứng và dừng lại tại điểm đã xác định.
A khi thời gian thực tế bằng với tổng thời gian 5 chu ky
Sử dụng lưu đồ giải thuật và phương pháp quy hoạch quỹ đạo bằng hàm đa thức bậc 3, chúng tôi tiến hành mô phỏng quy hoạch quỹ đạo trên Matlab Simulink.
4.5 Quy hoạch quỹ đạo theo hình tam giác
Chúng ta cần thiết lập quỹ đạo cho robot di chuyển từ điểm A đến điểm B trong thời gian t, sau đó từ điểm B đến điểm C cũng trong thời gian t, và cuối cùng từ điểm C trở về điểm A với thời gian t Quá trình này sẽ được mô phỏng theo hàm đa thức bậc ba Toàn bộ chu kỳ di chuyển này sẽ được lặp lại 5 lần.
27 dừng lại tại điểm A Lưu đồ giải thuật được trình bảy tương tự như quỹ đạo đường thang
Goi t là thời gian thực tế, trong khi Ty là tổng thời gian chạy của 5 chu kỳ Để tính toán thời gian hiện tại trong chu kỳ quy hoạch quỹ đạo, ta sử dụng phép chia lấy dư của t cho thời gian 1 chu kỳ là 3t Từ đó, quy hoạch quỹ đạo sẽ được thực hiện theo từng khoảng thời gian tương ứng và sẽ dừng lại tại điểm A khi thời gian thực tế đạt tổng thời gian của 5 chu kỳ.
Sử dụng lưu đồ giải thuật và phương pháp quy hoạch quỹ đạo bằng hàm đa thức bậc 3, chúng tôi tiến hành mô phỏng quy hoạch quỹ đạo trên Matlab Simulink.
CHUONG 5: MO PHONG KIEM CHUNG
5.1 Kiém ching géc quay va mô phỏng Simscape ¢ Truong hop 1: q, q,=0 q;=
Hinh 14 Tue thể robot trong trường hợp 1 Nhận xét: Tư thế robot ở điểm EE hoàn toàn đúng với góc quay mà ta mô phỏng s% Trường hợp 2: q,=0 q;, q,=Ý
Hinh 15 Tue thé robot trong trường hợp 2 Nhận xét: Tư thế robot ở điểm EE hoàn toàn đúng với góc quay mà ta mô phỏng
Tư thế của robot tại điểm EE hoàn toàn khớp với góc quay đã được mô phỏng Qua việc thử nghiệm từng trường hợp, chúng ta nhận thấy robot thực hiện đúng các góc quay mà chúng ta mong muốn.
5.2 Mô phỏng và đánh giá động học thuận
5.2.1 Mô phỏng trên phần mềm Matlab
Hình 17 Mô phỏng kiểm tra động học thuận trên Matlab e - Chương trình trong khối kiểm tra động học thuận DHT trong khối Matlab Function
13p,; x*cos(ql)*(7*cos(q2 + q3) + 10*cos(q2)); y*sin(q1)*(7*cos(q2 + q3) + 10*cos(q2));
5.2.2 Đánh giá kết quả của động học thuận trên Matlab ® Xét trường hợp:
Hình l8 Mô hình robot trong trường hợp Ì
Từ hình vẽ: ta thấy được tọa độ của EE lần lượt là x0; y=0; z nh 19 Kết quả tọa độ thu được trong trường hợp 1
Kết quả mô phỏng với tọa độ trên hình vẽ của EE cho thấy sự tương đồng, từ đó khẳng định rằng kết quả tính toán trước đó là chính xác.
Hình 20 Mô hình robot trong trường hợp 2
33 nh 21 Kết quả tọa độ thu được trong trường hợp 2
Kết quả mô phỏng với tọa độ trên hình vẽ của EE cho thấy sự tương đồng, từ đó khẳng định tính chính xác của kết quả tính toán trước đó Qua việc mô phỏng động học thuận trên Matlab, tọa độ ở các trường hợp đã phản ánh chính xác tọa độ của robot, chứng minh rằng kết quả tính toán động học thuận là đúng.
5.3 Mô phỏng và kiếm chứng kết quả động học nghịch
5.3.1 Mô phỏng trên phần mềm Matlab
Hình 22 Mô phỏng kiểm tra động học nghịch trén Matlab
34 e© Chương trình trong khối kiểm tra động học nghịch IK T6 trong khối Matlab Function function [ql, q2, q3] =DHN(x, y, z, s)
In the given mathematical framework, the relationship 13 = 70 is established with the variable s The angle ql is determined using the atan2 function with coordinates (y, x) The expression for cos3 involves a combination of trigonometric functions and the coordinates, specifically incorporating z and constants 11, 12, and 13 The angle q3 is derived from the cosine value, while the variables a and b represent transformations of x, y, and z The calculations for sin2 and cos2 are based on these transformations, leading to the determination of angle q2 through the atan2 function If the condition s equals 2 is met, further computations may be implemented.
To calculate the angles q1, q2, and q3 based on the coordinates (x, y, z), first determine q1 using the formula q1 = atan2(y, x) Next, compute cos3 with the equation cos3 = ((x*cos(q1) + y*sin(q1))^2 + (z - 11)^2 - 13^2 - 12^2) / (2 * 12 * 13) The angle q3 is then found using q3 = atan2(-sqrt(1 - cos3^2), cos3) For further calculations, set a = x*cos(q1) + y*sin(q1) and b = z - 11 The values a1, a2, a21, and a22 are derived from a and b, leading to sin2 = (a1*b - a21*a) / (a1*a22 - a21*a2) and cos2 = (a*a22 - a2*b) / (a1*a22 - a21*a2) Finally, calculate q2 using q2 = atan2(sin2, cos2) For the alternative case, if s is true, q1 is calculated as q1 = atan2(-y, -x) and cos3 is determined similarly, leading to q3 = atan2(sqrt(1 - cos3^2), cos3).
35 a= x*cos(ql)+y*sin(q1); b= z-11; all *cos(q3) + 12; al2 = -13*sin(q3); a21= 13*sin(q3); a22= 12 + 13*cos(q3); sin2 = (all*b-a21*a)/(al 1*a22-a21*al2); cos2 = (a*a22-al2*b)/(al 1*a22-a21*al 2); q2 = atan2(sin2,cos2); else
To calculate the angles ql, q2, and q3, we begin with ql defined as atan2(-y, -x) Next, we compute cos3 using the formula ((x*cos(q1) + y*sin(q1))^2 + (z - 11)^2 - 13^2 - 12^2) / (2*12*13) The angle q3 is then derived from atan2(-sqrt(1 - cos3^2), cos3) We define a as x*cos(q1) + y*sin(q1) and b as z - 11 The expressions for a1, a2, a21, and a22 are calculated as follows: a1 = a * cos(q3) + 12, a2 = -13*sin(q3), a21 = 13*sin(q3), and a22 = 12 + 13*cos(q3) Subsequently, we determine sin2 and cos2 using the formulas sin2 = (a1*b - a21*a) / (a1*a22 - a21*a2) and cos2 = (a*a22 - a2*b) / (a1*a22 - a21*a2) Finally, we obtain q2 from atan2(sin2, cos2).
5.3.2 Đánh giá kết quả của động học nghịch trên Matlab
Nhóm tiến hành mô phỏng với 2 trường hợp: ® Truong hop 1: ® Truong hop 2:
Hình 23 Kết quả mô phỏng của bộ nghiệm số 1 với trường hợp Ì
Hình 24 Kết quả mô phỏng của bộ nghiệm số 1 với trường hợp 2
Quy hoạch quỹ đạo theo đường thang ccccccccccccecsscsscsscseseesessessesrsesevsvsnssevseseeees 24
Chúng tôi cần lập kế hoạch quỹ đạo cho robot di chuyển từ điểm A đến điểm B trong thời gian t, sau đó quay trở lại từ điểm B về điểm A cũng trong thời gian t, theo hàm đa thức bậc ba Toàn bộ chu kỳ này sẽ được lặp lại 5 lần trước khi robot dừng lại tại điểm A.
Lưu đồ giải thuật cho phép tính toán thời gian hiện tại trong chu kỳ bằng cách sử dụng phép chia lấy dư của thời gian thực tế t cho thời gian I chu kỳ là 2t Với t là thời gian thực tế và Ty là tổng thời gian chạy của 5 chu kỳ, ta có thể xác định quỹ đạo hiện tại trong khoảng quy hoạch Từ đó, quy hoạch quỹ đạo sẽ được thực hiện tương ứng với từng khoảng thời gian, và sẽ dừng lại tại điểm xác định.
A khi thời gian thực tế bằng với tổng thời gian 5 chu ky
Bài viết này trình bày việc sử dụng lưu đỗ giải thuật và phương pháp quy hoạch quỹ đạo bằng hàm đa thức bậc 3 để mô phỏng quy hoạch quỹ đạo trên Matlab Simulink Thông qua việc áp dụng các kỹ thuật này, chúng tôi có thể tối ưu hóa quy trình quy hoạch quỹ đạo, mang lại kết quả chính xác và hiệu quả trong các ứng dụng thực tiễn.
Quy hoạch quỹ đạo theo hình tam g1ác c0 21 1222121211121 11 1251121111118 1 1E key 25
Chúng ta cần lập kế hoạch quỹ đạo cho robot di chuyển từ điểm A đến điểm B trong thời gian t, sau đó từ điểm B đến điểm C cũng trong thời gian t, và cuối cùng từ điểm C trở về điểm A trong thời gian t Quá trình này sẽ được thực hiện theo hàm đa thức bậc ba và toàn bộ chu kỳ sẽ được lặp lại 5 lần.
27 dừng lại tại điểm A Lưu đồ giải thuật được trình bảy tương tự như quỹ đạo đường thang
Thời gian thực tế (Goi t) và tổng thời gian chạy của 5 chu kỳ (Ty) cho phép chúng ta tính toán thời gian hiện tại trong chu kỳ quy hoạch quỹ đạo Bằng cách sử dụng phép chia lấy dư của t cho thời gian 1 chu kỳ là 3t, chúng ta có thể xác định được thời điểm dừng lại tại điểm A khi thời gian thực tế đạt bằng tổng thời gian của 5 chu kỳ.
Dựa vào lưu đồ giải thuật và phương pháp quy hoạch quỹ đạo sử dụng hàm đa thức bậc 3, chúng tôi tiến hành mô phỏng quy hoạch quỹ đạo trên Matlab Simulink để tối ưu hóa quá trình điều khiển.
MO PHONG KIEM CHỨNG e cccsserrxerrrrrrrierrrree 27
Mô phỏng và đánh giá động học thuận 2 222121211122 11211122281 eg 28
5.2.1 Mô phỏng trên phần mềm Matlab
Hình 17 Mô phỏng kiểm tra động học thuận trên Matlab e - Chương trình trong khối kiểm tra động học thuận DHT trong khối Matlab Function
13p,; x*cos(ql)*(7*cos(q2 + q3) + 10*cos(q2)); y*sin(q1)*(7*cos(q2 + q3) + 10*cos(q2));
5.2.2 Đánh giá kết quả của động học thuận trên Matlab ® Xét trường hợp:
Hình l8 Mô hình robot trong trường hợp Ì
Từ hình vẽ: ta thấy được tọa độ của EE lần lượt là x0; y=0; z nh 19 Kết quả tọa độ thu được trong trường hợp 1
Kết quả mô phỏng với tọa độ trên hình vẽ của EE cho thấy sự trùng khớp, từ đó khẳng định rằng kết quả tính toán trước đó là chính xác.
Hình 20 Mô hình robot trong trường hợp 2
Mô phỏng và kiêm chứng kết quả động học nghịch . 5 5c ccnn serrrere 31
Kết quả mô phỏng động học thuận trên Matlab cho thấy tọa độ của robot trong các trường hợp đã được xác định một cách chính xác, tương đồng với tọa độ trên hình vẽ của EE Điều này chứng tỏ rằng kết quả tính toán động học thuận là chính xác.
5.3 Mô phỏng và kiếm chứng kết quả động học nghịch
5.3.1 Mô phỏng trên phần mềm Matlab
Hình 22 Mô phỏng kiểm tra động học nghịch trén Matlab
34 e© Chương trình trong khối kiểm tra động học nghịch IK T6 trong khối Matlab Function function [ql, q2, q3] =DHN(x, y, z, s)
The calculations involve determining angles and trigonometric values based on given variables Specifically, the angle ql is calculated using the atan2 function, while cos3 is derived from the coordinates (x, y, z) and constants 11, 12, and 13 The angle q3 is then found using the cosine value obtained Subsequently, variables a and b are defined, leading to further calculations for sin2 and cos2, which are essential for finding the angle q2 This process is contingent on the condition that s equals 2.
To compute the angles q1, q2, and q3 in a 3D coordinate system, we start with q1 = atan2(y, x) The value of cos3 is derived from the equation involving x, y, and z, specifically cos3 = ((x*cos(q1) + y*sin(q1))^2 + (z - 11)^2 - 13^2 - 12^2) / (2 * 12 * 13) Next, q3 is calculated using q3 = atan2(-sqrt(1 - cos3^2), cos3) The variables a and b are defined as a = x*cos(q1) + y*sin(q1) and b = z - 11 The subsequent calculations for sin2 and cos2 lead to q2, where sin2 = (a * b - a21 * a) / (al1 * a22 - a21 * al2) and cos2 = (a * a22 - al2 * b) / (al1 * a22 - a21 * al2), with q2 = atan2(sin2, cos2) In an alternative scenario, if s is defined, q1 can also be computed as q1 = atan2(-y, -x), leading to a different calculation for cos3 and q3.
35 a= x*cos(ql)+y*sin(q1); b= z-11; all *cos(q3) + 12; al2 = -13*sin(q3); a21= 13*sin(q3); a22= 12 + 13*cos(q3); sin2 = (all*b-a21*a)/(al 1*a22-a21*al2); cos2 = (a*a22-al2*b)/(al 1*a22-a21*al 2); q2 = atan2(sin2,cos2); else
The calculation begins with determining the angle \( ql \) using the function \( \text{atan2}(-y, -x) \) Next, \( \text{cos3} \) is computed based on the coordinates \( x \), \( y \), and \( z \), incorporating constants 11, 12, and 13 The angle \( q3 \) is then derived from \( \text{cos3} \) using \( \text{atan2} \) Following this, variables \( a \) and \( b \) are defined, leading to the computation of \( \text{sin2} \) and \( \text{cos2} \) Finally, the angle \( q2 \) is determined using the \( \text{atan2} \) function with \( \text{sin2} \) and \( \text{cos2} \).
5.3.2 Đánh giá kết quả của động học nghịch trên Matlab
Nhóm tiến hành mô phỏng với 2 trường hợp: ® Truong hop 1: ® Truong hop 2:
Hình 23 Kết quả mô phỏng của bộ nghiệm số 1 với trường hợp Ì
Hình 24 Kết quả mô phỏng của bộ nghiệm số 1 với trường hợp 2
Bộ nghiệm số l đã cho kết quả chính xác cho góc quay q trong cả hai trường hợp Khi tiếp tục kiểm chứng với nhiều tình huống khác, chúng tôi vẫn nhận được kết quả đúng đắn.
Hnh 25 Kết quả mô phỏng của bộ nghiệm số 2 với trong hop 1
Hình 26 Kết quả mô phỏng của bộ nghiệm số 2 với trường hợp 2
Bộ nghiệm số 2 cho kết quả góc quay q, q; và o chính xác trong cả hai trường hợp Khi kiểm chứng với nhiều tình huống khác, chúng tôi vẫn nhận được kết quả chính xác.
Hình 27 Kết quả mô phỏng của bộ nghiệm số 3 với trường hợp Ì
Hình 28 Kết quả mô phỏng của bộ nghiệm số 3 với trường hợp 2
Bộ nghiệm số 3 đã cho kết quả chính xác cho góc quay q¡- q; trong cả hai trường hợp Khi kiểm chứng với nhiều trường hợp khác, bộ nghiệm này vẫn duy trì được độ chính xác cao.
Hình 29 Kết quả mô phỏng của bộ nghiệm số 4 với trường hợp Ì
Hinh 30 Két qua mô phỏng của bộ nghiệm số 4 với trường hợp 2
Bộ nghiệm số 4 đã cho kết quả chính xác cho góc quay q¡ - q; trong cả hai trường hợp Khi kiểm chứng bộ nghiệm này với nhiều tình huống khác, chúng tôi vẫn thu được kết quả chính xác.
Kết luận: Từ việc mô phỏng các bộ nghiệm trên phần mềm Matlab ta thu được tất cả bộ nghiệm đều chính xác
5.4 Kiếm chứng kết hợp Động học thuận, Động học nghịch và mô phỏng Simscape 5.4.1 Thiết kế file mô phỏng kiểm chứng động học thuận, nghịch và Simscape File mô phỏng là tổng hợp của 3 phần đã đề cập trong các chương I, 2, 3 nhằm kiểm chứng kết quả tính toán động học thuận, động học nghịch kết hợp với việc mô phỏng tư thế robot trén Simscape
Hình 31 File m6 phong kiém ching trén Matlab Simulink
Chúng tôi nhập ba giá trị vị trí x, y, z vào chương trình tính toán động học nghịch, từ đó nhận được ba góc quay tương ứng với ba khớp của robot Những góc quay này sau đó được sử dụng trong mô phỏng Simscape để tạo ra tư thế của robot trong không gian ba chiều Cuối cùng, các góc quay được đưa vào chương trình tính toán động học thuận để đề xuất ba giá trị x, y, z Nếu các công thức tính toán chính xác, giá trị vị trí ban đầu sẽ trùng khớp với kết quả của động học thuận Chương trình trong ba khối mô phỏng đã được trình bày chi tiết trong các chương 1, 2, 3.
5.4.2 Mô phỏng kiểm chứng tư thế robot với các bộ nghiệm
Ta sẽ tiền hành kiểm chứng lần lượt các bộ nghiệm của động học nghịch với nhiều trường hợp khác nhau
Hình 32 Kết quả mô phỏng các bộ nghiệm trong trong hop 1
Kết quả tính toán và tu thé robot cho thấy cả 4 bộ nghiệm đều xác định chính xác vị trí điểm cuối Đặc biệt, bộ nghiệm I-2 và 3-4 có góc quay trùng nhau, dẫn đến 2 tư thế robot khác nhau nhưng đều đưa end-effector đến cùng một vị trí.
Hình 33 Kết quả mô phỏng các bộ nghiệm trong trường hợp 2
Cả bốn bộ nghiệm đều xác định chính xác vị trí điểm cuối của robot, với bộ nghiệm I-2 và 3-4 cho ra cùng một góc quay Điều này dẫn đến hai tư thế robot khác nhau, nhưng đều đưa end-effector đến vị trí mong muốn.
Hình 34 Kết quả mô phỏng các bộ nghiệm trong trường hợp 3
Kết quả tính toán và tư thế robot cho thấy cả 4 bộ nghiệm đều đạt được vị trí điểm cuối chính xác Trong trường hợp này, 4 bộ nghiệm mang lại 4 tư thế robot khác nhau, đưa end-effector đến vị trí mong muốn.
Kết luận từ các mô phỏng cho thấy rằng kết quả tính toán động học thuận và nghịch đều chính xác Đặc biệt, khi góc quay là bội số của 90°, có thể tạo ra 2 tư thế robot từ 4 bộ nghiệm để đáp ứng vị trí điểm cuối đã cho Đối với các trường hợp khác, 4 bộ nghiệm sẽ tạo ra 4 tư thế robot tương ứng với 4 bộ góc quay khác nhau.
Trong việc điều khiển robot thực tế, góc quay của robot bị giới hạn bởi cấu hình phần cứng và không gian làm việc Vì vậy, các bộ nghiệm, dù đã được tính toán và mô phỏng kiểm chứng chính xác, vẫn cần được đánh giá trước khi áp dụng vào thực tế Để kiểm chứng quy hoạch quỹ đạo, phần mềm Simscape là một công cụ hữu ích.
5.5.1 Vé khong gian làm việc của robot
Chương trình tạo không gian làm việc cho robot
% Định nghĩa các thông số của robot
% Tạo lưới cho các góc khớp thetal = -360:20:360; % Khớp 1 theta2 = -45:20:225; % Khớp 2 theta3 = -90:20:90; % Khớp 3
% Khởi tạo máng đê lưu trữ các vị trí đầu cuôi end_effector_positions = [];
% Tính toán các vị trí đầu cuối cho mọi sự kết hợp của các góc khớp for tl = thetal for t2 = theta2 for t3 = theta3
% Tính toán vị trí đầu cuối x =cosd(tl) * 03 * cosd(2 + t3) + l2 * cosd(t2)); y =sind(tl) * (13 * cosd(t2 + t3) + 12 * cosd(t2)); z +13 * sind(t2 + t3) + 12 * sind(t2);
44 end_effector posifions = [end_effector positlons; x, y, Z|; end end end
