Thiết kế mô hình và bộ điều khiển đồng bộ cho hệ hai cánh tay robot 3 bậc tự do dựa trên mạng Can: Đồ án tốt nghiệp Khoa Chất lượng cao ngành Công nghệ kỹ thuật điều khiển và tự động hóa

HCM CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập - Tự do – Hạnh phúc KHOA ĐÀO TẠO CHẤT LƯỢNG CAO PHIẾU NHẬN XÉT ĐỒ ÁN TỐT NGHIỆP Dành cho giảng viên hướng dẫn Họ và tên sinh viên: Nguyễ

TỔNG QUAN

Đặt vấn đề

Ngày nay với sự yêu cầu ngày càng cao về hiệu suất và chất lượng sản phẩm đã dẫn đến hệ thống điều khiển ngày càng phức tạp và cần liên kết lại với nhau để cùng hợp tác và chia sẻ sao cho có thể hoàn thành nhiệm vụ một cách nhanh nhất Nhưng với những công việc như: sản xuất ô tô, lắp ráp sản phẩm, phẫu thuật, phân loại sản phẩm,… đòi hỏi tính kết hợp cao giữa các robot lại với nhau mà những công việc đó chỉ sử dụng một robot để thực hiện mặc dù ban đầu có thể tiết kiệm được chi phí nhưng hậu quả lại vô cùng lớn như làm xuống cấp và hư hỏng robot nhanh, có thể gây tai nạn cho người vận hành, làm ảnh hưởng đến chất lượng sản phẩm, … Vì vậy không nên sử dụng đơn lẻ một robot vào các công việc đòi hỏi tính ổn định và hợp tác với nhau thì mới có thể hoàn thành được

Hình 1.1: Robot cùng thực hiện nhiệm vụ nâng vật Ở Hình 1.1 cho thấy cụ thể hơn về khả năng hợp tác của các cánh tay robot, chỉ ra rằng sự hợp tác không chỉ dừng lại ở 2 cánh tay robot, mà nó có thể lớn hơn 2 và các cánh tay trên cùng thực hiện một nhiệm vụ (cụ thể ở đây là sẽ cùng nâng một vật nặng nếu vật đó quá lớn thì sẽ khó mà thực hiện bởi một cánh tay như bình thường và có thể làm rơi vỡ hàng hóa) và sẽ rất chính xác và tốc độ đòi hỏi người lập trình phải có kĩ năng trong việc tính toán thuật toán đồng bộ để áp dụng vào mô hình robot cũng như ngoại lực tác dụng lên vật và cánh tay là bao nhiêu và lựa chọn động cơ có mô men xoắn một cách phù hợp Làm việc đồng bộ đòi hỏi các robot không còn chỉ là nhận tín hiệu điều khiển và thực thi, mà còn là nhận thông tin từ các đối tượng khác và xử lý nó, kết hợp nó với tín hiệu điều khiển Khi đó, các robot có thể chia sẻ không gian làm việc cùng nhau và kết hợp với nhau để hoàn thành công việc Đó là quá trình làm việc đồng bộ của robot Trong tương lai gần, những robot làm việc đồng bộ với nhau sẽ là điều không thể thiếu vì những lợi ích mà nó mang lại so với robot đơn lẻ

Chương 1: Tổng quan Khoa đào tạo chất lượng cao làm việc như: Có tính kết hợp cao nên dễ dàng chia khối lượng công việc ra, từ đó tăng năng suất của mỗi robot Đồng bộ hóa trong robot là có thể cùng mang, giữ và kiểm soát ổn định cho quỹ đạo mong muốn chung của hai hay nhiều robot với nhau để cùng chia sẽ không gian làm việc chung, dù có ảnh hưởng của tác động nhiễu bên ngoài nhưng với sự kết hợp của thuật toán đồng bộ vào bộ điều khiển cho các robot với nhau sẽ có thể kiểm soát và theo dõi để đạt được sự đồng bộ hóa đến một quỹ đạo để có thể hoàn thành nhiệm vụ chung nào đó mà con người đã đặt ra Chẳng hạn như một nhóm lớn robot trước tiên có thể đồng bộ hóa hành vi và vị trí của chúng để tạo thành mô hình hoạt động hợp tác nhất định, sau đó theo dõi quỹ đạo mong muốn chung để hoàn thành nhiệm vụ đã cho Trong quy trình sản xuất, chẳng hạn như các ứng dụng sản xuất ô tô, nơi không thể đạt được bởi một hệ thống robot duy nhất, mà cần có sự lập kế hoạch và không gian làm việc chung để hợp tác nhiều robot khác nhau để cùng nâng, lắp ráp,

… một vật nặng cần có sự ổn định và phối hợp nhịp nhàng để không để vật liệu của ô tô bị rơi và gây tổn thất đến cho doanh nghiệp thì đồng bộ đóng một vai trò rất quan trọng

Trong điều khiển đồng bộ robot có những phương pháp như:

Bảng 1.1: So sánh ưu và nhược điểm của 2 phương pháp đồng bộ

Phương pháp đồng bộ Ưu điểm Nhược điểm Ứng dụng Đồng bộ chủ tớ

Dễ áp dụng bộ điều khiển vào mô hình

Chỉ phụ thuộc vào một chiều là hệ thống tớ chỉ theo hệ thống chủ

Trong công nghiệp sản xuất hàng hóa, phân loại sản phẩm cần có tính đồng bộ của robot tớ theo robot chủ Đồng bộ ràng buộc chéo

Có thể đồng bộ được hai chiều giữa hai cánh tay robot với nhau Sai số đồng bộ có thể là nhỏ nhất hoặc tiến về không

Hệ số  ảnh hưởng nhiều đến đáp ứng đồng bộ của mô hình

Có thể ứng dụng đồng bộ ở mọi lĩnh vực cần tính ổn định và độ tin cậy cao khả năng chịu lỗi cao mà không phải sợ robot gặp nhiễu hay bị tác động lực từ bên ngoài

Chương 1: Tổng quan Khoa đào tạo chất lượng cao Ý tưởng về làm việc đồng bộ đã được truyền cảm hứng từ nhiều ví dụ trong sinh học và khoa học đời sống Minh họa một vài ví dụ sinh học về hệ thống đồng bộ trong thiên nhiên: bầy kiến, đàn chim, … mục đích của việc hợp tác này là để thực hiện những việc đặc thù yêu cầu nhiều cá thể kết hợp, hoặc cũng có thể là khuếch đại năng suất cơ bản của một cá thể đơn lẻ và tập tính đồng bộ này giúp cho các loài đông vật có thể sống sót và tồn tại qua những năm tháng khi sự sống được hình thành trên trái đất Ai cũng biết rằng các hệ thống đồng bộ, nếu làm việc hợp tác theo tổ chức và có nguyên tắc thì sẽ có hiệu quả cao, có thể hoạt động tổng thể, được đảm bảo với khả năng chịu lỗi và các đặc tính mạnh mẽ Như vậy có thể thấy trong tự nhiên từ rất lâu đời đồng bộ đã có mặt ở trong đời sống [1]

Hình 1.2: Hình ảnh ví dụ về sự đồng bộ trong tự nhiên: (a) bầy kiến, (b) đàn chim

Nghiên cứu robot đồng bộ ở ngoài nước: ở một số trường đại học và công ty ngoài nước như đã nghiên cứu và phát triển và tạo ra được 2 cánh tay robot 7 bậc tự do để có thể thực hiện những công việc mang tính lặp đi lặp lại hay đòi hỏi trải qua nhiều quy trình để hoàn thành sản phẩm và các nhà nghiên cứu ở nước ngoài đã đưa các cánh tay robot kết hợp với các cảm biến khác nhau để có thể xử lí và thực hiện đồng bộ, cánh tay robot ngày càng ứng dụng cao vào đời sống của con người giúp cho con người có thể giảm bớt những việc nặng [2-5] Đồng bộ hệ 2 cánh tay robot có thể giúp ích cho đời sống của con người cụ thể một số việc như: pha trà, giải Rubik, nấu ăn, lắp sản phẩm vào hộp, phẫu thuật cho con người, phân loại và lắp ráp các sản phẩm lại với nhau theo quy trình, … Dưới đây là các công ty của các nước trên thế giới đã nghiên cứu và phát triển trong việc đồng bộ 2 cánh tay robot để cùng thực hiện các nhiệm vụ mang tính hợp tác lẫn nhau và đã đưa các sản phẩm ra thương mại

Chương 1: Tổng quan Khoa đào tạo chất lượng cao

Bảng 1.2: Tổng hợp các công ty về sản xuất robot đồng bộ ở ngoài nước

Tình hình thương mại Ảnh minh họa

Sản xuất các hệ thống tự động hóa của Nhật và Robot

Cánh tay robot của công ty Fanuc đã được thương mại hóa trên thị trường rất nhiều và công ty đã phát triển hệ thống đồng bộ gồm nhiều cánh tay kết hợp lại với nhau sử dụng các cảm biến để có thể cùng thực hiện nhiệm vụ nâng vật có khối lượng nặng, to lớn và cồng kềnh

Sản xuất robot công nghiệp và các giải pháp tự động hóa nhà máy

Cánh tay robot của KUKA ngày càng phát triển về độ linh hoạt của cánh tay robot và công ty đã phát triển và tạo ra robot 7 bậc tự do và những sản phẩm đã được thương mại hóa và bày bán trên thị trường và công ty đã kết hợp các cảm biến và sự động bộ của các cánh tay robot lại với nhau để thực hiện các nhiệm vụ trong các nhà máy

Chuyên sản xuất và cung cấp các sản phẩm truyền động điện, robot công nghiệp Được thương mại và bày bán ở trên khắp các nước trên thế giới và cánh tay robot đồng bộ kết hợp với cảm biến đã phát triển và có mặt ở khắp các nhà máy sản xuất giúp nâng cao năng suất và là giải pháp tối ưu cho mỗi doanh nghiệp muốn hướng đến phát triển công ty theo hướng tự động hóa

Chuyên trong ngành robot, điện, thiết bị điện nặng và công nghệ tự động hoá

Cánh tay robot đồng bộ đã được công ty phát triển và bày bán rất nhiều ở khắp các quốc gia trên thế giới robot của công ty ABB đã được các doanh nghiệp nước ngoài sử dụng rất nhiều tại các nhà máy giúp nâng cao năng suất và tối ưu thời gian tạo ra các sản phẩm cần độ chính xác cao

Sản xuất chủ yếu về lĩnh vực robot và các giải pháp tự động hóa

AUBO đã phát triển 2 cánh tay robot điều khiển động bộ kết hợp với cảm biến đã được bày bán ở khắp trên thế giới, robot đã ứng dụng vào đời sống của con người và ở các công ty nhà máy sản xuất cần đến các thiết bị tự động hóa để nâng cao chất lượng sản phẩm cũng như các sản phẩm cần độ chính xác cao

Sản xuất các thiết bị gia dụng và có các sản phẩm robot

Là một công ty lớn đóng góp các sản phẩm thương mại nhằm mục đích phục vụ nhu cầu đời sống của con người công ty đã có những sản phẩm về cánh tay robot đồng bộ có mặt trên khắp các nước trên thế giới nhằm mục đích giúp con người tối ưu hóa được sự thiếu hụt nhân lực cũng như năng suất làm việc, các công việc như: sắp xếp hàng hóa vào thùng và đưa vào nhà kho theo đúng với mã sản phẩm

Tại Bảng 1.2 cho ta thấy được về nghiên cứu robot đồng bộ ở ngoài nước là rất thành công đã sản xuất rất nhiều mô hình robot đồng bộ phục vụ cho đời sống của con người

Nghiên cứu robot đồng bộ ở trong nước: ở Việt nam thì tính hình nghiên cứu về

Robot thì đã nghiên cứu robot 6 bậc, 7 bậc đã đi đến thực nghiệm và đã đóng góp to lớn trong lĩnh vực kỹ thuật robot nước nhà Tuy nhiên về nghiên cứu đồng bộ từ hai robot thì chưa được nghiên cứu nhiều, đồng bộ chỉ đang ứng dụng và đưa vào các nhà máy sản xuất chứ chưa được nghiên cứu và phát triển nhiều, chưa đi vào thực tiễn và tạo ra mô hình, chưa có ứng dụng cảm biến để xử lí công việc mang tính ứng dụng cao như ở ngước ngoài nên vẫn còn mới và chưa được áp dụng vào đời sống Ở các trường đại học như: Đại học Hà Nội, Đại học Bách Khoa Hà Nội, Đại học Thái Nguyên, Đai học Vinh, …, cũng đã nghiên cứu và tìm hiểu về quy hoạch quỹ đạo, bộ điều khiển mạng thần kinh, bộ điều khiển trượt dựa trên mô hình động lực học cho hệ thống hai cánh tay robot 2 bậc tự do và đã có 4 bài báo nói về vấn đề trên [6-9] Tại trường ĐH Sư phạm Kỹ Thuật TP HCM đã có nhóm sinh viên K17 đã nghiên cứu và tạo ra mô hình đồng bộ 2 cánh tay robot 2 bậc tự do sử dụng động cơ bước và áp dụng phương pháp đồng bộ chủ tớ

Nội dung nghiên cứu của đề tài

Nội dung của đề tài bao gồm các phần sau:

Chương 2: Cơ sở lý thuyết

Chương 3: Thiết kế và thi công phần cứng

Chương 4: Tính toán động học và thuật toán điều khiển đồng bộ

Chương 5: Mô phỏng kết quả tính toán

Chương 6: Phương pháp điều khiển phân tán áp dụng mạng CAN BUS Chương 7: Thực nghiệm và đánh giá kết quả

Chương 8: Kết luận và hướng phát triển.

Các công cụ đánh giá

Các công cụ đánh giá được xác định thông qua tài liệu tham khảo

Sử dụng công cụ tìm kiếm như Google Scholar, Libgen để tìm hiểu các vấn đề nghiên cứu liên quan, nghiên cứu tổng quan, so sánh đánh giá ưu và nhược điểm của từng phương pháp sử dụng cho việc đồng bộ, quỹ đạo trong không gian để từ đó rút ra đề xuất cải tiến nhằm nâng cao chất lượng

