Thiết kế bộ điều khiển pid đồng bộ cho robot song song 4 bậc tự do

TỔNG QUAN

Tổng quan về hướng nghiên cứu

Kể từ khi ra đời, robot ngày càng được nghiên cứu, phát triển và ứng dụng rộng rãi trong nhiều lĩnh vực: nông nghiệp, công nghiệp, quân sự, giáo dục, y tế… Viện nghiên cứu McKinsey (MGI) dự đoán tới năm 2030 robot sẽ thay thế 800 triệu lao động [1] Tổ chức Lao động Quốc tế (ILO) cũng đưa ra cảnh báo cho Việt Nam trong

10 năm tới: robot có thể thay 86% công nhân dệt may - da giày, 75% lao động trong ngành sản xuất điện - điện tử [2]

Hình 1.1: Robot dần thay thế con người trong nhiều lĩnh vực

Theo tiêu chuẩn cấu trúc động học, robot được chia thành robot nối tiếp nếu cấu trúc động học có dạng chuỗi vòng hở và robot song song nếu có chuỗi vòng kín So với robot nối tiếp, robot song song có nhiều ưu điểm về không gian làm việc, độ chính xác, tốc độ làm việc, khả năng mang tải… Vì vậy, nghiên cứu về robot song song và các thuật toán điều khiển thu hút được sự quan tâm của nhiều nhà nghiên cứu ở trong và ngoài nước

1.1.1 Các nghiên cứu ngoài nước

Cấu trúc song song được cấp bằng sáng chế đầu tiên vào năm 1928 cho

Gwinnett, ông đã thiết kế một nền rạp chiếu phim dao động được theo các hiệu ứng hình ảnh, âm thanh [3]

Hình 1.2: Mô hình rạp chiếu phim của Gwinnett

Năm 1962, Gough và Whithall định nghĩa khái niệm robot song song [4] và các ứng dụng của nó gây được sự chú ý, khi Stewart giới thiệu buồng tập lái máy bay dùng để huấn luyên cho phi công vào năm 1965 [5]

Hình 1.3: Mô hình buồng tập lái máy bay của Stewart

Năm 1980, xuất phát từ nhu cầu khi tham quan một nhà máy sản xuất socola,

Reymond Clavel đã phát minh ra robot Delta Mô hình robot và cơ sở lý thuyết điều khiển được hoàn thiện vào năm 1991 [6]

Hình 1.4: Sơ đồ robot Delta

Kể từ đó, robot Delta và các mô hình robot song song ngày càng được phát triển và thương mại hóa bởi các thương hiệu nổi tiếng, như: OMRON, ABB, Kawasaki,

Hình 1.5: Robot Delta gắp sản phẩm

Robot song song là một cấu trúc chuỗi kín, với đế di động được liên kết với đế cố định thông qua các chuỗi động học nối tiếp, dẫn đến sự ràng buộc về cấu trúc hình học và hình thành các điểm kì dị trong không gian hoạt động của robot Gây ra nhiều khó khăn trong việc điều khiển robot và kiểm soát cấu hình kì dị, đòi hỏi những cải tiến liên tục về cấu hình phần cứng cũng như các thuật toán điều khiển để nâng cao khả năng ứng dụng robot song song trong thực tế [7]

Có nhiều thuật toán điều khiển đã được áp dụng trên các hệ thống robot song song, từ thuật toán PD cổ điển [8] tới các phương pháp điều khiển hiện đại, như: điều khiển bám quỹ đạo sử dụng phương pháp chuyển mạch thích nghi [9]; điều khiển trượt cho cơ cấu song song Delta 4 bậc tự do [10]; điều khiển bền vững [11]; điều khiển thích nghi [12] Các phương pháp này mặc dù cho kết quả điều khiển tốt, tuy nhiên các bộ điều khiển được xây dựng riêng lẻ cho từng chuỗi (cánh tay), không có sự liện hệ giữa các chuỗi với nhau Từ đây, vấn đề đồng bộ hóa giữa các chuỗi trong hệ thống robot song song được đặt ra như một nhu cầu cấp thiết nhằm đáp ứng các yêu cầu của hệ thống robot hiện đại (tốc độ và chính xác) Điều khiển đồng bộ hóa cung cấp lợi thế và là giải pháp duy nhất cho vấn đề phối hợp giữa các chuỗi với nhau [13] Bằng cách xây dựng khái niệm sai số xen kênh (cross-coupling error), bộ điều khiển đồng bộ đảm bảo cho tất cả các chuỗi hoàn thành từng nhiệm vụ riêng lẻ, đồng thời đồng bộ hóa các chuyển động giữa chúng Điều khiển PID đã trở thành phương pháp điều khiển tiêu chuẩn và được ứng dụng trong nhiều lĩnh vực vì điều khiển PID có cấu trúc đơn giản [14] Tuy nhiên, mặc dù điều khiển PID cho thấy hiệu quả trong các hệ tuyến tính, nhưng thường không được đánh giá cao trong các hệ thống phi tuyến như robot có số bậc cao Bên cạnh đó, hiệu suất của bộ điều khiển PID chịu ảnh hưởng nhiều bởi kinh nghiệm lựa chọn bộ thông số Nhiều nghiên cứu đã được phát triển để nâng cao chất lượng bộ điều khiển PID nhằm áp dụng trên các hệ thống phi tuyến Như điều khiển PID sử dụng điều khiển mờ [15] và mạng neural [16] đã được đề xuất và mang lại hiệu quả tốt Tuy nhiên, những nghiên cứu này đòi hỏi một mô hình chính xác hoặc một mô

Trang 5 hình hộp đen phức tạp Do đó, những ưu điểm của bộ điều khiển PID như tính đơn giản về cấu trúc và đặc tính không có mô hình không còn nữa So với điều khiển mờ và mạng neural, bộ ước lượng thời gian trễ (TDE) có thể cập nhật mô hình hệ thống trong thời gian thực với nhu cầu tính toán tối thiểu và hiệu suất tối ưu [17] TDE quan sát phản ứng của hệ thống trong quá khứ với thời gian đủ nhỏ để ước lượng bù động học cho nhiễu và các thành phần không chắc chắn [18]

1.1.2 Các nghiên cứu trong nước

Trong những năm gần đây, robot song song cũng là một chủ đề được nhiều nhà nghiên cứu trong nước quan tâm Năm 2006, tác giả Phạm Thị Hạnh đã phân tích động lực học cho robot song song 3 bậc tự do [19] Tiếp đó là mô hình robot Hexapod với thuật toán PID được tác giả Trần Thị Thanh Hải giới thiệu [20]

Hạn chế của các nghiên cứu trên là mới chỉ xây dựng mô hình mô phỏng, chưa thi công được mô hình thực tế Năm 2013, tác giả Phạm Anh Đức đã xây dựng được mô hình robot Delta nhưng chỉ chuyển động được theo quỹ đạo thẳng với sai số ±5mm [21] Đề tài “Thiết kế, chế tạo robot 4 bậc tự do mô phỏng chuyển động trên tàu thủy” của tác giả Đặng Văn Mười xây dựng một robot mô phỏng lại dao động trên tàu biển [22] Tuy nhiên, chưa giải quyết được hoàn thiện bài toán động học thuận và động học nghịch; sử dụng động cơ tuyến tính không có phanh điện nên bị trượt khỏi vị trí đặt trước Năm 2021, nhóm sinh viên Đinh Kiên Cường đã sử dụng

Raspberry lập trình điều khiển robot song song 4 bậc tự do nhận dạng và bám theo các sản phẩm dựa theo màu sắc (xử lý ảnh) [23] Điều khiển đồng bộ giữa các chuỗi trong robot song song là xu hướng chung và được quan tâm trong những năm gần đây Năm 2019, PGS Lê Tiến Dũng và cộng sự đã thiết kế được bộ điều khiển đồng bộ thích nghi cho robot song song phẳng có sai số quỹ đạo dưới 3% [24] Năm 2022, nhóm sinh viên Nguyễn Thanh Nhã đã bảo vệ thành công đề tài “Thiết kế mô hình và các bộ điều khiển đồng bộ cho robot song song 4 bậc tự do” do TS Trần Đức Thiện hướng dẫn [25], đề tài đã thiết kế được các bộ điều khiển PID, PID đồng bộ, trượt, trượt đồng bộ, mô phỏng chuyển động và áp

CHƯƠNG 1: TỔNG QUAN dụng thành công vào mô hình thực tế

