Đồ Án môn học thiết kế hệ thống cơ Điện tử me5512 Đề bài thiết kế robot scara 4 bậc tự do

- Đánh giá tính ổn định của hệ thống - Mô phỏng và phân tích, đánh giá các chỉ tiêu kỹ thuật của hệ thống - Lựa chọn các thiết bị cho hệ thống điều khiển: cảm biến, thiết bị điều khiển,

PHÂN TÍCH NGUYÊN LÝ VÀ THÔNG SỐ KỸ THUẬT

Tổng quan về Robot Scara

SCARA là viết tắt của Selective Compliance Assembly Robot Arm hay

Selective Compliance Articulated Robot Arm Có nghĩa là dễ dàng lựa chọn các khớp nối cánh tay robot

Vào năm 1981, Sankyo Seiki, Pentel và NEC đã giới thiệu một khái niệm hoàn toàn mới về robot lắp ráp, được phát triển dưới sự hướng dẫn của giáo sư Hiroshi Makino từ Đại học Yamanashi.

• Cân bằng tối ưu về kinh tế và hiệu suất

• Sử dụng động cơ động cơ servo và các mạch điều khiển có sẵn

• Sử dụng phần mềm có sẵn để điều khiển robot trên máy tính cá nhân Phần mềm sử dụng G-code để điều khiển robot

SCARA là một cấu hình tiêu chuẩn giữa các robot Ở giai đoạn thiết kế sơ bộ, chúng ta có những điểm sau đây:

• Robot SCARA 4 bậc tự do

• Độ chính xác lặp: (x, y) = ± 0,02 mm, (z) = ± 0,02 mm

• Sử dụng bộ truyền vít me – đai ốc cho khâu tịnh tiến

Các thiết bị điện tử phụ thuộc vào loại động cơ sử dụng, với động cơ bước và động cơ servo là lựa chọn hàng đầu để đạt độ chính xác cao Việc thiết kế các mạch điều khiển động cơ bước và servo là phần chính của hệ thống điện tử, ảnh hưởng lớn đến hiệu suất của robot Trong bài viết này, chúng ta sẽ tập trung vào các trình điều khiển động cơ bước và servo.

Một số lựa chọn ban đầu cần phải chú ý là:

• Loại động cơ: Đơn cực hoặc lưỡng cực

• Nguyên lý ổ đĩa: L/R hoặc PWM

• Chế độ hoạt động: Nửa bước hoặc toàn bước

• Đặc trưng mô-men xoắn - tốc độ

1.2.3 Phần mềm và điều khiển

Phần mềm điều khiển cần thực hiện được các nhiệm vụ sau:

• Đường dẫn liên tục trong không gian

• Chuyển động xen kẽ (Joint-interpolated motion)

Phối hợp chuyển động tuyến tính: tất cả các trục khởi động và dừng các bước chuyển động cùng một thời gian.

Nguyên lý hoạt động

2.1 Phân tích nguyên lý hoạt động

Robot hoạt động dựa vào chuyển động quay của các động cơ điện như động cơ bước và động cơ servo, được lắp đặt tại các khớp Các khớp một, hai và bốn sử dụng dây đai hoặc bánh răng để truyền động, trong khi khớp ba là khớp tịch tiến, sử dụng bộ trục vít-me đai ốc bi để chuyển đổi chuyển động quay thành chuyển động tịch tiến Để đạt được độ chính xác về vị trí và tốc độ, robot kết hợp với các cảm biến nhằm giới hạn chuyển động Hệ thống máy tính - controller điều khiển động cơ và cảm biến thông qua các phần mềm như Matlab/Simulink, G-code, tùy thuộc vào yêu cầu cụ thể.

Hệ thống điều khiển vòng kín (closed-loop) sử dụng 8 cầu nhiệm vụ và chương trình lập trình sẵn để xuất tín hiệu điều khiển tới các cơ cấu chấp hành, thực hiện các thao tác như quay động cơ, mở tay kẹp hay bắt đầu hàn Sau đó, hệ thống cảm biến sẽ phản hồi tín hiệu về bộ điều khiển, giúp điều chỉnh và xử lý các sai lệch nếu có Mục tiêu chính của hệ thống này là triệt tiêu tín hiệu nhiễu và kiểm soát hoạt động của robot, đảm bảo rằng robot hoạt động chính xác theo yêu cầu.

Hình 1.2 Nguyên lý hoạt động robot Scara

2.2 Các thông số kỹ thuật quan trọng của robot

• Tầm với (chiều dài cánh tay) : 225mm + 275mm

⬧ Hành trình trục z khớp 3 : 200mm

• Tốc độ tổng hợp (điểm làm việc) : [X] mm/s

• Chu kỳ thời gian (với tải 1kg) : 0,36s

• Tải trọng tối đa : 20kg

❖ Các thông số thiết kế

⬧ Khối lượng trục dẫn hướng: 𝑚 𝑡𝑟𝑢𝑐4 = 1.03𝑘𝑔

⬧ Đường kính trục dẫn hướng: 𝑑 𝑡𝑟𝑢𝑐4 = 20𝑚𝑚 = 2𝑐𝑚

⬧ Khối lượng vít me: 𝑚 𝑣𝑖𝑡𝑚𝑒 = 0.84kg

⬧ Khối lượng đai ốc bi: 𝑚 𝑑𝑎𝑖 𝑜𝑐 𝑏𝑖 = 0.35kg

⬧ Thông số mặt cắt ngang: ℎ 2 = 200𝑚𝑚, 𝑏 2 = 100𝑚𝑚, 𝑡 2 = 4𝑚𝑚

⬧ Thông số mặt cắt ngang: ℎ 1 = 200𝑚𝑚, 𝑏 1 = 100𝑚𝑚, 𝑡 1 = 6𝑚𝑚

Kết luận chương

Sau khi hoàn thành chương 1, em tìm hiểu được:

• Nguyên lí hoạt động của robot

• Các thông số kỹ thuật quan trọng

• Các thành phần tổng quan của hệ thống điều khiển như động cơ, cảm biến, bộ nguồn, bộ điều khiển động cơ, vi điều khiển

Qua đó hiểu được cấu tạo và nguyên lý chuẩn bị cho phần thiết kế hệ thống điều khiển trong chương tiếp theo

CHƯƠNG 2: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN

Robot Scara là một trong những loại robot có tính ứng dụng cao nhất hiện nay, được sử dụng phổ biến trong ngành công nghiệp Chúng hoạt động tự động theo quỹ đạo được thiết kế sẵn, phù hợp với nhiều mục đích như gắp thả linh kiện, lắp ráp linh kiện nhỏ, khắc lazer, in 3D, và hàn Để đảm bảo robot hoạt động hiệu quả, cần có một hệ thống điều khiển đáp ứng liên tục Trong chương này, tôi sẽ thiết kế một quỹ đạo và xây dựng hệ thống điều khiển cho robot dựa trên quỹ đạo đó.

1 Tính toán động học, động lực học Đầu tiên, em tính toán động học cho robot để phục vụ cho việc khảo sát vùng làm việc để biết được tầm với của robot, từ đó thiết kế một quỹ đạo trong tầm với đó

❖ Mô hình hoá robot Scara:

Hình 2.1 Mô hình robot Scara 4 bậc tự do