Sử dụng mô hình toán học, công thức tính toán cho robot để xây dựng mô hình toán học phục vụ việc tính toán động học và xây dựng mô hình động lực học làm cơ sở thiết kế mô hình, cách thức hoạt động và tính toán khối lượng các chi tiết để chọn lựa động cơ và hình dáng cho robot

Sử dụng phần mềm SolidWorks để thiết kế 2 cánh tay robot để xem xét và đánh giá và thi công mô hình giống như đã thiết kế Đồng thời sử dụng phần mềm

MATLAB-Simulink và Simscape để lập trình, kiểm chứng đánh giá kết quả mô hình

Sử dụng phần mềm Serial trong Python để đọc tín hiệu trả về từ vi điều khiển gửi thông qua kênh Serial từ USB máy tính khi chạy mô hình robot thực tế giữa tín hiệu đặt và tín hiệu thực tế để đánh giá kết quả mô hình

Các tiêu chí đánh giá hiệu quả của phương pháp Đánh giá về mức độ sai số của các kết quả điều khiển bám quỹ đạo và tín hiệu thực tế.

Giới hạn

Vì kết cấu cơ khí nên robot vẫn còn sai số và vẫn còn rung nhẹ khi hoạt động

Không đi sâu về động lực học của mô hình và chạy không tải.

CƠ SỞ LÝ THUYẾT

Định nghĩa robot

Robot là một cỗ máy hay cánh tay máy được con người thiết kế và lập trình dựa trên nhu cầu của mỗi cá nhân, tổ chức, doanh nghiệp, … chúng có khả năng thực hiện một loạt các yêu cầu phức tạp của người sử dụng một cách tự động Robot có thể được hướng dẫn bởi thiết bị điều khiển bên ngoài hoặc điều khiển có thể được nhúng bên trong Các robot có thể được chế tạo dựa trên hình dáng con người, động vật, … nhưng hầu hết các robot là những cỗ máy thực hiện nhiệm vụ, được thiết kế với trọng tâm là chức năng rõ ràng

Cánh tay robot là cỗ máy chuyên được ứng dụng vào quá trình sản xuất công nghiệp và được vận hành bởi con người Chúng có lợi thế là thiết kế linh hoạt với hướng dẫn sử dụng robot chi tiết được thiết kế có thể ứng dụng vào các mô hình sản xuất cho các nhà máy có thể hoạt động ở tốc độ nhanh nhất để tạo ra năng suất cao Chúng thường được sử dụng trong các công việc lắp ráp, sản xuất, sửa chữa thay thế linh kiện siêu nhỏ trong nhà máy, có thể hoạt động trong các môi trường khắc nghiệt, độc hại và nguy hiểm, có khả năng làm tự động.

Động học thuận tay máy

Hình 2.1:Mô hình hóa theo khâu và khớp của robot Để mô hình hóa robot với sự biểu diễn D-H, việc đầu tiên cần làm là đặt hệ trục tham chiếu lên mỗi và mọi khớp xoay Tất cả mọi khớp được biểu diễn theo trục Z Nếu khớp là khớp quay, trục Z sẽ được định hướng theo hướng dịch chuyển tạo ra sự quay theo quy tắc bàn tay phải Nếu khớp là khớp tịnh tiến, trục Z của khớp dọc theo hướng di chuyển Nếu gọi a i − 1 biểu diễn khoảng cách đường vuông góc chung giữa trục Z i − 1 và Z i thì hướng của trục X i − 1 sẽ dọc theo hướng của đoạn a i − 1 Hướng của

Chương 2: Cơ sở lý thuyết Khoa đào tạo chất lượng cao trục y được xác định theo tam diện thuận mà hệ trục tham chiếu có được Từ đó có bảng thông số DH

… n Trong đó: a i − 1 là khoảng cách giữa z i − 1 và z i so với x i − 1 , i − 1 là góc giữa z i − 1 và z i so với x i − 1 , d i là khoảng cách giữa x i − 1 và x i so với z i , i là góc giữa x i − 1 và x i so với z i

Ma trận chuyển vị từ hệ i đến hệ i-1 được thể hiện ở công thức với các thông số được xác định trong bảng D-H:

1 1 1 1 cos sin 0 sin sin s cos cos cos s 0 sin co

Ma trận chuyển vị của hệ từ hệ 0 đến hệ i được tính bằng cách nhân các ma trận chuyển vị ở từng khâu lại với nhau như công thức (2.2) như sau:

1 2 i i T = T T − i T (2.2) Động học thuận là quá trình tính toán vị trí và hướng của cơ cấu chấp hành cuối khi biết tất cả các giá trị biến khớp Kết quả của quá trình này thu được ma trận có dạng như công thức (2.3):

Trong đó R 3 3  là ma trận biểu diễn hướng, P 3 3  là ma trận biểu diễn vị trí của điểm đầu cuối so với trục toạ độ gốc.

Động học nghịch tay máy

Chương 2: Cơ sở lý thuyết Khoa đào tạo chất lượng cao Để có thể điều khiển được robot thì bài toán tính động học nghịch từ động học thuận được nghiên cứu từ những năm 1960 Sự phức tạp của vấn đề tính toán phụ thuộc vào cấu trúc của robot và số bậc tự do đóng vai trò quan trọng trong bài toán này Có các cách tính động học nghịch như: phương pháp đại số, phương pháp hình học, phương pháp hình học kết hợp với đại số Robot có bậc càng cao thì việc tính toán động học nghịch càng trở nên phức tạp

So sánh các phương pháp để giải bài toán động học nghịch:

• Ưu điểm: Tìm đầy đủ các nghiệm nên không bị sót nghiệm

• Nhược điểm: Nếu robot quá phức tạp việc tính toán hệ phương trình nhiều ẩn sẽ rất khó khăn

• Ưu điểm: Trực quan, dễ hình dung, dễ hiểu

• Nhược điểm: Dễ bị sót nghiệm

- Phương pháp kết hợp đại số và hình học:

• Ưu điểm: Trực quan, dễ hình dung, dễ hiểu

• Nhược điểm: Đòi hỏi cần có kiến thức chắc chắn giữa đại số và hình học.

Lý thuyết quy hoạch quỹ đạo

Quy hoạch quỹ đạo là tạo các tín hiệu vào tham chiếu cho bộ điều khiển robot để robot di chuyển theo quỹ đạo mong muốn của người lập trình Có rất nhiều cách để quy hoạch quỹ đạo từ điểm A (xA, yA) đến điểm B (xB, yB) cho robot Ở đây nhóm đã thực hiện quy hoạch quỹ đạo theo hàm đa thức bậc 3 (cubic) để tạo quỹ đạo điểm đến điểm cho cánh tay robot

Muốn đi qua hai điểm xác định trong không gian, từ vị trí đầu và cuối thì ta cần xác định được toạ độ điểm, thời gian đi từ điểm đầu đến điểm cuối và vận tốc và gia tốc của robot là bao nhiêu

Xét một khớp của robot ta có điều kiện:

Giá trị tại thời gian t = t 0 :

Giá trị tại thời điểm t = t f :

Chương 2: Cơ sở lý thuyết Khoa đào tạo chất lượng cao

= dt = (2.5) Áp dụng với hàm bậc 3 ta có:

Từ phương trình (2.6) (2.7) thay các giá trị tại thời điểm t t 0 , f ta suy ra:

Từ phương trình (2.8)→(2.11) ta thu được ma trận sau:

Lý thuyết điều khiển phân tán

Điều khiển phân tán là phương pháp được ứng dụng trong các hệ thống có qui mô vừa và lớn, mà tại đó thường được phân chia thành nhiều phân đoạn được điều khiển bởi nhiều vi điều khiển có kết nối mạng giao tiếp, có thể được phân bố tại nhiều vị trí cách xa nhau Phương pháp này khắc phục sự phụ thuộc vào một vi điều khiển làm trung tâm trong cấu trúc của điều khiển tập trung và tăng tính linh hoạt của hệ thống, mỗi mạng lưới khác nhau có thể được điều khiển bằng một hoặc một số vi điều khiển cục bộ được đặt rải rác tại các phòng điều khiển của từng phân đoạn, phân xưởng, ở vị trí không quá xa với quá trình kỹ thuật Các phân đoạn của các vi điều khiển này có sự tương tác qua lại với nhau, vì vậy vi điều khiển trung tâm có thể điều khiển và thu thập dữ liệu từ các phân đoạn khác nhau, cũng như giám sát dữ liệu để điều khiển toàn bộ quá trình hoạt động của hệ thống Trong đa số trường hợp, các bộ điều khiển được nối mạng với nhau và với một hoặc nhiều máy tính giám sát (MTGS) trung tâm qua bus hệ thống [10]

Hình 2.2: Mô hình điều khiển phân tán

Với mô hình đồng bộ 2 robot 3 bậc tự do nhóm thực hiện đồ án sẽ đi sau vào nghiên cứu điều khiển phân tán cho hệ thống chứ không sử dụng điều khiển tập trung.

Lý thuyết 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), được phát triển bởi công ty Bosch của Đức vào đầu những năm 1980 Sau đó, CAN đã được chuẩn hóa theo tiêu chuẩn ISO-

11898 và ISO-11519, thiết lập chính nó như là giao thức chuẩn để giao tiếp kết nối mạng trong ngành công nghiệp ô tô Thời gian đầu giao tiếp CAN được phát triển chủ yếu hỗ trợ cho ngành công nghiệp xe, vì vậy nó đã được sử dụng trong xe ô tô chở khách, tàu thuyền, xe tải, và các loại xe khác Ngày nay giao thức CAN được sử dụng trong nhiều lĩnh vực khác trong các ứng dụng với tên gọi mạng điều khiển nhúng (networked embedded control), bao gồm cả 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, máy móc sản xuất CAN được thiết kế để các vi điều khiển và các thiết bị giao tiếp với nhau mà không cần thông qua máy tính CAN là một giao thức hoạt động dựa trên các gói tin, ban đầu được thiết kế để ghép cái ECU (Electronic Control Unit) trong ô tô để tiết kiệm số lượng dây dẫn

Chuẩn giao thức CAN tiêu chuẩn được phổ biến hiện nay là CAN 2.0 Ngoài ra CAN còn có chuẩn giao thức mới nhất là CAN FD Trong nội dung đồ án, nhóm sẽ đi sâu khai thác chuẩn CAN 2.0 vì sự phổ biến và hiệu suất mà chuẩn này mang lại Trên bus CAN có thể chứa nhiều master để kết nối các ECU (Electronic Control Unit) như là những node (nút) trên đường truyền Cần ít nhất hai node trên đường truyền CAN để có thể giao tiếp

2.6.2 Thông số kỹ thuật CAN 2.0

Dưới đây là bảng thông số kỹ thuật CAN 2.0:

Bảng 2.2: Thông số kỹ thuật CAN 2.0

Kích thước dữ liệu Tối đa 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 trong việc truyền dữ liệu giữa các Node Dựa trên thứ tự ưu tiên cho ID của gói tin

Phương pháp đọc tín hiệu trên bus Dựa trên sự chênh lệnh điện áp 2 dây CAN_H và CAN_L CAN có sự đơn giản, độ tin cậy và hiệu suất cao, cung cấp một giao thức truyền thông hiệu quả giữa các bộ cảm biến, cơ cấu truyền động, điều khiển, và các Node khác 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) Có 4 loại frame khác nhau, đó là:

- 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

- Remote frame (Khung yêu cầu hay khung điều khiển) là khung được truyền từ một Node để yêu cầu Node khác truyền khung dữ liệu có ID trùng với khung yêu cầu

- Error frame (Khung lỗi): một loại khung đặc biệt độc đáo với các nguyên tắc định khung của SMS CAN Nó được truyền đi khi một Node phát xuất hiện lỗi & sẽ khiến toàn bộ các Node khác phát xuất hiện lỗi – chính vì như vậy chúng cũng sẽ gửi Error Frame Sau đó, bộ truyền sẽ auto phấn đấu truyền lại SMS

- Overload frame (Khung báo tràn) được sử dụng để tạo thêm độ trễ giữa các khung dữ liệu hoặc khung yêu cầu Mỗi Node trong bus CAN có thể truyền bất kỳ khi nào nếu phát hiện bus rảnh, nếu một Node nhận quá nhiều dữ liệu, nó có thể dùng khung này để ngăn sự truyền tiếp theo Chỉ có khung dữ liệu và khung yêu cầu là có

ID, cơ chế phân xử sẽ á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

Một Data frame (Hình 2.3) 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)

ID IDE=0 r0 DLC DATA CRC DEL ACK DEL EOF

Field Control Field Data Field CRC Field ACK Field EOF

Hình 2.3: Data frame 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, trong đó:

SOF (1bit): bit báo truyền CRC (15bit): kiểm tra lỗi

ID (11bit): ID của gói tin CRC DEL (1bit): bit ngăn cách

RTR (1bit): xác định frame truyền ACK (1bit): báo nhận tin nhắn

IDE (1bit): bit định dạng khung truyền ACK DEL (1bit): bit ngăn cách r (1bit): bit dự trữ EOF (1bit): bit kết thúc truyền

DLC (4bit): độ dài dữ liệu ITM: bit dừng

Data (0-8byte): dữ liệu của gói tin

Một Remote frame (Hình 2.4) bao gồm 6 vùng bit như Data frame (chỉ không có data field) với 2 đặc điểm quan trọng:

- Xác định Remote frame bằng mức logic bit RTR = 1

- Không có trường dữ liệu

ID IDE=0 r0 DLC CRC DEL ACK DEL EOF

Field Control Field CRC Field ACK Field EOF

Hình 2.4: Remote frame dạng chuẩn Các tín hiệu của Remote frame bao gồm:

SOF (1bit): bit báo truyền CRC DEL (1bit): bit ngăn cách

ID (11bit): ID của gói tin ACK (1bit): báo nhận tin nhắn

RTR (1bit): xác định frame truyền ACK DEL (1bit): bit ngăn cách

IDE (1bit): bit định dạng khung truyền EOF (1bit): bit kết thúc truyền r (1bit): bit dự trữ ITM: bit dừng

DLC (4bit): độ dài dữ liệu

Error frame là một khung đặc biệt và khác biệt với các quy định khung của tin nhắn CAN Error frame được truyền đi khi một Node phát hiện lỗi và sẽ khiến tất cả các Node khác phát hiện lỗi và chúng cũng sẽ gửi Error frame