Trên cơ sở các nghiên cứu trong và ngoài nước đã nêu, cho thấy rằng điều khiển đồng bộ là xu hướng tất yếu để nâng cao chất lượng điều khiển cho robot song song, đáp ứng các yêu cầu về kỹ thuật và nâng cao năng suất trong sản xuất công nghiệp hiện nay Việc nghiên cứu, áp dụng các thuật toán để tối ưu hóa các bộ điều khiển cũng phù hợp với xu thế phát triển tri thức con người Đây là cơ sở và động lực để người thực hiện lựa chọn đề tài “ Thiết kế bộ điều khiển PID đồng bộ cho robot song song 4 bậc tự do ”.

Mục tiêu của đề tài

Đề tài được nghiên cứu nhằm thực hiện các mục tiêu sau:

▪ Tìm hiểu mô hình robot song song 4 bậc tự do tại RicLAB

▪ Thiết kế bộ điều khiển PID, PID đồng bộ (SPID), và PID đồng bộ dùng ước lượng thời gian trễ (SPID – TDE) trên mô hình robot song song 4 bậc tự do.

Giới hạn của đề tài

Đề tài được thực hiện với một số giới hạn sau:

▪ Đề tài tập trung vào nghiên cứu các giải thuật điều khiển, không tập trung vào thi công robot

▪ Kết quả chạy thực nghiệm chưa được đánh giá bởi các thiết bị đo và quy trình đo lường phù hợp.

Phương pháp nghiên cứu

Trong quá trình thực hiện luận văn, tác giả đã sử dụng các phương pháp nghiên cứu sau:

▪ Phương pháp nghiên cứu tài liệu: thu thập, sàng lọc tài liệu về robot song song 4 bậc tự do, điều khiển đồng bộ, ước lượng thời gian trễ

▪ Phương pháp phi thực nghiệm: xây dựng mô hình mô phỏng trên MATLAB

Simulink, thiết kế các bộ điều khiển và tiến hành mô phỏng nhằm đánh giá chất lượng các bộ điều khiển theo lý thuyết

▪ Phương pháp thực nghiệm: áp dụng các bộ điều khiển trực tiếp trên mô hình robot song song 4 bậc tự do, thu thập kết quả và đánh giá.

Nội dung đề tài

Luận văn được hoàn thành với các nội dung sau:

Giới thiệu tổng quan về đề tài, các nghiên cứu trong và ngoài nước, mục tiêu, giới hạn và các phương pháp nghiên cứu được áp dụng khi thực hiện đề tài

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

Trình bày các cơ sở lý thuyết để thực hiện được đề tài: robot và robot song song, thuật toán PID, giải thuật điều khiển đồng bộ, bộ ước lượng thời gian trễ, các thiết bị phần cứng chính được sử dụng và giao thức truyền thông giữa chúng

❑ Chương 3: Giới thiệu robot song song 4 bậc tự do tại RicLAB

Chương này giới thiệu về robot song song 4 bậc tự do của phòng thí nghiệm

Robot và điều khiển thông minh (RicLAB), phân tích động học nghịch, động học thuận và quy hoạch không gian làm việc của robot

❑ Chương 4: Robot song song 4 bậc tự do

Dựa vào những cơ sở lý thuyết được đề cập trong chương 2, chương 4 sẽ tập trung vào thiết kế các bộ điều khiển PID, SPID và SPID-TDE, thiết kế giao diện giám sát và điều khiển

❑ Chương 5: Mô phỏng và thực nghiệm Để đánh giá được các bộ điều khiển trên, tác giả tiến hành mô phỏng trên phần mềm MATLAB Simulink và thực thi trên robot với các trường hợp không tải, mang tải hay bị mất hiệu suất động cơ

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

Chỉ ra những ưu, nhược điểm và hướng phát triển của luận văn

CƠ SỞ LÝ THUYẾT

Tổng quan về robot

Viện robot Hoa Kỳ (Robot Institute of America) định nghĩa “Robot là một tay máy hay cỗ máy đa chức năng có thể lập trình, được thiết kế để di chuyển vật liệu, chi tiết, dụng cụ, các thiết bị đặc biệt với những di chuyển được lập trình trước để thực hiện các nhiệm vụ khác nhau” Với khả năng làm việc liên tục, nhanh, chính xác, hoạt động được trong môi trường khắc nghiệt…, robot ngày càng được sử dụng rộng rãi trong sản xuất nông nghiệp, công nghiệp, xây dựng, quân sự, giáo dục, y tế, …

Hình 2.1: Ứng dụng robot delta trong phân loại và đóng gói sản phẩm

Hiệp hội robot quốc tế (IFR – International Federation of Robotics) chia robot thành 2 loại: robot công nghiệp và robot phục vụ Theo đó, robot công nghiệp là một cánh tay máy có ít nhất 3 trục, được cố định tại chỗ hoặc trên một nền tảng di động, có thể lập trình và hoạt động tự động, phục vụ cho quá trình tự động hóa công nhiệp

Robot công nghiệp có một hoặc nhiều cánh tay, khi tìm hiểu về cánh tay robot, ta cần quan tâm các khái niệm sau:

Hình 2.2: Cấu trúc một robot tay máy

▪ Khâu (Link) là các vật rắn riêng lẻ của robot Khâu chủ động là khâu nối với cơ cấu chấp hành, ngược lại là các khâu bị động

▪ Khớp (Joint) là thành phần nối 2 khâu với nhau Có 6 loại khớp: tịnh tiến, quay, trụ, cầu, vít, phẳng Phổ biến nhất là khớp quay và tịnh tiến

▪ Cơ cấu chấp hành cuối (End-Effecter) là khâu cuối cùng trên cánh tay robot, được gắn với tay gắp

▪ Số bậc tự do (Degree of Freedom – DoF) là số khả năng chuyển động của robot.

Robot song song

Tiêu chuẩn cấu trúc động học chia robot thành robot nối tiếp (Serial Robot - cấu trúc động học dạng chuỗi hở) và robot song song (Parallel Robot - các khâu hình thành nên chuỗi động học kín) [27]

CHƯƠNG 2: CƠ SỞ LÝ THUYẾT

Hình 2.3: Serial robot và Parallel robot

Robot song song bao gồm một nền tảng di chuyển được kết nối đến một đế cố định bởi tập hợp các chuỗi động học như nhau (thường gọi là các chân) Cơ cấu chấp hành cuối được cố định vào nền tảng di dộng Các nguồn phát động được đặt trên đế cố định hoặc ngay trên chân Robot hoàn toàn song song khi số chân lớn hơn hoặc bằng số bậc tự do

Hình 2.4: Cấu trúc của một robot song song

Nhìn chung, robot có cấu trúc song song có nhiều điểm mạnh hơn so với robot nối tiếp Bảng 2.1 miêu tả những ưu, nhược điểm của hai loại robot này dựa trên một số tiêu chí chính

Bảng 2.1: Ưu nhược điểm của robot song song và robot nối tiếp

STT Tính năng Robot song song Robot nối tiếp

1 Không gian làm việc Nhỏ hơn Lớn hơn

2 Độ cứng vững Cao hơn Thấp hơn

3 Độ chính xác Cao hơn Thấp hơn

4 Tỉ số tải/khối lượng Lớn hơn Thấp hơn

5 Tốc độ làm việc Nhanh hơn Chậm hơn

6 Tải trọng quán tính Nhỏ hơn Lớn hơn

7 Độ phức tạp Phức tạp Đơn giản

8 Điểm kỳ dị Nhiều hơn Ít hơn

9 Công suất tối đa Lớn hơn Nhỏ hơn

10 Tính toán động học thuận Rất khó Dễ hơn

11 Tính toán động học nghịch Dễ hơn Khó hơn

12 Sai số bám Trung bình Tích lũy

Robot song song có tốc độ xử lý nhanh hơn (khoảng 30% so với robot nối tiếp), phù hợp với các tác vụ pick-and-place nên được sử dụng nhiều trong đóng gói, phân loại sản phẩm, sản xuất điện tử…Quattro là một robot song song 4 bậc tự do, được

Adept Technologies giới thiệu vào năm 2009 có thể đạt 300 chu kỳ làm việc mỗi phút

Hình 2.5: Robot Quattro trong dây truyền sản xuất sữa chua

LinuxCNC có 6 bậc tự do, được ứng dụng trong máy cắt CNC với độ chính xác cao

Hình 2.6: Robot LinuxCNC được ứng dụng trong các máy cắt CNC

AC Servo

AC servo là loại động cơ điện chủ yếu được sử dụng trong các ứng dụng yêu cầu điều khiển chính xác với hiệu suất cao AC servo được tích hợp nhiều thiết bị điện tử và cảm biến, mang lại khả năng điều khiển và truyền thông tốt hơn

Cấu tạo AC Servo 3 pha gồm 3 phần:

