Luật điều khiển [1.11] đƣợc thiết kế trên Matlab/Simulink nhƣ Hình 3.1. Các biến trạng thái x và theta đƣợc đo trực tiếp từ mô hình sử khối QEI của vi điều khiển ARM. Hai biến trạng thái này sau đó đƣợc đƣa qua hai khối lấy vi phân rời rạc để tạo ra đƣợc vi phân của biến trạng thái x và theta. Ngõ ra của thuật toán đƣợc khuếch đại ku lần để phù hợp với đối tƣợng điều khiển. Ý nghĩa của tín hiệu ngỏ ra là tốc độ quay của động cơ DC kéo xe của con lắc (hay tốc độ di chuyển xe). Tốc động cơ DC đƣợc điều khiển bằng kỹ thuật điều khiển độ rộng xung. Hệ số ku được chọn bằng phương pháp thử và sửa sai.
45
Hình 3.1 Bộ điều khiển cuốn chiếu thiết kế trên Simulink.
Để thuật toán có thể chạy đƣợc trên mô hình thật, các khối chức năng nhƣ đọc tín hiệu hối tiếp, kiểm tra an toàn tránh xe va đạp vào hai đầu cuối của mô hình và truyền đáp ứng về máy tính cá nhân cũng cần đƣợc thiết kế thêm. Thuật toán Simulink hoàn chỉnh cho cả hai giải pháp thiết kế thủ công và tự động đƣợc thiết kế nhƣ Hình 3.2. Thuật toán có ba tín hiệu vào, năm tín hiệu ra:
- Position_Ref: Ngỏ vào đặt vị trí tham chiếu, có thể thay đổi dạng tín hiệu tham chiếu đƣa vào dễ dạng.
- Pos_inp: Ngỏ vào hồi tiếp vị trí, đơn vị tính là pulse (Số xung trả về từ bộ giải mã vòng quay đọc vị trí).
- Ang_inp: Ngỏ vào hồi tiếp góc con lắc, đơn vị tính là pulse (Số xung trả về từ bộ giải mã vòng quay đọc góc quay của con lắc).
- Pwm_out: Ngỏ ra của bộ điều khiển mang ý nghĩa là phần trăm độ rộng của chuỗi xung điều khiển tốc độ động cơ DC kéo xe của con lắc.
- Các ngỏ ra còn lại: pos_out, ang_out, pos_dot, ang_dot tương ứng là các ngỏ ra vị trí, góc, vi phân vị trí và vi phân góc của con lắc. Các ngỏ ra này đƣợc sẽ đƣợc gửi về máy tính các nhân để quan sát đánh giá khi cần thiết.
1 out
k1*c2*c
c2*c
-K-
d1 -K-
K Ts z-1
K Ts z-1
-C- -C-
4 theta-dot 3
theta 2 x-dot
1 x
46
Hình 3.2 Thuật toán Simulink cho giải pháp thủ công Các kết quả thực hiện và thực nghiệm bộ điều khiển:
- Kết quả sinh mã bộ điều khiển: Sau khi thiết kế, định nghĩa các tín hiệu cần thiết và cấu hình các thông số Simulink nhƣ đã trình bày ở phần trên. Thuật toán Hình 3.2 đƣợc sinh mã thành các tập tin ngôn ngữ C có phần mở rộng .c và .h nhƣ Hình 3.3
Hình 3.3 Kết quả sinh mã thuật toán.
- Dự án sau khi tích hợp mã lệnh sinh mã và viết thêm mã lệnh cho chương trình chính nhƣ Hình 3.4
5 Out5
4 Out4
3 Out3
2 Out1
1 Out2 Position Pulse Position in Metre
PulseToMetre Pos_Feedback
PositionError(m)
Angle Pulse
pos
PWM Out
ang
x_dot
theta_dot Controller 3
In3
2 In2 1
In1
ang_inp
pwm_out
ang_out pos_out
pos_dot
ang_dot pos_inp
Position_Ref
47
Hình 3.4 Dự án hoàn chỉnh.
- Kết quả chạy thực nghiệm bộ điều khiển hệ thống con lắc ngƣợc:
Đáp ng b c: Trong thí nghiệm này, góc con lắc và vị trí xe đƣợc điều khiển bám ở vị trí 0 từ vị trí khởi tạo của góc con lắc là 0.2 [rad]. Góc con lắc đƣợc thay đổi thủ công về giá trị 0.2 [rad], vị trí xe ở 0 [m]. Khi đến vị trí 0.2 [rad], bộ điều khiển đƣợc cho phép hoạt động để điều khiển xe về phía âm để đƣa con lắc về 0 [rad]. Khi con lắc đã ổn định, bộ điều khiển di chuyển xe về vị trí tham chiếu trở lại. Theo đáp ứng Hình 3.5, sau khoảng 2 giây hệ thống đã ổn định. Một lực nhỏ tác động vào con lắc tại thời điểm khoảng 7 giây để kiểm tra sự ổn định của bộ điều khiển khi có nhiễu. Kết quả bộ điều khiển đã đáp ứng tốt với nhiễu và duy trùy đƣợc sự ổn định của hệ thống sau khoảng 2 giây.
48 a)
b)
Hình 3.5 Đáp ứng bước của hệ thống: a) Đáp ứng góc con lắc, b) Đáp ứng vị trí xe.
Th y đ i vị tr xe liên t c: Trong thí nghiệm này, góc tham chiếu đƣợc giữ cố định là 0 [rad] trong khi tham chiếu vị trí thay đổi liên tục giữ 0.1 [m] và -0.1[m] với tần số 0.05 [Hz] nhƣ Hình 3.6. Kết quả cho thấy bộ điều khiển đã cân bằng đƣợc con lắc và bám tốt vị trí tham chiếu.
a)
0 2 4 6 8 10 12
-0.2 -0.15 -0.1 -0.05 0 0.05 0.1 0.15 0.2
time[s]
theta[rad]
0 2 4 6 8 10 12
-0.2 -0.15 -0.1 -0.05 0 0.05
time[s]
x[m]
0 5 10 15 20 25 30 35 40
-0.06 -0.04 -0.02 0 0.02 0.04 0.06
time[s]
theta[rad]
49 b)
Hình 3.6 Đáp ứng bám vị trí của hệ thống: a) Đáp ứng góc con lắc, b) Đáp ứng vị trí xe.