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.
+ 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 quá 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
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” IEEE, Trans. Autom.Control, vol 45, no 4, pp 725-729, 2000.
[2] M.Zhang and T.J.Tarn, “Hybrid control of Pendubot”, IEEE/ASME Trans. Mechatronics, vol 7, no. 1, pp79-86, 2002.
Sách tham khảo:
[3] Dương Hoài Nghĩa. Điều khiển hệ thống đa biến, Nhà xuất bản đại học Quốc gia thành phố Hồ Chí Minh, 2007.
[4] Nguyễn Thị Phương Hà. Lý thuyết điều khiển hiện đại, Nhà xuất bản đại học
Quốc gia thành phố Hồ Chí Minh, 2009.
[5] Nguyễn Doãn Phước. Lý thuyết điều khiển nâng cao, Nhà xuất bản khoa học và kỹ thuật, 2007.
Luận văn tham khảo:
[6] “Luận văn thạc sĩ: Xây dựng bộ điều khiển nhúng tuyến tính hóa vào ra cho hệ xe con lắc ngược”, (2011) HVTH: Nguyễn Văn Đông Hải, HDKH: PGS.TS Dương Hoài Nghĩa
[7] “Luận văn thạc sĩ: Tối ưu hóa điều khiển PID cho hệ Ball & Beam dùng giải thuật Genetic Algorithm”, (2015) HVTH: Nguyễn Sơn Hà, HDKH: PGS.TS Trần Thu Hà, Trường Đại Học Sư Phạm Kỹ Thuật thành phố Hồ Chí Minh.
[8] “Luận văn thạc sĩ: Điều khiển cân bằng hệ Pendubot”, (2016) HVTH: Phạm Văn Lợi, HDKH: TS Trương Đình Nhơn, Trường Đại Học Sư Phạm Kỹ Thuật thành phố Hồ Chí Minh.
Nguồn Internet:
[9] Texa instruments, TMS320F28335 digital signail controllers. Data manual.