▪ Stator: là các cuộn dây 3 pha được quấn quanh lõi thép từ tính (phần tĩnh)

▪ Rotor: thường là các nam châm vĩnh cửu có từ trường mạnh (phần động)

▪ Encoder: liên kết với trục động cơ, chuyển đổi chuyển động quay thành tín hiệu điện gửi về bộ điều khiển

Hình 2.8: Cấu tạo AC Servo

CHƯƠNG 2: CƠ SỞ LÝ THUYẾT Đặc điểm hoạt động:

▪ Bộ điều khiển điều khiển động cơ servo theo phương pháp điều chế độ rộng xung (PWM) Bộ điều khiển nhận xung từ Encoder và phân tích thành các thông tin vị trí, tốc độ để điều chỉnh động cơ hoạt động theo đúng yêu cầu

▪ Có 3 chế độ điều khiển: vị trí, tốc độ và moment.

Thuật toán PID

Bộ điều khiển PID (Proportional Integral Derivative, điều khiển 3 khâu) là một cơ chế phản hồi vòng kín được sử dụng nhiều nhất Bộ điều khiển sẽ tính sai số là hiệu của giá trị đặt và giá trị đo, sau đó điều chỉnh giá trị ngõ vào theo 3 khâu (P – I

– D) nhằm làm giảm sai số

Hình 2.9: Sơ đồ khối bộ điều khiển PID

Khâu tỉ lệ biến đổi giá trị ngõ ra tỉ lệ với sai số ngõ vào theo phương trình:

Trong đó: P out − ngõ ra khâu tỉ lệ

K p − hệ số tỉ lệ e t ( ) − sai số theo thời gian

Hệ số tỉ lệ K P xác định tốc độ bám theo giá trị đặt [28] Nếu hệ số K P quá cao (ngõ ra thay đổi lớn trong khi sai số thay đổi nhỏ) hệ thống sẽ không ổn định

Ngược lại, K P nhỏ sẽ làm cho bộ điều khiển kém nhạy, đáp ứng chậm Hình 2.8 mô tả ảnh hưởng của hệ số Kp tới ngõ ra của hệ thống

Khâu tích phân tỉ lệ với sai số ngõ vào và khoảng thời gian xảy ra sai số Đáp ứng khâu tích phân được xác định bởi công thức:

Trong đó: I out − ngõ ra khâu tích phân

K i − hệ số khâu tích phân t− thời gian

 − biến tích phân trung gian

Khâu tích phân (khi cộng thêm khâu tỉ lệ) sẽ tăng tốc độ bám của hệ thống và khử số dư sai số Tuy nhiên, vì khâu tích phân được tích lũy trong quá khứ nên có thể làm ngõ ra bị vọt lố

❖ Khâu vi phân (Derivative) Đáp ứng khâu vi phân tỉ lệ thuận với tốc độ biến thiên của sai số, được xác định bởi phương trình sau:

Trong đó: D out − ngõ ra khâu vi phân

K d − hệ số khâu vi phân e t ( ) − sai số theo thời gian

Khâu vi phân làm chậm tốc độ thay đổi ngõ ra, nên được sử dụng để nâng cao độ ổn định của bộ điều khiển Tuy nhiên, nó có thể làm hệ thống mất ổn định vì phép tính vi phân sẽ khuếch đại các tín hiệu nhiễu

Xác định được bộ thông số phù hợp sẽ có ảnh hướng lớn tới chất lượng bộ điều khiển PID Có nhiều phương pháp khác nhau để xác định các bộ thông số này, phụ thuộc vào đặc trưng, tốc độ đáp ứng… của hệ thống Hiện nay, nhiều phần mềm lập trình có hỗ trợ các công cụ tự động dò tìm các thông số bộ điều khiển PID Chúng tập hợp dữ liệu, xây dựng mô hình và đề xuất phương pháp điều chỉnh Kinh nghiệm của người thiết kế cũng là một yếu tố quan trọng, vì họ là người hiểu hệ thống nhất Vì vậy, để đạt được kết quả tối ưu nhất, người thiết kế sẽ thực hiện thêm phương pháp thử sai dựa trên bộ thông số thu được từ phần mềm.

Giải thuật điều khiển đồng bộ

Trong mô hình robot song song, cơ cấu chấp hành cuối được gắn trên đế di động và chịu tác động bởi các chuỗi động học khác nhau

Hình 2.10: Sơ đồ khối hệ thống điều khiển động bộ

Khi các chuỗi động học này được điều khiển bởi các bộ điều khiển độc lập thì chúng không có ràng buộc lẫn nhau Vì vậy, việc phối hợp hoạt động giữa các chuỗi

Trang 17 sẽ làm hệ thống hoạt động nhịp nhàng hơn Hình 2.14 giới thiệu một giải thuật điều khiển đồng bộ nhiều trục chuyển động

Giải pháp đồng bộ hóa các chuỗi động học đưa ra các khái niệm: sai số đồng bộ

(synchronization error) và sai số xen kênh (cross-coupling error) Sai số này ngoài chịu ảnh hưởng trực tiếp của sai số trên chính chuỗi điều khiển, nó còn phụ thuộc vào sai số trên các chuỗi lân cận Sai số đồng bộ có ảnh hưởng lớn đến sai số bám quỹ đạo của robot song song [13, 29].

Bộ ước lượng thời gian trễ - TDE

Phương trình động lực học của robot rất phức tạp, phụ thuộc vào cấu trúc, số bậc tự do của từng chuỗi Do vậy, việc điều khiển mô men xoắn của cánh tay robot là một thách thức không nhỏ Các nhóm phương án được đưa ra:

▪ Xây dựng các bộ điều khiển dựa trên phương trình động lực học của robot

(CTC, SMC, Adaptive control…) Phương án này gặp khó khăn do khó xác định được phương trình động học như đã được nêu Hơn nữa, trong mô hình động lực học của hệ thống luôn tồn tại các yếu tố chưa biết hoặc thay đổi theo thời gian, như: ma sát tại các khớp, hiệu ứng moment quán tính khi trọng lượng tải thay đổi, nhiễu tác động…

▪ Áp dụng các bộ điều khiển thông minh (Fuzzy, neural network) mà không cần xác định phương trình động lực học Tuy nhiên, các bộ điều khiển này chưa phù hợp trong các bài toán thực tế do ảnh hưởng tới hiệu suất điều khiển

▪ Năm 1995, Hsia và Jung đã đưa ra một bộ ước lượng thời gian trễ - Time

Delay Estimation (TDE) [30] TDE là một kỹ thuật nổi tiếng để bù đắp cho các yếu tố không chắc chắn của hệ thống

Giả sử rằng các phương trình động lực học của robot ít thay đổi trong một khoảng thời gian rất nhỏ, TDE quan sát thông tin trong quá khứ của ngõ vào và tín hiệu đo được ở ngõ ra để ước tính ảnh hưởng của nhiễu và các yếu tố không chắc chắn Bằng cách loại bỏ động lực học phi tuyến và đưa vào lỗi động lực học mong

CHƯƠNG 2: CƠ SỞ LÝ THUYẾT muốn đã đưa ra một luật điều khiển mới đó là Time Delay Control (TDC) TDE được chứng minh là có hiệu suất cao, tốc độ bù của TDE nhanh hơn mạng nơ-ron [31] q d

Hình 2.11: Sơ đồ khối của bộ điều khiển dùng TDE.

Phương pháp Jacobi

Jacobi là ma trận chứa các đạo hàm riêng bậc nhất dùng để xấp xỉ một hàm giữa hai không gian Xét hàm :F n → m được miêu tả bởi hệ phương trình sau:

Hệ (2.4) có thể biểu diễn dưới dạng vector:

Y X (2.5) Đạo hàm riêng từng phần (2.4) ta được:

Biểu diễn dạng vector của (2.6):

J X được gọi là ma trận Jacobi của hàm Đạo hàm 2 vế (2.8) theo thời gian, ta được:

Nếu p n và F khả vi tại p, thì ( )J X là xấp xỉ tuyến tính tốt nhất của F tại điểm p:

Nếu m = n, thì ma trận Jacobi là ma trận vuông, và định thức của nó được gọi là định thức Jacobi

Phương pháp lặp Newton – Raphson (Iterative Newton Raphson Medthod) dựa vào đặc tính vận tốc động học của mô hình phụ thuộc vào ma trận Jacobi trong công thức (2.10) Giá trị tìm được tỉ lệ với số vòng lặp của thuật toán

Trong đó: χ n − vị trí của khâu cuối hiện tại của robot χ n + −1 vị trí kế tiếp của khâu cuối trên robot

