TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO ĐỒ ÁN TỐT NGHIỆP THIẾT KẾ MÔ HÌNH VÀ BỘ ĐIỀU KHIỂN CHUYỂN ĐỘNG CHO HỆ ROBOT TRUYỀN ĐỘNG CÁP 6 BẬC TỰ
TỔNG QUAN
Đặt vấn đề
Sự phát triển của công nghệ và máy móc hiện đại đang thu hút sự chú ý lớn từ cả nước, đặc biệt là việc sử dụng robot trong các nhà máy và khu vực nguy hiểm Số lượng robot ngày càng tăng và có khả năng thay thế con người trong tương lai gần Robot được thiết kế để thực hiện các công việc lặp đi lặp lại, được điều khiển bởi các mạch điện tử đã được lập trình Với sự cải tiến về phần cứng và phần mềm, robot đáp ứng tốt các yêu cầu về độ chính xác, công suất và tốc độ, giúp con người giảm thiểu tiếp xúc với nguy hiểm trong môi trường khắc nghiệt và độc hại.
Cánh tay robot đã được nghiên cứu và ứng dụng từ lâu, góp phần nâng cao năng suất và chất lượng sản phẩm trong ngành công nghiệp Các cấu trúc cánh tay robot 6 bậc như Puma 560 và Denso vs 6556 được sử dụng rộng rãi nhờ động cơ đặt ở từng khớp và cấu hình thẳng đứng, phù hợp với không gian làm việc trống trải Tuy nhiên, với nhu cầu ngày càng cao về độ chính xác và linh hoạt trong môi trường khó khăn, các robot truyền thống gặp hạn chế trong khả năng tương tác Do đó, cấu trúc robot đa bậc đang được nghiên cứu và ứng dụng để cung cấp khả năng di chuyển linh hoạt, phục vụ cho bảo dưỡng và kiểm tra trong không gian chật hẹp, đảm bảo an toàn và tiết kiệm chi phí Những robot này có chiều dài lớn và nhiều bậc tự do, cho phép điều hướng trong môi trường phức tạp, nhưng yêu cầu động cơ công suất lớn, dẫn đến tăng giá thành, trọng lượng và kích thước, ảnh hưởng đến sự linh hoạt và khả năng mang tải.
Cấu trúc của robot đa bậc không được thiết kế theo cách truyền thống, với động cơ được di chuyển xuống đế và sử dụng cáp để truyền động đến các khớp Việc này giúp giảm kích thước và trọng lượng trên cánh tay, tăng độ linh hoạt và khả năng mang tải, cho phép robot di chuyển ổn định và chính xác trong không gian chật hẹp Hơn nữa, việc sử dụng cáp cũng giúp giảm chi phí thiết bị, tạo điều kiện cho robot được ứng dụng rộng rãi.
Việc quản lý độ căng của cáp trong cơ cấu truyền động cáp là rất quan trọng, vì cáp bị dãn sau thời gian sử dụng sẽ làm giảm độ chính xác của robot Do đó, cần thiết phải có cơ cấu tăng cáp tự động để điều chỉnh độ căng, đảm bảo hiệu quả làm việc ổn định cho robot Các cơ cấu căng cáp đã được nghiên cứu từ lâu, như cơ cấu điều khiển lực căng cáp sử dụng lò xo, với kích thước nhỏ gọn, trọng lượng nhẹ và giá thành thấp Bên cạnh đó, bộ truyền động cáp sử dụng pulleys và cuộn cáp cũng mang lại độ cứng cao, không cần bôi trơn, với cấu trúc nhỏ gọn nhưng vẫn đảm bảo hiệu suất truyền động lớn.
Trên toàn cầu, robot với cấu hình dây cáp đã được thiết kế và thi công một cách tỉ mỉ Đặc biệt, loại robot 4 bậc tự do sử dụng bộ điều khiển cáp cho phép tiếp cận từ xa.
CRAM được thiết kế để kiểm tra tính hợp lý trong thiết kế cơ khí và khả năng chịu tải Nó cũng phân tích các phản ứng mạnh mẽ trong cơ chế truyền động cáp, từ đó khẳng định khả năng của hệ thống khi có thêm nhiều bậc tự do.
Hệ thống được thiết kế với cấu trúc 2 bậc/khớp, mỗi bậc kết nối với một động cơ, cho phép phạm vi chuyển động lớn Đồng thời, hệ thống đảm bảo sự ổn định tại các điểm kỳ dị nhờ vào sơ đồ điều khiển nghịch đảo giả giảm chấn.
Mô hình robot với 3 liên kết điều khiển bằng 3 sợi cáp đã được phát triển để tăng khả năng chịu tải và duy trì sự linh hoạt Đồng thời, cấu trúc robot mô phỏng rắn cũng đang được cải tiến với khả năng chịu tải và độ chính xác cao, nhờ vào các khớp thiết kế theo kiểu hooke, giúp robot hoạt động linh hoạt trong môi trường phức tạp Để đáp ứng nhu cầu ngày càng cao về tính cơ động trong các môi trường khắc nghiệt, việc nâng cấp robot vận hành là cần thiết, dẫn đến cấu trúc ngày càng phức tạp.
Chính vì thế những cấu trúc robot đa bậc như robot truyền động cáp 6 bậc tự do đã được nghiên cứu và đã được ứng dụng thành công
Tại Việt Nam, các nhà máy chủ yếu tập trung vào nghiên cứu thiết kế và vận hành cánh tay robot công nghiệp truyền thống Bên cạnh đó, có loại robot chạy bằng cáp, nhưng chỉ áp dụng cho robot song song Hiện nay, việc thiết kế và vận hành robot cáp nối tiếp đang được các giảng viên và sinh viên nghiên cứu, điển hình là đồ án tốt nghiệp “Thiết kế và thi công mô hình robot 7 bậc tự do sử dụng cơ cấu truyền động cáp” do thầy Đặng Xuân Ba cùng hai sinh viên trường Đại học Sư Phạm Kỹ Thuật thực hiện.
Mô hình robot 7 bậc tự do đã được chế tạo thành công với thiết kế đẹp và sử dụng cơ cấu lò xo để căng cáp, đồng thời giải động học nghịch bằng phương pháp đại số Tuy nhiên, hệ thống vẫn phụ thuộc vào điều khiển tập trung, gây tăng gánh nặng cho vi xử lý trung tâm và gặp khó khăn trong việc sửa chữa hư hỏng Thêm vào đó, việc sử dụng encoder DC tại các khớp làm giảm hiệu quả vận hành của robot trong không gian từ trường lớn.
Mặc dù robot đa bậc có nhiều ưu điểm vượt trội so với robot công nghiệp truyền thống, nhưng lĩnh vực này vẫn chưa được chú trọng và ứng dụng nhiều Nghiên cứu và phát triển mô hình robot cáp nối tiếp là một lĩnh vực còn mới mẻ tại Việt Nam.
Nhóm đã thực hiện đề tài “Thiết kế mô hình và bộ điều khiển chuyển động cho hệ robot truyền động cáp 6 bậc tự do dựa trên mạng CAN” nhằm giải quyết những lý do đã nêu.
Mục tiêu đề tài
Tạo ra một cấu hình robot mới Nghiên cứu thêm phương pháp giải động học nghịch cho cấu trúc robot đa bậc Điều khiển robot bằng mạng CAN Bus
1.2.2 Mục tiêu đề tài: Đề tài cơ bản sẽ nghiên cứu giải quyết các vấn đề cụ thể như sau:
• Thiết kế mô hình robot truyền động cáp 6 bậc tự do
• Tính toán động học thuận, động học nghịch theo phương pháp đại số
• Kết hợp giữa phương pháp “Cyclic Coordiante Decent (CCD)” và phương pháp đại số để tính toán động học nghịch và giải quyết điều khiển hướng
• Mô phỏng giải thuật với quỹ đạo vị trí và quỹ đạo hướng
• Chế tạo mô hình thực nghiệm robot truyền động cáp 6 bậc tự do
• Xây dựng thuật toán điều khiển tạo khả năng linh hoạt cho robot
• Nghiên cứu và cấu hình mạng CAN áp dụng cho robot.
Phương pháp nghiên cứu
• Phân tích toán học (dựa vào các công thức toán và phần mềm hỗ trợ)
• Phân tích mô phỏng : sử dụng phần mềm Matlab Simulink để mô phỏng robot và giám sát tín hiệu ngõ ra, sai số và tín hiệu điều khiển
• Phân tích thí nghiệm : Sử dụng mô hình thực nghiệm để đánh giá với dữ liệu từ mô phỏng
• Các tiêu chí đánh giá hiệu quả của phương pháp : Độ chính xác và độ ổn định của robot.
Giới hạn đề tài
• Đề tài chỉ giải quyết các vấn đề liên quan đến động học của robot
• Thuật toán sử dụng không giải quyết được việc tránh vật cản nên hệ thống chỉ hoạt động tốt trong môi trường lý tưởng, không có vật cản
• Mô hình robot sử dụng vật liệu in 3D
• Do giới hạn về mặt cấu trúc phần cứng nên thuật toán chỉ có thể điều khiển được vị trí và 2 hướng của robot.
Giới thiệu nội dung
• Chương 1: Tổng quan đề tài
Chương 1 sẽ trình về đặt vấn đề của đề tài, mục tiêu đề tài, các phương pháp nghiên cứu được sử dụng trong đề tài, giới thiệu nội dung đề tài
• Chương 2: Cơ sở lý thuyết
Chương 2 trình bày tổng quan về robot, lý thuyết động học thuận và động nghịch robot, quy hoạch quỹ đạo cho robot theo không gian làm việc, lí thuyết về giải thuật Cylic Coordinate Descent, lý thuyết về điều khiển phân tán, lý thuyết mạng CAN Bus
• Chương 3: Thiết kế mô hình và tủ điều khiển
Chương 3 trình bày về phần cứng của hệ thống bao gồm mô hình và tủ điều khiển Thiết kế mô hình trên phần mềm solidworks, bao gồm thiết kế đế robot, thiết kế các khâu và khớp Thiết kế tủ điện, chọn lựa các thiết bị điện nặng và điện nhẹ
• Chương 4: Bài toán động học robot
Chương 4 trình bày về phân tích động học thuận và động học nghịch cho robot truyền động cáp 6 bậc tự do Động học thuận bao gồm đặt trục và vẽ không gian làm việc của robot Nêu lên khó khăn khi phân tích động học nghịch theo hướng đại số
• Chương 5: Áp dụng giải thuật Cylic Coordinate Descent kết hợp đại số
Sau khi phân tích sự khó khăn khi giải động học nghịch bằng phương pháp đại số, chương 5 sẽ trình bày áp dụng giải thuật Cylic Coordinate
Phương pháp Descent được sử dụng để giải quyết động học nghịch, đồng thời kết hợp với các phương pháp đại số nhằm giải quyết vấn đề định hướng của cơ cấu chấp hành cuối của robot.
Chương 6 trình bày kết quả mô phỏng động học, trong đó mô hình được thiết kế trên phần mềm SolidWorks và kết hợp với công cụ Simscape để thực hiện các đánh giá cần thiết.
Simulink để mô phỏng và đánh giá giải thuật Cylic Coordinate Descent kết hợp đại số để giải động học nghịch
• Chương 7: Phương pháp điều khiển
Chương 7 sẽ trình bày các phương pháp điều khiển hệ thống Thiết lập các thông số của mạng CAN, cách điều khiển AC Servo Tạo giao diện giám sát để thu thập dữ liệu theo thời gian thực
• Chương 8: Thi công, thực nghiệm và đánh giá
Chương 8 sẽ trình bày thực nghiệm hệ thống Chạy các quỹ đạo đã mô phỏng trên matlab simulink Đánh giá mạng CAN Bus
• Chương 9: Kết luận và hướng phát triển
CƠ SỞ LÝ THUYẾT
Tổng quan robot
Robot là những cỗ máy tự động, mô phỏng hoạt động của con người, được thiết kế để thay thế con người trong các công việc phức tạp và nguy hiểm Chúng được lập trình bởi máy tính để thực hiện nhiều hoạt động phức tạp thông qua bộ điều khiển và khớp nối chuyển động Hầu hết các khớp robot sử dụng động cơ điện với dạng chuyển động quay, phù hợp cho hệ thống tải nhẹ Robot có ưu điểm vượt trội trong việc thực hiện các công việc lặp lại với tốc độ và độ chính xác cao, đồng thời có độ bền lớn Chúng thường được ứng dụng trong lắp ráp, sản xuất, sửa chữa và làm việc trong các môi trường khắc nghiệt, độc hại.
Động học robot
Mục đích của bài toán động học thuận là xác định vị trí của khâu tác động cuối của tay máy dựa trên các biến khớp của nó Bên cạnh đó, động học thuận cũng được áp dụng để xác định không gian làm việc của robot.
Tay máy là một chuỗi động hở, bao gồm các khâu (Links) được kết nối với nhau qua các khớp Một đầu của chuỗi gắn với giá (Base), trong khi đầu còn lại kết nối với phần công tác (End effector) Mỗi khâu kết hợp với khớp phía trước tạo thành một cặp khâu-khớp, và mỗi cặp này được đặc trưng bởi hai loại thông số.
• Các thông số không thay đổi giá trị trong quá trình làm việc của tay máy được gọi là tham số
Các thông số thay đổi khi tay máy hoạt động được gọi là các biến khớp Để thể hiện sự thay đổi giữa các hệ tọa độ của các khớp liền kề, ta thực hiện một phép biến đổi đồng nhất Theo quy tắc Denavit-Hartenberg, các ma trận chuyển vị riêng lẻ có thể được kết hợp thành một ma trận chuyển vị thuần nhất Biến đổi này là một hàm có duy nhất một nghiệm, trong khi ba thông số còn lại được giữ cố định trong thiết kế cơ khí.
Hình 2.1: Các thông số trong hệ trục giữa Z i − 1 và Z i (một cặp khâu-khớp)
i − : Góc quay từ Z i − 1 đến Z i nhìn từ X i − 1
1 a i − : Khoảng cách từ Z i − 1 đến Z i dọc theo X i − 1
i : Góc quay từ X i − 1 đến X i nhìn từ Z i d i :Khoảng cách từ X i − 1 đến X i dọc theo Z i
Dịch chuyển từ hệ tọa độ Z i − 1 đến hệ tọa độ Z i gồm các bước:
• Bước 1: Quay quanh trục x i − 1 một góc i − 1
• Bước 2: Tịnh tiến dọc trục X i − 1 một khoảng a i − 1
• Bước 3: Quay trục Z i một góc i
• Bước 4: Tịnh tiến trục Z i một khoảng d i
Dịch chuyển từ hệ tọa độ Z i − 1 đến hệ tọa độ Z i viết dưới dạng ma trận:
(2.1) Trong đó: c i là viết tắt của cos( ) i , s i là viết tắt của sin( ) i , s i là viết tắt của sin( i ), c i là viết tắt của cos ( ) i
Từ (2.1), suy ra được ma trận tổng quát từ hệ i đến hệ i+1:
Ma trận chuyển vị của hệ từ hệ 0 đến hệ i được tính bằng cách nhân cac ma trận chuyển vị ở từng khâu lại với nhau
Động học thuận là quá trình xác định vị trí và hướng của cơ cấu chấp hành cuối dựa trên các giá trị biến khớp đã biết Kết quả của quá trình này được biểu diễn dưới dạng ma trận.
Trong đó R 3 3 x là ma trận biểu diễn hướng, P 3 3 x là ma trận biểu diễn vị trí của điểm đầu cuối so với trục tọa độ gốc
Mục đích của bài toán động học nghịch là xác định các biến khớp của robot khi biết được tọa độ và hướng của cơ cấu chấp hành cuối
Một số phương pháp giải bài toán động học nghịch:
• Phương pháp hình học kết hợp đại số
Robot có bậc cao sẽ làm cho việc tính toán động học nghịch trở nên phức tạp hơn, đòi hỏi việc áp dụng các phương pháp giải động học nghịch đặc biệt.
Quy hoạch quỹ đạo
Quy hoạch quỹ đạo là quá trình tạo ra các tín hiệu tham chiếu cho bộ điều khiển robot, giúp robot di chuyển theo quỹ đạo mong muốn Quỹ đạo điểm - điểm là quỹ đạo đi qua hai điểm đã được xác định trong một khoảng thời gian nhất định, trong khi quỹ đạo đường là quỹ đạo đi qua nhiều điểm theo một đường liên tục đã được định trước Đề tài này áp dụng đa thức bậc 3 để tạo quỹ đạo cho robot, với đa thức bậc 3 có dạng:
Để vị trí của cơ cấu chấp hành đi qua hai điểm xác định trong không gian làm việc, cần xác định các giá trị đầu và cuối cho các biến khớp từ vị trí đầu và cuối của đầu công tác trong bài toán động học ngược.
0 0 , f f q =q t q =q t Xét theo một biến khớp, tìm biểu thức cho q t ( )thỏa mãn điều kiện về vị trí (q) và vận tốc ( ) v ở thời điểm đầu và cuối
Giá trị tại thời điểm t 0 : q t ( ) 0 =q q t 0, ( ) 0 =v 0
Giá trị tại thời điểm t f : q t ( ) f = q q t f , ( ) f = v f
Với bốn điều kiện biên có thể giải để tìm được bốn nghiệm của phương trình (2.5):
Nếu chọn vận tốc đầu và vận tốc cuối là 0:
Lý thuyết giải thuật Cylic Coordinate Descent
Giải thuật CCD là phương pháp hiệu quả để giải quyết bài toán động học nghịch cho robot đa bậc, đặc biệt khi các phương pháp đại số hoặc hình học không mang lại kết quả.
CCD được ưa chuộng nhờ vào thuật toán đơn giản, yêu cầu ít yếu tố đầu vào, thời gian giải quyết nhanh chóng và độ chính xác cao Nó đặc biệt phù hợp cho các loại robot có nhiều bậc, cần sự khéo léo trong hoạt động.
Giải thuật CCD cần 3 đầu vào:
• Tọa độ cơ cấu chấp hành cuối P e
Hình 2.2: Giải thuật CCD 2.4.2 Quy trình thuật toán:
Giải thuật CCD dùng để giải động học nghịch cho cánh tay robot N bậc được tiến hành theo các bước:
1 Xác định tọa độ từng khớp (P c ), tìm (góc giữa 2 vectơ P P c f và P P c e ),
2 Xoay bậc hiện tại một góc , cập nhật vị trí của cơ cấu chấp hành cuối, tính toán và xoay bậc kế tiếp, Hình 2.3.b
3 Tính toán các góc N lần và tính toán khoảng cách giữa P e and P f , Hình
4 Nếu khoảng cách giữa P e and P f khác 0, tính toán lại các góc đến khi khoảng cách nhỏ hơn giá trị đặt, Hình 2.3.d a)
Hình 2.3: Quy trình thuật toán CCD
Thiết lập trạng thái ban đầu Áp dụng giải thuật CCD
Kết thúc Xác định Pc,Pe,Pf i i=N
Sai số vị trí khác 0 i >=1
Kết thúc Tính toán khoảng cách giữa Pe và Pf
Cập nhật vị trí Pe ĐÚNG
SAI ĐÚNG SAI Áp dụng giải thuật CCD
Hình 2.4: Lưu đồ giải thuật thuật toán CCD
2.4.3 Công thức tính góc θ và vecto quay d : cos e c f c e c f c
P e : Vị trí hiện tại của cơ cấu chấp hành cuối
P c : Vị trí hiện tại của bậc đang xét
P f : Vị trí mong muốn của cơ cấu chấp hành cuối
: góc giữa 2 vectơ P P c f và P P c e ) d : vecto hướng quay của
Lý thuyết điều khiển phân tán
2.5.1 Cấu trúc và thành phần của một hệ thống điều khiển và giám sát:
Các thành phần cơ bản của hệ thống điều khiển và giám sát quá trình bao gồm cảm biến và cơ cấu chấp hành, đóng vai trò là giao diện giữa thiết bị điều khiển và quá trình kỹ thuật Hệ thống điều khiển giám sát cũng là giao diện giữa người vận hành và máy móc Các thiết bị có thể được kết nối trực tiếp theo kiểu điểm-điểm hoặc thông qua mạng truyền thông.
Hình 2.5: Các thành phần cơ bản của một hệ thống điều khiển và giám sát
Một hệ thống điều khiển và giám sát bao các thành phần chức năng chính sau đây:
• Giao diện quá trình: các cảm biến và cơ cấu chấp hành, ghép nối vào/ra, chuyển đổi tín hiệu
Các thiết bị điều khiển tự động bao gồm bộ điều khiển chuyên dụng, bộ điều khiển khả trình PLC, thiết bị điều chỉnh số đơn lẻ và máy tính cá nhân, cùng với các phần mềm điều khiển tương ứng.
Hệ thống điều khiển giám sát bao gồm các thiết bị và phần mềm giao diện người-máy, cùng với các trạm kỹ thuật và trạm vận hành Hệ thống này cho phép giám sát và điều khiển ở mức độ cao, đảm bảo hiệu quả trong quá trình vận hành.
• Hệ thống truyền thông: Ghép nối điểm-điểm, bus cảm biến/chấp hành, bus trường, bus hệ thống
• Hệ thống bảo vệ, cơ chế thực hiện chức năng an toàn
2.5.2.1 Điều khiển phân tán so với điều khiển tập trung:
Hình 2.6: Mô hình điều khiển tập trung
Cấu trúc của hệ điều khiển tập trung được thể hiện qua một máy tính duy nhất điều khiển toàn bộ quá trình kỹ thuật Máy tính điều khiển có thể là các bộ điều khiển số trực tiếp, máy tính lớn, máy tính cá nhân hoặc thiết bị điều khiển khả trình Trong môi trường công nghiệp, máy tính điều khiển thường được đặt tại phòng điều khiển trung tâm, cách xa hiện trường, với các thiết bị cảm biến và cơ cấu chấp hành được kết nối trực tiếp với máy tính qua các cổng vào/ra.
Cách bố trí vào/ra tại máy tính điều khiển, hay còn gọi là vào/ra tập trung (central I/O), phù hợp cho các ứng dụng tự động hóa quy mô vừa và nhỏ Cấu trúc này đơn giản, giúp điều khiển hiệu quả các loại máy móc và thiết bị.
Hình 2.7: Mô hình điều khiển phân tán
Hệ thống điều khiển phân tán (DCS) là một phương pháp kiểm soát trong sản xuất, nơi các yếu tố điều khiển được phân tán trên toàn hệ thống thay vì tập trung tại một vị trí Mỗi thành phần tiểu hệ thống được điều khiển bởi một hoặc nhiều bộ điều khiển, kết nối qua mạng lưới giao tiếp và giám sát Cấu trúc điều khiển phân tán mang lại độ linh hoạt cao hơn và nâng cao hiệu suất cũng như độ tin cậy của hệ thống Việc phân tán chức năng xử lý thông tin và phối hợp điều khiển, dưới sự giám sát của các trạm vận hành trung tâm, mở ra nhiều khả năng ứng dụng mới, bao gồm lập trình cao cấp, điều khiển trình tự và tích hợp với cấp điều hành sản xuất Mô hình điều khiển phân tán có nhiều ưu điểm nổi bật.
• Tiết kiệm dây dẫn và công đi dây, nối dây
• Giảm kích thước hộp điều khiển
• Tăng độ linh hoạt hệ thống nhờ sử dụng các thiết bị có giao diện chuẩn và khả năng ghép nối đơn giản
• Thiết kế và bảo trì dễ dàng nhờ cấu trúc đơn giản
• Khả năng chuẩn đoán tốt hơn (các thiết bị hỏng được phát hiện dễ dàng)
• Tăng độ tin cây của toàn hệ thống
Nghiên cứu về điều khiển phân tán trong ngành hàng không vũ trụ ngày càng trở nên quan trọng khi nhiều hệ thống robot lớn được sử dụng cho việc sửa chữa và bảo trì thiết bị Khoảng cách từ bộ điều khiển đến các khớp robot và cảm biến trên các khâu lớn gây khó khăn trong việc đồng bộ hoạt động, dẫn đến chất lượng hoạt động không ổn định và tín hiệu cảm biến bị nhiễu Mô hình điều khiển phân tán với nhiều module tại từng khâu giúp xử lý thô các cơ cấu chấp hành và cảm biến, gửi dữ liệu qua mạng để đồng bộ hóa và chống nhiễu, đảm bảo sự phân bố tài nguyên ổn định Trong các hệ thống robot dưới nước, việc đảm bảo độ chính xác của tín hiệu cảm biến là rất quan trọng để bộ điều khiển có thể thu thập và xử lý dữ liệu từ xa Phương pháp điều khiển phân tán giúp giảm độ phức tạp thiết kế, tăng độ tin cậy và chính xác dữ liệu, đồng thời linh hoạt trong việc thay thế hoặc tích hợp thêm module đo lường hiện đại, trở thành lựa chọn phổ biến cho robot di động dưới nước.
Mạng CAN bus
CAN - Controller Area Network là một giao thức truyền thông bus nối tiếp chỉ thông qua 2 dây ( CAN-High và CAN-Low), phát triển bởi Bosch
Vào đầu những năm 1980, CAN đã được chuẩn hóa theo tiêu chuẩn ISO-11898 và ISO-11519, thiết lập nó như một giao thức chuẩn cho việc kết nối mạng trong ngành công nghiệp ô tô.
CAN được phát triển chủ yếu để hỗ trợ ngành công nghiệp ô tô, và hiện nay đã được ứng dụng rộng rãi trong các loại phương tiện như ô tô chở khách, tàu thuyền, xe tải và nhiều loại xe khác.
Giao thức CAN hiện nay được áp dụng rộng rãi trong nhiều lĩnh vực khác nhau, đặc biệt trong các ứng dụng mạng điều khiển nhúng Những lĩnh vực này bao gồm tự động hóa công nghiệp, ứng dụng y tế, xây dựng tự động hóa, máy dệt và máy móc sản xuất.
Chuẩn giao thức CAN tiêu chuẩn được phổ biến hiện nay là CAN 2.0
Chuẩn giao thức CAN FD là phiên bản mới nhất của CAN, nhưng trong đồ án này, nhóm sẽ tập trung vào chuẩn CAN 2.0 do tính phổ biến và hiệu suất cao mà nó mang lại.
2.6.2 Thông số kỹ thuật CAN 2.0:
Các thông số kỹ thuật của CAN 2.0 được thể hiện dưới đây:
Bảng 2.1 Thông số kỹ thuật CAN 2.0
Kích thước dữ liệu Tối da 8 byte/Frame
Tốc độ truyền Tối đa 1Mbps trong phạm vi 40m
Phạm vi tối đa 6 km với tốc độ 10Kbps
Số node tối đa 70 Node
Phương pháp tránh xung đột dữ liệu Dựa trên ưu tiên của ID gói tin
Phương pháp đọc tín hiệu trên bus Dựa trên sự chênh lệch điện áp 2 dây
CAN mang lại sự đơn giản, độ tin cậy và hiệu suất cao, cung cấp giao thức truyền thông hiệu quả giữa các Node trong các ứng dụng thời gian thực.
2.6.3 Cấu trúc tin nhắn CAN định dạng chuẩn:
Dữ liệu CAN được truyền dưới dạng các frame (khung dữ liệu), được cấu tạo gồm nhiều bit với những chức năng khác nhau Có 4 loại frame dữ liệu:
• Data frame (khung dữ liệu): là khung mang dữ liệu từ một bộ truyền dữ liệu đến các bộ nhận dữ liệu
Khung yêu cầu (Remote frame) là loại khung được gửi từ một Node nhằm yêu cầu một Node khác truyền khung dữ liệu có ID tương ứng với khung yêu cầu.
• Error frame (Khung báo lỗi) là khung được truyền bởi bất kỳ Node nào khi Node đó phát hiện lỗi bus
• Overload frame (Khung báo quá tải) được sử dụng để tạo thêm độ trễ giữa giữa các khung dữ liệu hoặc khung yêu cầu Mỗi Node trong bus
CAN có khả năng truyền dữ liệu bất cứ lúc nào khi phát hiện bus rảnh Nếu một Node nhận quá nhiều dữ liệu, nó có thể sử dụng khung này để ngăn chặn việc truyền tiếp Hiện nay, các bộ điều khiển CAN đã trở nên đủ tốt và thông minh, không còn cần thiết phải sử dụng khung này nữa.
Chỉ có khung dữ liệu và khung yêu cầu mang ID, và cơ chế phân xử sẽ được áp dụng cho hai loại khung này khi chúng được truyền trên bus.
Khung dữ liệu và khung yêu cầu có hai định dạng khác nhau là định dạng chuẩn (Standard) và định dạng mở rộng (Extended)
- Định dạng khung chuẩn sử dụng ID có độ dài 11 bit
- Định dạng khung mở rộng sử dụng ID có độ dài 29 bit
Chuẩn CAN 2.0A chỉ quy định sử dụng loại khung chuẩn Chuẩn CAN
2.0B sử dụng cả loại khung chuẩn và khung mở rộng
2.6.3.2 Khung truyền dữ liệu (Data frame):
Một Data frame - Hình 2.8, bao gồm 7 vùng bit khác nhau theo thứ tự là:
- Start of frame – SOF (Vùng bắt đầu khung)
- Arbitration field (Vùng phân xử)
- Control field (Vùng điều khiển)
- Data field (Vùng dữ liệu)
- CRC field (Vùng kiểm tra)
- ACK field (Vùng báo nhận)
- End of frame – EOF (Vùng kết thúc khung)
Hình 2.8: Khung truyền dữ liệu dạng tiêu chuẩn
Hình trên thể hiện đầy đủ các tín hiệu của 1 data frame tiêu chuẩn, chức năng từng vùng được trình bày trong Bảng 2.2
Bảng 2.2 Chức năng và số lượng bit các vùng trong Khung truyền dữ liệu
TÊN VÙNG SỐ LƯỢNG BIT CHỨC NĂNG
Bus Idle Bus ở trạng thái rảnh
ID 11 ID của gói tin
RTR 1 Xác định frame truyền
IDE 1 Bit định dạng khung truyền r 1 Bit dự trữ
DLC 4 Độ dài dữ liệu
Data 0-64 Dữ liệu của gói tin
CRC DEL 1 Bit ngăn cách
ACK 1 Báo nhận tin nhắn
ACK DEL 1 Bit ngăn cách
EOF 1 Bit kết thúc truyền
2.6.3.3 Khung điều khiển (Remote frame):
Một Remote Frame - Hình 2.9, bao gồm 6 vùng bit, có các đặc điểm nhận biết là:
- Không có trường dữ liệu (data field) so với khung truyền dữ liệu
Hình 2.9: Khung điều khiển
Hình trên thể hiện đầy đủ các tín hiệu của 1 remote tiêu chuẩn, chức năng từng vùng được trình bày trong
Bảng 2.3 Chức năng và số lượng bit các vùng trong Khung điều khiển
TÊN VÙNG SỐ LƯỢNG BIT CHỨC NĂNG
Bus Idle Bus ở trạng thái rảnh
ID 11 ID của gói tin
RTR 1 Xác định frame truyền
IDE 1 Bit định dạng khung truyền r 1 Bit dự trữ
DLC 4 Độ dài dữ liệu
CRC DEL 1 Bit ngăn cách
ACK 1 Báo nhận tin nhắn
ACK DEL 1 Bit ngăn cách
EOF 1 Bit kết thúc truyền
2.6.3.4 Khung báo lỗi (Error frame):
Error frame là một khung đặc biệt trong giao thức CAN, được truyền đi khi một Node phát hiện lỗi Khi một Node gửi Error frame, tất cả các Node khác cũng sẽ nhận diện lỗi và gửi Error frame của riêng mình.
The error frame, as shown in Figure 2.10, consists of a primary error flag that is 6 bits long and has the same value, along with an error delimiter made up of 8 recessive bits The error delimiter creates a space for other nodes on the bus to transmit their error flags.
(secondary) khi chúng phát hiện Error flag đầu tiên
Hình 2.10: Khung báo lỗi 2.6.4 Chuẩn CAN high speed:
CAN high speed có tốc độ giao tiếp lên đến 1 Mbps, vượt trội hơn nhiều so với CAN low speed (tối đa 125 kbps), rất phù hợp cho các hệ thống điều khiển truyền động yêu cầu trao đổi dữ liệu nhanh Nhờ vào tốc độ cao, CAN high speed đáp ứng tốt nhu cầu ứng dụng trong robot và tự động hóa hiện đại Tín hiệu CAN được truyền qua cặp dây xoắn (differential pair), giúp giảm thiểu ảnh hưởng của nhiễu từ bên ngoài Thiết kế và cấu tạo phần cứng của CAN được thể hiện rõ ràng trong hình dưới đây.
Bộ thu phát CAN high-speed CAN_H CAN_L
Bộ thu phát CAN high-speed CAN_H CAN_L
Hình 2.11: Kết nối phần cứng mạng CAN bus
Theo Hình 2.11, yêu cầu phần cứng để vi điều khiển tham gia mạng
1 Bộ vi điều khiển (Microcontroler): Xử lý việc truyền nhận dữ liệu và điều khiển hoạt động của các Node
2 Bộ điều khiển CAN (CAN protocol controler): nThực hiện các chức năng của giao thức CAN (Bit stuffing, phân xử quyền ưu tiên của tín hiệu, xử lý lỗi, kiểm tra CRC,…)
3 Bộ thu phát CAN (CAN transceiver): Tạo điện áp truyền cho bus, điều chỉnh, đảm bảo dòng điện cho thiết bị hoạt động và bảo vệ hệ thống bus
Bộ thu phát CAN có khả năng chuyển đổi tín hiệu từ dạng TTL sang tín hiệu vi sai theo tiêu chuẩn ISO 11898, với điện áp giữa CAN_H và CAN_L thường là 2V Để điều khiển bộ thu phát CAN, cần sử dụng dòng điện 30mA thông qua 2 điện trở đầu cuối.
Hình 2.12: Tín hiệu điện áp vi sai
Khi chênh lệch điện áp giữa 2 dây CAN_H và CAN_L là 0V, tín hiệu này được gọi là bit Recesive tương ứng với mức logic 1 của tín hiệu TTL
Ngược lại, khi 2 dây CAN_H và CAN_L tạo chênh lệch điện áp khoảng 2V
(phụ thuộc phần cứng mạng), tín hiệu này được gọi là bit Dominant tương ứng với mức logic 0 của tín hiệu TTL
Bảng 2.4: Điện áp trên BUS CAN
(Recessive bit) 2.5Volts 2.5Volts = U CANH − CANL = 0 volts
(Dominant) 3.5Volts 1.5Volts = U CANH − CANL = 2.5 volts Điện trở đầu cuối (Termination resistor): Hai điện trở (thường là 120
Điện trở Ohm cần được kết nối ở đầu và cuối của CAN bus, đóng vai trò quan trọng trong việc điều chỉnh trở kháng của bus Thiếu điện trở này sẽ làm giảm đáng kể độ tin cậy và khả năng chống nhiễu của việc truyền dữ liệu, thậm chí có thể dẫn đến việc không truyền được dữ liệu và gây hư hỏng thiết bị.
THIẾT KẾ MÔ HÌNH VÀ TỦ ĐIỆN
Thiết kế mô hình trên Solidworks
3.1.1 Thiết kế đế: Đế được làm từ vật liệu nhôm tấm và nhôm định hình
Hình 3.2: Mô hình đế robot
Khung đế được ghép từ các thanh nhôm định hình 20x20mm Khung có
Ba tấm nhôm 5 ly được sắp xếp song song, trong đó tấm đầu tiên kết nối với cánh tay robot, tấm thứ hai và thứ ba được gắn lò xo căng cáp, và tấm thứ ba được trang bị 6 động cơ.
Hình 3.6: Đế gá động cơ
Hình 3.7: a) Cơ cấu link 1, b) Cơ cấu link 2 và 3
Cơ cấu link được thiết kế với 5 chi tiết chính Chi tiết 1 kết nối toàn bộ cánh tay robot với đế, trong khi chi tiết 2 kết nối hai đầu link với nhau Chi tiết 3 là thân chính của link, có khả năng điều chỉnh chiều dài Chi tiết 4 và chi tiết 5 giữ link và joint cố định với nhau Cơ cấu link 2 và 3 có hai đầu có thể kết nối với joint.
Hình 3.8: Chi tiết 1 link
Chi tiết 1 Chi tiết 2 Chi tiết 3 Chi tiết 4 Chi tiết 5
Hình 3.9: Chi tiết 2 link
Hình 3.10: Chi tiết 3 link
Hình 3.11: Chi tiết 4 link
Hình 3.12: Chi tiết 5 link 3.1.3 Thiết kế joint:
Hình 3.13: Cơ cấu joint
Hình 3.14: Chi tiết 1 joint
Hình 3.15: Chi tiết 2 joint
Hình 3.16: Chi tiết 3 joint
Hình 3.17: Chi tiết 4 joint
Hình 3.19: Bạc đạn khớp
Bạc đạn khớp với đường kính trong 6mm và đường kính ngoài 15mm
Hình 3.20: Kết cấu end-effector
Thiết kế hệ thống điện
3.2.1 Lựa chọn thiết bị điều khiển:
• Vi điều khiển Master: Board điều khiển STM32F407 Discovery - Hình
3.21a, có tất cả 15 module giao tiếp, trong đó có 2 bộ điều khiển CAN tích hợp chuẩn V2.0B với tốc độ tối đa 1Mbps Có 6 module mạng
UART cho phép tốc độ truyền tối đa là 115200bps Module thu phát
TJA1050 - Hình 3.21b, được sử dụng làm giao tiếp giữa bộ điều khiển giao thức của mạng điều khiển mạng (CAN) và bus vật lý a) b)
Hình 3.21: a) Board STM32F407 Discovery; b) Module TJA1050
• Vi điều khiển Slave: Board điều khiển Adruino nano - Hình 3.22.a, có
Module có 14 ngõ vào/ra, bao gồm 2 ngõ vào ngắt ngoài để đọc encoder, 4 ngõ giao tiếp SPI và 4 ngõ cấp xung cho driver servo Điện áp hoạt động là 5VDC với xung nhịp 16MHz.
MCP2515 là một module chuyển đổi giao tiếp SPI thành tín hiệu CAN bus, được tích hợp sẵn TJA1050, cho phép giao tiếp CAN với bus vật lý.
Hình 3.22: a) Board Arduino nano; b) Module MCP2515
Sau khi lựa chọn các linh kiện phù hợp cho đề tài, nhóm đã tiến hành thiết kế và thi công mạch PCB Mạch điều khiển được thiết kế bằng phần mềm EasyEDA, như thể hiện trong Hình 3.23 và Hình 3.24.
Hình 3.23: Mạch PCB mặt trước
Hình 3.24: Mạch PCB mặt sau 3.2.2 Lựa chọn thiết bị công suất:
Động cơ AC Servo là thành phần chính trong hệ thống chấp hành, được lắp đặt trên khung đế và truyền động đến các khớp thông qua hệ thống cáp Để đảm bảo motor hoạt động chính xác theo vị trí yêu cầu, encoder cần được tích hợp để đọc xung và gửi về bộ điều khiển.
Yêu cầu lựa chọn động cơ:
- Moment của động cơ phải có khả năng kéo tải tất cả các khớp
- Điều khiển được ở chế độ xung
- Động cơ có thể tích hợp encoder trả về để điều khiển chính xác vị trí
Để tạo ra chuyển động liên tục và đáp ứng các yêu cầu kỹ thuật, nhóm đã quyết định sử dụng động cơ AC, có khả năng phối hợp với các motor khác.
Servo trong mô hình Motor được chọn là loại 100W - Hình 3.25, gồm 3 loại:
HG-KR13, HF-MP13, HF-KP13 Motor HG-KR13 có mô-men xoắn cực đại lớn nhất nên được sử dụng cho trục đầu tiên của mô hình
Bảng 3.1: Thông số kỹ thuật motor
Thông số HG-KR13 HF-KP13 HF-MP13
Bộ điều khiển sử dụng MR-J3-10A
Công suất định mức (kW) 0.1
Mô-men xoắn định mức (N.m) 0.32 0.32 0.32
Mô-men xoắn cực đại (N.m) 1.1 0.95 0.95
Tốc độ định mức (r/min) 3000
Tốc độ cực đại (r/min) 6000
Driver là thiết bị kết nối giữa vi điều khiển và motor, có chức năng nhận tín hiệu từ vi điều khiển để điều khiển động cơ quay thuận hoặc quay nghịch với số xung và tốc độ tương ứng Bên cạnh đó, driver còn đọc tín hiệu trả về từ encoder ở động cơ, giúp nâng cao độ chính xác trong việc điều khiển vị trí.
Hình 3.26: Driver điều khiển Motor Servo MR-J3-10A
Bảng 3.2: Thông số kỹ thuật driver MR-J3-10A
Ngõ ra Điện áp định mức 3 pha 170 VAC
Mạch chính Nguồn cấp, tần số 3 pha , 1 pha 200 – 230 VAC, 50/60 Hz
Nguồn cấp, tần số 1 pha 200 – 230 VAC, 50/60 Hz Dòng điện định mức 0.2 A
Mạch giao tiếp Nguồn cấp 24VDC ± 10%
Dòng cấp 0.3 A cho toàn bộ tín hiệu I/O
Hệ thống điều khiển Điều khiển sóng sine, điều khiển dòng
3.2.2.3 Tính toán chọn hộp số:
Phương pháp chọn hộp số cho từng khớp của robot được thực hiện bằng cách vẽ mô hình thực tế trên Solidworks Sau đó, các vật liệu thực tế được gán vào mô hình và xuất sang Matlab thông qua công cụ Simscape, như thể hiện trong Hình 3.28 Mô phỏng mô hình này giúp tối ưu hóa thiết kế và hiệu suất của robot.
Matlab-Simulink kết hợp với các quỹ đạo chuyển động Xem moment từng khớp để có thể tính toán hộp số động cơ - Hình 3.29, Hình 3.30
Sau khi thực hiện dùng tool Simscape tính toán và nhân chỉ số an toàn
Nhóm chọn các hộp số sau: tỉ lệ 1:15 cho khớp 1, tỉ lệ 1:10 cho khớp 2 và 3
Hình 3.28: Lấy moment ngõ ra khi cấp vị trí, vận tốc, gia tốc cho khớp
Hình 3.29: Moment 6 khớp với quỹ đạo tròn bán kính 60mm
Hình 3.30: Moment 6 khớp với quỹ đạo hướng
Tên Hình ảnh SL Đặc điểm
3 pha Điện áp định mức:
220VAC Dòng định mức: 50A Bảo vệ quá tải và ngắn mạch
3 pha Điện áp định mức:
220VAC Dòng định mức: 15A Dùng để bảo vệ
2 pha Điện áp định mức:
2 pha Điện áp định mức:
250VAC Dòng định mức: 5A Dùng để bảo vệ mạch
3 pha Điện áp định mức:
690VAC Dòng định mức: 25A Áp cuộn coil: 24VDC Dùng để đóng cắt mạch điều khiển và mạch công suất cấp cho driver
Nguồn 1 Điện áp ngõ vào: 220VAC Điện áp ngõ ra: DC24V Dòng ngõ ra: 5A
Lọc 2 Điện áp định mức: 250V Dòng định mức: 20A Lọc nguồn cấp cho driver
TÍNH TOÁN ĐỘNG HỌC ROBOT
Động học thuận
Hình 4.1: Đặt trục tọa độ (a) Hệ trục tổng quát (b) Hệ trục tại mỗi khớp (c)
Hệ trục mặt phẳng OXZ (d) Hệ trục mặt phẳng OXY
Mô hình robot 6 bậc truyền động cáp có cấu trúc với mỗi khớp tạo thành từ 2 bậc tự do, bao gồm một bậc có trục Z vuông góc với mặt đất và một bậc có trục Z song song với mặt đất Robot di chuyển trên mặt phẳng OXZ dựa trên các trục 2, 4, 6 (trục Z song song với đất) và trên mặt phẳng OXY dựa trên các trục 1, 3, 5 (trục Z vuông góc với đất).
• Hệ x y z 0 0 0 : hệ trục tọa độ gốc của robot
• Hệx y z 7 7 7 : hệ trục tọa độ cơ cấu chấp hành cuối
• Hệ x y z i i i : Hệ trục của bậc A i (0 i 7)
Sử dụng phương pháp Denavit-Hartenberg để tính động học thuận với các thông số:
Bảng 4.1: Bảng Denavit-Hartenberg i a i − 1 i − 1 d i i Range of variable (degree)
Ma trận chuyển đổi đồng nhất giữa hệ trục tọa độ {0} và hệ trục tọa {7}
Ma trận hệ tọa độ cơ cấu chấp hành cuối
4.1.2 Không gian làm việc của robot (Workspace):
Không gian làm việc của robot là khu vực mà cánh tay robot có thể hoạt động hiệu quả Đối với mô hình cánh tay robot 6 bậc mà nhóm đã thiết kế, không gian làm việc được hình thành dưới dạng hình cung, với các giới hạn góc quay được chỉ rõ trong Bảng 4.1.
Hình 4.2: Không gian làm việc của robot trên mặt phẳng OXZ
Hình 4.3: Không gian làm việc của robot trên mặt phẳng OXY
Động học nghịch
Để giải bài toán động nghịch, ngoài ma trận 7 0 T-(4.2), cần nắm rõ thông tin về ma trận chuyển đổi đồng nhất giữa hệ trục tọa độ {0} và hệ trục tọa độ {5}.
( ) ( ) 3 0 T − 1 5 0 T = 5 3 T (4.40) Cân bằng phần tử (3,4) của (4.40), ta có:
Cân bằng phần tử (2,4) của (4.40):
Cân bằng phần tử (3,4) của (4.48):
Cân bằng phần tử (2,4) của (4.48):
Kết luận
Thiết kế kết cấu cơ khí của robot có những đặc điểm đặc biệt, khiến việc giải động học nghịch trở nên khó khăn hơn so với các robot có cấu trúc nối tiếp thông thường Để tìm ma trận chuyển đổi từ hệ {0} đến hệ {7} của cơ cấu chấp hành cuối, cần ba yếu tố: vị trí của cơ cấu chấp hành cuối, vị trí và hướng của khớp 2 Tuy nhiên, trong thực tế, chỉ có thể xác định hướng và vị trí của cơ cấu chấp hành cuối, trong khi các thông tin khác rất khó xác định, do đó, việc sử dụng phương pháp đại số để giải động học nghịch cho loại robot này chưa phải là giải pháp tối ưu.
GIẢI THUẬT CYLIC COORDINATE DESCENT KẾT HỢP ĐẠI SỐ
Giới thiệu
Do đặc trưng của cấu trúc cơ khí, để giải động học nghịch cho robot cáp 6 bậc tự do, ta áp dụng giải thuật CCD và chia cấu trúc robot thành hai hệ riêng biệt Hệ T05 bao gồm hai khớp đầu tiên (4 bậc tự do) nhằm điều khiển vị trí của cơ cấu chấp hành cuối, trong khi hệ T07 gồm khớp cuối cùng (2 bậc còn lại) để điều khiển hướng của cơ cấu chấp hành cuối.
Cấu trúc được chia thành hai hệ T05 và T07, trong đó chuyển đổi không gian làm việc từ 3D sang 2D tương ứng với hình chiếu các góc $\theta$ lên các mặt phẳng OXZ và OXY Vị trí của cơ cấu chấp hành cuối $P_e$ trên mặt phẳng OXZ được biểu diễn bằng tọa độ $(x_{xz}; 0; z_{xz})$.
Trên mặt phẳng OXY, điểm P được biểu diễn dưới dạng P e^{xy} (P P x; y; 0) Trong hệ mặt phẳng OXZ, chúng ta áp dụng giải thuật CCD để xác định các giá trị của $\theta_2$, $\theta_4$, và $\theta_6$ Tương tự, trong hệ mặt phẳng OXY, giải thuật CCD cũng được sử dụng để tìm các giá trị của $\theta_1$, $\theta_3$, và $\theta_5$.
Hình 5.2: Hình chiếu các góc khớp lên 2 mặt phẳng OXY và OXZ
Giải thuật CCD cho hệ mặt phẳng XOZ
Hình 5.3: Hệ mặt phẳng XOZ
Sau khi chiếu lên mặt phẳng 2D, cơ cấu robot trên mặt phẳng OXZ có cấu trúc như Hình 5.3 Để áp dụng giải thuật CCD, cần 3 đầu vào để tìm
• Tọa độ cơ cấu chấp hành cuối P e : ( ;0; ) e xz x z
• Tọa độ điểm đặt P f : ( ;0; ) xz z f f x f
Tính khoảng cách Pc3(xz) và Pf05(xz)
Cập nhật vị trí Pc3 Xoay θ(i) sử dụng (θ,d) i=i-2 i=4 ĐÚNG ĐÚNG
SAI Sai số vị trí i > 4
Tính khoảng cách Pe(xz) và Pf(xz)
Cập nhật vị trí Pe Xoay θ(i) sử dụng (θ,d) i=i-2 i=6 ĐÚNG ĐÚNG
Hệ CCD XOZ-T05 Hệ CCD XOZ-T57
Hình 5.4: Lưu đồ giải thuật CCD cho mặt phẳng XOZ với A) Hệ T05 B)
Trong chương trình con "Hệ CCD XOZ-T05", các góc $\theta_2$ và $\theta_4$ sẽ được tính toán Chương trình bắt đầu vòng lặp bằng cách tính khoảng cách giữa vị trí khớp 3 ($P_{c3}$) và vị trí mong muốn của khớp 3 ($P_{f05}$) trên mặt phẳng XOZ Nếu sai số lớn hơn 0, chương trình sẽ thực hiện giải thuật CCD với hai góc theta Sau khi tính toán hai góc theta, chương trình con sẽ tính lại khoảng cách giữa $P_{c3}$ và $P_{f05}$ Vòng lặp sẽ kết thúc khi sai số vị trí giữa $P_{c3}$ và $P_{f05}$ đạt yêu cầu.
Trong chương trình con "Hệ CCD XOZ-T57", góc $\theta_6$ sẽ được tính toán khi $P_f$ bằng 0 Chương trình bắt đầu vòng lặp bằng cách tính khoảng cách giữa vị trí cơ cấu chấp hành $P_e$ và vị trí mong muốn $P_f$ trên mặt phẳng XOZ Nếu sai số lớn hơn 0, chương trình sẽ thực hiện giải thuật CCD với $\theta_6$ Sau khi tính được góc theta, chương trình sẽ tính lại khoảng cách giữa $P_e$ và $P_f$ Vòng lặp sẽ kết thúc khi sai số vị trí giữa $P_e$ và $P_f$ đạt yêu cầu.
Giải thuật CCD cho hệ mặt phẳng XOY
Hệ mặt phẳng OXY được thể hiện trong Hình 5.5, cho thấy cấu trúc của cơ cấu robot sau khi chiếu lên mặt phẳng 2D Để áp dụng giải thuật CCD, cần có 3 đầu vào để thực hiện quá trình tìm kiếm.
• Tọa độ cơ cấu chấp hành cuối P e : ( ; ;0) e xy x y
• Tọa độ điểm đặt P f : ( ; ;0) f xy f x fy
Tính khoảng cách giữa Pc3(xy) và Pf 5(xy)
Cập nhật vị trí Pc3 Xoay θ(i) sử dụng (θ,d) i=i-2 i=3 ĐÚNG
SAI Sai số vị trí i > 3
Tính khoảng cách Pe(xy) và Pf(xy)
Cập nhật vị trí Pe Xoay θ(i) sử dụng (θ,d) i=i-2 i=5 ĐÚNG
Hệ CCD XOY-T05 Hệ CCD XOY-T57
Hình 5.6: Lưu đồ giải thuật CCD cho mặt phẳng XOY với A) Hệ T05 B)
Trong chương trình con “Hệ CCD XOY-T05”, các góc $\theta_1$ và $\theta_3$ sẽ được tính toán Chương trình bắt đầu vòng lặp bằng cách tính khoảng cách giữa vị trí khớp 3 ($P_{c3}$) và vị trí mong muốn của khớp 3 ($P_{f05}$) trên mặt phẳng XOY Nếu sai số lớn hơn 0, chương trình sẽ thực hiện giải thuật CCD với hai góc theta Sau khi tính toán hai góc theta, chương trình sẽ cập nhật khoảng cách giữa $P_{c3}$ và $P_{f05}$ Vòng lặp sẽ kết thúc khi sai số vị trí giữa $P_{c3}$ và $P_{f05}$ đạt yêu cầu.
Trong chương trình con “Hệ CCD XOY-T57”, góc $\theta_5$ sẽ được tính toán khi $P_f$ bằng 0 Chương trình bắt đầu vòng lặp bằng cách tính khoảng cách giữa vị trí cơ cấu chấp hành $P_e$ và vị trí mong muốn $P_f$ Nếu sai số lớn hơn 0, chương trình sẽ thực hiện giải thuật CCD.
5 Sau khi tính được góc theta, chương trình con sẽ tính lại khoảng cách giữa
P e và P f Vòng lặp sẽ kết thúc khi sai số vị trí giữa P e và P f bằng 0.
Giải thuật CCD cho hệ T05
Xác định Pf 5(xy),Pf 5(xz) từ Pf 5
Cập nhật vị trí Pc3
Hình 5.7: Lưu đồ giải thuật CCD cho hệ T05 Ở chương trình con “Hệ CCD_T05”, góc 1 , 2 , 3 , 4 sẽ được tính toán
Chương trình con sẽ bắt đầu bằng việc chiếu và tính toán P f 05 lên hai mặt phẳng XOY và XOZ Sau đó, giải thuật CCD 2D sẽ được áp dụng lần lượt cho hai mặt phẳng Trong quá trình chuyển đổi giữa các mặt phẳng, vị trí của khớp 3 sẽ được cập nhật.
Giải thuật CCD cho hệ T57
Xác định Pf(xy),Pf(xz) từ Pf
Cập nhật vị trí Pe
Hình 5.8: Lưu đồ giải thuật CCD cho hệ T57 Ở chương trình con “Hệ CCD_T57”, góc 5 , 6 sẽ được tính toán
Chương trình con sẽ bắt đầu bằng việc chiếu và tính toán P f lên hai mặt phẳng XOY và XOZ Sau đó, giải thuật CCD 2D sẽ được áp dụng lần lượt cho hai mặt phẳng Trong quá trình chuyển đổi giữa các mặt phẳng, vị trí của cơ cấu chấp hành cuối (P e) sẽ được cập nhật.
Giải thuật CCD-ĐS cho robot cáp 6 bậc tự do
Phương pháp CCD-ĐS được sử dụng cho đến khi sai số giữa tọa độ cơ cấu chấp hành cuối (P_e) và điểm đặt (P_f) nhỏ hơn giá trị đã định trước.
Thiết lập vị trí ban đầu
Cập nhật vị trí Pc3
Cập nhật vị trí Pe
Hình 5.9: Lưu đồ giải thuật CCD-ĐS
Thuật toán bắt đầu bằng việc thiết lập trạng thái ban đầu cho hệ robot nhằm tránh điểm kì dị khi áp dụng thuật toán CCD cho các hệ 2D Sau đó, thuật toán sử dụng tọa độ đặt của cơ cấu chấp hành cuối \( P_f \) để tính toán tọa độ đặt của hệ T05 \( P_{f05} \) theo công thức, xác định \( P_c \) là tọa độ khớp 1 và khớp 2, trong khi \( P_e \) là tọa độ khớp 3 Dựa vào các dữ kiện \( P_f, P_c, P_e \), thuật toán CCD được áp dụng để tìm ra các góc khớp \( \theta_1, \theta_2, \theta_3, \theta_4 \) Sau khi tính toán các góc của khớp 1 và khớp 2, thuật toán cập nhật vị trí hiện tại của khớp 3 Cuối cùng, dựa vào tọa độ \( P_f \), \( P_c \) và \( P_e \), thuật toán tiếp tục áp dụng CCD cho hệ.
MÔ PHỎNG VÀ ĐÁNH GIÁ
Mô tả cấu trúc mô phỏng thuật toán CCD-ĐS
Sau khi hoàn thiện thiết kế mô hình và phát triển thuật toán giải động nghịch, nhóm sẽ sử dụng công cụ Simscape Simulink để mô phỏng và đánh giá thuật toán Công cụ này cho phép người dùng nhập vào các góc $\theta$ và xuất ra chuyển động của robot, giúp việc đánh giá trở nên dễ dàng và trực quan hơn.
Ngoài ra, giải thuật được đánh giá bằng số liệu, các góc từ động học nghịch sẽ được tính động học thuận để so sánh với giá trị đặt
Hình 6.1: Sử dụng Simscape Simulink để mô phỏng thuật toán
Hình 6.2: Cấu trúc mô phỏng chuyển động và đánh giá trên Matlab
Chương trình mô phỏng trên matlab simulink như Hình 6.2:
• Khối “Shape selection”: có chức năng chọn các quỹ đạo cho robot, bao gồm: quỹ đạo tròn, quỹ đạo tam giác và quỹ đạo hướng
• Khối “Trajectory Planing”: có chức năng quy hoạch quỹ đạo, đầu ra là
P P P cấp cho khối giải động học nghịch – giải thuật CCD-ĐS
Khối "Algebraic-CCD Algorithm" có nhiệm vụ giải quyết bài toán động học nghịch, với đầu vào là hướng và vị trí, và đầu ra là 6 góc $\theta$ được cung cấp cho khối Simscape và khối quan sát.
• Khối “Simscape Multibody”: có chức năng mô phỏng chuyển động robot ứng với các góc đã nhận từ khối giải động học nghịch
Hình 6.3: Tính động học thuận từ các góc nhận được và vẽ đồ thị
• Khối “Observation”: sau khi lấy các góc , các góc sẽ được tính động học thuận để so sánh với các giá trị đặt từ khối “Trajectory Planing” - Hình 6.3
Hình 6.4: Robot trong môi trường simscape mô phỏng chuyển động
Quỹ đạo tròn
Quỹ đạo tròn được viết theo dạng đa thức bậc 3 Bán kính quy hoạch là
Quỹ đạo có đường kính 60mm được quy hoạch trên mặt phẳng OYZ, với tâm đường tròn có tọa độ không gian là [780, Py, Pz] Chu kỳ quy hoạch là 15 giây, và để đánh giá thuật toán, kết quả mô phỏng được trình bày trong 3 chu kỳ quỹ đạo.
Hình 6.5: Quỹ đạo tròn trên mặt phẳng OYZ
Hình 6.6: Đáp ứng các góc theta với quỹ đạo tròn
Hình 6.7: Đáp ứng vị trí cơ cấu chấp hành theo 3 trục với quỹ đạo tròn
Quỹ đạo tròn được quy hoạch với P x cố định theo thời gian P P y , z thay đổi theo quỹ đạo Đáp ứng vị trí của cơ cấu chấp hành cuối như Hình 6.7
Hình 6.8: Sai số vị trí với quỹ đạo tròn
Tín hiệu sai số vị trí được xác định từ các góc động học thuận và các góc đặt Kết quả này được thể hiện trong Hình 6.8 Trong trường hợp quỹ đạo tròn, thuật toán được áp dụng để tính toán chính xác.
Cylic Coordinate Descent kết hợp đại số hoạt động tốt, kết quả