Nhận xét:
+ Khi cho tín hiệu nhiễu vào hệ thống với độ lớn và thời gian lớn hơn khả năng cho phép của hệ thống thì sẽ khơng thể swing-up và cân bằng
+ Nhìn theo các đồ thị thì khi khơng thể đáp ứng các thơng số vị trí link 2, vận tốc góc link 2 và đáp ứng ngõ ra đều khơng thể duy trì trạng thái như trước khi có nhiễu tác động.
CHƯƠNG 5: THIẾT KẾ THI CƠNG MƠ HÌNH THỰC TẾ VÀ ĐIỀU KHIỂN CÂN BẰNG HỆ PENDUBOT
5.1 Giới thiệu
Chương này trình bày thiết kế cơ khí của hệ Pendubot các thiết kế board mạch điều khiển sử dụng vi điều khiển TMS320F28335 của Texas Instruments. Các khối xử lý và giải thuật thực hiện các bộ điều khiển đã được nghiên cứu ở chương 5. Kết quả điều khiển mơ hình thực tế của hệ Pendubot qua trình giữ cân bằng ở vị trí cân bằng ổn định hướng lên.
5.2 Thiết kế cơ khí của hệ Pendubot
Hệ Pendubot thực tế được thiết kế gồm hai thanh: Link1 (l1, m1, v1, I1) và link2 (l2, m2, v2, I2) chuyển động trong mặt phẳng thẳng đứng. Một động cơ DC 24V là cơ cấu động lực của thanh thứ nhất, động cơ được bắt chặt vào đế thép cố định. Thanh thứ hai bắt vào khớp quay tự do. Cả hai đều có gắn encoder đọc giá trị góc quay tuyệt đối về. Link1 và link2 được chế tạo bằng thanh nhôm đúc. Pendubot được thiết kế gọn, nhẹ, chắc chắn khi chuyển động.
Hình 5.2 Mơ hình Pendubot thực tế của đề tài
5.3 Mạch điều khiển hệ Pendubot
5.3.1 Vi điều khiển DSP TMS320F28335
Vi điều khiển DSP TMS320F28335 là một vi điều khiển khá mạnh có các ngoại vi phù hợp để đọc các giá trị phản hồi và điều khiển hệ Pendubot, đồng thời hoạt động ở tốc độ 150MHz có bộ tính tốn xử lý số thực (FPU-Foating point Unit) đáp ứng các nhu cầu tính tốn xử lý của các bộ điều khiển. Mặc khác dòng vi điều khiển C2000 này đã được matlab hỗ trợ lập trình liên kết khá trực quan, nên việc thiết kế lập trình các bộ điều khiển thuận tiện và nhanh chóng hơn, dễ sửa lỗi hơn.
Một số chức năng điển hình của chip TMS320F28335
+ Tần số hoạt động lên đến 150MHz, tương ứng chu kỳ máy là 6.67ns nên tốc độ xử lý rất nhanh.
+ CPU 32 bit, hỗ trợ tính năng tính tốn với số dấu chấm động độ chính xác đơn.
+ Có 6 kênh điều khiển DMA cho ADC, McBSP, ePWM, XINTF, SARAM + Bộ nhớ trên chip 256Kx16Flash, 34Kx16SARAM
+ Boot ROM 8Kx16
+ Có 18 ngõ ra điều khiển chê độ rộng xung, 8 timer 32bit/9 timer 16bit, 2 bộ giao tiếp encoder 32bit, 2 bộ ADC
5.3.2 Module vi điều khiển TMS320F2335 value module
Do việc thiết kế board mạch cho chip DSP khó, cần cài đặt các chuẩn điện áp và chống nhiễu tín hiệu phức tạp, nên để đơn giản trong luận văn này sử dụng TMS320F28335 value module.
Hình 5.5 Board điều khiển TMS320F28335
Module này gồm:
+ Một module được tích hợp MCU chính là con chip DSP TMS230F28335 + IC TPS767D310 cấp nguồn điện cho MCU 3.3v và 1.9v
+ IC REF2130 tạo điện áp chuẩn tham chiếu cho các bộ ADC + Thạch anh tạo dao động ngoại 30MHz theo chuẩn công nghiệp
+ Các chân cấp nguồn và tín hiệu được kéo ra header 120 chân dạng cắm, tụ lọc nhiễu các chân tín hiệu MCU đảm bảo đúng yêu cầu thiết kế.
+ Tích hợp cổng kết nối dạng debug và chương trình nạp chương trình chuẩn TI JTAG IEEE1149.1
+ Led báo nguồn, nút Reset và các vùng switch lựa chọn vùng địa chỉ trong bộ nhớ chứa chương trình khởi động (bằng cách hàn hoặc tháo các điện trở)
Hình 5.6 Hình vẽ chi tiết các chân, kích thước của value module
5.3.3 Board điều khiển đọc các giá trị Encoder và xuất xung PWM
Board điều khiển bao gồm mạch cấp nguồn cho các module MCU, UART- USB giao tiếp với máy tính. Các thành phần của board điều khiển chính được trình bày như sau:
5.3.3.1 Sơ đồ nguyên lý chân ra của DSP TMS 320F28335 value module
5.3.3.2 Sơ đồ nguyên lý khối đo dịng và áp đơng cơ để tính thơng số động cơ
Kết nối theo sơ đồ bên dưới để đo dòng và áp cấp cho động cơ. Rồi qua cổng đệm và tăng áp để đưa vào chân ADC của DSP và tính tốn các thơng số của động cơ như trong chương 3 có đề cập
Hình 5.8 Mạch đệm đo dịng áp cấp cho động cơ
5.3.3.2 Sơ đồ nguyên lý khối đọc xung encoder
Khối kết nối cấp nguồn và đọc tín hiệu từ encoder (Encoder conector) là khối có nhiệm vụ cấp nguồn cho các encoder và đọc xung đo về vi điều khiển. Các tín hiệu xung từ encoder đọc về được đưa qua một IC đệm để đệm điện áp 5v thành 3.3v giao tiếp với vi điều khiển, đồng thời nắn tín hiệu đọc về từ encoder giảm thiểu nhiễu.
Hình 5.9 IC đệm cho encoder
Các cảm biến encoder: Dùng để đọc tín hiệu góc quay vận tốc quay của hai thanh của hệ Pendubot, điện áp cấp vào 5Vdc, tuy có nhiễu dây ngõ ra nhưng trong luận văn này chỉ sử dụng ra 2 xung vng pha A, B để xác định vị trí góc và chiều quay, độ phân giải của encoder là 400 xung trên vịng quay.
Hình 5.10 Encoder ứng dụng xác định vị trí
5.3.3.3 Sơ đồ nguyên lý khối phát xung PWM:
Sơ đố khối ngõ ra của bộ phát xung PWM điều khiển động cơ DC: có nhiệm vụ đệm tín hiệu xung PWM ngõ ra từ TMS230F28335 value module có mức điện áp 3.3v thánh mức điện áp 5v để cấp cho mạch cầu H lái động cơ. Khối được thiết kế với hai bộ ngõ ra gồm xung DIR quy định chiều quay và xung PWM nhưng chỉ sử dụng 1 ngõ ra cho hệ Pendubot.
Hình 5.11 Sơ đồ nguyên lý mạch đệm phát xung điều khiển PWM
5.3.3.4 Sơ đồ nguyên lý khối giao tiếp với máy tính
Sơ đồ nguyên lý khối giao tiếp với máy tính: sử dụng IC giao tiếp FT232R chuyển đổi tín hiệu giao tiếp SCI/UART từ vi điều khiển thành giao tiếp USB để truyền tín hiệu lên máy tính.
5.3.3.5 Sơ đồ nguyên lý mạch chuyển đổi xung vi sai qua xung PNP
Hình 5.13 Sơ đồ chuyển đổi xung vi sai qua xung NPN
5.3.3.6 Khối cấp nguồn cho mạch điều khiển
Có nhiệm vụ cung cấp nguồn Dc 24v cho mạch cầu H lái động cơ. Để cung cấp đủ công suất cho DSP, 2 Encoder, các IC đệm và mạch giao tiếp máy tính
Hình 5.15 Sơ đồ mạch điều khiển trung tâm
Kết quả thi công board mạch điều khiển thực tế: Sau khi thi công và lắp đặt mạch
điều khiển đã hoạt động tốt, các giá trị encoder đo về chính xác khơng bị nhiễu. Xung PWM xuất ra chính xác, mạch giao tiếp với máy tính chạy ổn định.
5.3.4 Mạch driver cầu H lái động cơ
Hình 5.16 Sơ đồ nguyên lý mạch cầu H
Kết quả thi công thực tế của mạch cầu H
+ Board cầu H-Brige dùng IC kích MC33833 của hãng free scale
+ Phần công suất dùng 4 POWER MOSFETs IRF3205 có thơng số VDSS=55v, RDS 100KHz
+ Ngõ vào opto cách ly PWM+, PWM-, DIR+, DIR-
+ Sử dụng IC kích Fet chuyên dụng MC338833 của freescale nên cycle đạt 100% ưu điểm hơn so với IC kích fet khác.
+ Có led báo nguồn, báo xung và chiều.
Hình 5.17 Mạch cầu H thực tế
5.4 Thiết kế chương trình điều khiển Pendubot
5.4.1 Giới thiệu các khối điều khiển và thư viện hỗ trợ của simulink
Để thiết kế chương trình ta sử dụng thư viện Matlab/simulink hỗ trợ lập trình đổ xuống chip TMS320F28335, taget support packet C2000lib. Hỗ trợ tất cả các MCU trong dòng vi điều khiển C2000 của TI. Trước hết ta phải lập trình các khối bằng matlab/ simulink, sau đó một trình biên dịch sẽ hỗ trợ phát một project mã C liên kết với Code composer studio v3.3 (CCS v3.3), sau đó CCS sẽ làm nhiệm vụ là môi trường build chạy và debug chương trình.
Việc kết hợp thư viện Target For TI C2000 được tích hợp sẵn trong Simulink của Matlab với Code Composer Studio (CCS) của hãng Taxes Intrument cho phép người sử dụng dịch các chương trình Simulink sang ngơn ngữ C chạy trên các chip DSP C2000 của TI như C280x fixed point DSP, C281x fixed point DSP, C28x3x floating point DSP, …
Hình 5.19 Thư viện target Preferences
Các module ngoại vi xuất nhập I/O, bộ đọc xung encoder QEP, bộ điều chế
độ rộng xung PWM, bộ chuyển đổi ADC, các khối truyền I2C, SCI, CAN của DSP
5.4.2 Sơ đồ giải thuật của chương trình điều khiển
Bắt đầu
Khởi tạo hoạt động cho các thiết bị ngoại
vi
Đọc xung phát từ encoder 1 và
encoder 2
Luật điều khiển cân bằng
Yes
No
|q1 - π/2|< 0.35 & |q1+q2-π/2|< 0.5
Luật điều khiển swing up
Hình 5.21 Sơ đồ giải thuật điều khiển swing up và cân bằng Pendubot
Giải thích sơ đồ giải thuật điều khiển Pendubot:
Đọc xung phát trên encoder 1 để biết q1 (góc giữa link 1 và trục hoành)
Đọc xung phát trên encoder 2 để biết q2 (góc giữa link 2 và phương của link 1) Nếu góc link 1 lệch với phương thẳng đứng một góc bé hơn 0.35 rad (~20º) và góc link 2 lệch với phương link 1 một góc bé hơn 0.15 rad (~8.6 º) thì sẽ sử dụng bộ điều khiển cân bằng để điều khiển Pendubot nếu ngược lại thì sẽ sử dụng bộ điều khiển Swing-up để điều khiển Pendubot.
Mỗi vòng điều khiển trên được thực hiện với chu kỳ được chọn là 10ms để đảm bảo các yêu cầu đo đạt các tín hiệu phản hồi. Việc chỉnh chu kỳ điều khiển được thực hiện trong mơ hình lập trình simulink như sau:
+ Vào Simulation -> Configuration Parameters …
+ Trong mục solver option chọn type (Fixed-step), solver (discrete (no continuous states)), fixed-step size chính là chu kỳ điều khiển của bộ điều khiển được tính bằng giây (ở đây tác giả chọn là 0.01 tức 10ms)
Hình 5.22 Thiết lập chu kỳ điều khiển cho bộ điều khiển trong simulink
5.4.3 Khối điều khiển hệ Pendubot thực tế
Đây là khối chương trình thực hiện việc điều khiển động cơ và đo đạc tín hiệu phản hồi từ hệ thống mơ hình hệ Pendubot thực tế. Bao gồm khối Motor control module phát xung điều khiển động cơ và khối Encoder module đọc các tín hiệu encoder về và xử lý thành tín hiệu vị trí góc và vận tốc góc.
5.4.3.1 Khối đọc xung (encoder module)
Đây là khối xử lý đọc tín hiệu phản hồi của Pendubot, nó làm nhiệm vụ đọc xung của hai encoder về xử lý thành các tín hiệu vị trí góc và vận tốc góc của 2 thanh (link1 và link 2) để cung cấp cho bộ điều khiển. Các ngoại vi đọc xung encoder được sử dụng ở mode x4 để tăng độ phân giải tín hiệu.
Khối đọc encoder link1:
Bao gồm bộ ngoại vi đọc encoder đọc vị trí xung về sau đó được sử lý tín hiệu góc link đo được sau đó đưa qua bộ xử lý từ xung thành giá trị góc radian.
Hình 5.23 Khối đọc encoder link1
Khối đọc encoder link2:
Bao gồm bộ ngoại vi đọc encoder 2 tích hợp trong động cơ đọc vị trí xung về sau đó được đưa qua bộ xử lý từ xung thành giá trị góc radian. Do góc q2 là góc tương đối giữa link1 và link2 nên các giá trị đọc về phải được tính bằng cách lấy giá trị encoder 2 trừ đi giá trị encoder1.
Hình 5.24 Khối đọc encoder link2
5.4.3.2 Khối xử lý điều khiển lái động cơ
Là khối có nhiệm vụ chuyển giá trị ngõ ra của bộ điều khiển thành giá trị xung PWM để cấp cho mạch cầu H lái động cơ.
+ Khối ePWM là khối xuất tín hiệu PWM của board điều khiển
+ Khối Digital Output là khối xuất tín hiệu ngõ ra của chân I/O xung quy định chiều quay của động cơ
+ Khối U control được lập trình với đoạn code xử lý
5.4.4 Khối truyền tín hiệu hiển thị lên máy tính (SCI transfer data to PC)
Khối này nhận tín hiệu từ các khối đọc xung và khối điều khiển giải thuật từ đó chuyển các giá trị nhận được thành mã ASCII truyền lên máy tính thơng qua
giao thức SCI/UART. Các giá trị truyền thành dạng frame truyền chuỗi và kết thúc ký tự xuống dịng.
Trên máy tính một chương trình windows form sẽ nhận các giá trị được gửi lên từ board điều khiển, hiển thị và vẽ đồ thị online. Vì các yêu cầu ngắt trong chương trình windows form nên việc hiển thị thực hiện với chu kỳ 50ms, nhưng các đồ thị đáp ứng và phản ánh đúng các giá trị đo đạc.
5.4.5 Chương trình điều khiển swing up dung phương pháp điều khiển hồi tiếp tuyến tính hóa riêng phần và giữ cân bằng hệ Pendubot dùng phương pháp điều khiển LQR
5.4.5.1 Chương trình thực tế của bộ điều khiển
Bộ điều khiển hồi tiếp tiếp tuyến tính hóa riêng phần sẽ điều khiển hệ
pendubot swing-up từ vị trí cân bằng hướng xuống / 2 0 0 0 tới vị trí cân
bằng khơng ổn định hướng lên / 2 0 0 0
Bộ điều khiển LQR sẽ điều khiển hệ pendubot giữ cân bằng nếu các góc nằm
trong tầm điều khiển sau: 1
7 7 q và 2 7 7 q . Nếu các góc nằm ngồi
giới hạn trên thì bộ điều khiển sẽ cho hệ thống Swing-up lại. Xây dựng mơ hình điều khiển hệ thống đã rời rạc hóa như sau:
Hình 5.25 Chương trình điều khiển hệ Pendubot thực tế
Trong đó các khối được thể hiện trong mơ hình có ý nghĩa như sau:
+ Khối eQEP1, có tác dụng đọc encoder gắn trên đọc cơ. Do encoder đọc
được từ giá trị 0 đến giá trị tràn do ta đặt rồi quay về giá trị 0 (khơng có giá trị <0 để xác định được góc âm) nên ta phải biến đổi quay về giá trị encoder đọc được giá trị góc tương ứng với thực tế. Do encoder loại 400 xung/vòng, sau khi đọc từ DSP, số xung đọc được mỗi vòng là 1600 xung. Ở đây học viên lấy giá trị thẳng đứng là
2
, chiều dương của góc lệch của link1 cùng chiều kim đồng hồ. Như vậy ta biến đổi sao cho vị trí thẳng đứng hướng lên là
2
, quaytheo chiều kim đồng hồ thì giá trị dương, ngược kim đồng hồ là giá trị âm. Việc biến đổi này được thực hiện trong
khối xu ly goc link1. Sau khi tín hiệu từ encoder đưa về được xử lý xong, chuyển
qua bộ “goc link1 thanh radian” chuyên đổi sang dạng radian (vì trong định dạng của ta là radian).
+ Khối eQEP2 có tác dụng đọc encoder gắn trên link2.
+ Các khối Embedded matlab “read2” có nhiệm vụ biến các giá trị góc lệch link1, link2, điện áp đặt vào động cơ thành các ký tự mã ASCII truyền lên cổng COM của máy tính, dùng cho việc thu thập dữ liệu
+ Các khối Embedded matlab “LQR balance control”, “Swing up Feedback control” “lua chon bo dieu khien” chứa chương trình giải thuật thực hiện xuất tín hiệu điều khiển động cơ khi có tín hiệu tương ứng. Các tín hiệu của khối này là:
- Ngõ vào (q1, q1_dot, q2, q2_dot) tương tướng với các giá trị biến trạng thái hệ thống.
- Ngõ ra “u” giá trị độ lớn độ rộng xung để điều khiển động cơ quay chậm hay nhanh.
- Ngõ ra “sign u” giá trị chiều quay của động cơ. Nếu “sign u” = 0 thì động cơ quay ngược kim đồng hồ. Ngược lại nếu “sign u” = 1 thì động cơ quay cùng chiều kim đồng hồ
- Ngõ ra “dien ap gui len” cung cấp giá trị điện áp đặt vào động cơ được chuyển đổi lên máy tính để thu thập giá trị của hệ thống.
+ Khối “SCI Transmit” có nhiệm vụ đưa các giá trị góc lệch của link1 và link2, vận tốc góc link1, link2 (tính theo rad), điện áp cấp cho động cơ (tính theo v). Tất cả các giá trị tham số được thu thập thông qua cổng COM.