Error frame (Hình 2.5) bao gồm Error flag có 6-bit có cùng giá trị và Error Delimiter là 8 Recessive bit Error Delimiter cung cấp một số khoảng trống trong đó các Node khác trong bus có thể gửi Error flag khi chúng phát hiện Error flag đầu tiên (Echo Error flag)

Overload frame (Hình 2.6) giống với Error frame về mặt định dạng và nó được truyền bởi một Node khi Node đó quá bận Overload frame không còn được sử dụng thường xuyên, vì ngày nay các bộ điều khiển CAN đủ tốt và thông minh để không sử dụng nó

CAN high speed tốc độ giao tiếp có thể lên đến 1 Mbps, cho tốc độ cao hơn nhiều so với CAN low speed (tối đa 125kbps), phù hợp với hệ hống điều khiển truyền động yêu cầu trao đổi dữ liệu tốc độ cao Vì vậy CAN high speed rất phù hợp với nhu cầu ứng dụng trong hệ thống robot và tự động hóa hiện đại Tín hiệu CAN được truyền trên cặp dây xoắn (differential pair), do đó nó ít bị ảnh hưởng bởi nhiễu từ bên ngoài Thiết kế và cấu tạo phần cứng CAN được thể hiện như hình dưới:

CAN Low Node 1 Điện trở đầu cuối

Bộ điều khiển giao thức CAN

CAN Low Node 6 TxD RxD

CAN Low Node n TxD RxD

Hình 2.7: Kết nối của các bus đến các node

Yêu cầu phần cứng của mạng CAN bus được ghép nối lại với nhau cần phải có 2 trở đầu cuối có giá trị 120 và được nối vào 2 chân Can High và Can Low của lưới mạng đó Các node được chọn là các vi điều khiển có thể giao tiếp được với mạng CAN bus hoặc có thể giao tiếp với mạng CAN qua khối trung gian

Theo đó, yêu cầu phần cứng yêu cầu để vi điều khiển tham gia mạng CAN gồm:

• Bộ vi điều khiển (Microcontroller) Xử lý việc truyền nhận dữ liệu và điều khiển hoạt động của các Node

• Bộ điều khiển CAN (CAN protocol controller)

Thự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, …)

• 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 Ngoài ra, bộ thu phát CAN còn thực hiện chức năng chuyển đổi tín hiệu từ dạng TTL sang dạng tín hiệu vi sai theo tiêu chuẩn ISO 11898 Theo tiêu chuẩn, điện áp giữa CAN_H và CAN_L thường là 2V, bộ thu phát CAN được điều khiển với dòng điện 30mA bằng

Hình ảnh so sánh tín hiệu, TTL và điện áp vi sai: bit Recessive bit Dominant CAN HIGH

Hình 2.8: Tín hiệu TTL và đ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 Recessive 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

Lý thuyết đồng bộ Cross-Coupling

Sử dụng khái niệm ghép nối chéo (cross coupling) [1], theo đó cả lỗi vị trí và lỗi đồng bộ hóa điểm đặt đều có thể tiệm cận bằng không.

Vị trí lỗi bám theo của cánh tay Robot so với vị trí đặt là:

Với: x i d ( ) t : là vị trí đặt của cánh tay robot, x t i ( ) : là vị trí thực tế của cánh tay robot

Mục tiêu điều khiển đồng bộ là đưa ra yêu cầu bổ sung là điều chỉnh mối quan hệ giữa các chuyển động đa trục, từ đó dẫn đến sai số vị trí e t i ( ) → 0theo thời gian t →  Mục tiêu của việc điều khiển động học:

Xác định lỗi đồng bộ hoá của vị trí của 2 cánh tay như sau:

Từ đó có thể thấy được, nếu  i ( ) t = 0 cho 2 cánh tay robot Mục tiêu đồng bộ hoá 2 trục theo có thể đạt được tự động Tín hiệu đặt là như nhau khi qua bộ điều khiển đồng bộ ràng buộc chéo thì tín hiệu điều khiển theo phương X và Y của robot sẽ như sau: d i i ix d i i iy

Với  ix , iy lần lượt là sai số đồng bộ của robot i so với robot (i+1) theo phương x và y, X i d , Y i d lần lượt là tín hiệu đặt theo phương x và y của robot

Luận văn này sẽ chọn phương pháp đồng bộ ràng buộc chéo (Cross-Coupling) làm hướng phát triển chính cho bộ điều khiển Sơ đồ khối của phương pháp đồng bộ này được thể hiện như Hình 2.9

Hình 2.9: Phương pháp đồng bộ Cross-Coupling

Lý thuyết đồng bộ Master-Slave

Phương pháp đồng bộ Master–Slave Mode (Master–Slave Approach (MSA)) Các Slaver nhận tín hiệu ngõ ra của Master làm tín hiệu điều khiển Luận văn này sẽ khảo sát qua về phương pháp này để so sánh ưu nhược điểm với phương pháp đồng bộ ràng buộc chéo Sơ đồ khối của phương pháp đồng bộ Master–Slave Mode (Master–Slave Approach (MSA)) áp dụng vào để khảo sát mô hình 2 cánh tay robot

Hình 2.10: Phương pháp đồng bộ Master-Slave

Lý thuyết truyền thông UART

UART là viết tắt của Universal Asynchronous Receiver/Transmitter (Bộ truyền nhận dữ liệu không đồng bộ) Nó không phải là một giao thức truyền thông như SPI và I2C, mà là một mạch vật lý trong bộ vi điều khiển hoặc một vi mạch độc lập Mục đích chính của UART là truyền và nhận dữ liệu nối tiếp Giao tiếp UART còn được gọi là giao tiếp 2 dây như Hình 2.11

Hình 2.11: Sơ đồ kết nối giao tiếp UART Hai đường dây mà mỗi thiết bị UART sử dụng để truyền dữ liệu đó là:

UART truyền thêm bit start và bit stop vào gói dữ liệu để xác định thời điểm bắt đầu và kết thúc để UART nhận biết khi nào bắt đầu đọc các bit Khi UARTnhận phát

Chương 2: Cơ sở lý thuyết Khoa đào tạo chất lượng cao hiện một bit start, nó bắt đầu đọc các bit đến ở một tần số cụ thể được gọi là tốc độ truyền (baud rate) Cả hai UART đều phải hoạt động ở cùng một tốc độ truyền Cả hai UART cũng phải được cấu hình để truyền và nhận cùng một cấu trúc gói dữ liệu

2.9.2 Nguyên lý hoạt động của UART

UART sẽ truyền dữ liệu nhận dữ liệu từ một bus dữ liệu Bus dữ liệu được sử dụng để gửi dữ liệu đến UART bởi một thiết bị khác như CPU, bộ nhớ hoặc vi điều khiển Dữ liệu được chuyển từ bus dữ liệu đến UART truyền ở dạng song song Sau khi UART truyền nhận dữ liệu song song từ bus dữ liệu, nó sẽ thêm một bit bắt đầu, một bit chẵn lẻ và một bit dừng, tạo ra gói dữ liệu Tiếp theo, gói dữ liệu được xuất ra nối tiếp từng bit tại chân Tx UART nhận đọc gói dữ liệu từng bit tại chân Rx của nó UART nhận sau đó chuyển đổi dữ liệu trở lại dạng song song và loại bỏ bit bắt đầu, bit chẵn lẻ và bit dừng Cuối cùng, UART nhận chuyển gói dữ liệu song song với bus dữ liệu ở đầu nhận như Hình 2.12

Hình 2.12: Truyền - nhận dữ liệu trong UART

Dữ liệu truyền qua UART được tập hợp thành gói (packet) như Hình 2.13 Mỗi gói chứa 1 bit start, 5 đến 9-bit dữ liệu (tùy thuộc vào UART), một bit chẵn lẻ (parity bit) tùy chọn và 1 hoặc 2 bit stop

Hình 2.13: Cấu trúc dữ liệu của UART

Start bit (bit bắt đầu): Đường truyền dữ liệu UART thường được giữ ở mức điện áp cao khi nó không truyền dữ liệu Để bắt đầu truyền dữ liệu, UART truyền sẽ kéo đường truyền từ mức cao xuống mức thấp trong một chu kỳ đồng hồ Khi UART nhận phát hiện sự chuyển đổi điện áp cao xuống thấp, nó bắt đầu đọc các bit trong khung dữ liệu ở tần số của tốc độ truyền.

THIẾT KẾ VÀ THI CÔNG PHẦN CỨNG

Thiết kế phần cơ khí

Yêu cầu cơ bản thiết kế phần cứng mô hình cánh tay robot 3 bậc tự do với không gian làm việc trong mặt phẳng Oxy Ưu tiên thiết kế robot kiểu SCARA để đảm bảo tối thiểu về mặt lực thế năng tác động lên khớp quay

Yêu cầu tiếp đến là chọn vật liệu và phương pháp truyền động cho các khâu để đảm bảo mô-men của động cơ có thể đáp ứng được và tránh tình trạng trượt bước

Yêu cầu về thẩm mỹ của robot cũng được quan tâm Vì vậy, thiết kế tỉ mỉ nhằm giảm trọng lượng ở các khâu mà còn giúp cho robot dễ nhìn

3.1.2 Thiết kế và tối ưu mô hình

Dựa vào các yêu cầu cơ bản đặt ra ở trên nhóm đã tiến hành thiết kế 2 robot 3 bậc tự do ban đầu như Hình 3.1

Hình 3.1: Mô hình cho 2 cánh tay robot 3 bậc tự do góc nhìn từ trên xuống

Sau khi tham khảo và lựa chọn phương thức truyền động cho hai cánh tay robot trên nhóm đã quyết định lựa chọn cơ cấu truyền động dây đai cho 2 robot trên Và các động cơ của các khớp sẽ được đặt ở cuối

Chương 3: Thiết kế và thi công phần cứng Khoa đào tạo chất lượng cao

Dưới đây là bảng vẽ 2D gồm có kích thước và tỉ lệ của mô hình, từ bảng vẽ đó nhóm tiến hành đi cắt Mica theo các tỉ lệ đã được trình bày Ở Hình 3.2 là mặt đế dưới của hộp động cơ

Hình 3.2: Mặt đáy của hộp động cơ Ở Hình 3.3 là hình chiếu của mặt sau cùng của hộp chứa động cơ

Hình 3.3: Mặt sau của hộp động cơ Ở Hình 3.4 là mặt trên cùng của hộp động cơ được thể hiện trên mặt phẳng 2D

Hình 3.4: Mặt trên cùng hộp động cơ Ở Hình 3.5 là mặt bên của hộp động cơ được thể hiện trên mặt phẳng 2D

Hình 3.5: Mặt bên hộp động cơ Ở Hình 3.6 là bảng vẽ kích thước chiều dài thanh liên kết thứ nhất (Link1) của cánh tay robot trên mặt phẳng 2D

Hình 3.6: Bảng vẽ kích thước link 1 của cánh tay robot Ở Hình 3.7 là bảng vẽ kích thước chiều dài thanh liên kết thứ hai (Link2) của cánh tay robot trên mặt phẳng 2D

Hình 3.7: Bảng vẽ kích thước link2 của cánh tay robot Ở Hình 3.8 là bảng vẽ kích thước chiều dài thanh liên kết thứ ba (Link3) của cánh tay robot trên mặt phẳng 2D

Hình 3.8: Bảng vẽ kích thước link3 của cánh tay robot

3.1.3 Tính toán và lựa chọn dây đai

Việc lựa chọn dây đai phù hợp với khoảng cách từ trục động cơ đến mỗi khớp điều khiển robot là rất quan trọng bởi yếu tố căng đai để robot có thể không bị trượt nếu làm dây quá chùng nên nhóm đã tính toán dây đai theo công thức (3.1)

Hình 3.9: Mô hình dây curoa quanh 2 puly

Trong đó: L là chu vi của dây đai; c là khoảng cách giữa 2 tâm của 2 puly; d 1,d 2 lần lượt là đường kính của puly 1 và puly 2

Nhóm đã sử dụng công thức (3.1) để tính và lựa chọn dây curoa thích hợp cho mô hình thực nghiệm 2 cánh tay của nhóm

3.1.4 Lựa chọn thiết bị phần cơ khí

Dựa trên động cơ đã có sẵn, nhóm đã lên ý tưởng thiết kế mô hình dựa trên các động cơ đó như Bảng 3.1

Bảng 3.1: Danh sách thiết bị phần cơ khí

STT Tên thiết bị Hình ảnh Đặc điểm Chức năng

- Được sản xuất từ vật liệu thép CT3

Sử dụng làm trục nối giữa cái khớp của robot

- Được sản xuất từ hợp kim nhôm

Sử dụng cho các trục của hộp số động cơ và truyền động cho khớp

- Được sản xuất từ hợp kim nhôm

Sử dụng để truyền động cho khớp 1 của robot

Sử dụng để truyền động cho khớp 1,2 và 3

- Được sản xuất từ vật liệu hợp kim nhôm

Sử dụng làm khớp quay truyền động ở khớp 1, 2 và 3

- Độ dày vòng bi 7mm.

- Được sản xuất từ vật liệu hợp kim nhôm

- Độ dày vòng bi 7mm.

Sử dụng làm khớp quay truyền động ở khớp 1, 2 và 3

- Được sản xuất từ vật liệu nhựa

- Chiều dày: 10mm, 5mm, 3mm và 2mm

Sử dụng để làm đế và các thanh liên kết robot

- Được sản xuất bởi vật liệu là đồng

Sử dụng để liên kết các khớp thanh liên kết của robot lại với nhau

- Được sản xuất bởi vật liệu là nhôm

Sử dụng để làm trục căng đai cho khớp

- Được sản xuất bởi vật liệu là nhôm

Sử dụng để giữ trục căng đai cho khớp

Tính toán và lựa chọn động cơ

Có rất nhiều cách để có thể tính toán và lựa chọn động cơ cho robot nhưng ở đề tài này thì nhóm sẽ lựa chọn phương pháp tính theo mô hình vẽ trên SolidWorks và đổ vật liệu đúng như khối lượng thực tế của robot và từ đó sẽ chuyển đổi qua