J χ q ( n , n )−ma trận Jacobi q n −góc khớp hiện tại của robot q d −vị trí các khớp mong muốn ρ−hệ số học.

Chuẩn truyền thông CAN

CAN (Controller Area Network) là một giao thức truyền thông nối tiếp, được

Bosh nghiên cứu phát triển từ năm 1983 nhằm thay thế cho các chuẩn truyền thông nối tiếp cũ và trở thành một tiêu chuẩn quốc tế (ISO 11898) vào năm 1994

Hình 2.12: Ứng dụng CAN trong xe ô tô

Ngày nay, CAN được ứng dụng nhiều trong: xe hơi, máy nông nghiệp, tàu ngầm, máy y khoa, máy dệt… do tính hiệu quả, ổn định và chi phí rẻ

Mạng CAN bao gồm nhiều node, mỗi node có thể giao tiếp với bất kỳ node nào khác trong mạng thông qua việc truyền và nhận các gói dữ liệu (message) Tuỳ theo mức độ ưu tiên, các message được gán cho một số định danh (ID) riêng Bus CAN chỉ gồm 2 dây dẫn: CAN_High và CAN_Low, kết thúc bằng điện trở 120 ohm ở mỗi đầu như hình 2.17

Hình 2.13: Cấu trúc mạng CAN

Truyền thông CAN có nhiều ưu điểm:

▪ Chi phí thấp, dễ lắp đặt và sửa chữa

▪ Prioritization of messages: khi nhiều thông điệp cùng được gửi, thì thông điệp có mức ưu tiên cao sẽ được truyền trước

▪ Một thông điệp có thể được nhận đồng thời tại nhiều node (thời gian thực)

▪ Khả năng phát hiện và báo lỗi, thông điệp lỗi sẽ được gửi khi bus rảnh

▪ Tính mở, là giao thức chung của nhiều nhà sản xuất ECU

Các thiết bị chính sử dụng trong đề tài

2.9.1 Động cơ Mitsubishi HG-KN43

Hình 2.14: Động cơ AC Servo HG-KN43 Động cơ Servo HG-KN43 thuộc nhóm động cơ quán tính thấp phục vụ cho các ứng dụng công suất tải nhỏ di chuyển với tốc độ cao HG-KN43 không hỗ trợ phanh điện từ

Bảng 2.2: Bảng thông số của động cơ HG-KN43

▪ Công suất động cơ: 400W ▪ Quán tính trục: 0,375 10 -4 kgm2

▪ Điện áp định mức: 200VAC ▪ Tốc độ định mức: 3000rmp

▪ Dòng định mức: 2,6A ▪ Tốc độ tối đa: 5000 rmp

▪ Momen định mức: 1,3Nm ▪ Không có phanh

▪ Momen tới hạn: 3,8Nm ▪ Độ phân giải encoder: 17 bit

Mitsubishi MR-JE 40A thuộc dòng Drive Servo sử dụng cho các động cơ hoạt động với điện áp 200-230VAC, 50/60Hz Sử dụng chuỗi xung hoặc tín hiệu tương tự để điều khiển Các chế độ điều khiển: vị trí, tốc độ hoặc mô-men xoắn

Hình 2.15: Driver Mitsubishi MR – JE 40A

▪ Công suất 400W; dòng điện tối đa 40A

▪ Điện áp: 1 pha 200VAC / 3 pha 200VAC

▪ Phương pháp điểu khiển PWM / Analog; Encoder 17 bit

Là bộ kit phát triển sử dụng vi xử lý STM32 F4 Cortex -M4, tích hợp sẵn mạch nạp ST-LINK/V2 Board được trang bị một số cảm biến: cảm biến góc, micro, bộ chuyển đổi âm thanh lớp D, 4 LED đơn và các nút nhấn

Hình 2.16: Kit lập trình STM32F407G

ROBOT SONG SONG 4 BẬC TỰ DO

Tổng quan mô hình hệ thống

Robot bao gồm 2 phần: phần thân robot và tủ điều khiển

Hình 3.1: Thân robot song song 4 bậc tự do

Thân robot gồm 5 phần chính:

2 Đế cố định, được gắn vào khung robot bằng các khớp nối

3 Khâu chủ động (khâu 1) của robot, được liên kết với các trục động cơ AC servo thông qua các mặt bích

4 Khâu thụ động (khâu 2) có nhiệm vụ dẫn lực từ khâu chủ động tới đế di động thông qua các khớp cầu

5 Đế di động của robot

Phần tủ điều khiển bao gồm board mạch điều khiển và các bộ driver động cơ

AC servo Mạch điều khiển gồm 2 board STM32F407G, có nhiệm vụ giao tiếp với máy tính và điều khiển các động cơ thông qua các bộ driver

Hình 3.2: Tủ điều khiển robot.

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

Hình 3.3: Sơ đồ khối toàn mạch

Hệ thống có sơ đồ khối như hình 3.3, gồm các khối:

▪ Khối nguồn: cung cấp nguồn 5VDC cho các board STM32, …

▪ Khối Personal Computer giúp giám sát và điều khiển robot qua giao diện

▪ Khối Controller: gồm 2 STM32F407VG và một board controller

STM32F407 – Master có nhiệm vụ trao đổi dữ liệu với Personal Computer

CHƯƠNG 3: ROBOT SONG SONG 4 BẬC TỰ DO và đọc dữ liệu từ các encoder sau đó tính toán điều khiển các động cơ

AC Servo Bởi vì mỗi board STM32 chỉ có 2 ngõ ra analog, nên

STM32-Master chỉ điều khiển trực tiếp động cơ 1 và 2, đồng thời nó gửi kết quả điều khiển động cơ 3 và 4 qua STM32-Slave thực hiện thông qua giao thức CAN Việc trao đổi dữ liệu giữa 2 board STM32F407VG, các bộ drive và tín hiệu gửi về từ encoder được quản lý bởi Board Controller

Sơ đồ đấu nối toàn mạch:

Hình 3.4: Sơ đồ đấu nối toàn mạch.

Board Controller

Như đã trình bày ở phần trước, Board Controller có nhiệm vụ trung gian trong việc giao tiếp, xử lý tín hiệu in/out giữa STM32F407_Master –

STM32F407VG_Slave và các Driver AC Servo Sơ đồ nguyên lý của mạch được trình bày trong hình 3.5, bao gồm các khối: mạch cách ly, khối giao tiếp CAN BUS, khối dịch mức điện áp và mạch khuếch đại điện áp, các mạch đệm để thực hiện giao tiếp giữa VĐK và các driver AC Servo

Hình 3.5: Sơ đồ nguyên lý Board Controller

CHƯƠNG 3: ROBOT SONG SONG 4 BẬC TỰ DO

Bảng 3.1: Bảng các ngõ vào/ra Board Controller

SON Servo-on Bật SON để cấp nguồn cho mạch, và làm cho bộ servo amplifier sẵn sàng hoạt động

Tắt SON để tắt nguồn cấp vào mạch và làm cho động cơ chuyển động mà không sử dụng nguồn

Làm ngừng cung cấp nguồn ngay lập tức khi chân EMG được tắt

Khi tắt EMG, thì dynamic Brake được vận hành và dừng đột ngột servo motor Tại thời điểm này thì màn hình sẽ hiển thị cảnh báo AL E6 (Servo emergency stop warning)

Khi vận hành bình thường, không nên dùng EMG để thay thế STOP và RUN vì làm giảm tuổi thọ của Driver

Nếu mà khởi động quay thuận và khởi động quay nghịch hoặc cấp xung cho động cơ trong suốt thời gian dừng khẩn cấp, thì động cơ sẽ quay lập tức ngay khi cảnh báo được reset Trong suốt thời gian dừng khẩn cấp, luôn tắt RUN COMMAND

RES RESET Bật RES hơn 50ms để reset cảnh báo

Một vài cảnh báo không thể tắt bằng cách reset (RES)

Bật RES trong “alarm free status” để ngắt base circuit

Base Circuit không được ngắt thì phải set parameter No

Sử dụng để chọn tốc độ cho vận hành

Dùng để chọn hướng quay thuận của torque servo motor

Dùng để chọn hướng quay nghịch của torque servo motor

P15R 15VDC power supply Đầu ra 15VDC đến P15R.LG Có sẵn nguồn để cung cấp cho TC, TLA, VC, VLA Dòng cho phép là 30mA

Sử dụng để điều khiển torque trong phạm vi torque full của motor Áp từ 0 đến 8 VDC điện áp từ TC- LG Torque lớn nhất tại 8 VDC

Torque tại 8 VDC INPUT có thể thay đổi tại Parameter No.PC13

Chân chung cho TLA, TC, VLA

