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
đều đọc hỗ trợ đầy đủ và cấu hình dễ dàng trực quan.
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.
+ Khối ePWM là khơi tạo xung. Việc điều xung này có tác dụng điều khiển động cơ thay đổi moment để cân bằng link1 và link2 ở tại vị trí cân bằng trên.
5.5 Kết quả đo được thực tế khi dùng bộ LQR.
Kết quả thu được khi cho hệ thống chạy mơ hình thật. Ta thu được dữ liệu sau.
Hình 5.26 Đáp ứng thực tế của góc link1
Hình 5.28 Đáp ứng ngõ ra của bộ điều khiển
CHƯƠNG 6: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN CỦA ĐỀ TÀI CỦA ĐỀ TÀI
6.1 Kết quả đạt được
Trong chuyên đề này học viên đã thực hiện các công việc sau:
+ Hệ thống đạt ổn định trong khoảng rộng. Tuy nhiên, khi sử dụng LQR, nếu đạt được thơng số gần phù hợp thì hệ thống sẽ đi về ổn định. Cịn ở phương pháp tuyến tính hóa vào ra rồi sử dụng LQR thì trừ phi chọn được thơng số rất tốt ta mới đảm bảo hệ thống đứng im tại vị trí cân bằng. Ngược lại hệ thống sẽ dao động nhẹ quanh điểm cân bằng.
+ Việc tuyến tính hóa vào ra đối với hệ thống vẫn giữ nguyên các đặc trưng phi tuyến của hệ thống. Do đó khoảng ổn định cân bằng của hệ thống rộng hơn nhiều so với chỉ đơn thuần tuyến tính hóa hệ thống quanh điểm làm việc rồi điều khiển LQR.
+ Nghiên cứu và áp dụng giải thuật hối tiếp tuyến tính hóa riêng phần để giúp hệ Pendubot tự swing-up lên vị trí cân bằng.
+ Nghiên cứu họ vi điều khiển C2000 của Texas Instrument điển hình là chip điều khiển TMS320F28335, các thư viện và cách lập trình bằng ngơn ngữ C và ngôn ngữ matlab/Simulink để phục vụ cho việc thi công thực tế các bộ điều khiển.
+ Lập trình bộ điều khiển giữ cân bằng LQR nhúng giải thuật thực tế trên board TMS320F28335 nhằm điều khiển giữ thăng bằng mơ hình cơ khí thực tế của hệ Pendubot.
+ Mơ hình thực nghiệm đã tự swing-up lên vị trí cân bằng và giữ cân bằng ở đó. Đặc biệt khi tác động một lực (nhiễu) quá mạnh làm hệ thống ra khỏi vùng mà hệ thống điều khiển được thì theo qn tính hệ thống sẽ quay tự do, nếu trở lại được vị trí mà hệ thống điều khiển được thì hệ thống vẫn hoạt động và điều khiển cân bằng
+ Góc θ1 (góc nhọn giữa link 1 và trục theo phương thẳng đứng) tối đa mà hệ
vẫn trở về được trạng thái cân bằng là: 1
7 7
+ Góc θ2 (góc nhọn giữa link 1 và link 2) tối đa mà hệ vẫn trở về được trạng
thái cân bằng là: 2
7 7
6.2 Giới hạn của đề tài
+ Link 1, link 2 và vật nặng phải được cố định và có khối lượng khơng đổi nếu không hệ thống sẽ không thể hoạt động được.
+ Khi góc θ1 (góc nhọn giữa link 1 và trục theo phương thẳng đứng) là:
1
7 7
và góc θ2 (góc nhọn giữa link 1 và link 2) là: 2
7 7
thì hệ sẽ
khơng tự reset về vị trí ban đầu là thẳng đứng hướng xuống để Swing up lên lại. + Góc nhiễu tối đa của hệ là
7 7 6.3 Đóng góp của đề tài:
+ Luận văn đã đưa ra được cái nhìn tổng quan về các phương pháp điều khiển của các nhà nghiên cứu trước đó.
+ Luận văn đưa ra phương pháp điều khiển cho q trình giữ cân bằng tại vị trí cân bằng ngược.
6.4 Hướng phát triển của đề tài:
+ Phần cứng: Xây dựng con lắc đôi (trên link 2 sẽ gắn thêm một con lắc tự do nữa)
+ Phần điều khiển:
Nghiên cứu giải thuật điều khiển Swing _up phù hợp hơn.
Nghiên cứu khả năng ổn định của hệ thống khi có vật nặng có khối
lượng thay đổi trên đỉnh của link 2.
Sử dụng các phương pháp nhận dạng bằng Neuron để nhận dạng các
hàm của hệ thống phi tuyến. Từ đó ta sẽ tuyến tính hóa vào-ra và điều khiển cho hệ phức tạp hơn.
TÀI LIỆU THAM KHẢO
Bài báo khoa học:
[1] I. Fantoni, R. Lozzano, and M. Spong, “Energy Based control of the Pendubot”