MATLAB Simscape để quan sát mô-men xoắn cũng như vận tốc cần đạt được để mỗi khớp của cánh robot có thể chuyển động

Việc tính toán và lựa chọn động cơ như vậy giúp phần nào giảm tải được chi phí khi làm mô hình thực tế tránh việc lựa chọn động cơ không đủ torque để kéo các khớp di chuyển đúng như mong đợi gây tốn kém và hư hỏng mô hình

Hệ số an toàn để lựa chọn động cơ cho từng khớp của robot là:

3.2.1 Tính toán lựa chọn động cơ cho cánh tay robot 1

3.2.1.1 Lựa chọn động cơ cho khớp 3 của robot 1

Moment xoắn của khớp 3 của cánh tay robot 1 được mô phỏng chạy quỹ đạo trên MATLAB Simulink được thể hiện như Hình 3.10

Hình 3.10: Moment xoắn khớp 3 của robot 1 Moment xoắn lớn nhất của khớp 3 là: or 31 0.0565 t = Nm (3.3)

Vậy cần phải lựa chọn động cơ cho khớp 3 của robot 1 kết hợp với hệ số an toàn ở (3.2) có moment xoắn là:

Vận tốc của khớp 3 của cánh tay robot 1 cần để chạy quỹ đạo được mô phỏng trên MATLAB Simulink như Hình 3.11

Hình 3.11: Vận tốc khớp 3 của cánh tay robot 1 Dựa vào Hình 3.11 ta có vận tốc góc lớn nhất trong một giây robot đạt được là:

Dựa vào công thức (3.4) ta phải chọn động cơ cho khớp 3 của robot 1 có moment xoắn lớn hơn hệ số torque31 đã được tính ra ở công thức (3.4) và phải thỏa mãn vận tốc góc quay được trong một giây là lớn hơn 13.35 o /s

Moment xoắn của khớp 1 của cánh tay robot 1 được mô phỏng chạy quỹ đạo trên MATLAB Simulink được thể hiện như

3.2.2 Tính toán lựa chọn động cơ cho cánh tay robot 2

Hình 3.16: Moment xoắn khớp 3 của robot 2

Moment xoắn lớn nhất của khớp 3 là: or 32 0.0565 t = Nm (3.12)

Hình 3.21: Vận tốc khớp 1 của cánh tay robot 2

Dựa vào Hình 3.21 ta có vận tốc góc lớn nhất trong một giây robot đạt được là:

Dựa vào công thức (3.19) ta phải chọn động cơ cho khớp 1 của robot 2 có moment xoắn lớn hơn hệ số torque12 đã được tính ra ở công thức (3.19) và phải thỏa mãn vận tốc góc quay được trong một giây là lớn hơn 12.26 o /s.

Thiết kế phần điện

Yêu cầu điều khiển bao gồm có: nguồn điện, vi điều khiển, driver, động cơ AC servo, truyền thông can bus theo sơ đồ khối như Hình 3.22

Hình 3.22: Sơ đồ khối tổng quan phần điện

3.3.2 Thiết kế mạch điện điều khiển cho tủ điện

3.3.2.1 Sơ đồ kết nối mạch điều khiển

Sơ đồ kết nối mạch điều khiển Master với 2 Slave như:

TJA 1050 Tx PB8/ CAN1 Rx Rx

SLAVE1 STM32F407 PB9/CAN1 Tx

SLAVE2 STM32F407 PB9/CAN1 Tx

Hình 3.23: Sơ đồ kết nối Master và Slave 1 và 2

Sơ đồ kết nối mạch điều khiển robot 1 với Slave 1 chịu trách nhiệm điều khiển các Slave 1.1, 1.2 và 1.3 điều khiển ở các khớp như Hình 3.24

Hình 3.24: Sơ đồ kết nối Slave 1 với các Slave 1.1, 1.2 và 1.3

Sơ đồ kết nối mạch điều khiển robot 2 với Slave 2 chịu trách nhiệm điều khiển các Slave 2.1, 2.2 và 2.3 điều khiển ở các khớp như Hình 3.25

Hình 3.25: Sơ đồ kết nối Slave 2 với các Slave 2.1, 2.2 và 2.3

3.3.2.2 Thiết kế mạch in PCB

Sơ đồ mạch nguyên lý PCB của mạch điều khiển mô hình 2 cánh tay robot 3 bậc tự do được thể hiện như Hình 3.26

Hình 3.26: Sơ đồ nguyên lý mạch điện của Slave Ở Hình 3.26 trong sơ đồ thể hiện rõ các Module trong đó chức năng cũng như nhiệm vụ của hệ thống Có các khối chính như Khối điều khiển trung tâm gồm các Arduino nano, khối mạng CAN gồm các Module giao tiếp như: mcp2515, tja1050 và khối lọc nhiễu hệ thống giúp nguồn ổn định khi hoạt động

Sơ đồ nguyên lý mạch PCB gồm 1 Master và 2 Slave được thiết kế như Hình 3.27

Hình 3.27: Sơ đồ nguyên lý mạch điện PCB Master điều khiển 2 Slave Ở Hình 3.27 thể hiện rõ các khối chính ở đây là khối MCU là các board STM32F407 được thể hiện như trên bảng vẽ với các khối mạng CAN được sử dụng là module tja1050 và khối đèn led hiển thị trạng thái của hệ đang điều khiển

3.3.3 Lựa chọn thiết bị phần điện

Với những thiết bị cơ khí đã chọn ở trên thì thiết bị lựa chọn cho phần điện cũng đúng với yêu cầu điều khiển của đề tài của nhóm như Bảng 3.2

Riêng với động cơ AC servo mặc dù là thiết bị có sẵn nhưng nhóm đã tính toán và lựa chọn động cơ ở trên nên thấy rằng động cơ trên đủ moment xoắn cũng như vận tốc để thực hiện đề tài này

Bảng 3.2: Danh sách thiết bị phần điện

STT Tên thiết bị Hình ảnh Số lượng Đặc điểm

- 1 pha 230VAC Dòng điện: 10A Tần số: 50/60Hz Biến thiên điện áp cho phép:

1 pha cho phép 207-253VAC Biến thiên tần số: ±5%

Tham số đầu ra encoder: 1 đến 65535 Xung/vòng

Công suất (Động cơ sử dụng trong đồ án): 100W, 200W, 400W

+ STM32F407VG Discovery microcontroller lõi 32-bit ARM Cortex-M4F core, 1

MB Flash, 192 KB RAM in an LQFP100 package

+ On board ST-LINK/V2 với hai chế độ nạp

+ Nguồn cấp qua cổng USB + Nguồn cấp ngoài 3V và 5V + Có nút nhấn và nút Reset tích hợp

+ Có 2 bộ điều khiển CAN tích hợp hỗ trợ chuẩn V2.0B với tốc độ tối đa 1Mbps Có

6 module mạng UART cho phép tốc độ baud rate tối đa là 115200

Module thu phát TJA1050 đượ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 CAN và bus vật lý Thiết bị này cung

Chương 3: Thiết kế và thi công phần cứng Khoa đào tạo chất lượng cao cấp khả năng truyền cho bus và khả năng nhận tới bộ điều khiển CAN, khả năng kết nối lên đến 110 node

Module chuyển đổi CAN MCP2515 thực hiện chuyển đổi giao tiếp SPI sang CAN, tương thích tốt với Arduino và hỗ trợ mạnh mẽ nhiều chế độ điều khiển Module có 1 bộ điều khiển CAN và 1 bộ thu phát TJA1050 tích hợp hỗ trợ tốt chuẩn V2.0B và tốc độ tối đa đạt 1Mbps

Nano 6 Điện áp hoạt động:5VDC Điện áp đầu vào giới hạn: 6- 20VDC

Số chân Digital I/O:14 (trong đó có 6 chân PWM)

Số chân Analog: 8 (độ phân giải 10bit, nhiều hơn Arduino Uno 2 chân)

Bộ nhớ flash: 32KB với 2KB dùng bởi bootloader

SRAM: 2KB EEPROM: 1KB Xung nhịp:16MHz

Nguồn cung cấp cho các mạch điều khiển Chuyển mức điện áp xoay chiều đầu vào 220V đầu ra: 6V, 9V, 11V

800x600x300mm Chất liệu: Kim loại có sơn tĩnh điện

Thi công phần cứng

3.4.1 Thi công phần cơ khí

Dựa vào thiết kế và tính toán vẽ mô hình trên SolidWorks 2021 nhóm đã thi công thực nghiệm theo thiết kế ra được 2 cánh tay robot 3 bậc tự do như Hình 3.28

Hình 3.28: Mô hình thi công cơ khí 2 cánh tay robot 3 bậc tự do

Nhóm đã lên ý tưởng và thi công tủ điện điều khiển của 2 cánh tay robot 3 bậc tự do như Hình 3.29

Hình 3.29: Thi công tủ điện điều khiển cho 2 cánh tay robot

Dựa vào thiết kế mạch in PCB (Printed Circuit Board) nhóm đã trình bày ở trên và đã thi công và được 3 mạch điều khiển giao tiếp CAN BUS với ba mạng lưới CAN giao tiếp với 11 nút như Hình 3.30

Hình 3.30: Mạch điều khiển 2 cánh tay robot với giao thức truyền thông CAN BUS

Thiết kế giao diện điều khiển

Dựa vào nhu cầu của người điều khiển cần quan sát đồ thị đáp ứng của robot chạy thực tế nên nhóm đã thiết kế giao diện điều khiển gồm 4 màn hình như bên dưới

Với giao diện màn hình thứ nhất như Hình 3.31 sẽ là màn hình giới thiệu của nhóm

Hình 3.31: Màn hình giao diện điều khiển thứ nhất

Với giao diện màn hình thứ hai như Hình 3.32 sẽ là màn hình điều khiển hai cánh tay robot kết hợp với biểu diễn quỹ đạo đặt và quỹ đạo thực tế cũng như sai số của mỗi robot theo phương x và y sẽ được thể hiện rõ như bên hình dưới

Hình 3.32: Màn hình giao diện điều khiển thứ hai

Nhìn vào Hình 3.32 đồ thị thể hiện rõ cho người xem biết với màu đỏ là biểu diễn cho robot 1 và màu xanh là biểu diễn cho robot 2 cũng như quỹ đạo thực tế mà 2 robot đang chạy ở đồ thị sai số ở bên dưới cũng thể hiện rõ của từng robot so với quỹ đạo đặt

Với giao diện màn hình thứ ba như Hình 3.33 trên màn hình ở mỗi đồ thị sẽ vẽ ra từng góc theta đặt cũng như là góc thực tế của cánh tay robot thứ nhất chạy được là bao nhiêu từ đó tính ra được sai số của mỗi khớp so với tín hiệu đặt từ đó biểu diễn lên đồ thị sai số của mỗi khớp là bao nhiêu

Hình 3.33: Màn hình giao diện điều khiển thứ ba Với giao diện màn hình thứ tư như Hình 3.34 trên màn hình ở mỗi đồ thị sẽ vẽ ra từng góc theta đặt cũng như là góc thực tế của cánh tay robot thứ hai chạy được là

Chương 3: Thiết kế và thi công phần cứng Khoa đào tạo chất lượng cao bao nhiêu từ đó tính ra được sai số của mỗi khớp so với tín hiệu đặt từ đó biểu diễn lên đồ thị sai số của mỗi khớp là bao nhiêu

Hình 3.34: Màn hình giao diện điều khiển thứ tư

Chương 4: Tính toán động học và thuật toán điều khiển robot đồng bộ Khoa đào tạo chất lượng cao

TÍNH TOÁN ĐỘNG HỌC VÀ THUẬT TOÁN ĐIỀU KHIỂN

Sơ đồ khối hệ thống

Hệ thống có sơ đồ khối điều khiển được trình bày như sau:

- Quy hoạch quỹ đạo, tính toán giải thuật đồng bộ và động học cho 2 robot và gửi vị trí của mỗi khớp của robot đến từng Slave

- Nhận góc thực tế của mỗi khớp từ Slave 1 và 2.

- Xuất dữ liệu lên giao diện

- Nhận góc và gửi vị trí đã tính toán đến mỗi Slave

- Nhận tín hiệu góc thực tế từ các Slave và gửi lên Master

- Nhận góc và gửi vị trí đã tính toán đến mỗi Slave

- Nhận tín hiệu góc thực tế từ các Slave và gửi lên Master

Hình 4.1 Sơ đồ khối của hệ thống điều khiển đồng bộ 2 cánh tay robot 3 bậc tự do

Master (vi điều khiển STM32F407) sẽ đảm nhận vai trò nhận góc encoder từ hai vi điều khiển Slave1 (vi điều khiển STM32F407) và Slave 2 (vi điều khiển STM32F407) phân tích tính toán quy hoạch quỹ đạo, giải thuật đồng bộ, động học và gửi góc quay cho 2 module trên thông qua mạng CAN BUS (Network1) Sau đó 2 module Slave xử lí góc quay nhận từ Master và gửi về cho các vi điều khiển ở các khớp của Robot arm 1 tương ứng Slave1.1, Slave1.2 và Slave1.3 thông qua Network2 và Robot arm 2 tương ứng Slave2.1, Slave2.2 và Slave2.3 thông qua Network3

Tại các module điều khiển các khớp của Robot arm 1 và Robot arm 2 sẽ chịu trách nhiệm trong việc điều khiển Driver của AC Servo đồng thời đọc tín hiệu encoder và gửi về 2 module Slave thông qua Network2 và Network3.

Động học của robot

Đầu tiên ta tiến hành xác định số thanh và khớp của robot, gắn trục z dọc theo khớp thứ i, ở đây ta có 2 lựa chọn, về hướng là hướng lên và hướng xuống ta chọn theo hướng lên Tiếp theo đó ta tiến hành kẻ đường vuông góc chung với 2 trục tọa

Chương 4: Tính toán động học và thuật toán điều khiển robot đồng bộ Khoa đào tạo chất lượng cao độ và gắn trục x dọc theo đường vuông góc chung giữa 2 trục z, hướng từ khớp thứ i đến i+1, theo quy tắc bàn tay phải ta xác định được trục y

Hình 4.2 Mô hình 1 robot và cách đặt trục