Chân được kết nối bên trong PA0 Pin A0 Kênh CH_A đọc Encoder 1

PA1 Pin A1 Kênh CH_B đọc Encoder 1

PD12 Pin D12 Kênh CH_A đọc Encoder 2

PD13 Pin D13 Kênh CH_B đọc Encoder 2

PC6 Pin C6 Kênh CH_A đọc Encoder 3

PC7 Pin C7 Kênh CH_B đọc Encoder 3

PE9 Pin E9 Kênh CH_A đọc Encoder 4

PE11 Pin E11 Kênh CH_B đọc Encoder 4

PB8 Pin B8 Kênh giao tiếp CAN LOW

PB9 Pin B9 Kênh giao tiếp CAN HIGH

Bài toán động học robot

Robot có cấu trúc như sau:

Hình 3.7: Cấu trúc robot song song 4 bậc tự do

▪ Đế di động tâm P được nối với đế di chuyển tâm O bởi 4 chuỗi động học như nhau Mỗi chuỗi động học có 1 khâu chủ động liên kết với 1 khâu bị động thông qua khớp cầu

▪ Khâu chủ động B A i i có chiều dài L 1 được dẫn động bởi 4 động cơ gắn trên đế cố định tâm O

▪ Khâu bị động bao gồm hai thanh hình trụ với bốn khớp cầu tạo thành một cấu trúc hình bình hành Phía dưới 4 khâu bị động i i

A D được kết nối tới đế di chuyển tâm P

▪ Đế di chuyển tâm P được thiết kế có bậc tự do bên trong

Hình 3.8: Đế di chuyển tâm P

3.4.1 Bài toán động học nghịch Động học nghịch cho robot là tìm các góc khớp q i i ( =1, 2,3, 4) từ vị trí

OP của cơ cấu chấp hành cuối và góc θ của đế di động Hệ tọa độ

Oxyzđược đặt tại tâm của đế cố định

Hình 3.9: Hệ tọa độ của robot được đặt tại tâm đế cố định

So với hệ tọa độ Oxyz, các chuỗi động học lần lượt được đặt lệch một góc tương ứng là:  1 E o ;  2 5 o ;  3 "5 o ;  4 15 o

Hình 3.10: Cấu trúc hình học của một chuỗi động học

CHƯƠNG 3: ROBOT SONG SONG 4 BẬC TỰ DO Điểm B i thuộc đường tròn ( O r , ), tọa độ điểm B i với cánh tay thứ i là:

Hình 3.10 mô tả cấu trúc hình học của một chuỗi động học robot Với biến góc khớp q i và độ dài cánh tay L i Toạ độ điểm Ai được xác định bởi vector:

( r L 1cosq i )cos i ( r L 1cosq i )sin i L 1sinq i T

1 1 cos cos cos sin sin

Hình 3.11: Hệ tọa độ của đế di động và các kích thước đặc trưng

C i i = là điểm nối tất cả các phần tử của đế di động Điểm D i 1 và D i 2 là tâm của khớp cầu được kết nối với đế di chuyển Góc  là góc quay của đế và một số hằng số hình học ( d d h h , , , i i ) Với: d là chiều dài phần tử A (dọc theo trục P x ); h là chiều dài của cặp thanh nối phần A và B của đế di động (được đo giữa các điểm

C 1so với C 4 và C 2 so với C 3 ); d i và h i là khoảng cách giữa các điểm C i và D i (dọc theo trục P x và P y tương ứng)

Từ các thông số hình học của đế di chuyển, các véc-tơ PD i được xác định như sau:

Việc xác định động học nghịch cho robot được thực hiện theo các bước:

❖ Bước 1 : Dựa vào cấu trúc hình học của robot như hình 3.10, ta có ràng buộc sau:

Trong đó: OP =  X Y Z  T là vector giữa 2 hệ tọa độ gắn trên cơ cấu chấp hành cuối và đế cố định

Từ phương trình (3.8), ta suy ra:

Phương trình (3.9) xác định tọa độ vector A D i i của cánh tay thứ i, cụ thể:

2 sin cos cos sin z sin

Giả sử A D i i = K ix K iy K iz  T , ta có:

❖ Bước 2 : Thay (3.10), (3.11), (3.12), (3.13) vào (3.14) ta được: sin cos 0 i i i i i

Trong đó: ,E F i i và G i lần lượt là:

▪ Đối với cánh tay thứ nhất:

2 sin 2 cos sin 2 sin cos sin cos

▪ Đối với cánh tay thứ 2:

2 sin 2 cos sin 2 sin cos sin cos

Từ (3.16) và (3.21), ta được phương trình bậc hai biến t i như sau:

( G i −F t i ) i 2 +2 E t i i +( F i +G i )=0 (3.22) Phương trình (3.22) có 2 nghiệm là:

Từ phương trình (3.21) và (3.23), ta tính được các biến góc khớp q i Đồng thời, dựa vào không làm việc của robot, ta được nghiệm:

3.4.2 Bài toán động học thuận

Do robot song song 4 bậc tự do có cấu trúc đặc biệt có bậc tự do ở đế nên việc tính toán động học thuận của robot song song 4 bậc tự do rất khó khăn Khi tính toán bằng phương pháp đại số thì nó dẫn đến một phương trình bậc 8 nên không thể tính toán được Để giải quyết bài toán trên, người thực hiện tính toán động học thuận bằng phương pháp Jacobi

Ma trận Jacobi thể hiện quan hệ giữa vận tốc khâu cuối và vận tốc của các khớp: χ Jq (3.25)

Trong đó: χ= X Y Z  T là vận tốc của khâu cuối

J−ma trận Jacobi của hệ thống

 1 2 3 4  q q q q T q= là vận tốc góc các khớp robot

Từ (3.10), (3.11), (3.12), (3.13) và (3.15) ta suy ra được hệ phương trình sau:

X d d r hs L cq c Y h rs hc L cq s

X d d rc hs L cq c Y h rs hc L cq s

X K hs L cq c Y M hc L cq s Z L sq L

CHƯƠNG 3: ROBOT SONG SONG 4 BẬC TỰ DO Đạo hàm (3.28) ta được:

Y M hc L cq s Y hs q L s sq Z L sq Z q L cq

Y M hc L cq s Y hs q L sq s Z L sq Z q L cq

A X K hs L cq c A Y M hc L cq s A Z L sq

A = X +K −L cq c A = +Y M −L cq s A = +Z L sq A Đặt B = diag (  B 11 , B 22 , B 33 , B 44  ) , với:

B = X +K −hs L c sq + Y+M +hc L s sq +ZL cq +L sq cq

B = X +K −hs L sq c + Y+M +hc L sq s +ZL cq +L cq sq

B = X +K L sq c + Y +M L sq s +ZL cq +L sq cq

Khi đó, hệ phương trình (3.29) được biểu diễn:

Từ (3.25) và (3.30) ta tìm được ma trận Jacobi:

Từ đây, sử dụng công thức (2.18) ta cập nhật được vị trí tiếp theo của khâu cuối cùng:

Trong đó: χ n − vị trí của khâu cuối hiện tại của robot χ n+ 1 − vị trí kế tiếp của khâu cuối trên robot

J χ q ( n , n )−ma trận Jacobi q n −góc khớp hiện tại của robot q d −góc khớp mong muốn ρ−hệ số học Jacobi.

Không gian làm việc của robot

Không gian làm việc của robot là vùng không gian mà cơ cấu tác động cuối có thể thao tác được Thể tích và hình dáng của không gian làm việc phụ thuộc vào kết cấu các chuỗi và tập xác định các biến góc khớp Quy hoạch được không gian làm việc sẽ giúp xác định được khả năng làm việc của robot Để xác định được không gian làm việc của robot, ta tiến hành mô phỏng bài toán động học thuận với chiều dài khâu chủ động L1 = 17.5cm, khâu bị động L2 50cm, miền xác định của các góc khớp q i  − 40 ;40 o o  Hình 3.12 thể hiện hình dạng và kích thước không gian làm việc của robot nhìn từ các hệ toạ độ khác nhau

CHƯƠNG 3: ROBOT SONG SONG 4 BẬC TỰ DO a) b) c) d)

Hình 3.12: Không gian làm việc của robot: a) Không gian 3 chiều Oxyz; b) Không gian 2 chiều Oxy; c) Không gian 2 chiều Oxz; d) Không gian 2 chiều Oyz.

THIẾT KẾ BỘ ĐIỀU KHIỂN

Lưu đồ giải thuật

