Phương trình động lực học của hệ tay máy đôi – đối tượng trong mô hình thực nghiệm cũng có dạng như phương trình (3.1). Nhưng các thành phần trong phương trình động lực học của tay máy đôi có dạng khác, cụ thể như sau:
Các thành phần trong phương trình động lực học của tay máy thứ nhất. Ma trận H1(q1); C1(q1,q˙1); g1(q1); J01 xác định được như sau:
H1(q1) = H11 H12 H21 H22 ;C1(q1, q˙1) = C11 C12 C21 C22 ;g1(q1) = g11 g22 ;J01 = J111 J121 J211 J221 .
Các thành phần trong ma trận quán tính H1(q1) được xác định: H11 = 2 X j=1 J1j + 2 X j=1 m1j lg21j +m12d211 + 2m12lg12d11cosθ12 H12 =H21 = J12 +m12lg212+m12lg12d11cosθ12 H22 =J12 +m12lg212.
Các thành phần trong ma trận tương hỗ và ly tâm C1(q1,q˙1) được xác định: C11 =− m12lg12d11sinθ12θ˙12
C12 =− m12lg12d11sinθ12( ˙θ11 + ˙θ12) C21 = m12lg12d11sinθ12θ˙11
C22 = 0
Các thành phần trong vectơ trọng trường g1(q1) được xác định: g11 =(m11glg11+m12gd11)cosθ11 +m12glg12cos(θ11 +θ12) g22 =m12glg12cos(θ11 +θ12)
Các thành phần trong ma trận Jacoby J01 được xác định: J111 =−d11sinθ11 − d12sin(θ11 +θ12) J121 =−d12sin(θ11 +θ12)
J211 = d11cosθ11 +d12cos(θ11 +θ12) J221 = d12cos(θ11 +θ12)
Các thành phần trong phương trình động lực học của tay máy thứ hai được xác định tương tự như các thành phần trong phương trình động lực hoạc của tay máy thứ nhất.
Để phục vụ cho kiểm chứng của hệ thống thực nghiệm tay máy đôi- đối tượng, kết quả mô phỏng thuật toán điều khiển tựa mô hình với hệ tay máy đôi – đối tượng, mỗi tay máy có hai bậc tự do sẽ được trình bày trong phần tiếp theo. 5.3. Mô phỏng với hê tay máy đôi – đối tượng, mỗi tay máy có hai bậc tự do
a) Điều khiển hỗn hợp lực, vị trí chuyển động theo trục x:
Kết quả mô phỏng điều khiển hỗn hợp lực, vị trí chuyển động theo trục x của đối tượng sử dụng thuật toán điều khiển tựa mô hình. Với quỹ đạo chuyển động của đối tượng dọc theo trục x, khi vị trí theo trục y được giữ nguyên được thể hiện trong Hình 5.3 - Hình 5.5
0 5 10 15 20 0.05
0.1 0.15
a) Quỹ đạo chuyển động
0 5 10 15 20 -2 0 2 4 6 8 10 12 10 -4
b) Sai số vị trí của đối tượng Hình 5.3: Quỹ đạo chuyển động của đối tượng dọc theo trục x.
0 5 10 15 20 -0.9 -0.8 -0.7 -0.6 -0.5 -0.4 -0.3 -0.2 -0.1 0 0.1
Hình 5.4: Mô men của các khớp của tay máy thứ nhất.
0 5 10 15 20 -0.1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9
Hình 5.5: Mô men của các khớp của tay máy thứ nhất.
hợp lực và vị trí theo trục x cho kết quả tốt, sai lệch vị trí theo trục x lớn nhất là 0,013%. Đối tượng được chuyển động tới vị trí mong muốn với sai lệch tĩnh rất nhỏ gần như bằng không. Tại vị trí cân bằng mô men tác động lên các khớp của hai tay máy có giá trị không đổi, điều đó chứng tỏ đối tượng được giữ ổn định.
b) Điều khiển hỗn hợp lực, vị trí chuyển động theo trục y:
Kết quả mô phỏng điều khiển hỗn hợp lực, vị trí chuyển động theo trục y của đối tượng sử dụng thuật toán điều khiển tựa mô hình. Với quỹ đạo chuyển động của đối tượng dọc theo trục y , khi vị trí theo trục x được giữ nguyên được thể hiện trong Hình 5.6 0 2 4 6 8 10 12 0.47 0.48 0.49 0.5 0.51 0.52 0.53 0.54 0.55 0.56
a) Quỹ đạo chuyển động
0 2 4 6 8 10 12 -0.5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 10 -3
b) Sai số vị trí của đối tượng Hình 5.6: Quỹ đạo chuyển động của đối tượng dọc theo trục y.
Từ kết quả mô phỏng Hình 5.6 có thể thấy rằng bộ điều khiển hỗn hợp lực và vị trí theo trục y cho kết quả tốt, sai lệch vị trí theo trục y lớn nhất là 0,038%. Đối tượng được chuyển động tới vị trí mong muốn với sai lệch tĩnh rất nhỏ 0,01%. Tại vị trí cân bằng đối tượng được giữ ổn định.
c) Điều khiển hỗn hợp lực, hướng chuyển động của đối tượng:
Kết quả mô phỏng điều khiển hỗn hợp lực, hướng chuyển động của đối tượng sử dụng thuật toán điều khiển tựa mô hình. Với quỹ đạo hướng chuyển động của đối tượng được thể hiện trong Hình 5.7
0 5 10 15 20 25 30 35 40 45 50 0 1 2 3 4 5 6 7 8 9 10
a) Quỹ đạo chuyển động
0 5 10 15 20 25 30 35 40 45 50 -0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18
b) Sai số góc xoay của đối tượng Hình 5.7: Quỹ đạo hướng chuyển động của đối tượng.
Từ kết quả mô phỏng Hình 5.7 có thể thấy rằng bộ điều khiển hỗn hợp lực và hướng chuyển động cho kết quả tốt, sai lệch hướng lớn nhất là 1,6%. Đối tượng được chuyển động tới hướng mong muốn với sai lệch tĩnh rất nhỏ 0,2%. Tại vị trí cân bằng đối tượng được giữ ổn định.
d) Điều khiển hỗn hợp lực, vị trí chuyển động theo trục x, hướng chuyển
động của đối tượng:
Kết quả mô phỏng điều khiển hỗn hợp lực, vị trí chuyển động theo trục x, hướng chuyển động của đối tượng sử dụng thuật toán điều khiển tựa mô hình. Với quỹ đạo vị trí và hướng chuyển động của đối tượng được thể hiện trong Hình 5.8 – Hình 5.9 0 5 10 15 20 25 0.09 0.095 0.1 0.105 0.11 0.115 0.12 0.125 0.13
a) Quỹ đạo chuyển động
0 5 10 15 20 25 -1 -0.5 0 0.5 1 1.5 2 2.5 3 10 -4
b) Sai số vị trí của đối tượng Hình 5.8: Quỹ đạo hướng chuyển động của đối tượng dọc theo trục x.
0 5 10 15 20 25 -1 0 1 2 3 4 5 6 7 8
a) Quỹ đạo chuyển động
0 5 10 15 20 25 -0.1 -0.08 -0.06 -0.04 -0.02 0 0.02 0.04 0.06 0.08 0.1
b) Sai số góc xoay của đối tượng Hình 5.9: Quỹ đạo hướng chuyển động của đối tượng.
Từ kết quả mô phỏng Hình 5.8 – Hình 5.9 có thể thấy rằng bộ điều khiển hỗn hợp lực, vị trí theo trục x, hướng chuyển động cho kết quả tốt. Sai lệch vị trí theo trục x lớn nhất là 0,075%, hướng lớn nhất là 1%. Đối tượng được chuyển động tới vị trí, hướng mong muốn với sai lệch tĩnh rất nhỏ lần lượt là 0,002% và 0,25%. Tại vị trí cân bằng đối tượng được giữ ổn định.
e) Điều khiển hỗn hợp lực, vị trí chuyển động theo trục y, hướng chuyển động của đối tượng:
Kết quả mô phỏng điều khiển hỗn hợp lực, vị trí chuyển động theo trục y, hướng chuyển động của đối tượng sử dụng thuật toán điều khiển tựa mô hình. Với quỹ đạo vị trí và hướng chuyển động của đối tượng được thể hiện trong Hình 5.10 – Hình 5.11 0 5 10 15 20 25 30 0.525 0.53 0.535 0.54 0.545 0.55 0.555 0.56 0.565 0.57
a) Quỹ đạo chuyển động
0 5 10 15 20 25 30 -12 -10 -8 -6 -4 -2 0 10-4
b) Sai số vị trí của đối tượng Hình 5.10: Quỹ đạo chuyển động theo trục y của đối tượng.
0 5 10 15 20 25 30 Time [s] -1 0 1 2 3 4 5 6 7 8
a) Quỹ đạo chuyển động
0 5 10 15 20 25 30 -0.2 -0.15 -0.1 -0.05 0 0.05
b) Sai số góc xoay của đối tượng Hình 5.11: Quỹ đạo hướng chuyển động của đối tượng.
Từ kết quả mô phỏng Hình 5.10 – Hình 5.11 có thể thấy rằng bộ điều khiển hỗn hợp lực, vị trí theo trục y, hướng chuyển động cho kết quả tốt. Sai lệch vị trí theo trục y lớn nhất là 0,08%, hướng lớn nhất là 1,8%. Đối tượng được chuyển động tới vị trí, hướng mong muốn với sai lệch tĩnh rất nhỏ lần lượt là 0,025% và 0,5%. Tại vị trí cân bằng đối tượng được giữ ổn định.
f) Điều khiển hỗn hợp lực, vị trí chuyển động theo trục x và trục y của
đối tượng:
Kết quả mô phỏng điều khiển hỗn hợp lực, vị trí chuyển động theo trục x và trục y của đối tượng sử dụng thuật toán điều khiển tựa mô hình. Với quỹ đạo vị trí của đối tượng được thể hiện trong Hình 5.12 – Hình 5.13
Từ kết quả mô phỏng Hình 5.12 – Hình 5.13 có thể thấy rằng bộ điều khiển hỗn hợp lực, vị trí cho kết quả tốt. Sai lệch vị trí theo trục x, trục y lớn nhất lần lượt là 0,02% và 0,03%. Đối tượng được chuyển động tới vị trí mong muốn
0 5 10 15 20 25 30 35 40 0.08 0.09 0.1 0.11 0.12 0.13 0.14 0.15 0.16 0.17
a) Quỹ đạo chuyển động
0 5 10 15 20 25 30 35 40 -2 0 2 4 6 8 10 12 14 16 10 -4
b) Sai số vị trí của đối tượng Hình 5.12: Quỹ đạo chuyển động theo trục x của đối tượng.
0 5 10 15 20 25 30 35 40 0.525 0.53 0.535 0.54 0.545 0.55 0.555 0.56 0.565 0.57
a) Quỹ đạo chuyển động
0 5 10 15 20 25 30 35 40 -2 0 2 4 6 8 10 12 10-4
b) Sai số vị trí của đối tượng Hình 5.13: Quỹ đạo chuyển động theo trục y của đối tượng.
với sai lệch tĩnh rất nhỏ lần lượt là 0,002%. Tại vị trí cân bằng đối tượng được giữ ổn định.
Từ kết mô phỏng Hình 5.3 – Hình 5.13 thấy rằng bộ điều khiển làm việc tốt và ổn định. Đối tượng được giữ chặt và chắc chắn trong quá trình di chuyển, kết quả có được quỹ đạo vị trí và hướng của đối tượng đã bám với quỹ đạo đặt. Sai lệch tĩnh nhỏ, quỹ đạo chuyển động dọc theo trục x sai lệch tĩnh nhỏ hơn 0,08%, quỹ đạo chuyển động dọc theo trục y sai lệch tĩnh nhỏ hơn 1%, quỹ đạo hướng chuyển động sai lệch tĩnh nhỏ hơn 1%, độ quá điều chỉnh nhỏ dưới 1%. Hình 5.3b – Hình 5.13b cho thấy tại vị trí cân bằng sai lệch tĩnh là hằng số, chứng tỏ đối tượng được giữ ổn định tại vị trí cân bằng.
Phần tiếp theo, xây dựng hệ thống thực nghiệm và thử nghiệm hệ thống theo kịch bản được mô phỏng như phần 5.3 sẽ được trình bày.
5.4. Cấu trúc và hoạt động của hệ thống thực nghiệm
Hình 5.14: Sơ đồ khối hệ thống thực nghiệm.
FDDI: Fiber Distributed Data Interface (dữ liệu phân tán sợ quang). Thuật toán được viết và xây dựng trên Matlab/Simulink và sử dụng phần mềm Control desk biên dịch. Lúc này các tín hiệu từ bộ điều khiển qua card DS1103, qua hộp giao diện để đưa tín hiệu điều khiển tới driver của động cơ servo thông qua một board kết nối. Tín hiệu từ driver của động cơ sẽ điều khiển chuyển động cho động cơ servo từ đó điều khiển chuyển động các khớp của tay máy đôi.
được chuyển tới Matlab/Simulink thông qua hộp giao diện của DS1103 (qua cổng INC), trên phần mềm Matlab/Simulink tín hiệu từ encoder sẽ được chuyển thành tín hiệu góc xoay của các khớp, và phản hồi lại cho bộ điều khiển.
Ví trí của đối tượng sẽ được tính gián tiếp qua góc xoay của các khớp. Còn hướng (góc xoay) của đối tượng sẽ được đo bằng cảm biến MPU6050. Vị trí, hướng của đối tượng sẽ được phản hồi lại cho bộ điều khiển.
5.4.1. Các thiết bị phần cứng
Để điều khiển chuyển động cho các khớp của tay máy đôi sử dụng bốn động cơ servo điều khiều cho bốn khớp độc lập. Động cơ servo sử dụng là động cơ xoay chiều và của hãng Misubishi, thông số, mã hiệu, đặc tính của động cơ được trình bày trong phần dưới đây, chi tiết được trình bày trong phụ lục.
a) Động cơ AC servo Misubishi HC-KFS43:
Điện áp cung cấp: 220VAC. Công suất: 400W.
Tốc độ vòng quay: 3000 vòng/phút.
Encoder 17 bit, tốc độ phân giải 131072 xung/vòng. Momen xoắn: 1.3 Nm, momen xoắn lớn nhất: 3.8 Nm.
Động cơ AC Servo HC-KF43B tương thích với Servo Amplifiler: MR- J2S-40A/B/CP/CL.
b) Driver MR-J2S-40A:
Để điều khiển cho động cơ servo sử dụng driver MR-J2S-40A, các thông số của MR-J2S-40A như sau:
Điện áp nguồn cấp: 3-pha 200 VAC hoặc 1-pha 230 VAC. Điện áp ra: 3 pha 200-230 V.
Công suất: 400 W. Loại servo Amplifier.
Phương pháp điều khiển: Điều chế độ rộng xung PWM, điều khiển dòng điện.
c) Hệ thống cảm biến phản hồi góc xoay của đối tượng:
Để xác định góc xoay của đối tượng, phản hồi về cho bộ điều khiển. Tác giả dùng cảm biến MPU6050 (module GY-521) kết nối với Arduino và gửi tín hiệu phản hồi về card DS1103 bằng giao tiếp analog.
Hệ thống phản hồi góc của đối tượng được xây dựng xung quanh cảm biến MPU6050, vi điều khiển Arduino Uno R3 sẽ đọc tín hiệu cảm biến trả về thông qua giao thức I2C, sau đó nhờ DAC MCP4822 giao tiếp với arduino bằng SPI xuất tín hiệu analog tới DS1103. Arduino hoạt động ở tần số mặc định 8Mhz, giao tiếp I2C họa động với tần số 400kHz, giao tiếp SPI mặc định 4Mhz.
5.4.2. Phần mềm điều khiển
Tác giả sử dụng card điều khiển thời gian thực dSPACE 1103 để kiểm chứng thuật toán điều khiển đã được đề xuất trong chương 3, vừa sử dụng được chương trình điều khiển đã xây dựng mô phỏng trên Matlab/Simulink, vừa không phải xây dựng và viết lại chương trình điều khiển khi sử dụng các chip vi điều khiển để thực hiện.
Bộ DSP - DS1103 là sản phẩm xử lí tín hiệu số do hãng dSPACE của Đức sản xuất, được tích hợp gồm: Phần cứng card DS1103, phần mềm Controldesk và Matlab/Simulink để tạo thành bộ điều khiển hoàn chỉnh, có khả năng tính toán tốc độ cao trong điều khiển theo thời gian thực và thao tác trực tiếp hệ thống.
Quá trình điều khiển là sự liên hệ mật thiết giữa các khối phần cứng và phần mềm. Sự luân chuyển các tham số giữa các khối này với nhiệm vụ được phân định như sau:
+ Phần mềm Matlab/Simulink: Thiết kế thuật toán điều khiển.
+ Phần mềm Controldesk: biên dịch, quan sát các tham số tác động, các tín hiệu đầu ra thực nghiệm.
+ Card DS1103: Giao tiếp, truyền các tín hiệu tương tác trực tiếp giữa máy tính và các thiết bị ngoại vi.
Card DS1103 được tích hợp vi xử lí, ADC, DAC tốc độ cao. DS1103 đo lường và tạo ra các tín hiệu tương tự, tín hiệu số đơn bit qua các cổng I/O, tín hiệu số đa bít từ các bộ encoder, tạo xung điều khiển PWM, truyền thông trong hệ thống thông tin công nghiệp thông qua các chuẩn giao tiếp RS – 232 và Can.
b) Phần mềm Controldesk
Controldesk dSPACE là phần mềm đi kèm với card DS1103 để thực hiện được thực nghiệm. Phần mềm cung cấp tất cả các chức năng như trình biên dịch, các công cụ để đưa được tín hiệu điều khiển cho động cơ, xây dựng giao diện, hiển thị lên màn hình. Người dùng có thể mô tả các chương trình điều khiển, cũng như mô tả động lực học của hệ thống thông qua Simulink của Matlab. Controldesk liên hệ với Matlab để nhận dữ liệu vào thực hiện chương trình. 5.4.3. Board kết nối
Để kết nối giữa phần cứng và phần mền (động cơ servo, driver và DS1103) sử dụng một board kết nối với mục đích:
- Cách ly cho bộ điều khiển DS1103 với phần cứng (phần mạch lực). - Linh hoạt khi kết nối, linh hoạt chế độ làm việc của động cơ.
Momen sau khi được tính từ bộ điều khiển DS1103 được đưa tới driver để điều khiển động cơ thông qua board kết nối.
Chuyển chế độ điều khiển từ điều khiển momen sang điều khiển vị trí và ngược lại. Tín hiệu điều khiển động cơ quay thuận, quay ngược được lấy từ tín