Trong Hình 4.2 ta có hệ trục toạ độ X0Y0Z0 so với hệ trục toạ độ X1Y1Z1 ta thấy khoảng cách giữa 2 trục Z0, Z1 được đo dọc theo X0 là 0, góc tạo giữa Z0, Z1 quay quanh trục X0 là 0, khoảng cách giữa X0, X1 dọc theo trục Z1 là 0, góc quay giữa X0,

X1 là 𝜃1 Đối với hệ trục toạ độ X1Y1Z1 so với hệ trục toạ độ X2Y2Z2 ta thấy khoảng cách giữa 2 trục Z1, Z2 được đo dọc theo X1 là L1 góc tạo giữa Z1, Z2 quay quanh trục X1 là 0, khoảng cách giữa X1, X2 dọc theo trục Z2 là 0, góc quay giữa X1, X2 là 𝜃2 Đối với hệ trục toạ độ X2Y2Z2 so với hệ trục toạ độ X3Y3Z3 ta thấy khoảng cách giữa 2 trục Z2, Z3 được đo dọc theo X2 là L2 góc tạo giữa Z2, Z3 quay quanh trục X2 là 0, khoảng cách giữa X2, X3 dọc theo trục Z3 là 0, góc quay giữa X2, X3 là 𝜃3

Bảng 4.1: Bảng DH của robot i 𝑎i-1 𝛼i-1 di 𝜃i

Trong đó: 𝑎i-1 là khoảng cách từ 𝑍̂i-1đến 𝑍̂I được đo dọc theo 𝑋̂i-1, 𝛼i-1 là góc quay từ 𝑍̂i-1 đến 𝑍̂i xung quanh trục 𝑋̂i-1 (theo quy tắc bàn tay phải), di là khoảng cách từ 𝑋̂i-1 đến 𝑋̂i đo dọc theo trục 𝑍̂i.,  i là góc quay từ trục 𝑋̂i-1 đến 𝑋̂i xung quanh trục 𝑍̂i.

Vị trí điểm cuối chiếu lên hệ trục toạ độ thứ 3:

4.2.1 Động học thuận của 1 robot

Công thức tổng quát ma trận chuyển vị từ hệ trục thứ i tới hệ trục số i-1:

1 1 1 1 cos sin 0 sin sin s cos cos cos s 0 sin co

Ma trận chuyển vị từ hệ trục thứ 0 tới hệ trục số 1:

Ma trận chuyển vị từ hệ trục số 0 tới hệ trục số 3:

Vị trí điểm đầu cuối so với hệ trục tham chiếu:

1 1 2 1 2 3 1 2 3 c cos ) cos( ) sin ) sin( ) os( sin(

4.2.2 Động học nghịch của 1 robot Động học nghịch là từ phương trình toạ độ Px, Py, Pz so với toạ độ gốc được tính ra từ động học thuận ta tiến hành áp dụng các phương pháp tính toán để tìm ra được các góc ban đầu của robot có thể đến được với các điểm đầu cuối như phương trình tổng quát ta tìm được từ đó có thể áp dụng phương pháp đại số, phương pháp hình học, để giải và tìm ra được bộ nghiệm cho robot có thể hoạt động

Từ các bộ nghiệm tìm được chúng ta có thể chọn và sử dụng tùy mục đích Có nhiều cách để tính động học nghịch cho robot nhưng ở bài này sẽ áp dụng phương pháp tính động học nghịch cho robot bằng phương pháp đại số

Ta tiến hành đặt    = + + 1 2 3 , S =sin, C =cos , C i =cos i , S i =sin i , sin( i j ) ij

S =  + , và C ij = cos(  i + j ) trong đó i≠j với i, j ∈ (1,2,3) Vì cơ cấu ta chọn robot khi làm việc ở khâu cuối luôn vuông góc với trục x nên ta chọn  = 0

Ta có: EE 0 P Vị trí điểm đầu cuối so với hệ trục tham chiếu đã tính toán được từ động học thuận ở trên:

Từ ma trận (4.9) ta có:

+ Tính 𝜽 𝟐 : Đặt lần lượt các biến n x và n y như sau:

Tổng bình phương n x và n y ta được:

Tử phương trình (4.12) ta tìm được C 2 :

Từ phương trình (4.11) ta đặt:

Từ phương trình (4.11), (4.13) và (4.15) ta có:

Ta tìm được S2 như sau:

Từ phương trình (4.13) và phương trình (4.17) ta tìm được  2 :

Vậy  2 có 2 nghiệm khi S 2 = 1−C 2 2 và S 2 = − 1−C 2 2

Từ phương trình (4.11) ta có:

Từ phương trình (4.20) ta rút ra biến C1 và S1:

Thay S1 vào C1 ta thu được C1:

Từ phương trình (4.14), (4.19) và (4.23) ta tìm được  3 :

Không gian toạ độ trong robot

Mô tả hệ toạ độ B so với hệ toạ độ A Ở đây, toạ độ gốc của hệ toạ độ B là không trùng khớp với hệ toạ độ A, nó được tạo bởi từ một vector mới, vector từ A đến gốc của hệ toạ độ B được gọi là A P BORG Cũng như B xoay một góc so với A gọi là B A R Cho B P , chúng ta có thể tính A P như sau:

Hình 4.3 Hệ toạ độ B so với A

P = R P + P (4.25) Đơn giản hơn về việc mô tả vector trong hệ toạ B so với hệ toạ độ A

Một ánh xạ (mapping) từ hệ toạ độ này sang hệ toạ độ khác như toán tử ở dạng ma trận Điều này giúp việc viết phương trình nhỏ gọn và rõ ràng hơn phương trình (4.25) về mặt khái niệm Ta có thể viết mô hình toán học cho trong (4.25) ở dạng ma

Chương 4: Tính toán động học và thuật toán điều khiển robot đồng bộ Khoa đào tạo chất lượng cao trận được gợi ý bởi (4.26), ta xác định ma trận 4x4 và sử dụng vector vị trí 4x1 để (4.26) có cấu trúc:

Trong đó: “1” được thêm vào để đủ dạng vector 4x1 Hàng “[0 0 0 1]” được thêm vào cuối hàng để được ma trận 4x4

Mô hình robot thực tế:

Hình 4.4 Hệ toạ độ robot trong không gian

4.3.3 Động học thuận của 2 robot

Hình 4.5 Chuyển đổi từ gốc toạ độ sang Robot 1 Để chuyển đổi ta thực hiện:

1 Tịnh tiến theo chiều âm của phương x một khoảng bằng

P = − d  Với d: khoảng cách giữa 2 robot

Ma trận chuyển đổi hệ toạ độ 1 với hệ toạ độ 0 là:

  vì không xoay hệ toạ độ khi chuyển đổi

Vị trí điểm đầu cuối của Robot 1 trong hệ toạ độ không gian gốc là:

Với:c 1 =cos( ) 1 ,s 1 =sin( ) 1 ,c 12 =cos(  1 + 2 ),s 12 =sin(  1 + 2 ),

Hình 4.6 Chuyển đổi từ hệ toạ độ gốc sang Robot 2 Để chuyển đổi ta thực hiện:

1 Tịnh tiến theo chiều dương của phương x một khoảng bằng

=   Với d: khoảng cách giữa 2 robot

2 Quay hệ Oxyz quanh trục y 1 góc 180 o theo phương pháp Euler Angle

Ma trận chuyển đổi hệ toạ độ 2 với hệ toạ độ 0 là:

Vị trí điểm đầu cuối của Robot 2 trong hệ toạ độ không gian gốc là:

4.3.4 Động học nghịch của 2 robot

Từ đó nhóm thu được c :

Vậy  2 có 2 nghiệm khi S 2 = 1−C 2 2 và S 2 = − 1−C 2 2

Từ phương trình (4.40) ta rút ra biến c1 và s1:

Thế phương trình (4.42) vào phương trình (4.41) ta tìm được c1:

Từ phương trình (4.44) ta tìm được  3 :

Từ phương trình (4.47) ta tìm được c 2 :

Từ phương trình (4.49) ta rút ra biến c1 và s1:

Thế phương trình (4.51) vào phương trình (4.50) ta tìm được c1:

Từ phương trình (4.53) ta tìm được  3 :

Không gian làm việc và quy hoạch quỹ đạo

Không gian làm việc của robot là khoảng không gian mà ở đó cánh tay robot có thể với tới hay nằm trong khoảng không gian mà nó hoạt động được và hoạt động tốt Với mô hình cánh tay robot 3 bậc mà nhóm đã lựa chọn thì không gian làm việc của robot này phụ thuộc vào giới hạn các góc quay của khớp 1, khớp 2 và khớp 3 Vì nhóm thực hiện đề tài với sự đồng bộ của 2 robot, nên mỗi robot sẽ có không gian làm việc đơn lẻ sau đó kết hợp với nhau để tìm ra không gian làm việc chung mà 2 robot cùng có thể hoạt động tốt ở đó

Bất kỳ robot nào cũng có một giới hạn nhất định về mặt cơ khí khiến cho các góc xoay của các khớp chỉ hoạt động được ở một khoảng nhất định Để biết được không gian làm việc của robot ta tiến hành giới hạn góc xoay cho các khớp Giới hạn góc quay các khớp ta dựa trên mô hình mô phỏng SolidWorks để biết được góc quay tối đa mà thiết kế cơ khí của mô hình có thể đạt được là bao nhiêu Sau khi đo đạc mô hình SolidWorks phiên bản 2021 ta được giới hạn  1 = 15 ,165 ,  2 = − 135 ,135  ,  3 = − 135 ,135 

Tiến hành mô phỏng quỹ đạo bằng hàm “plot(x, y)” của MATLAB (Hình 3) ta có được không gian làm việc của robot với bộ nghiệm 1:  1 = −   80 ,80   ;

 = −  và bộ nghiệm 2:  1 = − 80 ,80 ;  2 =0 , 130−  Sau đó tổng hợp lại ta mới được không gian làm việc đúng của robot ở các góc giới hạn ban đầu

Chương 4: Tính toán động học và thuật toán điều khiển robot đồng bộ Khoa đào tạo chất lượng cao a) b) c)

Hình 4.7 Không gian làm việc Robot 1 a) Không gian làm việc bộ nghiệm 1, b) Không gian làm việc bộ nghiệm 2, c) Không gian làm việc kết hợp của 2 bộ nghiệm

Với không gian làm việc 1 tương ứng bộ nghiệm với góc  2  0, bộ nghiệm 2 tương ứng bộ nghiệm với góc  2  0

Tương tự không gian làm việc của robot 2 được mô tả như hình sau a) b)

Chương 4: Tính toán động học và thuật toán điều khiển robot đồng bộ Khoa đào tạo chất lượng cao c)

Hình 4.8 Không gian làm việc Robot 1 a) Không gian làm việc bộ nghiệm 1, b) Không gian làm việc bộ nghiệm 2, c) Không gian làm việc kết hợp của 2 bộ nghiệm

Với không gian làm việc 1 tương ứng bộ nghiệm với góc  2  0, bộ nghiệm 2 tương ứng bộ nghiệm với góc  2  0

Không gian làm việc chung của 2 Robot

Sau khi dựa vào giới hạn cơ khí và mục tiêu điều khiển, nhóm lựa chọn bộ nghiệm thứ nhất để phù hợp với yêu cầu thiết kế quỹ đạo chung cho 2 robot làm việc trong không gian lớn nhất Do đó nhóm lựa chọn bộ nghiệm thứ nhất của từng robot để vẽ không gian làm việc chung với hệ toạ độ đặt của 2 robot theo mô hình Hình 4.4 Ở đây, ta đặt hai hệ toạ độ gốc của 2 robot cách nhau 616 mm cùng hướng với trục Ox, ngược chiều nhau và hướng vào nhau Hình 4.9 Khoảng cách 616 mm được ước lượng để hai robot có thể có cùng không gian làm việc chung với nhau là lớn nhất theo chiều đặt như trên

Hình 4.9 Vị trí đặt của 2 robot

Không gian làm việc chung của 2 robot như trên được mô phỏng như hình sau:

Hình 4.10 Không gian làm việc chung của 2 Robot

Không gian giao nhau giữa 2 màu xanh lá và xanh dương là không gian làm việc chung của 2 robot.

Thiết kế bộ điều khiển đồng bộ ràng buộc chéo

Theo công thức (2.16) được trình bày ở phần lý thuyết nhóm tiến hành thiết kế bộ điều khiển ràng buộc chéo cho mô hình của nhóm theo các bước sau:

- Quy hoạch quỹ đạo cho robot

- Lấy vị trí góc thực tế của robot trả về qua encoder từ đó qua khối động học thuận để tính ra vị trí thực tế của robot Khi có vị trí đặt và vị trí thực tế nhóm bắt đầu tính sai số của từng robot, đưa vào khối thuật toán đồng bộ ràng buộc chéo sau đó tính ra được vị trí cần để cấp cho 2 robot như Hình 4.11

Hình 4.11: Khối thuật toán đồng bộ ràng buộc chéo

- Lấy vị trí đầu ra vừa tính được ở trên đưa vào khối động học nghịch sẽ tính ra được các góc quay cần để cấp cho robot 1 và robot 2.

MÔ PHỎNG KẾT QUẢ TÍNH TOÁN

Chuyển đổi mô hình Simscape sang MATLAB Simulink

Để chuyển đổi từ SolidWorks sang Simulink ta cần thực hiện các bước được thể hiện như Hình 5.1

Hình 5.1 Sơ đồ chuyển đổi SolidWorks sang Simulink

Sau khi hoàn thành mô hình trên SolidWorks bước tiếp theo cần phải đặt hệ trục tọa độ như những gì đã tính toán ở phần động học thuận, sau đó thông qua phần mềm Simscape Multibody sẽ giúp chuyển đổi từ SolidWorks sang MATLAB, để sử dụng mô hình đã được chuyển đổi cần phải “Import file” đã chuyển đổi vào MATLAB

Từ mô hình cánh tay được thiết kế trên phần mềm SolidWorks 2021, sử dụng nền tảng Simscape Multibody để chuyển sang MATLAB – Simulink 2020b Kết quả được thể hiện như Hình 5.2

Hình 5.2 Mô hình Simscape Multibody đã chuyển đổi của 2 robot 3 bậc tự do.