Để điều khiển được 4 động cơ AC servo cung cấp động lực cho 4 cánh tay robot, người thực hiện cần sử dụng 2 board điều khiển STM32F407VG, do mỗi bộ điều khiển chỉ có 2 ngõ ra analog Board điều khiển chính (Master) và board điều khiển phụ (Slave) được giao tiếp thông qua giao thức CAN

Board điều khiển Master thực hiện nhiệm vụ đọc và chuyển đổi xung encoder, tính toán điện áp để điều khiển 4 động cơ, xuất tín hiệu điều khiển động cơ khớp 1 và

2 Đồng thời gửi thông số điều khiển khớp 3 và 4 qua board điều khiển Slave

- Đọc và chuyển đổi encoder

- Truyền, nhận tín hiệu UART

- Tính toán tín hiệu điều khiển moment

- Xuất tín hiệu điều khiển khớp 1 và 2

- Gửi tín hiệu điều khiển khớp 3 và 4 cho Slave Đ

- Lựa chọn bộ điều khiển và quỹ đạo

Hình 4.1: Lưu đồ giải thuật board điều khiển board Master

Lưu đồ giải thuật của board Master được thể hiện như hình 4.1 Khi mới cấp nguồn, Master thiết lập lại các thông số ban đầu của hệ thống, sau đó đọc và xử lý tín hiệu từ encoder gắn trên các động cơ thông qua Board controller Master kiểm tra xem có tín hiệu được gửi từ máy tính mỗi 5ms, nếu có thì tiến hành chuyển đổi tín hiệu, lựa chọn bộ điều khiển, đặt lại thời gian, quy hoạch quỹ đạo (xác định vị trí và tính toán giá trị góc theo thời gian thực thông qua phương trình động học nghịch)

CHƯƠNG 4: THIẾT KẾ BỘ ĐIỀU KHIỂN

Tiếp theo, bộ điều khiển sẽ tính toán và gửi thông số điều khiển cho AC servo 1, 2 và

Slave (điều khiển AC servo 3,4) thông qua các bộ driver Cuối cùng, Master gửi dữ liệu lên máy tính và kiểm tra có tín hiệu kết thúc điều khiển hay không

Nhận tín hiệu từ Master mỗi 5ms (CAN)

- Xuất tín hiệu điều khiển khớp 3 và 4

Thiết lập các thông số ban đầu

Hình 4.2: Lưu đồ giải thuật board Slave

Chương trình điều khiển board Slave được thể hiện trong hình 4.2 Khi khởi động, Slave sẽ thiết lập các thông số mặc định, nhận tín hiệu từ Master mỗi 5ms

Chuyển đổi tín hiệu nhận được sang giá trị điện áp điều khiển AC servo 3 và 4 thông qua các driver.

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

4.2.1 Phương trình tổng quát động lực học robot

Phương trình tổng quát động lực học của cánh tay robot n bậc tự do [32]:

= d τ M q q + C q,q q + G q + F q,q + τ (4.1) Trong đó: τ−vector mô men xoắn từ cơ cấu chấp hành

 − n q,q,q lần lượt là vector góc, vector vận tốc góc và vector gia tốc góc của các khớp

C q vector Coriolis (li tâm)/hướng tâm

F q,q vector mô men ma sát

Luôn tồn tại ma trận đường chéo xác định dương:

Khi đó, phương trình (4.1) được viết lại:

Phương trình (4.2) được đơn giản:

Dễ thấy vector N q,q,q ( ) bao gồm nhiễu và các thành phần không chắc chắn, như: momen ma sát, Coriolis, trọng trường

4.2.2 Bộ ước lượng thời gian trễ

Giả sử N q,q,q ( ) liên tục trên từng đoạn với khoảng thời gian nhỏ L, và giá trị

N q,q,q tại thời gian t xấp xỉ giá trị N q,q,q ( ) tại thời điểm t−L:

Với Lthường được chọn là khoảng thời gian lấy mẫu của hệ thống

Từ (4.4) ta tính được giá trị của N q,q,q ( ) ( t L − ) :

N q,q,q N q,q,q τ Mq (4.7) Ở đây, N q,q,q ˆ ( ) ( ) t chính là ước lượng thời gian trễ (TDE) của N q,q,q ( ) tại thời điểm t Như vậy, để tính được N q,q,q ˆ ( ) ( ) t ta chỉ cần biết 2 thông số đó là τ t L − và q ( t L − ) thay vì phải tính động lực học phức tạp của robot như (4.1) Thời gian lấy mẫu Lcàng nhỏ thì ước lượng TDE (4.7) càng chính xác

4.2.3 Thiết kế bộ điều khiển PID

PID là bộ điều khiển kinh điển, được sử dụng nhiều nhất trong các bộ điều khiển phản hồi Mô hình điều khiển PID hình 4.1 mô tả việc áp dụng các bộ điều khiển PID một cách độc lập cho mỗi chuỗi động lực

Hình 4.3: Mô hình điều khiển PID cho robot song song 4 bậc tự do

Phương trình biểu điễn cho bộ điều khiển PID là:

Trong đó: u( )t  − 4 vector tín hiệu điều khiển trên mỗi chuỗi động học

K K K các ma trận hệ số, là ma trận chéo xác định dương

Phương pháp thử - sai được áp dụng để dò tìm các thông số K p ,K K i , d của hệ thống

4.2.4 Thiết kế bộ điều khiển SPID

Bộ điều khiển PID được thiết kế nhằm mục đích bám quỹ đạo Nhưng robot song song là một cấu trúc chuỗi kín, điều khiển theo nguyên tắc 4 chuỗi động học cùng tác động lên một đế di động di chuyển theo các tác vụ xác định Tuy nhiên, bộ điều khiển này chỉ xét riêng sai số của từng chuỗi động học mà không xét đến sự phối hợp hoạt động của các trục hoặc các chuỗi động học song song khác Điều này không phù hợp với bản chất động học của robot song song, là có sự phối hợp giữa các chuỗi động học cùng tác động lên cơ cấu chấp hành cuối Vì vậy, việc thiết kế và áp dụng các giải thuật điều khiển đồng bộ là cần thiết

Trong phần này, người thực hiện thiết kế bộ điều khiển PID đồng bộ (SPID –

Synchronized PID) nhằm mục đích cải thiện chất lượng của hệ thống, đưa sai số bám quỹ đạo tiến về 0 trong thời gian hữu hạn Hình 4.2 mô tả cấu trúc của bộ điều khiển

PID đồng bộ cho rotbot song song 4 bậc tự do

Hình 4.4: Mô hình bộ điều khiển SPID

Sai số bám (tracking error) được định nghĩa: e = q - q d (4.9)

Trong đó: e − 4 vector sai số bám của các khớp q − 4 vector tín hiệu đáp ứng của các khớp q d  − 4 vector tín hiệu đặt của các khớp Để cải thiện độ chính xác của hệ thống, yếu tố đồng bộ về sai số

(synchronization error) được xem xét Sai số đồng bộ được xác định là một tập hợp con của các cặp có thể có của hai khớp lân cận, được định nghĩa theo công thức sau:

Hệ phương trình (4.10) có thể được biểu diễn lại dưới dạng ma trận:

    e S e (4.11) Để đưa cả hai sai số bám e và sai số đồng bộ e s hội tụ về 0, một sai số xen kênh (cross-coupling error) được định nghĩa là kết hợp hai sai số này theo hệ phương trình sau:

Hệ phương trình (4.12) được viết lại:

Theo [13], nếu lim c 0 t→ e = thì e→0 và e s →0

CHƯƠNG 4: THIẾT KẾ BỘ ĐIỀU KHIỂN Đặt

. t c = +  dt e e β T e (4.15) Áp dụng (4.15) vào (4.8), ta có phương trình biểu diễn cho bộ điều khiển SPID là:

4.2.5 Thiết kế bộ điều khiển SPID-TDE

Bộ điều khiển SPID-TDE được thiết kế như hình 4.3:

Hình 4.5: Sơ đồ bộ điều khiển SPID-TDE

Theo đó, tín hiệu điều khiển cho robot là:

Theo [33, 34], đồng thời áp dụng sai số xen kênh cho hệ thống, ta được:

Trong đó: q d t ( ) −là vector gia tốc góc mong muốn

K K i (=diag K ( i 11,K i 22, ,K inn ) ) và K d (=diag K ( d 11,K d 22, ,K dnn ) )là các ma trận hệ số PID

Thế (4.7) và (4.18) vào (4.17) ta được:

   τ = M q + K e + e K e + τ Mq (4.19) Đạo hàm 2 vế (4.15) ta được: c = + e e β T e (4.20)