Hệ quy chiếu được thiết lập theo phương pháp Denavit-Hartenberg (D-H) trong Hình 2.1 cho phép xác định các hệ quy chiếu động tại các khâu, như thể hiện trong Hình 2.2 Các bộ thông số động học theo quy ước D-H được trình bày trong Bảng 2.

Hình 2.2 Các hệ quy chiếu động được đặt tại các khâu theo phương pháp D-H

Bảng 2.1 Bảng thông số D-H của Robot Scara

Hệ tọa độ i di ai i

• Trong đó: q1, q2, q3, q4 là biến khớp, tham số động học cho như sau:

Bảng 2.2 Các tham số động học

156mm 225mm 275mm 95mm 51mm

❖ Như vậy các ma trận biến đổi từ hệ Oi-1 về hệ Oi như sau:

❖ Ma trận biến đổi thuần nhất mô tả hướng và vị trí của khâu thao tác so với khâu cơ sở:

• Ma trận biểu diễn hướng của khâu thao tác:

• Vị trí điểm thao tác:

❖ Giả thiết K , φ lần lượt là vị trí và hướng của khâu thao tác so với khâu cơ sở

Hình 2.3 Tọa điểm cuối khâu thao tác

➔ Đối với bài toán thuận ta cần tìm xK,yK,zK và φ

❖ Khâu công tác của Robot có dạng (với ϴ=φ, α=π):

Ta có TK = T suy ra:

❖ So sánh các phần tử tương ứng của các ma trận ở 2 vế, ta được kết quả của bài toán thuận:

❖ Sử dụng chương trình MATLAB, ta mô phỏng không gian làm việc của robot như sau:

Hình 2.4 Không gian làm việc của robot Scar

❖ Đối với bài toán ngược ta cần tìm q 1 , q 2 , q 3 , q 4 với x K ,y K ,z K đã biết

❖ Khi biết q2 ta viết lại (2.8):

❖ Từ hệ phương trình (2.9) giải ra ta được:

❖ Vậy nghiệm của bài toán ngược là:

1.1.2 Bài toán vận tốc – gia tốc

❖ Vận tốc góc các khâu

Từ các biểu thức lý thuyết:

Ta tính được vận tốc góc các khâu như sau:

❖ Vận tốc khối tâm các khâu

Vị trí khối tâm C1, C2, C3, C4 được xác định bởi các kích thước l1, l2, l3, l4 như Hình 2.5

Hình 2.5 Vị trí khối tâm các khâu

Từ biểu thức lý thuyết :

Ta tính được vận tốc khối tâm các khâu như sau:

• Toạ độ khối tâm các khâu trong hệ toạ độ khâu r̂Ci (i) :

• Toạ độ khối tâm các khâu trong hệ toạ độ cơ sở 𝑟̂ 𝐶

• Vận tốc khối tâm các khâu 𝑣 𝐶

❖ Gia tốc góc các khâu

Ta tính được gia tốc góc các khâu như sau:

❖ Gia tốc tịnh tiến các khâu

Ta tính được gia tốc tịnh tiến các khâu như sau:

1.1.3 Thiết kế quỹ đạo làm việc Để khảo sát quỹ đạo làm việc của bài toán đặt ra, ta chọn quỹ đạo làm việc gắp thả vật để khảo sát :

Hình 2.6 Quỹ đạo gắp vật

Tọa độ và hướng các điểm:

S (0.5; 0 ;0.2; 0) là điểm khởi động của robot

Bảng 2.3 Bảng quỹ đạo và thời gian tương ứng

Để thiết kế quỹ đạo cho Robot, phương pháp xấp xỉ đa thức bậc n hoặc dạng phối hợp thường được sử dụng Trong báo cáo này, nhóm đã chọn quỹ đạo theo đa thức bậc 3, vì nó đơn giản trong quá trình thiết kế.

Để đảm bảo tính liên tục của vận tốc trong quá trình hoạt động của robot, cần thiết phải tránh quỹ đạo giật cục và gấp khúc Điều này giúp giảm thiểu sốc và đảm bảo hiệu suất hoạt động ổn định cho robot.

❖ Thiêt kế quỹ đạo trong không gian thao tác, quỹ đạo đường thẳng

• Tổng quát : ta có phương trình đường thẳng đi qua 2 điểm M(x , y ,z ) o o o và N (x , y ,z ) e e e là:

• Độ dời dịch chuyển của điểm thao tác E theo phương x,y,z là đa thức bậc 3 theo thời gian:

• Với thời gian đặt cho quỹ đạo là te(s) để đi từ M đến N theo một đường thẳng Với điều kiện đầu:

Ta xác định được các hệ số như sau:

Sơ đồ Simulink thiết kế quỹ đạo:

Hình 2.7 Mô hình Simulink thiết kế quỹ đạo chuyển động

Quỹ đạo các biến khớp:

Hình 2.8 Quỹ đạo các góc khớp

Chúng ta đã xác định các biến khớp tương ứng với quỹ đạo đã được thiết kế, và những biến khớp này sẽ đóng vai trò là đầu vào trong quá trình thiết kế bộ điều khiển.

Sử dụng phương pháp Lagrange để tính động lực học robot Scara, xét hệ nhiều vật có n bậc tự do với liên kết giữ, dừng

Phương trình Lagrange loại 2 dạng ma trận:

❖ Vector toạ độ suy rộng: 𝑞 = [𝑞1 𝑞 2 … 𝑞 𝑛 ] 𝑇

❖ Phương trình động năng của hệ:

• Vận tốc khối tâm vật k: 𝑣 𝐶

• Ma trận momen quán tính khối vật k: 𝐼 𝐶

(𝑘)𝑅 𝑘 0𝑇 Phương trình (2.27) có thể được viết lại như sau:

❖ Gọi M(q) là ma trận khối lượng suy rộng:

❖ Phương trình thế năng của hệ do trọng lực: Π(𝑞) = ∑ Π 𝑖 (𝑞) = − ∑ 𝑚 𝑖 𝑔 (0)𝑇 𝑟 𝐶

❖ Lực suy rộng không thế:

• τ = Bu: momen/lực dẫn động tại khớp k

• QF * = JE,T T.F (0) : lực tác dụng tại điểm E bất kỳ

Như vậy phương trình (2.26) có thể được viết lại thành:

• C(q, q̇): ma trận Coriolis và ly tâm

𝜕𝑞 𝑖 ): số hạng Christoffel symbols ở dạng thứ nhất Vậy phương trình (2.34) trở thành:

❖ Ma trận Jacobi tịnh tiến:

• Toạ độ khối tâm các khâu so với hệ toạ độ cơ sở (tính được từ (2.16)):

• Tính các ma trận Jacobi tịnh tiến:

• Vận tốc góc các khâu (tính được từ (2.13)):

• Tính các ma trận Jacobi quay:

❖ Ma trận khối lượng suy rộng M(q):

• Tính (𝐽 𝑇 𝑘 𝑇 𝑚 𝑘 𝐽 𝑇 𝑘 ) từ các ma trận Jacobi tịnh tiến tính được ở (2.39):

(𝑘) 𝑅 𝑘 0𝑇 𝐽 𝑅 𝑘 ) từ các ma trận Jacobi quay tính được ở (2.41):

• Tính ma trận khối lượng suy rộng M(q):