Mô phỏng động học

Quy hoạch quỹ đạo Động học nghịch Robot Động học thuận Đánh giá kết quả

Hình 5.3 Sơ đồ giải thuật mô phỏng động học

Dựa vào sơ đồ giải thuật như Hình 5.3, tiến hành viết chương trình trên MATLAB – Simulink 2020b như Hình 5.4 Các giá trị tính được từ hàm quy hoạch quỹ đạo sẽ

Chương 5: Mô phỏng kết quả tính toán Khoa đào tạo chất lượng cao được đưa vào khối động học nghịch, ngõ ra của hàm động học nghịch là giá trị các góc xoay của từng khâu đồng thời là giá trị đầu vào của khối robot, từ khối robot có thể lấy ra được góc quay của mô hình robot dựa vào cảm biến trong mô hình Simscape Tiếp tục qua khối động học thuận để tính được vị trí của 2 robot trong không gian để đưa ra kết luận đánh giá kết quả mô phỏng về sai số vị trí đặt và vị trí của robot trong mô phỏng

5.2.1 Thiết kế mô phỏng động học

Hình 5.4 Mô hình mô phỏng động học Đầu tiên nhóm mô phỏng không gian làm việc chung của 2 robot để kiểm tra động học thuận và động học nghịch của cả 2 robot Lần lượt quỹ đạo mô phỏng sẽ là từ điểm tới điểm, quỹ đạo qua 3 điểm, 4 điểm và quỹ đạo hình tròn Mô phỏng này được thực hiện trên phần mềm MATLAB Simulink 2020b, chế độ mô phỏng là Fixed step, thời gian lấy mẫu là 0.001s

5.2.2 Kết quả mô phỏng động học

Nhóm thu lại kết quả gồm 4 đồ thị với 2 đồ thị biểu thị khả năng bám theo của 2 robot theo trục X, Y và 2 đồ thị hiển thị sai số vị trí Với Ref X, Y là toạ độ đặt của 2 robot theo trục X và Y, “Robot i” vị trí thực tế của robot thứ i Đối với đồ thị sai số vị trí với “Robot i” là sai số vị trí của robot thứ i Với a) Đồ thị quỹ đạo theo trục X của 2 robot, b) Đồ thị quỹ đạo theo trục Y của 2 robot, c) Sai số vị trí theo trục X của

2 robot, d) sai số vị trí theo trục Y của 2 robot Đối với điểm tới điểm

Mô phỏng quỹ đạo đi từ điểm (-40, -50) đến điểm (-50, 70) trong không gian chung của 2 robot Thời gian mô phỏng 10s

Hình 5.5 Quỹ đạo mô phỏng từ điểm tới điểm

Chương 5: Mô phỏng kết quả tính toán Khoa đào tạo chất lượng cao

Dữ liệu toạ độ X và Y thu được như Hình 5.6 a) b) c) d)

Hình 5.6 Dữ liệu X, Y của robot trên mô phỏng theo quỹ đạo từ điểm tới điểm Đối với quỹ đạo đường thẳng, theo dữ liệu phân tích như Hình 5.6, ta thấy robot

1 và robot 2 bám so với tín hiệu đặt X và Y, đồng thời trả về sai số bằng 0 Do đó 2 robot chạy đúng theo quỹ đạo cho trước và hoạt động tốt trong không gian làm việc chung Đối với quỹ đạo 3 điểm (tam giác)

Mô phỏng quỹ đạo đi qua 3 điểm (-40, -50), (-50, 70) và (40, 70) trong không gian chung của 2 robot Thời gian mô phỏng 20s

Hình 5.7 Mô hình robot chạy theo quỹ đạo hình tam giác

Dữ liệu toạ độ X và Y thu được như Hình 5.8

Chương 5: Mô phỏng kết quả tính toán Khoa đào tạo chất lượng cao a) b) c) d)

Hình 5.8 Dữ liệu X, Y của robot trên mô phỏng theo quỹ đạo 3 điểm Đối với quỹ đạo 3 điểm (hình tam giác), theo dữ liệu phân tích như Hình 5.8, ta thấy robot 1 và robot 2 bám so với tín hiệu đặt X và Y, đồng thời trả về sai số bằng

0 Do đó 2 robot bám chạy đúng theo quỹ đạo cho trước và hoạt động tốt trong không gian làm việc chung Đối với quỹ đạo 4 điểm

Mô phỏng quỹ đạo đi qua 4 điểm (-40, -50), (-50, 70), (40, 70) và (40, -50) trong không gian chung của 2 robot Thời gian mô phỏng 15s

Hình 5.9 Mô hình robot chạy theo quỹ đạo 4 điểm

Hình 5.10 Dữ liệu X, Y của robot trên mô phỏng theo quỹ đạo 4 điểm Đối với quỹ đạo 4 điểm (hình tứ giác), theo dữ liệu phân tích như Hình 5.10, ta thấy robot 1 và robot 2 bám so với tín hiệu đặt X và Y, đồng thời trả về sai số bằng

0 Do đó 2 robot bám chạy đúng theo quỹ đạo cho trước và hoạt động tốt trong không gian làm việc chung Đối với quỹ đạo nhiều điểm (hình tròn)

Mô phỏng quỹ đạo hình tròn tâm (0,0) bán kính 50 mm trong không gian chung của 2 robot Thời gian mô phỏng 15s

Hình 5.11 Mô hình robot chạy theo quỹ đạo hình tròn

Hình 5.12 Dữ liệu X, Y của robot trên mô phỏng theo quỹ đạo đường tròn Đối với quỹ đạo hình tròn, theo dữ liệu phân tích như Hình 5.12, ta thấy X1, X2 và Y1, Y2 bám so với tín hiệu đặt X và Y, đồng thời trả về sai số bằng 0 Do đó 2 robot bám chạy đúng theo quỹ đạo cho trước và hoạt động tốt trong không gian làm việc chung.

Mô phỏng thuật toán điều khiển đồng bộ

Đối với điều khiển động bộ 2 cánh tay robot cần bổ sung các thuật toán điều khiển để mô tả hiện tượng đồng bộ trên robot 1 so với robot 2 Đầu tiên nhóm mô phỏng robot ở chế độ mô phỏng là Fixed step, quỹ đạo hình tròn tâm (0, 0) bán kính 50mm, thời gian lấy mẫu là 0.001s và thời gian mô phỏng là 30s

Nhóm thực hiện giả lập lỗi khi tác động link 1 của 2 robot Thời gian tác động lỗi vào link 1 của robot 1 là từ 5s đến 10s Thời gian tác động lỗi vào link 1 của robot 2 là từ 15s đến 20s Thời gian còn lại 2 robot hoạt động không bị tác động lỗi và chạy theo tín hiệu đặt của không gian quỹ đạo chung Bộ mô phỏng lỗi được nhóm thực hiện theo phương pháp cộng thêm góc quay cho link 1 tại thời gian xảy ra lỗi Kết quả mô phỏng hiển thị bởi các đồ thị với Ref X, Y là toạ độ đặt của 2 robot theo trục

X và Y, “Robot i” vị trí thực tế của robot thứ i Đối với đồ thị sai số vị trí với “Robot

Chương 5: Mô phỏng kết quả tính toán Khoa đào tạo chất lượng cao i” là sai số vị trí của robot thứ i Với a) Đồ thị quỹ đạo theo trục X của 2 robot, b) Đồ thị quỹ đạo theo trục Y của 2 robot, c) Sai số vị trí theo trục X của 2 robot, d) sai số vị trí theo trục Y của 2 robot

Mô hình mô phỏng Simulink được nhóm thiết kế như Hình 5.13:

Hình 5.13 Mô phỏng 2 robot khi không có bộ điều khiển đồng bộ

Kết quả mô phỏng khi không có phương pháp đồng bộ như Hình 5.14 a) b) c) d)

Hình 5.14 Bộ mô phỏng lỗi cho 2 robot khi không có phương pháp đồng bộ

Theo Hình 5.14, nhóm thu được kết quả lúc thời điểm 5s đến 10s robot 1 bị lệch quỹ đạo so với tín hiệu đặt còn robot 2 thì không bị ảnh hưởng Tương tự tại thời

Chương 5: Mô phỏng kết quả tính toán Khoa đào tạo chất lượng cao điểm 15s đến 20s robot 2 bị lệch quỹ đạo so với tín hiệu đặt còn robot 1 thì không bị ảnh hưởng Do đó có thế kết luận 2 robot hoàn toàn không bị phụ thuộc và không có sự đồng bộ với nhau Khi 1 trong 2 robot bị lỗi thì robot còn lại không bám theo mà tiếp tục chạy theo quỹ đạo đặt Do đó nhóm tiến hành tìm hiểu và thiết kế bộ điều khiển đồng bộ để 2 robot có sự tương tác với nhau

5.3.1 Kết quả thuật toán đồng bộ chủ tớ Đầu tiên nhóm mô phỏng kết quả thuật toán đồng bộ chủ tớ khi tín hiệu X, Y đọc từ encoder của robot 1 sẽ là tín hiệu X, Y đặt của robot 2 như Hình 5.15

Hình 5.15 Mô phỏng phương pháp đồng độ MASTER SLAVE

Kết quả mô phỏng khi dùng phương pháp đồng bộ chủ tớ như Hình 5.16 a) b) c) d)

Hình 5.16 Mô phỏng đồng bộ chủ tớ khi tác động lỗi lần lượt vào link 1 của 2 robot

Theo Hình 5.16, nhóm thu được kết quả lúc thời điểm 5s đến 10s robot 1 (robot chủ) bị lệch quỹ đạo so với tín hiệu đặt thì robot 2 (robot tớ) bị ảnh hưởng và bám theo robot 1 Đồng thời tại thời điểm 15s đến 20s robot 2 bị lệch quỹ đạo so với tín hiệu đặt nhưng robot 1 thì không bị ảnh hưởng Do đó có thế kết luận 2 robot có ảnh hưởng qua lại với nhau Khi robot 1 bị tác động lực thì robot 2 cũng bám theo như tín hiệu mà robot trả về, ngược lại khi robot 2 bị tác động lực thay đổi khỏi quỹ đạo đặt thì robot 1 không bám theo mà vẫn chạy theo quỹ đạo ban đầu Từ đó để có sự tương tác chéo giữa 2 robot thì nhóm tiến hành tìm hiểu và thuật toán đồng bộ mới

5.3.2 Kết quả thuật toán đồng bộ ràng buộc chéo Đối với thuật toán đồng bộ ràng buộc chéo thì đầu ra của robot 1 và 2 sẽ là đầu vào của chúng Nhóm thiết kế bộ điều khiển trên mô phỏng như Hình 5.17 giống với công thức (2.16) ở phần lý thuyết

Hình 5.17 Mô hình mô phỏng thuật toán đồng bộ chéo (Cross Coupling Controller)

Kết quả mô phỏng khi dùng phương pháp đồng bộ ràng buộc chéo như Hình 5.16

Hệ số  lần lượt là 20, 1, 0.1

Chương 5: Mô phỏng kết quả tính toán Khoa đào tạo chất lượng cao c) d)

Hình 5.18 Mô phỏng đồng bộ ràng buộc chéo khi tác động lỗi vào link 1 của 2 robot

Theo Hình 5.18, nhóm thu được kết quả lúc thời điểm 5s đến 10s robot 1 (robot chủ) bị lệch quỹ đạo so với tín hiệu đặt thì robot 2 (robot tớ) bị ảnh hưởng và bám theo robot 1 Đồng thời tại thời điểm 15s đến 20s robot 2 bị lệch quỹ đạo so với tín hiệu đặt thì robot 1 bị ảnh hưởng và bám theo robot 2 Do đó có thế kết luận 2 robot có ảnh hưởng qua lại với nhau Khi robot 1 bị tác động lực thì robot 2 cũng bám theo ngược lại khi robot 2 bị tác động lực thay đổi khỏi quỹ đạo đặt thì robot 1 bám theo vị trí robot 2 Do đó việc mô phỏng thuật toán đồng bộ chéo đã thu được kết quả tốt khi  =20

Khi  =1 thì sự đồng bộ và khả năng bám được biểu thị qua Hình 5.19: a) b) c) d)

Hình 5.19 Mô phỏng đồng bộ ràng buộc chéo khi tác động lỗi vào link 1 của 2 robot

Theo Hình 5.19, khi  =1 thì sai số bám của robot 1 và robot 2 khác nhau, cụ thể so với trường hợp  thì sai số bám khác nhau rõ rệt hơn Từ đó có thể kết luận hệ số  ảnh hưởng tới khả năng đồng bộ của 2 robot

Khi  =0.1, thì sự đồng bộ và khả năng bám của tính hiệu được biểu thị qua

Hình 5.20 Mô phỏng đồng bộ ràng buộc chéo khi tác động lỗi lần lượt vào link 1 của 2 robot a) Vị trí X, b) Vị trí Y, c) Sai số X, d) sai số Y

Theo Hình 5.20, hệ số  =0.1 cho thấy khả năng đồng bộ thấp nhất, 2 robot vẫn bám theo nhưng sai số bám khác nhau Do đó có thể kết luận về hệ số  , khi  cao thì sự đồng bộ tốt hơn so với  thấp

Khi so sánh giữa 2 phương pháp đồng bộ so với không có đồng bộ, nhóm thu được kết quả như Hình 5.21 và Hình 5.22 Với Ref là tín hiệu đặt của 2 robot, Robot i là vị trí theo trục X và Y của trường hợp không sử dụng phương pháp đồng bộ của robot thứ i Robot i MS là vị trí theo trục X và Y của trường hợp sử dụng phương pháp đồng bộ chủ tớ của robot thứ i Robot i CCC là vị trí theo trục X và Y của trường hợp sử dụng phương pháp đồng bộ ràng buộc chéo của robot thứ i Đồ thị sai số với “Robot i” là sai số của tín hiệu đặt so với tín hiệu của robot thứ i khi không sử dụng phương pháp đồng bộ “Robot i MS” là sai số của tín hiệu đặt so

