CHƯƠNG 4: KIỂM CHỨNG BẰNG THỰC NGHIỆM TRÊN MÔ HÌNH CẦN CẨU TREO 2D
4.1. Xây dựng bàn thí nghiệm
4.1.1. Mô hình vật lý
Cần cẩu được thiết kế với chiều cao là 2m, chiều rộng là 1.6 (m), chiều dài dây treo là 1.2 (m), tải trọng tối đa là 60 (kg). Động cơ truyền động là động cơ không đồng bộ ba pha tốc độ 60 (vòng/phút), công suất 90 (W), có hộp số giảm tốc. Động cơ không đồng bộ ba pha được ghép nối với một biến tần công nghiệp của hãng Omron loại 3G3JX, thiết bị đo tốc độ và vị trí xe là một encoder (E40S6-1024-3-T-24) có độ phân giải là 1024[P/R].
Mô hình hệ thống điều khiển cần cẩu treo trong phòng thí nghiệm được biểu diễn trong hình ảnh sau đây:
Hình 4.1: Hệ thống điều khiển và giám sát cần cẩu treo 2D
Hệ thống điều khiển và giám sát cần cẩu treo trong phòng thí nghiệm bao gồm các thiết bị sau đây:
104 (1) Hệ cần cẩu treo 2D
(2) Bộ điều khiển (3) Máy tính giám sát 4.1.2. Thiết kế phần cứng
Bộ điều khiển được thiết kế trên dòng vi điều khiển 32-bit STM32F407 của hãng STMicroelectronics, mạch phần cứng cụ thể được thể hiện trong hình 4.2:
Hình 4.2: Mạch phần cứng điều khiển
4.1.2.1. Vi điều khiển STM32F4
Bộ điều khiển được thiết kế trên dòng vi điều khiển 32-bit STM32F407 của hãng STMicroelectronics, với các thông số kỹ thuật chính như: 1-Mbyte bộ nhớ Flash, 192 Kbyte RAM, tần số hệ thống lên tới 168 MHz, điện áp cấp từ 1.8 đến 3.6V…
Dòng vi điều khiển ARM cũng hỗ trợ nhiều ngoại vi đa dụng bao gồm: Các cổng I/O đa dụng, bộ điều khiển ngắt ngoại, bộ điều khiển thời gian đa dụng và mở rộng, đồng hồ thời gian thực, 17 kênh timers/counter, 3 kênh ADC 12 bit, 2 kênh DAC 12-bit … cùng với ngoại vi giao tiếp như:
USART, SPI, I2C, CAN, USB, ETHERNET … 4.1.2.2. Cảm biến vị trí
Tốc độ và vị trí của xe con được đo bằng Encoder E40S6-1024-3-T-24, hỗ trợ 3 kênh A, B và Z.
Cảm biến có 5 dây kết nối, được mô tả trong bảng.
105
Màu dây Chức năng
Đen Đầu ra kênh A
Trắng Đầu ra kênh B
Da cam Đầu ra kênh Z
Xanh dương GND
Nâu VCC
Bảng 4.1: Dây kết nối của Encoder
Từ tín hiệu trả về từ Encoder, ta xác định được tốc độ của xe con theo công thức:
12 1024 pulse speed
T (4.1)
Trong đó, pulse là số xung ngắt cạnh xuống đọc được từ encoder, 1024 là độ phân giải (số xung/vòng quay), 12 (cm) là độ dịch chuyển ứng với một vòng quay của động cơ, T là thời gian lấy mẫu.
4.1.2.3. Cảm biến góc nghiêng
Cảm biến góc nghiêng gia tốc MPU6050 (hình 4.2) là một trong những giải pháp cảm biến dùng để đo chuyển động chi phí thấp, hiệu suất cao đầu tiên trên thế giới được thiết kế cho các ứng dụng công suất. MPU6050 có tới 6 trục cảm biến tích hợp trong một chip duy nhất bao gồm 3 trục con quay hồi chuyển tính vận tốc góc và 3 trục cảm biến gia tốc.
Hình 4.3: Cảm biến MPU6050
106
Bên cạnh đó, MPU6050 còn có một đơn vị chuyên xử lý tín hiệu do cảm biến thu thập và thực hiện các tính toán cần thiết. Điều này giúp giảm bớt đáng kể phần xử lý tính toán của vi điều khiển, cải thiện tốc độ xử lý và cho ra tín hiệu phản hồi nhanh hơn. Đây chính là điểm khác biệt đáng kể của MPU6050 so với các cảm biến gia tốc và gyro khác. Truyền nhận dữ liệu giữa cảm biến MPU6050 với vi điều khiển qua giao tiếp I2C.
Ngoài ra, trong mạch phần cứng còn có các khối như: mạch FT232 chuyển đổi “USB to COM”
để giao tiếp giữa vi điều khiển với chuẩn truyền thông RS232 của máy tính qua giao tiếp UART, mạch khuếch đại không đảo sử dụng IC LM358 có nhiệm vụ khuếch đại điện áp cấp tham chiếu cho biến tần để điều tốc động cơ, mạch rơ-le có nhiệm vụ đóng cắt để đảo chiều động cơ.
4.1.3. Thiết kế phần mềm
4.1.3.1. Bộ lọc Kalman cho cảm biến MPU6050
Bộ lọc Kalman, được Rudolf Kalman công bố năm 1960, là thuật toán đệ quy sử dụng chuỗi các giá trị đo lường, bị ảnh hưởng bởi có sai số và nhiễu, cùng các biến số ước đoán nhằm tăng độ chính xác so với việc sử dụng duy nhất một giá trị đo lường. Bộ lọc Kalman thực hiện phương pháp truy hồi đối với chuỗi các giá trị đầu vào bị nhiễu, nhằm tối ưu hóa giá trị ước đoán trạng thái của hệ thống.
Trước tiên, ký hiệu giá trị vận tốc góc và gia tốc đọc được từ cảm biến theo các trục x,y,z lần lượt như sau:gyro gyro gyro a a ax, y, z, x, y, z.
Do khi để yên MPU6050, giá trị của vận tốc góc đo được từ cảm biến khác không và dao động quanh giá trị cố định nên tồn tại giá trị offset. Để xác định giá trị offset ban đầu, ta lấy giá trị trung bình sau nhiều lần đo. Giá trị offset và giá trị góc nghiêng sẽ được cập nhật liên tục bằng bộ lọc Kalman. Cách tính góc nghiêng theo trục x bằng cảm biến gyro như sau:
_ ( ).
x gyro gyrox offset T
, với T là thời gian lấy mẫu. (4.2)
Tuy nhiên do cách tính tích phân nên giá trị góc thường bị trôi sau khoảng thời gian đủ lớn.
Cách tính góc nghiêng sử dụng gia tốc như sau:
x acc_ arctan x
y
a
a (4.3)
107
Phương pháp tính góc nghiêng theo gia tốc hoàn toàn không bị trôi nhưng khi có rung lắc mạnh thì giá trị góc tính sẽ có độ quá điều chỉnh lớn. Do đó, ta kết hợp hai phương pháp tính góc nghiêng trên và sử dụng bộ lọc Kalman để khắc phục nhược điểm.
Đầu tiên, ta xét mô hình trạng thái dự đoán và trạng thái đo lường của quá trình như sau:
1 1
. .
k k k k
k k k
x A x Bu w
z H x v
(4.4)
Trong đó: xk x offset
là các giá trị ra của bộ lọc Kalman, uk gyrox, zk x acc_ ,wk, vk là các nhiễu quá trình và nhiễu đo, tại bước lấy mẫu thứ k.
Các ma trận A, B, H được chọn như sau:
1 , , 1 0
0 1 0
T T
A B H
(4.5)
Lưu đồ thuật toán của bộ lọc Kalman được thể hiện trong hình:
Hình 4.4: Lưu đồ thuật toán bộ lọc Kalman
Trong đó, Kklà ma trận hệ số tỉ lệ được cập nhật, Q và R lần lượt là ma trận hiệp phương sai của nhiễu quá trình và nhiễu đo, ở đây ta coi là không đổi.
108
Bộ lọc Kalman cho ta giá trị đáng tin cậy của góc nghiêng, đồng thời hiệu chỉnh được giá trị offset của cảm biến trong quá trình vận hành.
4.1.3.2. Giao diện HMI
Giao diện HMI được thiết kế bằng ngôn ngữ C#, với các chức năng như kết nối cổng COM, gửi các thông số tới bộ điều khiển, thu thập số liệu, vẽ đồ thị giám sát các biến quá trình như tín hiệu điều khiển, chuyển động của xe và góc lắc của tải. Giao diện trên máy tính lấy được biểu diễn trong hình 4.5.
Hình 4.5: Giao diện HMI trên máy tính