CHƯƠNG 6 : Thiết kế quỹ đạo chuyển động của Robot
2) Không gian làm việc của Robot
Từ các thông số của Robot :
Chiều dài các khâu là : L1 = 240mm ; L2 = 350 mm ; l3 = 325 mm .
Giới hạn các góc khớp : q1 [0;32] rad .
q2 [0 ;23] rad. q3 [0;23]rad.
3) Thiết kế quỹ đạo trong không gian khớp .
Trong trường hợp chuyển động từ điểm tới điểm , người ta có thể quan tâm hoặc không quan tâm đến việc khâu thao tác đi qua một số điểm trung gian . Khi đó kỹ thuật thiết kế quỹ đạo trong không gian khớp hay được sử dụng . thiết kế quỹ đạo trong không gian khớp đòi hỏi phải xây dựng được các hàm phụ thuộc thời gian của các biến khớp và các đjao hàm bậc nhất , bậc hai của nó . Qua đó mô tả được chuyển động cần thiết của Robot .
a) Đề bài :
Mô tả : Ban đầu, từ vị trí gắp vật ,robot di chuyển vị trí khâu cuối tịnh tiến theo trục z đi lên 1 đoạn. Tiếp theo robot đưa khâu thao tác đi theo nửa cung
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 42
tròn để sang vị trí đặt phôi. Cuối cùng,robot hạ điểm thao tác theo trục z đến vị trí đặt mong muốn rồi thả vật.
Chọn phương án dịch chuyển các khâu sao cho các tọa độ khớp qk (k = , …)là các đa thức bậc 3 của thời gian như sau :
= + + +
̇ = + +
̈ = +
Gọi qi0,qie lần lượt là vị trí đầu và vị trí cuối của quỹ đạo .
ti là thời gian dịch chuyển từ vị trí đầu đến vị trí cuối của đoạn quỹ đạo . te là tổng thời gian chuyển động từ vị trí O sau đó quay về vị trí O .
Vận tốc điểm đầu và điểm cuối quỹ đạo chọn bằng 0 .
- Áp dụng công thức để tính các hệ số trong pt (1) cho từng đoạn quỹ đạo chuyển động:
ai0 = qi0
ai1 = 0
ai2 = ( )
ai3 = ( )
+ Phương trình vận tốc cho từng đoạn quỹ đạo: ̇ = + 2 + 3 . + Phương trình gia tốc cho từng đoạn quỹ đạo: ̈ = 2 + 6 .
b) Ví dụ cụ thể .
Quá trình chuyển động gồm 3 giai đoạn:
+ Giai đoạn 1: robot đi từ A(300,400,200) tới điểm B(300,400,500) với quỹ đạo là đường thẳng .
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 43
+ Giai đoạn 2: robot đi từ B(300,400,500) tới điểm C(300,-400,500) với quỹ đạo là đường tròn tâm H(100,0,500) ,bán kính R = √200 + 400 = 200 5
(mm) .
+ Giai đoạn 3:robot di từ C(300,-400,500) tới D(300,-400,200) với quỹ đạo là đường thẳng .
Thiết kế quỹ đạo chuyển động trong không gian thao tác :
Giai đoạn 1: robot đi từ A(300,400,200) tới điểm B(300,400,500) với quỹ đạo là đường thẳng .
• Ta có quy luật : = + + + .
• Chọn thời gian di chuyển : t1 = 2s .
• Quãng đường di chuyển: q = 300 .
• Điểm đầu A(300,400,200).
• Điểm cuối B(300,400,500).
Suy ra q10 = 200 , q1e = 500 . Từ đó ta có :
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 44 ⎩ ⎪ ⎨ ⎪ ⎧ = 200= 0 = 3(500 − 200) 2 = 225 = 2(200 − 500) 2 = −75 Vậy nên : = + − ̇ = − ̈ = −
Đồ thị quỹ đạo , vận tốc , gia tốc theo thời gian gđ 1
Giai đoạn 2: robot đi từ B(300,400,500) tới điểm C(300,-400,500) với quỹ đạo là đường tròn tâm H(100,0,500) ,bán kính R = √200 + 400 = 200 5
(mm) .
• Ta có quy luật : = + + + .
• Chọn thời gian di chuyển : t1 = 2s .
• Quãng đường di chuyển(độ dài cung): q = 2r.arctan (2) .
• Điểm đầu B(300,400,500) .
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 45
Suy ra q10 = r.arctan(2) , q1e = - r.arctan(2) .
Từ đó ta có : ⎩ ⎪ ⎨ ⎪ ⎧ = 4456= 0 =3(−4456 − 4456) 2 = −6684 =2(4456 + 4456) 2 = 2228 Vậy nên : = − + ̇ = − + ̈ = − + x H y
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 46
Đồ thị quỹ đạo , vận tốc , gia tốc theo thời gian gđ 2
Giai đoạn 3:robot di từ C(300,-400,500) tới D(300,-400,200) với quỹ đạo là đường thẳng .
• Ta có quy luật : = + + + .
• Chọn thời gian di chuyển : t1 = 2s .
• Quãng đường di chuyển: q = 300 .
• Điểm đầu C(300,-400,500) . • Điểm cuối D(300,-400,200) . Suy ra q10 = 500 , q1e = 200 . Từ đó ta có : ⎩ ⎪ ⎨ ⎪ ⎧ = 500= 0 = 3(200 − 500) 2 = −225 =2(500 − 200) 2 = 75 Vậy nên :
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 47
= − +
̇ = − +
̈ = − +
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 48
CHƯƠNG 7 Xây dựng mô hình toán
và khảo sát điều khiển động cơ
I. Giới thiệu chung 1. Động cơ bước là gì? 1. Động cơ bước là gì?
Động cơ bước là một loại động cơ điện có nguyên lý và ứng dụng khác biệt
với đa số các động cơ điện thông thường. Chúng thực chất là một động cơ đồng bộ dùng để biến đổi các tín hiệu điều khiển dưới dạng các xung điện rời rạc kế tiếp nhau thành các chuyển động góc quay hoặc các chuyển động của rôto có khả năng cố định rôto vào các vị trí cần thiết.
Về cấu tạo, động cơ bước có thể được coi là tổng hợp của hai loại động cơ: Động cơ một chiều không tiếp xúc và động cơ đồng bộ giảm tốc công suất nhỏ.
2. Hoạt động
Động cơ bước không quay theo cơ chế thông thường, chúng quay theo
từng bước nên có độ chính xác rất cao về mặt điều khiển học. Chúng làm việc
nhờ các bộ chuyển mạch điện tử đưa các tín hiệu điều khiển vào stato theo thứ tự và một tần số nhất định. Tổng số góc quay của rôto tương ứng với số lần chuyển mạch, cũng như chiều quay và tốc độ quay của rôto phụ thuộc vào thứ tự chuyển đổi và tần số chuyển đổi.
3. Ứng dụng
Trong điều khiển chuyển động kỹ thuật số, động cơ bước là một cơ cấu chấp hành đặc biệt hữu hiệu bởi nó có thể thực hiện trung thành các lệnh đưa ra dưới dạng số.
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 49
Động cơ bước được ứng dụng nhiều trong ngành Tự động hoá, chúng được
ứng dụng trong các thiết bị cần điều khiển chính xác. Ví dụ: Điều khiển robot, điều khiển tiêu cự trong các hệ quang học, điều khiển định vị trong các hệ quan trắc, điểu khiển bắt, bám mục tiêu trong các khí tài quan sát, điều khiển lập trình trong các thiết bị gia công cắt gọt, điều khiển các cơ cấu lái phương và chiều trong máy bay...
Trong công nghệ máy tính, động cơ bước được sử dụng cho các loại ổ đĩa
cứng, ổ đĩa mềm, máy in...
II. Điều khiển động cơ bước
Trong phạm vi đồ án này, nhóm điều khiển động cơ bước dựa trên phương pháp “Điều chế độ rộng xung”
1. Giới thiệu phương pháp điều khiển tần số , băm xung cho động cơ step .
Điều chế độ rộng xung (tiếng Anh: Pulse-width modulation (PWM)),
hay Điều chế thời gian xung (tiếng Anh: pulse-duration modulation (PDM)), là
một kỹ thuật điều chế được sử dụng để mã hóa một thông điệp thành một tín
hiệu xung. Mặc dù kỹ thuật điều chế này có thể được sử dụng để mã hóa thông tin để truyền tải, việc sử dụng chính của nó là cho phép điều khiển nguồn điện cung cấp cho các thiết bị điện, đặc biệt là để tải quán tính như động cơ. Ngoài ra, PWM là một trong hai thuật toán chính được sử dụng trong bộ sạc pin
quang điện năng lượng mặt trời, thuật toán kia là giám sát điểm công suất cực
đại.
Giá trị trung bình của điện áp (và dòng điện) cung cấp cho tải được kiểm
soát bằng cách thay đổi việc đóng cắt giữa nguồn và tải tắt với tốc độ rất
nhanh. Thời gian đóng càng lâu so với thời gian cắt, thì tổng công suất cung cấp cho tải càng cao.
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 50
Tần số đóng cắt PWM phải cao hơn nhiều so với tần số ảnh hưởng đến tải (các thiết bị sử dụng điện), để dạng sóng cuối cùng được đưa tới tải phải càng mịn càng tốt. Tốc độ (hoặc tần số) mà tại đó các nguồn cấp phải đóng cắt có thể rất khác nhau tùy thuộc vào tải và ứng dụng, ví dụ
Việc đóng cắt phải được thực hiện nhiều lần một phút đối với một bếp
điện; 120 Hz trong một bộ dimmer đèn sợi đốt; giữa một vài kiloHertz (kHz),
cho đến hàng chục kHz trong một bộ điều khiển động cơ; và khoảng vài chục hoặc vài trăm kHz trong các bộ khuếch đại âm thanh và các bộ nguồn máy tính.
Thuật ngữ chu kỳ làm việc mô tả tỷ lệ giữa thời gian “bật” với thời gian điều
chỉnh hoặc “chu kỳ” làm việc; chu kỳ làm việc thấp tương ứng với công suất thấp, bởi vì nguồn điện bị cắt trong phần lớn thời gian điều chỉnh. Chu kỳ làm việc được thể hiện theo phần trăm, 100% là bật hoàn toàn.
Ưu điểm chính của PWM đó là tổn hao công suất trên các thiết bị đóng cắt (Chuyển mạch) rất thấp. Khi khóa chuyển mạch tắt thì không có dòng điện nào đi qua, và khi bật thì nguồn sẽ được đưa sang phụ tải, thì hầu như không có sụt áp trên thiết bị chuyển mạch. Tổn hao công suất, là tích của điện áp và dòng điện, do đó trong cả hai trường hợp gần như bằng không. PWM cũng hoạt động tốt với điều khiển kỹ thuật số, mà vì tính chất bật/tắt, ta có thể dễ dàng thiết lập chu kỳ làm việc cần thiết.
PWM cũng đã được sử dụng trong một số hệ thống truyền thông, trong đó
chu kỳ làm việc của nó được sử dụng để truyền tải thông tin qua một kênh truyền thông.
2. Nguyên lý
Điều chế độ rộng xung sử dụng một sóng xung hình chữ nhật có độ rộng
được điều chế dẫn đến sự biến thiên của giá trị trung bình của dạng sóng. Nếu
chúng ta xem xét một sóng xung ( ), với chu kỳ , giá trị thấp , giá trị
cao , và chu kỳ làm việc D (xem hình 1), giá trị trung bình của dạng sóng
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 51
Hình 1: một sóng xung, thể hiện các định nghĩa của , và D.
= 1 ( ) .
Vì ( ) là một sóng xung, giá trị của nó là trong khoảng < <
. , và trong khoảng . < < . Biểu thức trên trở thành:
= 1 +
= 1( . . + (1 − ) )
= . + (1 − ) .
Biểu thức sau này có thể khá đơn giản trong nhiều trường hợp trong đó =
0 khi = . . Từ đó, rõ ràng là giá trị trung bình của tín hiệu ( ) trực tiếp
phụ thuộc vào chu kỳ làm việc D.
Cách đơn giản nhất để tạo ra một tín hiệu PWM là phương pháp giao thoa,
chỉ yêu cầu cần có một sóng răng cưa hoặc sóng tam giác (dễ dàng tạo ra bằng
cách sử dụng một bộ tạo dao động đơn giản) và một mạch so sánh. Khi giá trị của
tín hiệu tham chiếu (tín hiệu đặt) (sóng sin màu đỏ trong hình 2) lớn hơn sóng điều biến (màu xanh lam), thì tín hiệu PWM (màu đỏ tía) sẽ ở trạng thái cao, nếu không thì ở trạng thái thấp.
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 52
Hình 2: Một phương pháp đơn giản để tạo ra mạch xung PWM tương ứng với một tín hiệu cho trước là PWM giao thoa: tín hiệu (ở đây là sóng sin
màu đỏ) được so sánh với dạng sóng răng cưa (màu xanh lam). Khi tín hiệu sau là nhỏ hơn tín hiệu trước, tín hiệu PWM (màu đỏ tía) sẽ ở trạng thái cao
(1). Nếu không thì nó ở trạng thái thấp (0).
3. Thiết kế bộ điều khiển PID và mô phỏng trên MATLAB a) Mô hình toán học: a) Mô hình toán học:
Mô hình động cơ bước nam châm vĩnh cửu (PMS)
Phần này cung cấp một dẫn xuất ngắn gọn của một mô hình phi tuyến của động cơ bước 2 pha PM.
Phương trình động lực học của trục rotor được đưa ra bởi:
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 53
trong đó: – mô men quán tính của trục rotor
F – hệ số ma sát nhớt
– mô men tải
Phương trình (1) tạo cơ sở cho mô hình động lực học chung của rotor động cơ bước PM. Do đó với động cơ PM có P cặp cực rôto và hai pha ( ) tại 0 và ( / 2), các phương trình không gian trạng thái sau có thể được suy ra:
=
= (− sin + cos ) − −
= − + sin
+ (2)
= − − cos −
trong đó: là gia tốc góc của tải
là đạo hàm dòng điện qua cuôn dây a
là đạo hàm dòng điện qua cuôn dây b
Phương trình xét trục nối đàn hồi:
Xem xét trường hợp khi tải được kết nối với động cơ qua một trục cứng dài có độ cứng k, thì quán tính của động cơ sẽ là và quán tính tải sẽ là như trong hình 3.
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 54
Hình 3 - Sơ đồ nối trục đàn hồi
Trong trường hợp này, phương trình tải trọng và trục đàn hồi sẽ là:
= (3)
= 1 [− ( − ) − ( − )]
Một kỹ thuật điều khiển cổ điển cho PMSM dựa trên biến đổi vectơ (u) và (i) được biểu diễn trong hệ quy chiếu gắn với stato cố định (a, b) thành các vectơ biểu diễn trong một khung (d, q) xoay dọc theo vectơ lực dẫn động giả tưởng sao cho:
= cos sin
−sin cos
Phương trình trạng thái (2) thể hiện dưới dạng dòng điện và điện áp trong tọa độ xoay (d, q) trở thành: = = − − − 1 [ ( − ) + ( − )] = − + . + (4) = − − . − +
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 55
b) Mô phỏng với MATLAB Simulink
Trong MATLAB, chạy file Thong_so_mo_hinh_hoa_Stepper_motor.m
chứa các thông số của động cơ bước.
% momen quan tinh cua dong co
Jm = 0.08; % kg.m^2
% he so can nhot cua khop noi truc
B = 3; % Nms/rad % dien cam cua cuon day
L = 3; % Henry % dien tro cua dong co
R = 3; % Omh % so cap cuc
P = 6; % khong thu nguyen % he so cua dong co
Km = 2; % Nm/rad
% he so can nhot cua truc dong co
F = 0.01; % Nms/rad % momen tai
TL = 5; % Nm
k = 0.9;
Tiến hành mô phỏng hệ phương trình (4) với Simulink
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 56
Hình 4 – Sơ đồ Simulink mô phỏng động cơ bước
Hình 5 – Khối Subsystem “Dong co buoc“
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 57
Hình 6 – Tham số PID điều khiển vận tốc góc
Hình 7 - Kết quả đáp ứng vận tốc góc
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 58
Hình 9 - Kết quả đáp ứng vị trí góc quay
Nhận xét: Đánh giá bộ điều khiển
Từ kết quả đáp ứng nhận được trên các đồ thị, ta rút ra một số nhận xét: + Bộ điều khiển vị trí góc quay tương đối đạt yêu cầu khi độ quá điều chỉnh và thời gian đáp ứng nhỏ
+ Bộ điều khiển vận tốc góc chưa đạt yêu cầu do độ quá điều chỉnh rất lớn. Do vậy, bộ điều khiển này phải cần tiếp tục nghiên cứu và khắc phục
c) Điều khiển động cơ bước thực tế với vi điều khiển: Phần cứng:
- Arduino UNO
- Driver điều khiển động cơ A4988 - Breadboard
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 59
- Dây nối
Kết nối driver, động cơ bước vào Arduino như sau:
Code Arduino tham khảo
// Run a A4998 Stepstick from an Arduino UNO.
// Paul Hurley Aug 2015 - http://www.instructables.com/id/Drive- a-Stepper-Motor-with-an-Arduino-and-a-A4988-/
int x;
#define BAUD (9600)
void setup() {
Bùi Văn Mạnh – KT Cơ Điện tử 03-k60 Page 60
Serial.begin(BAUD);
pinMode(6,OUTPUT); // Enable pin - chân khởi động - nối vào GND sẽ giúp ta bật động cơ bước, nối vô VCC động cơ bước được thả ra. Nôm na: GND = servo.attach, VCC = servo.detach
pinMode(5,OUTPUT); // Step pin pinMode(4,OUTPUT); // Dir - pin digitalWrite(6,LOW); // Set Enable low }
void loop() {
digitalWrite(6,LOW); // Đặt Enable ở trạng thái LOW digitalWrite(4,HIGH); // Đặt Dir ở trạng thái HIGH Serial.println("Cho chay 200 steps (1 vong)"); for(x = 0; x < 200; x++) // Cho chay 1 vong