CHƢƠNG 5 : BỘ ĐIỀU KHIỂN DẪN ĐƢỜNG
5.3 Bộ điều khiển PID hai lớp
5.3.1 Mô phỏng bộ điều khiển PID hai lớp
a. Tổng quan khối hệ thống điều khiển
Hình 5.11: Chi tiết các thành phần trong khối điều khiển
Đầu vào khối điều khiển :
Tín hiệu điều khiển từ khối điều khiển (từ 1000÷2000).
Các giá trị trạng thái của multicopter trong q trình chạy mơ phỏng + Gia tốc dài gắn thân của multicopter: ax, ay, az.
+ Vận tốc góc gắn thân của multicopter p, q ,r
Các giá trị mơi trƣờng gồm nhận đƣợc trong q trình mô phỏng động học: +Temp (nhiệt độ)
+ Press (áp suất)
Bảng 4: Tín hiệu đầu vào khối điều khiển
Đầu vào Mô tả Khoảng giá trị Đơn vị
Ch1 Điều khiển góc roll - Aileron 1000-2000 [ ]
Ch2 Điều khiên góc pitch - Elevator 1000-2000 [ ]
Ch3 Điều khiển góc yaw - Rudder 1000-2000 [ ]
Ch4 Điều khiển ga chung - Throttle 1000-2000 [ ]
60
Ch6 Không sử dụng 1000-2000 [ ]
p Tốc độ góc theo trục X -249÷249 Deg/s
q Tốc độ góc theo trục Y -249÷249 Deg/s
r Tốc độ góc theo trục Z -249÷249 Deg/s
ax Gia tốc dài theo trục X -19.62÷19.62 g(**)
ay Gia tốc dài theo trục Y -19.62÷19.62 g
az Gia tốc dài theo trục Z -19.62÷19.62 g
Press Áp suất khí quyển N/m^2
temp Nhiệt độ mơi trƣờng Độ C
Đầu ra:
Xung PWM (thực chất là giá trị đại diện cho độ rộng của xung PWM) điều khiển động cơ (8 tín hiệu PWM với máy bay x8 hoặc 4 tín hiệu PWM với máy bay x4). Giá trị độ rộng xung PWM tƣơng ứng nằm trong khoảng từ 1000÷2000 (ứng với chu kì là 2000).
Bảng 5: Tín hiệu đầu ra motor
Đầu ra Mô tả Khoảng giá trị Đơn vị
m1 Độ rộng xung PWM điều khiển motor1 1000-2000 [ ] m2 Độ rộng xung PWM điều khiển motor2 1000-2000 [ ] m3 Độ rộng xung PWM điều khiển motor3 1000-2000 [ ] m4 Độ rộng xung PWM điều khiển motor4 1000-2000 [ ] m5 Độ rộng xung PWM điều khiển motor5(nếu có) 1000-2000 [ ] m6 Độ rộng xung PWM điều khiển motor6(nếu có) 1000-2000 [ ] m7 Độ rộng xung PWM điều khiển motor7(nếu có) 1000-2000 [ ] m8 Độ rộng xung PWM điều khiển motor8(nếu có) 1000-2000 [ ]
61
b. Khối chuẩn hóa góc pitch và góc roll
Hình 5.12: Khối cảm biến tổng hợp và khối chuẩn hóa góc pitch và roll trong
Chức năng:
Khối xử lý cảm biến sử dụng bộ lọc Kalman để đƣa ra giá trị góc pitch và góc roll. Mơ phỏng q trình lọc dữ liệu từ cảm biến với các yếu tố sát với thực tế bao gồm nhiễu của cảm biến gia tốc góc(accelerometer noise, kí hiệu Q_angle), nhiễu cảm biến tốc độ góc (gyroscope noise, kí hiệu Q_gyroBias).
Đầu vào
Tốc độ góc p, q, r (trong hệ quy chiếu gắn thân) lấy từ cảm biến Gyro trong khối động học (Dynamic). Đơn vị là radian/s.
Gia tốc dài ax, ay, az (trong hệ quy chiếu gắn thân) lấy từ cảm biến gia tốc góc accelerometer trong khối động học (Dynamic) . Đơn vị là g (1g = 9.8m/s^2).
Bảng 6: Các tín hiệu đầu vào khối cảm biến
Đầu vào Mô tả Khoảng giá trị Đơn vị
p Tốc độ góc theo trục X -249÷249 Deg/s
q Tốc độ góc theo trục Y -249÷249 Deg/s
62
ax Gia tốc dài theo trục X -19.62÷19.62 g(**)
ay Gia tốc dài theo trục Y -19.62÷19.62 g
az Gia tốc dài theo trục Z -19.62÷19.62 g
Đầu ra
Góc Roll, Pitch, Yaw (Các góc Euler, đơn vị là Deg). Bảng 7: Các tín hiệu đầu ra khối cảm biến
Đầu ra Mô tả Khoảng giá trị Đơn vị
Roll_angle Góc Roll (1 trong 3 góc Euler) -90÷90 Độ Pitch_angle Góc Pitch (1 trong 3 góc Euler) -90÷90 Độ Yaw_angle Góc Yaw (1 trong 3 góc Euler) -180÷180 Độ
c. Khối chuẩn hóa tốc độ góc p, q, r
Hình 5.13: Khối chuẩn hóa tốc độ góc p, q, r
Chức năng:
Chuẩn hóa tín hiệu tốc độ góc về khoảng giá trị -1÷1. Do tốc độ góc nhận đƣợc từ cảm biến nên giá trị tốc độ góc bị giới hạn bởi khả năng của cảm biến (Gyro). Trong mơ hình sử dụng, giới hạn tốc độ góc đo đƣợc của cảm biến là -
63
249÷249 độ/s (xem thơng số trong khối IMU thuộc khối Dynamic) nên khối này có tác dụng chia tốc độ góc cho 250 (xấp xỉ 249) để chuẩn hóa.
Đầu vào:
Tốc độ góc p q r (với hệ quy chiếu gắn thân). Đơn vị là Deg/s.
p q r là tốc độ góc đo đƣợc từ khối mô phỏng IMU trong khối Dynamic model.
Bảng 8: Các tín hiệu đầu vào khối chuẩn hóa tốc độ góc
Đầu vào Mơ tả Khoảng giá trị Đơn vị
p Tốc độ góc theo trục X -249÷249 Deg/s
q Tốc độ góc theo trục Y -249÷249 Deg/s
r Tốc độ góc theo trục Z -249÷249 Deg/s
Đầu ra:
Tốc độ góc đã đƣợc giảm 1/250 lần.
Bảng 9: Các tín hiệu đầu ra khối chuẩn hóa tốc độ góc
Đầu ra Mơ tả Khoảng giá trị Đơn vị
Tốc độ góc Roll chuẩn hóa Tốc độ góc Roll chuẩn hóa -1÷1 [ ] Tốc độ góc Pitch chuẩn hóa Tốc độ góc Roll chuẩn hóa -1÷1 [ ] Tốc độ góc Yaw chuẩn hóa Tốc độ góc Roll chuẩn hóa -1÷1 [ ]
64
d. Khối PID (2 lớp)
Hình 5.14: Sơ đồ khối mơ phỏng điều khiển PID hai lớp
Khối PID lớp 1:
Đầu vào:
Là hiệu số (sai số) giữa giá trị góc roll và pitch mong muốn (điều khiển) với giá trị góc đo đƣợc từ cảm biến.
Giá trị điều khiển đƣợc chuẩn hóa nhƣ đã nêu ở Khối lọc tín hiệu điều khiển . Bảng 10: Các tín hiệu đầu vào khối PID lớp 1
Đầu vào Mô tả Khoảng giá trị Đơn vị
Roll-Ch1 Sai số góc Roll điều khiển và góc Roll hiện tại -3.57÷3.57 [ ] Pitch-Ch2 Sai số góc Pitch điều khiển và góc Pitch hiện
tại
-3.57÷3.57 [ ]
Ch5 Tín hiệu kích PID hoạt động [ ]
65
Bảng 11: Cái tín hiệu đầu ra khối PID lớp 1
Đầu ra Mô tả Khoảng giá trị Đơn vị
OutPIDRoll Tín hiệu ra của bộ PID đk góc Roll -1÷1 [ ] OutPIDPitch Tín hiệu ra của bộ PID đk góc Pitch -1÷1 [ ]
Khối PID (lớp 2):
Đầu vào:
Bảng 12: Các tín hiệu đầu vào khối PID lớp 2
Đầu vào Mô tả Khoảng giá trị Đơn vị
p - OutPIDRoll Sai số giữa tốc độ góc Roll đo đƣợc với đầu ra của PID góc Roll
-2÷2 [ ]
q - OutPIDPitch Sai số giữa tốc độ góc Pitch đo đƣợc với đầu ra của PID góc Pitch
-2÷2 [ ]
r – Ch4 Sai số giữa tốc độ góc Yaw đo đƣợc với tín hiệu điều khiển tốc độ góc Yaw từ kênh 4
-2÷2 [ ]
Ch5 Tín hiệu kích PID hoạt động [ ]
Đầu ra:
Bảng 13: Các tín hiệu đầu ra khối PID lớp 2
Đầu ra Mô tả Khoảng giá trị Đơn vị
OutPIDRollRate Tín hiệu điều khiển tốc độ góc Roll -1÷1 [ ] OutPIDPitchRate Tín hiệu điều khiển tốc độ góc Pitch -1÷1 [ ] OutPIDYawRate Tín hiệu điều khiển tốc độ góc Yaw -1÷1 [ ]
66 e. Kết quả chọn hệ số PID = 4.8, = 0.18, = 0.04, = 0.1 Hình 5.15: Đồ thị ứng với mức ga thấp nhất Hình 5.16: Đồ thị ứng với mức ga trung bình Hình 5.17: Đồ thị ứng với mức ga cao nhất
67 = 4.5, = 0.15, = 0.04, = 0.1
Hình 5.18: Đồ thị ứng với mức ga thấp nhất
Hình 5.19: Đồ thị ứng với mức ga trung bình
68
Kết quả thu đƣợc khi tiến hành thử nghiệm bộ điều khiển PID 2 lớp trong thực tế Bộ hệ số thứ nhất : =4.8, = 0.18, = 0.04, = 0.1
Hình 5.21: Đồ thị dữ liệu thu đƣợc từ bộ hệ số PID thứ nhất Bộ hệ số thứ 2 : = 4.5, = 0.15, = 0.04, = 0.1 Bộ hệ số thứ 2 : = 4.5, = 0.15, = 0.04, = 0.1
69
Bộ hệ số thứ ba : = 4.6, = 0.15, = 0.04, = 0.1
70
d. Kết quả bay theo quỹ đạo
Kết quả mô phỏng áp dụng bộ điều khiển bay theo quỹ đạo xây dựng ở chƣơng 2
71