❖ Ma trận Coriolis và ly tâm C(q, q̇) (được định nghĩa ở (2.35) và (2.36)):

Từ (2.43) nhận thấy chỉ có m11, m12, m21 chứa biến q2, các mij khác là hằng số, nên ta chỉ cần tính:

Còn lại các cijk khác đều bằng 0

❖ Ma trận g(q) (được định nghĩa ở (2.32)):

Lấy g (0) = [0 0 -9.81] T , kết hợp với các ma trận 𝐽 𝑇 𝑖 tính được ở (2.39) ta tính được ma trận g(q):

❖ Các lực không thế Qkt (được định nghĩa ở (2.33)):

• Ta bỏ qua lực cản nhớt nên τ d = Dq̇ = 0

• Đặt momen/lực dẫn động tại khớp k: 𝜏 = 𝐵𝑢 = [

• Tính lực tác dụng tại điểm E bất kỳ:

Với P là trọng lượng của tải

❖ Như vậy từ biểu thức (2.37), sau khi áp dụng ta thu được phương trình như sau:

Và từ đó rút ra được hệ phương trình vi phân chuyển động:

Với các giá trị mij từ (2.43) và các giá trị Cij từ (2.45)

2 Mô hình hóa, xác định hàm truyền, đánh giá tính ổn định

Mô hình hóa động cơ giúp xác định hàm truyền, một hàm toán học mô phỏng mối quan hệ giữa đầu ra và đầu vào Qua đó, chúng ta có thể đánh giá tính ổn định và sai lệch tĩnh của hệ thống điều khiển.

2.1 Mô hình hóa động cơ điện một chiều

Hình 2.9 Sơ đồ khối robot hoàn chỉnh bao gồm hộp số

*motor driver: bộ điều khiển động cơ

*encoder:cảm biến phản hồi tốc độ, vị trí của động cơ

*motor inertia: quán tính động cơ

*motor friction: ma sát động cơ

*load inertia: quán tính tải trọng

𝐽 𝑚 là momen quán tính của động cơ

𝐽 𝑙 là momen quán tính của khâu gắn trên khớp đó

𝜏 𝑚 là momen ở trục động cơ

𝜏 𝑙 là momen tải quán tính

𝜃 𝑚 là góc quay ở trục động cơ

𝐵 𝑚 là hệ số giảm chấn của động cơ

𝐵 𝑙 là hệ số giảm chấn của tải

Ta có hệ số giảm tốc :

Sử dụng nguyên lý D’Alambert , chúng ta có :

𝑑𝑡 = 𝜔 𝑠 là tốc độ góc của tải (Rad / s)

𝑑𝑡 là gia tốc góc của tải (𝑅𝑎𝑑/𝑠 2 ) Phương trình trên tương đương với :

Sử dụng nguyên lý trên cho trục động cơ chính ta được :

Kết hợp các phương trình trên ta thu được:

𝐽 𝑡𝑑 = 𝐽 𝑚 + 𝑛 2 𝐽 𝑙 là momen quán tính tác động ở trục động cơ

𝐵 𝑡𝑑 = 𝐵 𝑚 + 𝑛 2 𝐵 𝑙 là hệ số giảm chấn tác dụng ở trục động cơ

2.1.2 Phương trình về điện Động cơ điện một chiều (Động cơ điện DC) kích từ độc lập, được điều khiển bằng điện áp phần ứng Sơ đồ nguyên lý của loại động cơ này được thể hiện trên hình, trong đó dòng kích từ 𝑖 𝑘 được giữ không đổi

Hình 2.10 Sơ đồ nguyên lý động cơ

• Tín hiệu vào là điện áp u đặt vào phần ứng [Volt; V]

• Tín hiệu ra là vận tốc góc ω của động cơ [rad/s]

Sử dụng 3 phương trình cơ bản như sau:

❖ Phương trình mạch điện phần ứng:

⬧ 𝐾 𝑒 – hằng số sức điện động [Vs/rad]

⬧ 𝐾 𝑒 𝜔 – sức điện động phần ứng [V]

Biến đổi Laplace 2 vế phương trình, ta được:

Sơ đồ khối tương ứng:

Hình 2.11 Sơ đồ khối mạch điện phần ứng

❖ Phương trình momen điện từ của động cơ

Với dòng kích từ 𝑖𝑘 không đổi thì từ thông khe khí 𝛷 = k 2 𝑖 𝑘 là không đổi và momen điện từ M của động cơ tỉ lệ với dòng điện phần ứng:

⬧ 𝐾 𝑚 = 𝑘 1 𝛷 = 𝑘 1 𝑘 2 𝑖 𝑘 là hằng số momen của động cơ, [N.m/A]

 𝑘 1 - hằng số phụ thuộc vào kết cấu động cơ

 𝑘 2 - hằng số đặc trưng đoạn tuyến tính của từ thông thay đổi theo 𝑖 𝑘

Biến đổi Laplace hai vế ta được:

Sơ đồ khối tương đương :

Hình 2.12 Sơ đồ khối phần momen điện từ

❖ Phương trình cân bằng momen trên trục động cơ

⬧ 𝐽 𝑡𝑑 – momen quán tính của động cơ và tải quy về trục động cơ, [kg.m2]

⬧ 𝐵 𝑡𝑑 – hệ số ma sát nhớt của động cơ và tải quy về trục động cơ, [kg.m2]

⬧ 𝑀 𝑡 – momen phụ tải (nhiễu), [Nm]

Biến đổi Laplace 2 vế phương trình:

Hình 2.13 Sơ đồ khối phần momen cân bằng trên động cơ

Kết hợp 3 sơ đồ khối thành phần, ta thu được sơ đồ khối của động cơ:

Hình 2.14 Sơ đồ khối động cơ ( Điện áp -> vận tốc)

Hàm truyền của động cơ DC với tín hiệu vào là điện áp và tín hiệu ra là vận tốc:

Trong đồ án này, chúng tôi tập trung vào việc điều khiển vị trí của robot, đặc biệt là góc quay tại các khớp Do đó, khi mô hình hóa động cơ, tín hiệu đầu ra cần phải phản ánh sự dịch chuyển của góc.

Gọi 𝜃 𝑚 – góc quay của động cơ Do đó đạo hàm của góc quay là vận tốc góc:

Chuyển sang miền tần số, ta được:

Lúc này, hàm truyền của động cơ trở thành:

𝑅 – hằng số thời gian điện từ, 𝜏 𝑐 = 𝐽 𝑡𝑑

𝐵 𝑡𝑑 – hằng số thời gian cơ Hàm truyền của động cơ DC với tín hiệu vào là điện áp và tín hiệu ra là góc quay:

Sơ đồ khối của động cơ DC với tín hiệu vào là điện áp và tín hiệu ra là góc quay:

Hình 2.15 Sơ đồ khối động cơ ( Điện áp -> Góc quay)

2.2 Mô hình hóa và xác định hàm truyền các khâu

2.2.1 Mô hình hóa các khâu

Khâu 4 chỉ sử dụng tổ hợp động cơ DC kết hợp với hộp giảm tốc mà không cần thêm cơ cấu truyền động, do đó hàm truyền của khâu 4 chính là hàm truyền của động cơ đã được xây dựng.

Trục vít me – đai ốc bi khâu 3 (Khâu 3 là khâu tịnh tiến- sử dụng vít me đai ốc bi như đã thiết kế ở đồ án 1)

Hình 2.16 Sơ đồ trục vít me- đai ốc bi

Xét bộ truyền vít me – đai ốc bi như hình

Tín hiệu vào: vận tốc góc 𝑤(𝑡) của vít me, [rad/s]

Tín hiệu ra: lượng dịch chuyển 𝑦(𝑡) của khâu 3, [m]

Gọi p là bước của vít me, [m] Ta có phương trình quan hệ:

Biến đổi Laplace 2 vế với điều kiện đầu bằng 0:

Lập tỷ số tín hiệu ra trên tín hiệu vào, ta thu được hàm truyền tích phân:

Sơ đồ khối tương đương:

Hình 2.17 Sơ đồ khối trục vít me- đai ốc bi

Kết hợp sơ đồ khối các phần tử động cơ và trục vít me bi, ta thu được sơ đồ khối mô tả khâu 3 như sau:

Hình 2.18 Sơ đồ khối khâu 3

Khâu 2 chỉ sử dụng động cơ DC kết hợp với hộp giảm tốc mà không cần thêm cơ cấu truyền động Do đó, hàm truyền của khâu 2 chính là hàm truyền của động cơ đã được xây dựng.

Khâu 1 chỉ sử dụng tổ hợp động cơ DC kết hợp với hộp giảm tốc mà không có thêm cơ cấu truyền động, do đó hàm truyền của khâu 1 chính là hàm truyền của động cơ đã được xây dựng.

2.2.2 Momen quán tính các khâu

• Giả sử tải trọng là vật có dạng lập phương, cạnh 𝑎 = 8𝑐𝑚 = 0,08𝑚, bán kính trục dẫn hướng 𝑟 𝑡𝑟𝑢𝑐4 = 𝑑 𝑡𝑟𝑢𝑐4

2 = 0,01𝑚, ta tính được momen quán tính của cụm (trục+tải) so với trục quay z:

❖ Bảng thông số động cơ servo sử dụng trong robot SCARA:

Bảng 2.4 Thông số động cơ khâu 4

Hằng số sức điện động 𝐾 𝑒 (𝑉𝑠/𝑟𝑎𝑑) 56/60

Hệ số momen xoắn 𝐾 𝑚 (𝑁𝑚/𝐴) 0,496 Quán tính của rotor 𝐽 𝑚 (𝑘𝑔 𝑚 2 ) 0,326.10 −4 Điện trở 𝑅(Ω) 1,83 Độ tự cảm 𝐿(𝐻) 4,72 10 −3

Quán tính của rotor 𝐽 𝑚 (𝑘𝑔 𝑚 2 ) 0,769 10 −4 Điện trở 𝑅(Ω) 1,83 Độ tự cảm 𝐿(𝐻) 4,72 10 −3

Bảng 2.6 Thông số động cơ khâu 1 và khâu 2

❖ Tính các hàm truyền sử dụng công cụ MATLAB:

Hình 2.19 Chương trình MATLAB tính hàm truyền

Hình 2.20 Kết quả hàm truyền các khâu

Với Gs1, Gs2, Gs3, Gs4 là hàm truyền khâu 1,2,3,4

2.3 Đánh giá tính ổn định, xác định sai lệch tĩnh

Khảo sát tính ổn định của hàm truyền các khâu theo tiêu chuẩn Nyquist

Hình 2.21 Đồ thị Nyquist hàm truyền khâu 1 Hình 2.22 Đồ thị Nyquist hàm truyền khâu 2

Đồ thị Nyquist của hàm truyền các khâu không có điểm nào chạm vào (-1+0j), điều này cho thấy không có điểm cực nào nằm trên trục ảo hoặc bên phải trục ảo Do đó, hệ kín sẽ đảm bảo tính ổn định.

❖ Xác định sai lệch tĩnh

Cả 3 khâu đều có hàm truyền dạng:

Xét tín hiệu đầu vào có dạng 𝑢(𝑡) = 1(𝑡)

Hình 2.25 Sơ đồ khối tính sai lệch tĩnh với đầu vào u(t)=1(t)

Sai lệch tĩnh được tính theo công thức:

Theo tiêu chuẩn đánh giá sai lệch tĩnh, hệ thống ổn định

❖ Đánh giá tính đáp ứng

Hình 2.26 Đường đáp ứng quá độ hàm truyền khớp 1

⬧ Thời gian tăng: 105s (là khoảng thời gian kể từ khi hệ thống đạt 10% cho đến khi đạt 90% giá trị xác lập)

⬧ Trạng thái ổn định: Giá trị xác lập là 1, vậy sai lệch tĩnh bằng 0 và hệ ổn định

Mặc dù hệ thống đã ổn định, thời gian xác lập vẫn quá lâu và tốc độ đáp ứng chưa đủ nhanh Do đó, cần thiết phải thiết kế một bộ điều khiển nhằm nâng cao tốc độ đáp ứng và rút ngắn thời gian xác lập của hệ thống.

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

THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN

Mô hình hóa, xác định hàm truyền, đánh giá tính ổn định

• Đặt momen/lực dẫn động tại khớp k: 𝜏 = 𝐵𝑢 = [

• Tính lực tác dụng tại điểm E bất kỳ:

Với P là trọng lượng của tải

❖ Như vậy từ biểu thức (2.37), sau khi áp dụng ta thu được phương trình như sau:

Và từ đó rút ra được hệ phương trình vi phân chuyển động:

Với các giá trị mij từ (2.43) và các giá trị Cij từ (2.45)

2 Mô hình hóa, xác định hàm truyền, đánh giá tính ổn định

Mô hình hóa động cơ giúp xác định hàm truyền, một hàm toán học mô tả mối quan hệ giữa đầu ra và đầu vào Qua đó, chúng ta có thể đánh giá tính ổn định và sai lệch tĩnh của hệ thống điều khiển.

2.1 Mô hình hóa động cơ điện một chiều

Hình 2.9 Sơ đồ khối robot hoàn chỉnh bao gồm hộp số

*motor driver: bộ điều khiển động cơ

*encoder:cảm biến phản hồi tốc độ, vị trí của động cơ

*motor inertia: quán tính động cơ

*motor friction: ma sát động cơ

*load inertia: quán tính tải trọng

𝐽 𝑚 là momen quán tính của động cơ

𝐽 𝑙 là momen quán tính của khâu gắn trên khớp đó

𝜏 𝑚 là momen ở trục động cơ

𝜏 𝑙 là momen tải quán tính

𝜃 𝑚 là góc quay ở trục động cơ

𝐵 𝑚 là hệ số giảm chấn của động cơ

𝐵 𝑙 là hệ số giảm chấn của tải

Ta có hệ số giảm tốc :

Sử dụng nguyên lý D’Alambert , chúng ta có :

𝑑𝑡 = 𝜔 𝑠 là tốc độ góc của tải (Rad / s)

𝑑𝑡 là gia tốc góc của tải (𝑅𝑎𝑑/𝑠 2 ) Phương trình trên tương đương với :

Sử dụng nguyên lý trên cho trục động cơ chính ta được :

Kết hợp các phương trình trên ta thu được:

𝐽 𝑡𝑑 = 𝐽 𝑚 + 𝑛 2 𝐽 𝑙 là momen quán tính tác động ở trục động cơ

𝐵 𝑡𝑑 = 𝐵 𝑚 + 𝑛 2 𝐵 𝑙 là hệ số giảm chấn tác dụng ở trục động cơ

2.1.2 Phương trình về điện Động cơ điện một chiều (Động cơ điện DC) kích từ độc lập, được điều khiển bằng điện áp phần ứng Sơ đồ nguyên lý của loại động cơ này được thể hiện trên hình, trong đó dòng kích từ 𝑖 𝑘 được giữ không đổi

Hình 2.10 Sơ đồ nguyên lý động cơ

• Tín hiệu vào là điện áp u đặt vào phần ứng [Volt; V]

• Tín hiệu ra là vận tốc góc ω của động cơ [rad/s]

Sử dụng 3 phương trình cơ bản như sau:

❖ Phương trình mạch điện phần ứng:

⬧ 𝐾 𝑒 – hằng số sức điện động [Vs/rad]

⬧ 𝐾 𝑒 𝜔 – sức điện động phần ứng [V]

Biến đổi Laplace 2 vế phương trình, ta được:

Hình 2.11 Sơ đồ khối mạch điện phần ứng

❖ Phương trình momen điện từ của động cơ

Với dòng kích từ 𝑖𝑘 không đổi thì từ thông khe khí 𝛷 = k 2 𝑖 𝑘 là không đổi và momen điện từ M của động cơ tỉ lệ với dòng điện phần ứng:

⬧ 𝐾 𝑚 = 𝑘 1 𝛷 = 𝑘 1 𝑘 2 𝑖 𝑘 là hằng số momen của động cơ, [N.m/A]

 𝑘 1 - hằng số phụ thuộc vào kết cấu động cơ

 𝑘 2 - hằng số đặc trưng đoạn tuyến tính của từ thông thay đổi theo 𝑖 𝑘

Biến đổi Laplace hai vế ta được:

Sơ đồ khối tương đương :

Hình 2.12 Sơ đồ khối phần momen điện từ

❖ Phương trình cân bằng momen trên trục động cơ

⬧ 𝐽 𝑡𝑑 – momen quán tính của động cơ và tải quy về trục động cơ, [kg.m2]

⬧ 𝐵 𝑡𝑑 – hệ số ma sát nhớt của động cơ và tải quy về trục động cơ, [kg.m2]

⬧ 𝑀 𝑡 – momen phụ tải (nhiễu), [Nm]

Biến đổi Laplace 2 vế phương trình:

Hình 2.13 Sơ đồ khối phần momen cân bằng trên động cơ

Kết hợp 3 sơ đồ khối thành phần, ta thu được sơ đồ khối của động cơ:

Hình 2.14 Sơ đồ khối động cơ ( Điện áp -> vận tốc)

Hàm truyền của động cơ DC với tín hiệu vào là điện áp và tín hiệu ra là vận tốc:

Trong đồ án này, chúng tôi tập trung vào việc điều khiển vị trí của robot, đặc biệt là góc quay tại các khớp Do đó, khi mô hình hóa động cơ, tín hiệu đầu ra cần phản ánh đại lượng dịch chuyển của góc.

Gọi 𝜃 𝑚 – góc quay của động cơ Do đó đạo hàm của góc quay là vận tốc góc:

Chuyển sang miền tần số, ta được:

Lúc này, hàm truyền của động cơ trở thành:

𝑅 – hằng số thời gian điện từ, 𝜏 𝑐 = 𝐽 𝑡𝑑

𝐵 𝑡𝑑 – hằng số thời gian cơ Hàm truyền của động cơ DC với tín hiệu vào là điện áp và tín hiệu ra là góc quay:

Sơ đồ khối của động cơ DC với tín hiệu vào là điện áp và tín hiệu ra là góc quay:

Hình 2.15 Sơ đồ khối động cơ ( Điện áp -> Góc quay)

2.2 Mô hình hóa và xác định hàm truyền các khâu

2.2.1 Mô hình hóa các khâu

Khâu 4 sử dụng tổ hợp động cơ DC kết hợp với hộp giảm tốc mà không cần thêm cơ cấu truyền động Do đó, hàm truyền của khâu 4 chính là hàm truyền của động cơ đã được xây dựng.

Trục vít me – đai ốc bi khâu 3 (Khâu 3 là khâu tịnh tiến- sử dụng vít me đai ốc bi như đã thiết kế ở đồ án 1)

Hình 2.16 Sơ đồ trục vít me- đai ốc bi

Xét bộ truyền vít me – đai ốc bi như hình

Tín hiệu vào: vận tốc góc 𝑤(𝑡) của vít me, [rad/s]

Tín hiệu ra: lượng dịch chuyển 𝑦(𝑡) của khâu 3, [m]

Gọi p là bước của vít me, [m] Ta có phương trình quan hệ:

Biến đổi Laplace 2 vế với điều kiện đầu bằng 0:

Lập tỷ số tín hiệu ra trên tín hiệu vào, ta thu được hàm truyền tích phân:

Sơ đồ khối tương đương:

Hình 2.17 Sơ đồ khối trục vít me- đai ốc bi

Kết hợp sơ đồ khối các phần tử động cơ và trục vít me bi, ta thu được sơ đồ khối mô tả khâu 3 như sau:

Hình 2.18 Sơ đồ khối khâu 3

Khâu 2 chỉ sử dụng động cơ DC kết hợp với hộp giảm tốc mà không có cơ cấu truyền động bổ sung, do đó hàm truyền của khâu 2 chính là hàm truyền của động cơ đã được thiết lập.

Khâu 1 chỉ sử dụng tổ hợp động cơ DC kết hợp với hộp giảm tốc mà không cần thêm cơ cấu truyền động Do đó, hàm truyền của khâu 1 chính là hàm truyền của động cơ đã được xây dựng.

2.2.2 Momen quán tính các khâu

• Giả sử tải trọng là vật có dạng lập phương, cạnh 𝑎 = 8𝑐𝑚 = 0,08𝑚, bán kính trục dẫn hướng 𝑟 𝑡𝑟𝑢𝑐4 = 𝑑 𝑡𝑟𝑢𝑐4

2 = 0,01𝑚, ta tính được momen quán tính của cụm (trục+tải) so với trục quay z:

❖ Bảng thông số động cơ servo sử dụng trong robot SCARA:

Hệ số momen xoắn 𝐾 𝑚 (𝑁𝑚/𝐴) 0,496 Quán tính của rotor 𝐽 𝑚 (𝑘𝑔 𝑚 2 ) 0,326.10 −4 Điện trở 𝑅(Ω) 1,83 Độ tự cảm 𝐿(𝐻) 4,72 10 −3

Bảng 2.6 Thông số động cơ khâu 1 và khâu 2

❖ Tính các hàm truyền sử dụng công cụ MATLAB:

Hình 2.19 Chương trình MATLAB tính hàm truyền

Hình 2.20 Kết quả hàm truyền các khâu

Với Gs1, Gs2, Gs3, Gs4 là hàm truyền khâu 1,2,3,4

2.3 Đánh giá tính ổn định, xác định sai lệch tĩnh

Khảo sát tính ổn định của hàm truyền các khâu theo tiêu chuẩn Nyquist

Đồ thị Nyquist của hàm truyền các khâu không có điểm nào nằm tại (-1+0j), do đó không có điểm cực nào trên trục ảo hoặc bên phải trục ảo Điều này cho thấy hệ kín sẽ ổn định.

❖ Xác định sai lệch tĩnh

Cả 3 khâu đều có hàm truyền dạng:

Xét tín hiệu đầu vào có dạng 𝑢(𝑡) = 1(𝑡)

Hình 2.25 Sơ đồ khối tính sai lệch tĩnh với đầu vào u(t)=1(t)

Sai lệch tĩnh được tính theo công thức:

Theo tiêu chuẩn đánh giá sai lệch tĩnh, hệ thống ổn định

❖ Đánh giá tính đáp ứng

Mặc dù hệ thống đã ổn định, thời gian xác lập vẫn quá lâu và tốc độ đáp ứng chưa đủ nhanh Vì vậy, cần thiết phải thiết kế bộ điều khiển nhằm tăng cường tốc độ đáp ứng và rút ngắn thời gian xác lập của hệ thống.

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

Bộ điều khiển PID (Proportional-Integral-Derivative) là một cơ chế điều khiển vòng phản hồi phổ biến trong ngành công nghiệp Nó hoạt động bằng cách tính toán sai số, tức là sự khác biệt giữa tín hiệu đầu ra thực tế và tín hiệu mong muốn Mục tiêu của bộ điều khiển PID là giảm thiểu sai số này để cải thiện hiệu suất hệ thống.

46 bằng cách điều chỉnh tín hiệu đầu vào Sai lệch càng nhỏ thì hệ thống càng đạt đến trạng thái làm việc mong muốn

Hình 2.30 Mô hình bộ điều khiển PID

Trong đồ án này, chúng tôi sử dụng luật điều khiển PID để điều khiển robot đạt đến vị trí mong muốn trong không gian khớp Tín hiệu điều khiển của PID Controller được xác định bởi sai lệch về vị trí biến khớp theo thời gian, tức là sai số giữa vị trí hiện tại và vị trí mong muốn.

• 𝑒(𝑡) – sai lệch giữa giá trị đặt mong muốn (setpoint) và giá trị thực tế đo được

• 𝑞𝑑(𝑡) – giá trị đặt mong muốn (setpoint) tại thời điểm đo

• 𝑞(𝑡) – giá trị thực tế của biến khớp tại thời điểm đo

Tiến hành xây dựng bộ điều khiển PID trong MATLAB như sau:

• Thiết lập sơ đồ Simulink cho bộ điều khiển

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

• Trong MATLAB-Simulink đã có sẵn khối PID Controller, ta sẽ sử dụng trực tiếp khối này mà không cần xây dựng lại

Hình 2.32 Sơ đồ khối PID điều khiển động cơ

Việc lựa chọn các thông số cho PID Controller có thể thực hiện qua nhiều phương pháp khác nhau Phương pháp đơn giản nhất là điều chỉnh bằng tay, thử nghiệm các bộ thông số cho đến khi đạt được tín hiệu mong muốn, tuy nhiên cách này tốn thời gian và không khoa học Một phương pháp khác là sử dụng các kỹ thuật toán học như Ziegler-Nichols, Chien-Hrones-Reswick, hoặc tối ưu độ lớn Cuối cùng, phương pháp được sử dụng trong đồ án này là ứng dụng công cụ PID Tuner tích hợp trên MATLAB để tối ưu hóa các thông số.

Hình 2.33 Giao diện công cụ PID Tuner trên Matlab

Xác định được các thông số của PID Controller:

Bảng 2.7 Bảng thông số PID

Ta sẽ đánh giá chất lượng của bộ điều khiển đã thiết kế với đầu vào mẫu

Hình 2.34 Đồ thi đáp ứng khâu 1 với bộ điều khiển PID

• Thời gian tăng: 0,00435s (là khoảng thời gian kể từ khi hệ thống đạt 10% cho đến khi đạt 90% giá trị xác lập)

• Trạng thái ổn định: Giá trị xác lập là 1, vậy sai lệch tĩnh bằng 0 và hệ ổn định

Mô phỏng, phân tích và đánh giá các chỉ tiêu kỹ thuật của hệ thống

Sau khi thiết kế bộ điều khiển PID, hệ thống đạt trạng thái ổn định với giá trị xác lập là 1, sai lệch tĩnh bằng 0 Mặc dù xuất hiện vọt lố nhỏ, điều này là khó tránh khỏi và hoàn toàn chấp nhận được Đặc biệt, tốc độ đáp ứng nhanh và thời gian xác lập giảm đáng kể, góp phần nâng cao hiệu quả hoạt động của hệ thống.

4 Mô phỏng, phân tích và đánh giá các chỉ tiêu kỹ thuật của hệ thống

Sau khi hoàn thành thiết kế bộ điều khiển PID, bước tiếp theo là tiến hành mô phỏng để đánh giá hiệu quả và tính hợp lý của bộ điều khiển đã thiết kế Quy trình thực hiện bài toán bao gồm các bước cụ thể để đảm bảo kết quả chính xác và đáng tin cậy.

⬧ Xác định các thông số vật lý của robot

⬧ Mô hình hóa bằng phần mềm SolidWorks

⬧ Xuất bản CAD tương ứng sang MATLAB/SimMechanics

⬧ Xây dựng bộ điều khiển PID và mô phỏng

Hình 2.38 Trình tự xây dựng mô hình hệ thống điều khiển

• Sau khi xác định thông số vậy lý và xây dựng mô hình CAD trong

Solidworks, ta đưa mô hình vào MATLAB Simulink

Hình 2.39 Mô hình CAD của robot trong Matlab

Hình 2.40 Khối Scara robot trong Simulink

World: hệ quy chiếu toàn cục

MechanismConfiguaration: chọn vecto trọng lực

Transform:chuyển từ hệ quy chiếu toàn cục sang hệ quy hệ quy chiếu cục bộ của khâu 0 tại gốc

Part1_1_RIGID: khâu 0 (đế), chuyển hệ trục tọa độ từ khớp 0 lên khớp 1

Revolute: tạo chuyển động quay ở khớp 1

Part2_1_RIGID: khâu 1, chuyển hệ trục tọa độ từ khớp 1 lên khớp 2

Revolute1: tạo chuyển động quay ở khớp 2

Part3_1_RIGID: khâu 2, chuyển hệ trục tọa độ từ khớp 2 lên khớp 3

Parimastic: tạo chuyển động tịnh tiến ở khớp 3

• Cuối cùng, ta ghép nối mô hình robot với bộ điều khiển PID để hình thành mô hình điều khiển cho toàn bộ robot

Hình 2.41 Mô hình hệ thống điều khiển toàn bộ robot

Hệ thống nhận đầu vào từ các biến khớp (q1, q2, q3, q4), những giá trị này phụ thuộc vào vị trí của các điểm thao tác trong không gian Các biến khớp này cần phải thỏa mãn quỹ đạo hình học đã được xác định trước và được giải quyết thông qua bài toán động học ngược.

• Tín hiệu vào sẽ đi qua các khối Điều khiển PID, khối động cơ DC và khối mô hình 3D của robot

Đầu ra của hệ thống bao gồm quỹ đạo và vận tốc của các biến khớp, cùng với tín hiệu sai lệch, đóng vai trò quan trọng làm đầu vào cho bộ điều khiển PID.

Hình 2.42 Khối Position Reference (Thiết kế quỹ đạo)

Hình 2.43 Khối động cơ DC

4.2 Mô phỏng và kết quả

Ta mô phỏng và thu được đồ thị về quỹ đạo của từng biến khớp, sai số và vận tốc của chúng như sau:

Hình 2.44 Giá trị biến khớp khâu 1

Hình 2.46 Sai số biến khớp khâu 1

Hình 2.55 Sai số biến khớp khâu 4 Đánh giá:

• Giá trị biến khớp các khâu đặt vào và đầu ra là trùng khớp

• Sai số biến khớp các khâu là rất nhỏ ( khâu 1: 6,25 10 −4 𝑟𝑎𝑑 , khâu 2: 1,56 10 −2 𝑟𝑎𝑑, khâu 3: 3,017 10 −2 𝑚𝑚, khâu 4: 6,94 10 −3 𝑚𝑚)

➔ Như vậy bộ điều khiển ta đã thiết kế là phù hợp

Lựa chọn các phần tử cho hệ thống điều khiển

5 Lựa chọn các phần tử cho hệ thống điều khiển

Hệ thống điều khiển robot bao gồm 2 phần tử cơ bản và quan trọng: Hệ thống điều khiển động cơ servo và bộ điều khiển robot

5.1 Hệ thống điều khiển servo

Cụm thiết bị điều khiển servo gồm 2 thành phần: Bộ điều khiển và động cơ servo

Trong công nghiệp hiện nay, có hai lựa chọn chính để điều khiển động cơ servo: sử dụng biến tần hoặc Servo Driver Biến tần, vốn được sử dụng chủ yếu cho động cơ AC trong các ứng dụng công suất lớn như máy nén khí và băng tải nặng, giờ đây đã được cải tiến để có thể điều khiển động cơ servo Trong khi đó, Servo Driver được thiết kế theo mô-đun, với mỗi driver tương thích với một số loại động cơ servo nhất định, tùy thuộc vào từng nhà sản xuất.

Trong đồ án này, em sẽ lựa chọn cụm thiết bị driver SGDV-5R5A và động cơ servo SGMAV – 10A cho khâu 1 và khâu 2, SMGAV-08A cho khâu 3, và

SMGAV-06A cho khâu 4 (hãng Yaskawa)

Hình 2.56 Động cơ và bộ điều khiển hãng Yaskawa

Hình 2.57 Thông số kỹ thuật của dòng động cơ SGMAV

*Rate Output: Đầu ra định mức ( công suất )

*Rate Torque: Moomen xoắn định mức

*Rate Speed: Tốc độ định mức

*Max Speed: Tốc độ tối đa

*Applicable SERVOPACK: Mã bộ điều khiển tương ứng với mã động cơ

Hình 2.58 Giao diện của bộ điều khiển SGDV-5R5A

Driver chia làm 3 vùng chính:

• Phần Panel Operator: Bao gồm 1 màn hình LED hiển thị và nút bấm, sử dụng đểcấu hình các chức năng, cài đặt chế độ cho driver hoạt động

• Phần cấp nguồn (nằm ở sườn bên trái), gồm có:

⬧ Cổng L1, L2, L3: Cấp nguồn điện cho động cơ

⬧ Cổng L1C, L2C: Cấp nguồn điện cho driver hoạt động

⬧ Cổng U, V, W: Cổng nối với động cơ

• Phần cổng input/output (nằm ở sườn bên phải), gồm có:

⬧ Cổng CN5: Cổng kết nối ra màn hình annalog

⬧ Cổng CN3: Cổng kết nối với các thiết bị điều khiển

⬧ Cổng CN7: Cổng kết nối USB dành cho máy tính

⬧ Cổng CN1: Cổng I/O vào ra, nhận các tín hiệu điều khiển từ bộ điều khiển trung tâm, xuất các tín hiệu cảnh báo, …

⬧ Cổng CN8: Cổng chức năng cho các thiết bị an toàn

⬧ Cổng CN2: Kết nối với encoder của động cơ

Driver là một module điều khiển phản hồi vòng kín, tương tự như khối điều khiển PID trong thiết kế hệ thống điều khiển Các tín hiệu điều khiển được nhận từ các cổng như CN3, CN7 hoặc CN1, cho phép người dùng lựa chọn các bộ điều khiển trung tâm phù hợp với yêu cầu ứng dụng Khi sử dụng cổng CN1, người dùng có thể thêm bộ cáp để mở rộng chân kết nối, phục vụ cho việc kết nối với vi điều khiển hoặc các thiết bị cần nhiều chân I/O.

Vị trí của động cơ được kiểm soát thông qua hai chân đầu vào PULS (chân số 7 và 8) và SIGN (chân số 11 và 12) Trong đó, PULS cung cấp xung cho driver, giúp động cơ di chuyển tương ứng với xung từ encoder gắn trên động cơ Chân SIGN điều khiển chiều quay, với mức H tương ứng với chiều quay thuận và mức L cho chiều quay nghịch Để cài đặt chế độ cho driver, chọn chế độ Factory Settings.

Hình 2.59 Phương pháp xuất xung điều khiển servo ở chế độ điều khiển vị trí

Từ encoder 20bit, số xung/vòng quay của động cơ có thể được tính toán thông qua tỷ lệ hộp giảm tốc Điều này cho phép chúng ta xác định số xung cần thiết để động cơ quay một vòng sau khi đi qua hộp giảm tốc.

𝑛 = 1048576 × 𝑘 Với k là hệ số giảm tốc

Khi đó muốn điều khiển động cơ quay được 𝑎 𝑣ò𝑛𝑔 ta cần cấp 𝑛 × 𝑎 xung vào chân PULS Để đảo chiều thì thay đổi mức logic ở chân SIGN

Trong đồ án này, mục tiêu thiết kế hệ thống điều khiển là đảm bảo robot đáp ứng yêu cầu về vị trí, vì vậy chế độ điều khiển vị trí của robot được áp dụng.

Để điều khiển 3 bộ driver, cần tối thiểu 3 chân PULS và 3 chân SIGNS Bên cạnh đó, cũng cần thêm một số cổng INPUT khác để cho phép servo hoạt động và cảnh báo lỗi.

Trong công nghiệp, vi điều khiển được sử dụng phổ biến là STM32 và Arduino Mega 2560 Sau khi phân tích thông số kỹ thuật của chúng em nhận thấy

STM32 vượt trội hơn Arduino Mega 2560 về tốc độ, bộ nhớ,… nên sẽ phù hợp với robot cần độ chính xác cao như SCARA

Dựa trên kiến thức về vi điều khiển và khả năng tiếp cận dễ dàng với nhiều tài nguyên mở, cùng với sự hỗ trợ từ cộng đồng lớn và tính linh hoạt trong lập trình, tôi đề xuất lựa chọn vi điều khiển ARM-Cortex M4.

Hình 2.60 Board vi điều khiển STM32F407 DISCOVERY

Thông số kỹ thuật cơ bản của vi điều khiển STM32F407VGT6:

• Tần số xung nhịp: Tối đa 168MHz

• Timer: gồm 14 timers, trong đó 6 timer có 4 kênh xuất PWM điều khiển động cơ, 2 timer có 2 kênh PWM

• Có 3 bộ chuyển đổi Analog to Digital 12 bit, mỗi bộ 16 kênh Có 1 bộ chuyển đổi Digital to Analog

• Giao thức truyền thông: 3 x I2C, 6 x USART, 3 x SPI, 2 x CAN,

Các mạch điện và ghép nối phần tử

Hệ thống điều khiển sử dụng 4 driver để điều khiển 4 động cơ servo, với điện áp hoạt động là 3 pha AC 200V Driver nhận nguồn điện từ 1 pha AC 200V, có thể được lấy từ 2 dây của hệ thống điện 3 pha Đồng thời, bộ điều khiển sử dụng nguồn điện 5VDC.

Hệ thống cấp nguồn bao gồm các thiết bị chính như:

• 1KM: Bộ khởi động từ cho nguồn điều khiển

• 2KM: Bộ khởi động từ cho nguồn chính

• 1PL: Đèn cảnh bảo lỗi động cơ

• 1SA, 2SA, 3SA: Bộ chống sét và bảo vệ quá tải

• Các công tắc để đóng cắt các nguồn điện

Sơ đồ ghép nối mạch nguồn đã nộp kèm bản thuyết minh

Nguồn điện 3 pha được cung cấp qua các chân L1, L2, L3, tương ứng với ba dây pha Hệ thống bao gồm Relay Terminal để mở rộng kết nối đến các driver Động cơ được kết nối vào các cổng U, V, W trên driver cùng với dây tín hiệu encoder.

L1C, L2C lấy nguồn 1 pha từ 2 dây pha của nguồn điện 3 pha để cấp nguồn cho driver hoạt động

6.2 Mạch ghép nối phần tử điều khiển Để điều khiển vị trí, ta sử dụng chân PULS và SIGN để điều khiển vị trí và chiều quay của động cơ

Ngoài ra còn các chân INPUT có chức năng khác như:

• S-ON: Cho phép driver hoạt động

• P-CON: Positon Control, chế độ hoạt động là điều khiển vị trí

• P-OT: Giới hạn, hãm chiều quay của động cơ theo chiều thuận

• N-OT: Giới hạn, hãm chiều quay của động cơ theo chiều nghịch

• ALO1, ALO2, ALO3: Các bit báo lỗi của driver

• COIN: Chân báo động cơ đã tới đúng vị trí mong muốn

• TGON: Chân báo động cơ vượt quá vận tốc đặt

• SRDY: Chân báo tín hiệu động cơ đã sẵn sàng hoạt động hay chưa

Để điều khiển và giám sát hoạt động của driver, mỗi động cơ cần tối thiểu 16 cổng I/O, do đó 4 động cơ sẽ yêu cầu tổng cộng 64 cổng Các chân I/O trên mạch điều khiển đã được chọn đủ để đáp ứng nhu cầu điều khiển hệ thống.

Sơ đồ ghép nối servo driver ở chế độ điều khiển vị trí đã nộp kèm bản thuyết minh

Kết luận chương

Sau khi hoàn thành Chương 2, tôi đã thiết kế một hệ thống điều khiển và mô phỏng, đồng thời đánh giá hiệu quả của bộ điều khiển Tôi đã lựa chọn các phần tử phù hợp cho hệ thống và thiết kế mạch đấu nối nguồn cùng với mạch điều khiển.

MÔ PHỎNG ĐIỀU KHIỂN ROBOT

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

Hình 3.1 Giao diện điều khiển robto scara

Giao diện được thiết kế đầy đủ chức năng, hiển thị rõ ràng các thông tin cần thiết, bao gồm hai khối chính: khối hiển thị và khối chức năng.

Tại đây, chúng ta có thể theo dõi sự di chuyển của robot theo chương trình mô phỏng, giúp vẽ quỹ đạo và hiển thị tọa độ khâu cùng không gian làm việc một cách trực quan.

• Ta cũng có thể theo dõi sự thay đổi về tọa độ các khớp theo thời gian, qua đó kiểm chứng độ chính xác của chương trình mô phỏng

Hình 3.2 Thông tin cơ bản của chương trình mô phỏng

Hình 3.3 Thông tin về góc, vận tốc khi thiết lập quỹ đạo Hình 3.4 Thông tin về từng góc khớp

Hình 3.5 Thông tin về tọa độ điểm thao tác Hình 3.6 Thông tin về PID

Chương trình mô phỏng cung cấp cho người dùng khả năng theo dõi chi tiết các thông tin quan trọng, bao gồm sự biến thiên của giá trị góc, vận tốc và gia tốc của từng khớp, tọa độ điểm thao tác, cũng như dạng quỹ đạo.

Hình 3.7 Chức năng động học thuận Hình 3.8 Chức năng động học nghịch

Hình 3.9 Chức năng thiết kế quỹ đạo

Chương trình điều khiển bao gồm các chức năng:

• Nút nhấn SET, RESET: Bắt đầu và đặt lại chương trình mô phỏng

Người dùng có thể nhập các biến khớp 𝜃 1, 𝜃 2, 𝑑 3, 𝜃 4 để điều khiển robot di chuyển theo bài toán động học thuận Tọa độ của các biến khớp sẽ được hiển thị theo thời gian trong quá trình robot di chuyển, như minh họa trong Hình 3.2.

Inverse cho phép người dùng nhập tọa độ x, y, z và góc yaw của điểm thao tác cuối E Các tọa độ biến khớp sẽ được hiển thị trong phần set của Kinematics tiến Đồng thời, tọa độ các biến khớp theo thời gian khi robot di chuyển cũng sẽ được hiển thị như trong Hình 3.2.

Người dùng có thể thiết kế quỹ đạo trong không gian góc khớp và không gian điểm thao tác bằng cách nhập tọa độ x, y, z và góc yaw mong muốn, cùng với vận tốc và gia tốc tối đa Sau khi hoàn tất, chỉ cần nhấn nút "Go To" để robot di chuyển theo quỹ đạo đã được xác định đến tọa độ mục tiêu.

Kết quả mô phỏng

Mô phỏng động học thuận với thông số:

Hình 3.10 Kết quả mô phỏng động học thuận

2.2 Động học ngược theo quỹ đạo

Mô phỏng động học ngược với thông số:

Hình 3.11 Kết quả mô phỏng động học ngược theo quỹ đạo đường thẳng

Hình 3.12 Kết quả mô phỏng động học ngược theo quỹ đạo đường tròn

Sau khi hoàn thành Chương 3, tôi đã phát triển một giao diện điều khiển tích hợp các tính năng điều khiển động học thuận, động học nghịch và khả năng điều khiển robot theo quỹ đạo cố định.

Tiêu đề	Thiết Kế Robot SCARA 4 Bậc Tự Do
Tác giả	Hoàng Xuân Tùng
Người hướng dẫn	THS. Hoàng Văn Bạo
Trường học	Đại Học Bách Khoa Hà Nội
Chuyên ngành	Cơ Điện Tử
Thể loại	Đồ án môn học
Năm xuất bản	2023-2024
Thành phố	Hà Nội

Định dạng
Số trang	76
Dung lượng	4,2 MB