Chương 5: Mô phỏng kết quả tính toán Khoa đào tạo chất lượng cao với tín hiệu của robot thứ i sử dụng phương pháp đồng bộ chủ tớ “Robot i CCC” là sai số của tín hiệu đặt so với tín hiệu của thứ i sử dụng phương pháp đồng bộ ràng buộc chéo syns eX, syns eY là sai số đồng bộ hoá của 2 robot theo trục X, Y khi không sử dụng phương pháp đồng bộ, syns eX MS, syns eY MS là sai số đồng bộ hoá của 2 robot theo trục X, Y khi sử dụng phương pháp đồng bộ chủ tớ, syns eX CCC, syns eY CCC là sai số đồng bộ hoá của 2 robot theo trục X, Y khi sử dụng phương pháp đồng bộ ràng buộc chéo a) c) d)

Hình 5.21 Tổng hợp các phương pháp điều khiển theo trục X của robot 1 và robot 2 a)

Chương 5: Mô phỏng kết quả tính toán Khoa đào tạo chất lượng cao b) c)

Hình 5.22 Tổng hợp các phương pháp điều khiển theo trục Y của robot 1 và robot 2 Nhận xét kết quả mô phỏng:

- Đối với phương pháp đồng bộ chủ tới thì chỉ có robot chủ lỗi thì robot tớ mới bám theo quỹ đạo, nên sai số bám và sai số đồng bộ gần như bằng không Còn khi robot tớ bị tác động lực đi ra khỏi quỹ đạo đặt thì robot chủ không bám theo làm cho sai số bám và sai số đồng bộ ở trường hợp này rất cao

- Đối với phương pháp đồng bộ ràng buộc chéo tuy cả 2 có sai số đồng bộ và sai số bám nhưng giá trị này thấp hơn nhiều so với phương pháp đồng bộ chủ tớ Nếu trong điều kiện lý tưởng như trong mô phỏng thì có thể tăng hệ số  lên giúp cho sai số đồng bộ thấp lại và 2 robot hoạt động cùng nhau tốt hơn.

PHƯƠNG PHÁP ĐIỀU KHIỂN PHÂN TÁN ÁP DỤNG MẠNG

Sơ đồ khối của hệ thống điều khiển phân tán áp dụng mạng CAN

Hình 6.1: Sơ đồ khối của hệ thống phân tán cho hệ đồng bộ 2 cánh tay robot 3 bậc tự do Chức năng của từng khối trong hệ thống được trình bày dưới đây:

- Xử lí thuật toán đồng bộ

- Thu thập dữ liệu encoder từ các Slave module

- Xử lí phân luồng tin nhắn CAN (Network1)

- Hiệu chỉnh quy đổi và gửi giá trị góc để điều khiển robot 1 và robot 2

- Điều khiển đồng bộ 8 Slave module và cánh tay robot

Chương 6: Phương pháp điều khiển phân tán áp dụng mạng CAN BUS Khoa đào tạo chất lượng cao

- Nhận góc quay của cánh tay robot từ Master module

- Chấp hành hiệu lệnh điều khiển của Master

- Xác định góc quay để gửi cho các Module Slave 1.1, 1.2 và 1.3 điều khiển robot 1

- Thu thập dữ liệu góc encoder từ các Module Slave 1.1, 1.2 và 1.3

- Xử lí phân luồng tin nhắn CAN (Network1 và Network2)

- Nhận góc quay của cánh tay robot từ Master module

- Xác định góc quay để gửi cho các Module Slave 2.1, 2.2 và 2.3 điều khiển robot 2

- Thu thập dữ liệu góc encoder từ các Module Slave 2.1, 2.2 và 2.3

• Slave module 1.1, 1.2, 1.3, 2.1, 2.2 và 2.3 (Arduino Nano)

- Nhận góc quay từ Slave module 1 và 2

- Xác định chiều quay của động cơ

- Thu thập dữ liệu encoder của các encoder gửi cho Slave module 1 và 2

Thiết kế bộ điều khiển phân tán

Master module chứa các giá trị điều khiển và đảm nhận vai trò nhận góc encoder từ hai vi điều khiển Slave1 Module và Slave 2 Module phân tích tính toán và gửi góc quay cho 2 module trên thông qua mạng CAN BUS (Network1) Sau đó 2 module Slave xử lí góc quay nhận từ Master và gửi về cho các vi điều khiển ở các khớp của Robot arm 1 tương ứng Slave1.1, Slave1.2 và Slave1.3 thông qua Network2 và Robot arm 2 tương ứng Slave2.1, Slave2.2 và Slave2.3 thông qua Network3

Tại các module điều khiển các khớp của Robot arm 1 và Robot arm 2 sẽ chịu trách nhiệm trong việc điều khiển Driver của AC Servo đồng thời đọc tín hiệu encoder và gửi về 2 module Slave thông qua Network2 và Network3

Cấu hình các thông số quan trọng cho mạng CAN:

Message ID được đặt thích hợp đảm bảo mức độ ưu tiên của tin nhắn CAN được đảm bảo tuân theo mức độ phân cấp của hệ thống Theo đó, Master module sử dụng những Message ID có giá trị thấp nhất (quyền ưu tiên cao nhất của Network1), sau đó là Slave module sử dụng những Message ID có giá trị thấp nhất (ưu tiên cao nhất của Network2 và Network3) và sau cùng là những Slave module 1.1, 1.2, 1.3, 2.1, 2.2 và 2.3 sử dụng những Message ID giá trị cao nhất (ưu tiên thấp nhất của Network

2 và Network3) Giữa các Slave module còn được phân cấp ưu tiên trên đường truyền tùy thuộc vào sự ưu tiên của khớp quay mà nó phụ trách trong hệ thống Quyền ưu tiên thường được sắp xếp theo thứ tự giảm dần từ khớp đầu tiên đến khớp cuối cùng Quyền ưu tiên có thể có sự khác biệt tùy thuộc vào loại robot và hệ thống thực tế Để có thể xây dựng các chương trình giao tiếp dữ liệu bằng giao thức CAN, yêu cầu người dùng phải cài đặt thư viện Waijung Blockset vào thư viện MATLAB Simulink và sử dụng các phần cứng có chức năng hỗ trợ giao tiếp CAN Bus

Các bước thiết lập chương trình truyền data bằng giao thức CAN sử dụng thư viện Waijung trên phần mềm MATLAB Simulink phiên bản 2020b

Thông số quan trọng cần chú ý đó là cấu hình tần số xung clock của vi điều khiển được sử dụng trong hệ thống là STM32F407 Discovery nếu dùng thư viện Waijung thì cấu hình thạch anh ngoại là 8Mhz và tốc độ xung clock cao nhất là 168Mhz sau khi qua bộ chia 4 của module thì tần số tối đa của vi điều khiển là 42MHz

Một bit timing trên đường truyền CAN được cấu hình như sau:

Hình 6.2: CAN bit segment Time Quantum là đơn vị thời gian nhỏ nhất cho tất cả các giá trị cấu hình

Bảng 6.1: Số tq cho phép cài đặt trong một bit timing

Prop_Seg + Phase_Seg1 1…16tq

Bảng tính trên sử dụng Prop_Seg = 0

SYNC_SEG: được sử dụng để đồng bộ các node bus

PROP_SEG: được sử dụng để bù tín hiệu delay thông qua network

PHASE_SEG1 được sử dụng để bù cho các cạnh lỗi pha và có thể kéo dài trong quá trình tái đồng bộ

PHASE_SEG2 là tối đa của PHASE_SEG1 và thời gian xử lý thông tin dài Nó cũng được sử dụng để bù các cạch lỗi pha và có thể được rút ngắn trong quá trình tái đồng bộ Giá trị tối thiểu của PHASE_SEG2 là giá trị của SJW

SJW giá trị luôn luôn bằng 0, điều này có nghĩa là Synchronization Jump Width là 1tq Nếu một giá trị được nhập vào, nó chỉ sao chép đến thanh ghi thích hợp

Tính toán Time Quantum để tốc độ truyền data của hệ thống CAN đạt 1Mbps với tần số xung clock của hệ thống là 42Mhz và tần số xung clock của hệ thống CAN là 6Mhz khi qua bộ chia trước là 7 Độ dài đường truyền CAN bus: 10m với độ trễ dây dẫn khoảng 5ns/m Độ trễ đáp ứng của CAN transceiver: 260ns

Thời gian trễ do dây dẫn bus: 10 * 5 = 50ns

Tốc độ baud rate là 1Mbps

Số Time Quantum 1 bit time:

PROP SEG busdelay tranceiver delay t = t +t = + = ns (6.5)

Resync Jump Width (SJW): 1tq

BS PROP SEG ROUNDUP ROUNDUP tq tq ns

BS SJW PROP SEG BS tq

Từ đó cấu hình được mạng CAN cho các module của hệ thống

Cấu trúc dữ liệu tin nhắn được thiết kế và tối ưu theo lượng dữ liệu cần trao đổi để tránh gây quá tải đường truyền (tối đa 8 byte) Bên cạnh đó giải thuật còn phải so sánh độ dài dữ liệu khi nhận so với lúc gửi đi để đảm bảo bảo toàn dữ liệu tuyệt đối, đặc biệt là phương thức mã hóa cũng như giải mã phù hợp cả bên gửi và bên nhận Cấu trúc dữ liệu tin nhắn cơ bản các module như sau:

Hình 6.3: Cấu trúc dữ liệu tin nhắn gửi đi trong CAN Ở hệ thống nhóm xây dựng khi Master module xử lý và tính toán xong thì gửi dữ liệu đi trên ID tương ứng đã được định sẵn địa chỉ nhận của Slave module dựa trên cấu trúc dữ liệu tin nhắn CAN như ở Hình 6.3

Với Master module ở Frame tin nhắn gửi đi cho robot 1 là một kiểu số nguyên có dấu như sau:

Byte 6: Lệnh điều khiển của Module Slave 1

Với Master module ở Frame tin nhắn gửi đi cho robot 2 là một kiểu số nguyên có dấu như sau:

Byte 6: Lệnh điều khiển của Module Slave 2

Trước khi gửi đi thì để chính xác hơn trong quá trình gửi thì nhóm đã thực hiện Scale dữ liệu lên 100 rồi mới tiền hành truyền

6.2.2 Lưu đồ giải thuật qua các khối điều khiển

6.2.2.1 Lưu đồ giải thuật cho Master Module

Tính giá trị đặt các khớp quay của 2 cánh tay robot từ khối quy hoạch quỹ đạo, động học và thuật toán đồng bộ

Xử lý nút nhấn bên giao diện điều khiển

Gửi lệnh điều khiển và giá trị đặt các khớp quay cho Slave Module 1 và 2 qua CAN Network1

Nhận giá trị Encoder trả về vị trí của robot hiện tại từ

Slave Module qua CAN Network1

Xuất dữ liệu lên giao diện điều khiển

Hình 6.4: Lưu đồ giải thuật khối Master

Master module (STM32F407 Discovery) là khối trung tâm điều khiển xử lý chính của hệ thống có nhiệm vụ tính toán thuật toán đồng bộ đồng thời xử lí các góc trả về từ các khớp để xử lý đồng thời gửi dữ liệu thu thập được lên giao diện thông qua UART Lưu đồ giải thuật cho khối Master Module như Hình 6.4

Master Module chịu trách nhiệm nhận tín hiệu từ giao diện điều khiển để có thể chọn được quỹ đạo phù hợp cho robot và đi qua thuật toán đồng bộ để tính ra được vị trí cần để cấp vào cho khối động học nghịch từ đó có thể tính toán ra các góc cho từng khớp của 2 cánh tay robot phù hợp Đồng thời Master cũng có nhiệm vụ thu thập các góc chạy thực tế của robot để hiển thị sang giao diện điều khiển để người sử dụng có thể biết được sai số cũng như góc thực tế robot có chạy đúng với quỹ đạo đặt hay không

Lưu đồ giải thuật nút nhấn điều khiển được sử dụng bên giao diện như Hình 6.5

Robot hoạt động theo quỹ đạo đã chọn

Hình 6.5: Lưu đồ giải thuật xử lí nút nhân bên giao diện điều khiển

Sau khi khai báo và chọn quỹ đạo trên giao diện thì ta nhấn Start thì hệ thống kiểm tra nếu có thì sẽ bắt đầu chạy quỹ đạo mà đã chọn còn nếu nhấn Stop thì hệ thống dừng lại

6.2.2.2 Lưu đồ giải thuật cho Slave 1 Module

Slave 1 Module (STM32F407 Discovery) là khối điều khiển cho robot 1 và chịu trách nhiệm trong việc nhận góc quay từ Master Module thông qua Network1 sau đó tính toán và gửi góc quay phù hợp đến các Slave module 1.1, 1.2 và 1.3 Đồng thời cũng nhận góc thực tế từ các khớp của Robot 1 gửi về thông qua Network2 Lưu đồ giải thuật khối Slave 1 Module như Hình 6.6

Nhận giá trị đặt các khớp quay của robot 1 từ Master module qua CAN Network1

Tính toán giá trị các góc nhận để phù hợp cho các khớp của robot 1

Nhận giá trị Encoder của 3 khớp trả về vị trí của robot 1 thông qua CAN Network2

Gửi lệnh điều khiển và giá trị đã tính toán đến các khớp quay cho Slave Module 1.1, 1.2 và 1.3 qua CAN Network2

Hình 6.6: Lưu đồ giải thuật cho Slave 1 Module

6.2.2.3 Lưu đồ giải thuật cho Slave 2 Module

Slave 1 Module (STM32F407 Discovery) là khối điều khiển cho robot 1 và chịu trách nhiệm trong việc nhận góc quay từ Master Module thông qua Network1 sau đó tính toán và gửi góc quay phù hợp đến các Slave module 2.1, 2.2 và 2.3 Đồng thời cũng nhận góc thực tế từ các khớp của Robot 2 gửi về thông qua Network3 Lưu đồ giải thuật khối Slave 1 Module như

Nhận giá trị đặt các khớp quay của robot 2 từ Master module qua CAN Network1

Tính toán giá trị các góc nhận để phù hợp cho các khớp của robot 2

Nhận giá trị Encoder của 3 khớp trả về vị trí của robot 2 thông qua CAN Network3

Gửi lệnh điều khiển và giá trị đã tính toán đến các khớp quay cho Slave Module 2.1, 2.2 và 2.3 qua CAN Network3

Hình 6.7: Lưu đồ giải thuật cho Slave 2 Module