Công thức (4.21) là luật điều khiển PID đồng bộ dùng ước lượng thời gian trễ cho robot song song 4 bậc tự do Thành phần ( τ t L − − Mq ( t L − ) ) được gọi là bộ ước lượng thời gian trễ Có thể thấy rằng, luật điều khiển này đơn giản, không yêu cầu động lực học của robot Thay vào đó, nó cần xác định trạng thái ngõ vào điều khiển t L − τ và gia tốc góc q ( t L − ) trong quá khứ để bù động lực cho robot

Chương trình MATLAB Simulink

Chương trình điều khiển được viết bằng MATLAB Simulink dựa trên thư viện

Waijung Blockset 1 cho các board điều khiển STM32F407VG

Hình 4.6: Chương trình điều khiển board Master

Slave giao tiếp với Master qua giao thức CAN và nhận nhiệm vụ điều khiển

Hình 4.7: Chương trình điều khiển board Slave

Giao diện GUI

Giao diện GUI (Graphical User Interface) giúp con người giao tiếp với hệ thống thông qua chữ viết hoặc hình ảnh thay vì các câu lệnh phức tạp Trong đề tài này, người thực hiện sử dụng phần mềm Visual Studio để thiết kế giao diện giám sát và điều khiển hệ thống robot

Hình 4.8: Giao diện giám sát và điều khiển

MÔ PHỎNG VÀ THỰC NGHIỆM

Kiểm chứng động học robot song song 4 bậc tự do

5.1.1 Tạo môi trường mô phỏng trên MATLAB Simulink

Các chi tiết của robot song song 4 bậc tự do được thiết kế trên phần mềm

Solidworks, và được liên kết với nhau bởi các khớp Thư viện Simscape Multibody giúp chuyển đổi file CAD sang file XML để sử dụng trên MATLAB Simulink

Hình 5.1: Mô hình Simscape robot song song 4 bậc tự do

Từ mô hình chuyển đổi như hình 5.2, ta có thể tiến hành mô phỏng, kiểm chứng các thuật toán điều khiển trên MATLAB Simulink

Hình 5.2: Mô hình chuyển đổi trên MATLAB Simulink

5.1.2 Kiểm chứng động học nghịch Để kiểm chứng tính đúng đắn của bài toán động học nghịch đã trình bày trong mục 3.4.1 Người thực hiện tiến hành mô phỏng trên MATLAB Simulink với cấu trúc như hình 5.3

Hình 5.3: Chương trình mô phỏng kiểm chứng động học nghịch

Với mỗi giá trị góc khớp được đưa vào, khối mô hình Simscape The 4-Dof

Parallel Robot Model sẽ cho kết quả là vị trí của robot trong không gian Kết quả này lại được đưa vào ngõ vào khối Inverse Kinematics đã được lập trình trong chương trình động học nghịch Kết quả ngõ ra (q i i =1, 2, 3, 4) sẽ được so sánh với góc khớp đã đặt

❖ Trường hợp 1 : mô phỏng với góc đặt q_in =  0 0 0 0  T

Hình 5.4: Chương trình mô phỏng và đáp ứng trên mô hình Simscape TH1

CHƯƠNG 5: MÔ PHỎNG VÀ THỰC NGHIỆM

❖ Trường hợp 2 : mô phỏng với góc đặt q_in =  40 40 20 20  T

Hình 5.5: Chương trình mô phỏng và đáp ứng trên mô hình Simscape TH2

Từ kết quả mô phỏng của 2 trường hợp, ta thấy kết quả ngõ ra gần giống với giá trị đặt, cho thấy các tính toán động học nghịch là chính xác

5.1.3 Kiểm chứng động học thuận Để kiểm chứng bài toán động học thuận, người thực hiện xây dựng mô hình như hình 5.6 Theo đó, toạ độ đặt được đưa vào khối Inverse_Kinematics xử lý và đưa đưa ra góc các khớp Các giá trị góc này được đưa vào khối Forward Kinematics để đưa ra toạ độ tính toán của robot So sánh kết quả mô phỏng đạt được với kết quả đặt để đưa kết luận Hình 5.6 cho kết quả mô phỏng tương ứng với vị trí đặt x = 0, y 0, z = -400 và θ = 40 o

Hình 5.6: Chương trình mô phỏng kiểm chứng động học thuận TH1

▪ q d −ma trận góc đặt các khớp

▪ X Y Z the, , , −vị trí đặt của robot

▪ j he so, _ − số vòng lặp và hệ số học của phương pháp Jacobian

▪ X _ ,f Y_ ,f Z_ ,f the_ f − vị trí ngõ ra tính toán được

▪ n− số vòng lặp được thực hiện

Thực hiện kiểm chứng động học thuận với trường hợp: x = 0, y = 20, z = -400 và θ = 20 o

Hình 5.7: Chương trình mô phỏng kiểm chứng động học thuận TH2

Từ kết quả mô phỏng trong 2 trường hợp, nhận thấy giá trị ngõ ra mô phỏng bám sát với giá trị đặt với sai số rất nhỏ Bài toán động học thuận đã được chứng minh.

Kết quả mô phỏng các bộ điều khiển

Đề tài tập trung vào xây dựng một bộ điều khiển đồng bộ (SPID), đồng thời cải tiến bộ điều khiển dùng bộ ước lượng thời gian trễ (TDE) Để đánh giá được chất lượng các bộ điều khiển, người thực hiện xây dựng mô hình mô phỏng trên MATLAB

Simulink, với nhiều yếu tố tác động như thay đổi tải, mất hiệu suất động cơ

Hình 5.8 thể hiện chương trình mô phỏng trên MATLAB Simulink cho robot thực hiện quỹ đạo hình tròn Thiết lập thời gian lấy mẫu fix time = 0,0001s, phương pháp xấp xỉ Runge-Kutta (ODE4)

Hình 5.8: Chương trình mô phỏng các bộ điều khiển

Các thông số bộ điều khiển PID, SPID, SPID-TDE được cho như bảng 5.1 sau khi thực hiện phương pháp thử sai

Bảng 5.1: Thông số mô phỏng các bộ điều khiển

Bộ điều khiển Thông số điều khiển

Tiến hành mô phỏng các bộ điều khiển theo các trường hợp:

▪ Trường hợp 1: robot hoạt động bình thường, không mang tải

▪ Trường hợp 2: robot hoạt động với tải trọng 2kg

▪ Trường hợp 3: robot hoạt động theo tình huống sau: giây thứ 10 động cơ

1 giảm hiệu suất 10%; giây thứ 15 động cơ 3 giảm hiệu suất 20%; giây thứ 20 động cơ 2 giảm hiệu suất 30% và giây thứ 30 động cơ 4 giảm hiệu suất 35%

Quỹ đạo mô phỏng là đường tròn tâm (0, 0,−500), bán kính R = 100mm và tần số f = 0,2Hz có phương trình toán học như sau:

Mô phỏng robot di chuyển theo quỹ đạo (5.1), tín hiệu đặt và đáp ứng của từng bộ điều khiển: a) b)

CHƯƠNG 5: MÔ PHỎNG VÀ THỰC NGHIỆM c) d)

Hình 5.9: Tín hiệu đặt và đáp ứng từng khớp của các bộ điều khiển trong TH1 a) Khớp 1 b) Khớp 2 c) Khớp 3 d) Khớp 4

Hình 5.10: Sai số bám từng khớp của các bộ điều khiển trong TH1 a) Khớp 1 b) Khớp 2 c) Khớp 3 d) Khớp 4

Hình 5.11: Sai số đồng bộ giữa khớp i và (i+1) trong TH1 a) Khớp 1 và 2 b) Khớp 2 và 3 c) Khớp 3 và 4 d) Khớp 4 và 1

▪ Hình 5.9 cho thấy cả 3 bộ điều khiển PID, SPID và SPID-TDE đều cho đáp ứng tốt, bám sát giá trị đặt

▪ Hình 5.10 và 5.11 cho thấy sai số bám và sai số đồng bộ của 3 bộ điều khiển đều nhỏ (nhỏ hơn 0,005rad) Tuy nhiên, bộ điều khiển đồng bộ

SPID cho các sai số tốt hơn bộ điều khiển PID Bộ điều khiển SPID-TDE cho kết quả tốt nhất khi các sai số bám, sai số đồng bộ gần như bằng 0

Mô phỏng robot theo quỹ đạo tròn (5.1) khi mang tải trọng 2kg

Hình 5.12: Sai số bám từng khớp của các bộ điều khiển trong TH2 a) Khớp 1 b) Khớp 2 c) Khớp 3 d) Khớp 4

❖ Nhận xét : Từ hình 5.10 tới hình 5.13 cho thấy:

▪ Khi mang tải sai số bám và sai số đồng bộ giữa các khớp trong trong bộ điều khiển PID và SPID tăng lên