6.2.2.4 Lưu đồ giải thuật cho Slave Module điều khiển các khớp của robot

GIAO TIẾP VỚI STM ĐIỀU KHIỂN ĐỘNG CƠ

Khai báo các biến, cài đặt giá trị ban đầu

THỰC NGHIỆM VÀ ĐÁNH GIÁ KẾT QUẢ

Kết quả phần cứng

Dưới đây là kết quả phần cứng mà nhóm đã đạt được sai khi hoàn thành đề tài gồm có tủ điều khiển như Hình 7.1

Hình 7.1: Tủ điện điều khiển cho hệ thống

Và nhóm đã hoàn thành được mô hình cũng như là tủ điều khiển được thể hiện ở Hình 7.2

Hình 7.2: Mô hình 2 cánh tay robot và tủ điện điều khiển

Chương 7: Thực nghiệm và đánh giá kết quả Khoa đào tạo chất lượng cao

Kết quả thực nghiệm

Sau khi nhóm chạy thực nghiệm robot sử dụng UART để đọc tín hiệu, dùng phần mềm QT design để thiết kế giao diện và ngôn ngữ python để lập trình giao diện Đồng thời nhóm lưu dữ liệu lại và dùng MATLAB để vẽ đồ thị trình bày trong bài báo cáo này

Lần lượt là 4 quỹ đạo khi không có phương pháp đồng bộ đó là quỹ đạo đường thẳng, quỹ đạo 3 điểm (tam giác), quỹ đạo 4 điểm (tứ giác) và quỹ đạo nhiều điểm (đường tròn) Kết quả thực nghiệm được trình bày ở các Hình 7.3 đến Hình 7.14, với

“Ref Robot i” là tín hiệu đặt theo trục X, Y cho robot thứ i, “Robot i” là tín hiệu vị trí

X, Y của robot thứ i trong thực tế Được trình bày theo 4 đồ thị a) đáp ứng của Robot thứ i theo trục X, b) đáp ứng của Robot thứ i theo trục Y, c) sai số vị trí của Robot thứ i theo trục X, d) sai số vị trí của Robot thứ i theo trục Y

Quỹ đạo đường thẳng quy hoạch từ điểm (0, 200) đến điểm (0,100) chạy trong 2 chu kỳ và quay trở lại vị trí ban đầu Thời gian đi từ điểm (0, 200) tới (0, 100) là 2s Thời gian chạy của 2 robot là 11s a) b) c) d)

Hình 7.3 Quỹ đạo đường thẳng

Theo Hình 7.3, ta thấy sai số của quỹ đạo đi từ điểm tới điểm theo trục X là 0.2 mm, theo trục Y là 1.5 mm đối với sai số lớn nhất Do đó đối với quỹ đạo đường thẳng 2 robot hoạt động tốt trong không gian làm việc và sai số nhỏ

Chương 7: Thực nghiệm và đánh giá kết quả Khoa đào tạo chất lượng cao a) b) c) d) e) f)

Hình 7.4 Đáp ứng các góc của robot 1 a) b) c) d) e) f)

Hình 7.5 Đáp ứng các góc của robot 2

Theo Hình 7.4 và Hình 7.5, đáp ứng khi chạy theo quỹ đạo từ điểm tới điểm của các góc tại các khớp tốt, khớp 1 của 2 robot có sai số nhỏ nhất so với khớp 2 và khớp

3 là khoảng 0.1 độ cho sai số lớn nhất Đối với khớp 2 và khớp 3 thì sai số lớn hơn, khoảng từ -0.5 đến 0.5 độ

Quỹ đạo tam giác quy hoạch qua 3 điểm (0, 200), (-50, 150) và (50, 150) trong

Hình 7.6 Đáp ứng của Robot theo quỹ đạo tam giác

Theo Hình 7.6, quỹ đạo qua 3 điểm robot đáp ứng tốt ở cả trục X và Y sai số khoảng -1 đến 1 mm a) b) c) d) e) f)

Hình 7.7 Đáp ứng góc robot 1

Hình 7.8 Đáp ứng góc robot 2

Theo Hình 7.7 và Hình 7.8, sai số bám theo của 2 robot là thấp cụ thể từ -0.8 đến 0.4 độ, hầu như các khớp của 2 robot chỉ khoảng -0.2 đến 0.2 độ Do đó có thể kết luận 2 robot hoạt động tốt với quỹ đạo 3 điểm trong không gian làm việc chung của

Quỹ đạo hình tứ giác quy hoạch từ điểm (-40, 100), (40, 100), (40, -10) và (-40, -10) Quỹ đạo chạy trong vòng 11 giây a) b) c) d)

Hình 7.9 Đáp ứng robot theo quỹ đạo hình tứ giác

Theo Hình 7.9, sai số đáp ứng thấp khoảng -1mm đến 1mm a) b) c) d) e) f)

Hình 7.10 Đáp ứng góc robot 1 a) b) c) d) e) f)

Hình 7.11 Đáp ứng góc của robot 2 Theo Hình 7.10 và Hình 7.11, sai số bám theo của 2 robot là thấp cụ thể từ -0.6 đến 0.6 độ, hầu như các khớp của 2 robot chỉ khoảng -0.3 đến 0.3 độ Do đó có thể

Chương 7: Thực nghiệm và đánh giá kết quả Khoa đào tạo chất lượng cao kết luận 2 robot hoạt động tốt với quỹ đạo 4 điểm trong không gian làm việc chung của 2 robot

Quỹ đạo hình tròn quy hoạch trên đường tròn có tâm (0, 100) bán kính 50mm, chu kì 1 vòng tròn là 3 giây Thời gian 2 robot hoạt động là 11 giây a) b) c) d)

Hình 7.12 Đáp ứng robot theo quỹ đạo hình tròn

Theo Hình 7.12, mô hình quy hoạch quỹ đạo hình tròn thì đáp ứng tốt nhưng sai số còn lớn cụ thể khoảng -2mm đến 1.5mm a) b) c) d) e) f)

Hình 7.13 Đáp ứng góc robot 1 theo quỹ đạo hình tròn

Hình 7.14 Đáp ứng góc của robot 2 theo quỹ đạo hình tròn

Theo kết quả đồ thị cho thấy robot thực tế có thể hoạt động tốt các loại quỹ đạo

2 điểm, 3 điểm, 4 điểm và quỹ đạo nhiều điểm trong không gian làm việc chung của

2 robot Nhưng vẫn còn tồn tại sai số do thiết kế cơ khí của nhóm chưa được tốt, vẫn còn dùng dây đai để dẫn động, từ sai số dây đai dẫn đến sai số vị trí của điểm đầu cuối Để kiểm chứng được khả năng đồng bộ của các thuật toán thực tế nhóm tiến hành tác động lực làm robot 1 và 2 lệch khỏi quỹ đạo cho trước Đầu tiên bắt đầu từ giây thứ 4 đến giây thứ 5 nhóm giữ lại link 1 của robot 1 và từ 7.5s đến 8.1s giây nhóm giữ lại link 1 của robot 2 để xem kết quả đồng bộ Kết quả được trình bày ở các đồ thị sau:

Kết quả khi không có đồng bộ a) b)

Chương 7: Thực nghiệm và đánh giá kết quả Khoa đào tạo chất lượng cao c) d)

Hình 7.15 Khả năng đồng bộ của robot khi không có phương pháp đồng bộ

Theo Hình 7.15, 2 robot không đồng bộ với nhau, khi 1 trong 2 bị lỗi thì robot còn lại vẫn chạy theo quỹ đạo ban đầu của nó

Khi áp dụng thuật toán đồng bộ chủ tớ a) b) c) d)

Hình 7.16 Khả năng đồng bộ của robot khi áp dụng thuật toán đồng bộ chủ tớ

Khi áp dụng thuật toán đồng bộ chủ tớ vào 2 robot thì nhóm thu được kết quả gần như giống với mô phỏng, sai số nhỏ, khả năng bám của robot tớ bám theo robot chủ là khá tốt Sau khi tác động lỗi 1 trong 2 robot thì cả 2 cũng có thể quay về quỹ đạo ban đầu nhanh, thời gian đáp ứng khoảng 0.2s

Khi áp dụng thuật toán đồng bộ ràng buộc chéo

Vì hệ số  ảnh hưởng tới khả năng đồng bộ và sai số bám vị trí nên nhóm tiến hành khảo sát nhiều trường hợp  để chọn hệ số phù hợp với mô hình của đề tài Với  =0.9 a) b) c) d)

Hình 7.17 Khả năng đáp ứng thuật toán đồng bộ ràng buộc chéo khi hệ số  = 0.9

Ta thấy giá trị  =0.9, sai số bám theo tín hiệu đặt quá lớn, gây dao động rất nhiều làm cho robot hoạt động không tốt trong giá trị này

Chương 7: Thực nghiệm và đánh giá kết quả Khoa đào tạo chất lượng cao c) d)

Ta thấy giá trị  =0.7, sai số bám theo tín hiệu đặt cũng quá lớn, gây dao động rất nhiều làm cho robot hoạt động không tốt trong giá trị này

Hình 7.19 Khả năng đáp ứng thuật toán đồng bộ ràng buộc chéo khi hệ số  = 0.5 Với  = 0.4

Chương 7: Thực nghiệm và đánh giá kết quả Khoa đào tạo chất lượng cao a) b) c) d)

Hình 7.20 Khả năng đáp ứng thuật toán đồng bộ ràng buộc chéo khi hệ số  = 0.4 Với  =0.3 a) b) c) d)

Nhận xét: Đối với  = 0.9 và  = 0.7 sai số đồng bộ và sai số bám quá cao gây ra giao động lớn ảnh hưởng tới khả năng hoạt động của robot Đối với  = 0.5 sai số đồng bộ thấp hơn, hiện tượng giao động ít hơn nhưng vẫn còn xảy ra ở những thời điểm không bị tác động vào robot Đối với  = 0.3 thì sai số bám thấp, robot không bị giao động khi không tác dụng lực lên robot, đồng thời khả năng bám theo quỹ đạo của hệ số  này cao, tuy nhiên khả năng đồng bộ giữa 2 robot còn thấp Do đó nhóm lựa chọn hệ số  = 0.4 vừa có khả năng bám quỹ đạo tốt đồng thời cải thiện được sự đồng bộ giữa 2 robot và hoạt động tốt đối với mô hình thực tế mà nhóm thiết kế Để đánh giá cụ thể được mức độ đáp ứng tốt của các thuật toán với nhau và đối với khi không áp dụng thuật toán đồng bộ, nhóm có thu thập dữ liệu khi robot chạy thực tế và dùng phần mềm MATLAB để vẽ đồ thị và kết quả như Hình 7.22 và Hình 7.23 a) b) c)

Hình 7.22 Tổng hợp sai số vị trí, sai số đồng bộ dựa trên vị trí của robot so với tín hiệu đặt khi áp dụng các thuật toán đồng bộ

Chương 7: Thực nghiệm và đánh giá kết quả Khoa đào tạo chất lượng cao a) b) c)

Hình 7.23 Tổng hợp các phương pháp điều khiển theo trục Y của robot 1 và robot 2

KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN

Kết luận

Sau một thời gian nghiên cứu và thực hiện, luận văn “Thiết kế mô hình và bộ điều khiển đồng cho hệ hai cánh tay robot 3 bậc tự do dựa trên mạng CAN” đưa ra những kết luận như sau: ˗ Thiết kế và thi công được mô hình 2 cánh tay robot 3 bậc tự do trên phần mềm SolidWorks 2021 ˗ Tính toán được các thuật toán của mô hình 2 cánh tay robot 3 bậc tự do: Động học thuận, động học nghịch, quy hoạch quỹ đạo, giải thuật đồng bộ chủ tớ và ràng buộc chéo ˗ Mô phỏng, kiểm tra được các kết quả tính toán của thuật toán 2 cánh tay robot

3 bậc tự do trên phần mềm MATLAB Simulink 2020b ˗ Có kiến thức về truyền thông giao tiếp CAN BUS và điều khiển AC servo cũng như đọc tín hiệu từ Encoder trả về ˗ Giao tiếp dữ liệu giữa các Master và Slave bằng giao thức CAN BUS với 3 Network và 11 Node đề gửi tín hiệu đến động cơ AC servo điều khiển các khớp của mô hình 2 cánh tay robot 3 bậc tự do ˗ Mô phỏng được sự thay đổi quỹ đạo của 2 cánh tay robot 3 bậc tự do trong trường hợp có hoặc không có sự tác động của ngoại lực làm thay đổi quỹ đạo của robot trong trường hợp đồng bộ và không có đồng bộ giữa 2 giải thuật chủ tớ và ràng buộc chéo để so sánh ˗ Thiết kế được giao diện điều khiển mô hình 2 cánh tay robot 3 bậc tự do.

Phương hướng phát triển

˗ Hệ đồng bộ không chỉ dừng lại ở hai cánh tay robot mà sẽ phát triển hơn trong một hệ thống có thể nhiều cánh tay thao tác đồng bộ với nhau ˗ Sử dụng robot có số bậc tự do cao hơn khi thực hiện đồng bộ sẽ mở rộng không gian làm việc cũng như khả năng linh hoạt hơn trong không gian Oxyz ˗ Nghiên cứu về động lực học để có thể áp dụng được nhiều bộ điều khiển nâng cao trong robot đồng bộ ˗ Kiểm soát sai số đồng bộ của robot đối với thuật toán ràng buộc chéo (Cross-Coupling).

Tiêu đề	Thiết Kế Mô Hình Và Bộ Điều Khiển Đồng Bộ Cho Hệ Hai Cánh Tay Robot 3 Bậc Tự Do Dựa Trên Mạng Can
Tác giả	Nguyễn Tấn Lực, Hà Thiên Tranh
Người hướng dẫn	TS. Trần Đức Thiện
Trường học	Trường Đại Học Sư Phạm Kỹ Thuật Thành Phố Hồ Chí Minh
Chuyên ngành	Công nghệ Kỹ thuật Điều khiển và Tự động hóa
Thể loại	đồ án tốt nghiệp
Năm xuất bản	2022
Thành phố	Tp.Hồ Chí Minh

Định dạng
Số trang	129
Dung lượng	26,55 MB