▪ Chất lượng bộ điều khiển SPID tốt hơn PID Bộ điều khiển SPID-TDE vẫn đảm bảo chất lượng khi các sai số bám, sai số đồng bộ gần như bằng 0

Mô phỏng robot di chuyển theo quỹ đạo (5.1) theo tình huống sau: giây thứ 10 động cơ 1 giảm hiệu suất 10%; giây thứ 15 động cơ 3 giảm hiệu suất 20%; giây thứ

20 động cơ 2 giảm hiệu suất 30% và giây thứ 30 động cơ 4 giảm hiệu suất 35%

Hình 5.14: Sai số bám từng khớp của các bộ điều khiển trong TH3 a) Khớp 1 b) Khớp 2 c) Khớp 3 d) Khớp 4 Sai số đồng bộ: a) b) c) d)

▪ Mặc dù liên tiếp chịu tác động của các hiệu ứng xấu, nhưng tính bền vững của các bộ điều khiển vẫn được thể hiện

▪ Thuật toán đồng bộ đã giúp bộ điều khiển SPID có chất lượng điều khiển tốt hơn bộ điều khiển PID

▪ Với khả năng bù các thành phần động lực học cho hệ thống, bộ điều khiển SPID-TDE cho chất lượng điều khiển vượt trội hơn hẳn, khi mà các sai số bám, sai số đồng bộ gần như bằng 0.

Kết quả thực nghiệm

Quỹ đạo cho robot di chuyển là đường tròn có phương trình (5.2) với thời gian thực hiện quỹ đạo là 40s và thời gian lấy mẫu là 0.005s

Các giai đoạn thực hiện như sau:

▪ Từ 0s đến 5s: robot di chuyển theo đường thẳng (có quỹ đạo là đa thức bậc 5) từ vị trí ban đầu có tọa độ (0, 40, 440.3) − − đến vị trí có tọa độ

▪ Từ 5s đến 35s: robot di chuyển theo quỹ đạo đường tròn có bán kính

R0mm, tâm I(0;0;-500) với tần số f = 0.1Hz trong 3 chu kỳ

▪ Từ 35s đến 40s: robot di chuyển về vị trí ban đầu theo quỹ đạo đường thẳng Để so sánh, đánh giá chất lượng các bộ điều khiển, người thực hiện cho robot thực hiện quỹ đạo trên với 3 trường hợp như sau:

▪ Trường hợp 1: robot hoạt động bình thường, không mang tải

▪ Trường hợp 2: robot hoạt động mang tải trọng 2kg

▪ Trường hợp 3: robot hoạt động theo tình huống sau: giây thứ 10 động cơ

1 giảm hiệu suất 15%; giây thứ 15 động cơ 3 giảm hiệu suất 20%; giây thứ 20 động cơ 2 giảm hiệu suất 25% và giây thứ 30 động cơ 4 giảm hiệu suất 30%

Bảng 5.2 thể hiện thông số các bộ điều khiển được áp dụng

Bảng 5.2: Bảng thông số các bộ điều khiển thực nghiệm

Bộ điều khiển Giá trị các thông số

5.3.1 Kết quả thực nghiệm trường hợp 1

Robot chạy bình thường, không mang tải với chu kỳ T = 10s Đáp ứng của các khớp:

Hình 5.16: Tín hiệu đặt và đáp ứng của các khớp trong TH1 a) Khớp 1 b) Khớp 2 c) Khớp 3 d) Khớp 4 Sai số bám quỹ đạo các khớp: a) b)

Hình 5.17: Sai số bám quỹ đạo các khớp trong TH1 a) Khớp 1 b) Khớp 2 c) Khớp 3 d) Khớp 4 Sai số đồng bộ giữa các khớp: a) b) c) d)

Hình 5.18: Sai số đồng bộ giữa khớp i và khớp (i+1) trong TH1 a) Khớp 1 và 2 b) Khớp 2 và 3 c) Khớp 3 và 4 d) Khớp 4 và 1

Sai số xen kênh của 2 bộ điều khiển SPID và SPID-TDE a) b) c) d)

Hình 5.19: Sai số xen kênh của các khớp trong TH1 a) Khớp 1 b) Khớp 2 c) Khớp 3 d) Khớp 4 Tín hiệu điều khiển các khớp

CHƯƠNG 5: MÔ PHỎNG VÀ THỰC NGHIỆM a) b) c) d)

Hình 5.20: Tín hiệu điều khiển các khớp a) Khớp 1 b) Khớp 2 c) Khớp 3 d) Khớp 4

Robot mang theo tải trọng 2kg Sai số bám quỹ đạo các khớp:

Hình 5.21: Sai số bám quỹ đạo các khớp trong TH2 a) Khớp 1 b) Khớp 2 c) Khớp 3 d) Khớp 4 Sai số đồng bộ của các bộ điều khiển: a) b)

Sai số xen kênh của bộ điều khiển SPID và SPID-TDE: a) b) c) d)

Hình 5.23: Sai số xen kênh của các khớp trong TH2 a) Khớp 1 b) Khớp 2 c) Khớp 3 d) Khớp 4 Tín hiệu điều khiển các khớp: a) b) c) d)

Hình 5.24: Tín hiệu điều khiển các khớp trong TH2 a) Khớp 1 b) Khớp 2 c) Khớp 3 d) Khớp 4

Robot không mang tải, bị lỗi truyền động theo tình huống sau: giây thứ 10 động cơ 1 giảm hiệu suất 15%; giây thứ 15 động cơ 3 giảm hiệu suất 20%; giây thứ 20 động cơ 2 giảm hiệu suất 25% và giây thứ 30 động cơ 4 giảm hiệu suất 30%

CHƯƠNG 5: MÔ PHỎNG VÀ THỰC NGHIỆM a) b) c) d)

Hình 5.25: Sai số bám quỹ đạo các khớp trong TH3 a) Khớp 1 b) Khớp 2 c) Khớp 3 d) Khớp 4 Sai số đồng bộ giữa các khớp: a) b)

Hình 5.27: Sai số xen kênh của các khớp trong TH3 a) Khớp 1 b) Khớp 2 c) Khớp 3 d) Khớp 4 Tín hiệu điều khiển các khớp: a) b) c) d)

Hình 5.28: Tín hiệu điều khiển các khớp trong TH3 a) Khớp 1 b) Khớp 2 c) Khớp 3 d) Khớp 4

5.3.4 Đánh giá kết quả thực nghiệm Đánh giá chất lượng bộ điều khiển thông qua quan sát đồ thị mang tính chất chủ quan, định tính Để đánh giá một cách khách quan và chính xác hơn, người thực hiện sử dụng phương pháp sai số trung bình bình phương (RMSE):

Trong đó: n− số mẫu dữ liệu

O i − giá trị đáp ứng của hệ thống

Sai số trung bình bình phương của sai số bám và sai số đồng bộ:

Bảng 5.3: Sai số trung bình bình phương của sai số bám

Bộ điều khiển Khớp 1 Khớp 2 Khớp 3 Khớp 4

Bảng 5.4: Sai số trung bình bình phương của sai số đồng bộ

Bộ điều khiển Khớp 1-2 Khớp 2-3 Khớp 3-4 Khớp 4-1

Sai số trung bình bình phương của sai số bám:

Sai số trung bình bình phương sai số đồng bộ:

Sai số trung bình bình phương của sai số bám:

Sai số trung bình bình phương sai số đồng bộ

▪ Ngoài sự ràng buộc vật lý của một hệ thống khép kín trong robot song song, thuật toán điều khiển đồng bộ giúp các chuỗi động học có sự liên kết chủ động với nhau Bảng 5.4; 5.6 và 5.8 cho thấy các bộ điều khiển đồng bộ SPID, SPID-TDE có sai số đồng bộ giữa các chuỗi nhỏ hơn hẳn bộ điều khiển PID Do vậy, chất lượng điều khiển của SPID-TDE và

▪ Mặc dù liên tiếp chịu tác động xấu khi mất hiệu suất ở các động cơ, bộ điều khiển SPID-TDE vẫn đáp ứng tốt hơn hẳn nhờ vào khả năng ước lượng và loại bỏ các thành phần không chắc chắn, đồng thời bù động lực học cho hệ thống Bảng 5.3; 5.5 và 5.7 cho thấy bộ điều khiển SPID-

TDE có trung bình bình phương sai số bám nhỏ hơn đáng kể so với 2 bộ điều khiển còn lại

▪ Chất lượng bộ điều khiển SPID-TDE > SPID > PID

Định dạng
Số trang	114
Dung lượng	25,71 MB