Để chứng minh bộ điều khiển đã thiết kế trong chương 2 là ổn định, trong phần này ta sẽ tiến hành mô phỏng hệ thống Quadrotor trên phần mềm mô phỏng Matlab của công ty MathWorks phiên bản 2016a. Đây là phần mềm cho phép người dùng mơ phỏng tính tốn, thực nghiệm các mơ hình trong thực tế cũng như kỹ thuật.
3.2.1. Thơng số vật lý mơ hình Quadrotor
Trong q trình mơ phỏng, các thơng số vật lý được chọn gần đúng với mơ hình thật theo tài liệu tham khảo [10]. Chi tiết các thơng số được trình bày trong bảng 3.1.
Thơng số Mô tả Giá trị Đơn vị
𝐼𝑋𝑋 Moment quán tính của khung theo trục x 7.5e-3 kg. m2
𝐼𝑌𝑌 Moment quán tính của khung theo trục y 7.5e-3 kg. m2
𝐼𝑍𝑍 Moment quán tính của khung theo trục z 1.3e-2 kg. m2
𝑙
Khoảng cách từ tâm Quadrotor đến tâm của
một cánh quạt 0.23 m
𝐽𝑟 Moment quán tính của rotor 6e-5 kg. m2
𝑚 Khối lượng Quadrotor 0.650 kg
𝑏 Lực đẩy 3.13e-5 N. s2
𝑑 Lực kéo 7.5e-7 N. m. s2
g Gia tốc trọng trường 9.81 m/s2
Trang 49
3.2.2. Sơ đồ khối Quadrotor
Hệ thống Quadrotor được thiết kế trên Simulink với giao diện như sau:
Hình 3.3. Sơ đồ khối Quadrotor
3.2.3. Khối Quadrotor Dynamics
Hình 3.4. Khối Quadrotor Dynamics
Khối này được xây dựng trong Matlab/ Simulink từ phương trình (2.34) đó chính là mơ hình tốn học của Quadrotor.
Trang 50
3.2.4. Khối System Input Calculation
Hình 3.5. Khối System Input Calculation
Khối System Input Calculation được xây dựng trong Matlab/ Simulink từ phương trình (3.3) để tính tốn vector đầu vào [𝑈1 𝑈2 𝑈3 𝑈4]
3.2.5. Khối Motor Speed Caculation.
Hình 3.6. Khối Motor Speed Caculation
Khối Motor Speed Caculation được xây dựng trong Matlab/ Simulink từ phương trình (3.4) để tính tốn tốc độ đầu vào của 4 cánh quạt 𝛺1, 𝛺2, 𝛺3, 𝛺4.
Trang 51
3.2.6. Luật điều khiển U1, U2 , U3, U4
Hình 3.7. Luật điều khiển U1, U2 , U3, U4
Khối luật điều khiển U1, U2 , U3, U4 được xây dựng trong Matlab/ Simulink từ các phương trình (3.19) và (3.29 đến 3.31) để tính tốn luật điều khiển cho mơ hình Quadrotor phương pháp điều khiển Backstepping.
Trang 52
CHƯƠNG 4
KẾT QUẢ MÔ PHỎNG 4.1. Thông số mong muốn trường hợp tổng quát
Thông số Mơ tả Giá trị
phi_d Góc Roll mong muốn Quadrotor đạt được 50 theta_d Góc Pitch mong muốn Quadrotor đạt được 80
psi_d Góc Yaw mong muốn Quadrotor đạt được 100 z_d Độ cao mong muốn Quadrotor đạt được 10m
Bảng 4.1. Thơng số mong muốn của mơ hình trường hợp tổng qt
❖ Góc nghiêng Roll
Hình 4.1. Góc nghiêng Roll
Đồ thị hình 4.1 biểu diễn góc nghiêng Roll trong luật điều khiển U2 (Luật điều khiển góc nghiêng). Trong thời gian đầu tín hiệu có dao động nhưng sau khoảng 3s tín hiệu đã hồn tồn ổn định.
Trang 53
❖ Đáp ứng góc nghiêng Roll (50)
Hình 4.2. Đáp ứng góc nghiêng Roll
Đồ thị hình 4.2 biểu diễn đáp ứng góc nghiêng Roll của Quadrotor trong khơng gian. Trong thời gian đầu tín hiệu điều khiển có dao động nhưng sau khoảng 3s tín hiệu điều khiển bám theo tín hiệu đặt và ổn định.
❖ Góc lật Pitch
Hình 4.3. Góc Pitch
Đồ thị hình 4.3 biểu diễn góc Pitch trong luật điều khiển U3 (Luật điều khiển góc lật Pitch). Trong thời gian đầu tín hiệu có dao động nhưng sau khoảng 3s tín hiệu đã hồn tồn ổn định
Trang 54
❖ Đáp ứng góc lật Pitch (80)
Hình 4.4. Đáp ứng góc lật Pitch
Đồ thị hình 4.4 biểu diễn đáp ứng góc lật Pitch của Quadrotor trong khơng gian. Trong thời gian đầu tín hiệu điều khiển có dao động nhưng sau khoảng 3s tín hiệu điều khiển bám theo tín hiệu đặt và ổn định.
❖ Góc xoay Yaw
Hình 4.5. Góc xoay Yaw
Đồ thị hình 4.5 biểu diễn góc Yaw trong luật điều khiển U4 (Luật điều khiển góc xoay). Trong thời gian đầu tín hiệu có dao động nhưng sau khoảng 3s tín hiệu đã hoàn toàn ổn định
Trang 55
❖ Đáp ứng góc xoay Yaw (100)
Hình 4.6. Đáp ứng góc xoay Yaw
Đồ thị hình 4.6 biểu diễn đáp ứng góc xoay Yaw của Quadrotor trong khơng gian. Trong thời gian đầu tín hiệu điều khiển có dao động nhưng sau khoảng 3s tín hiệu điều khiển bám theo tín hiệu đặt và ổn định.
❖ Độ cao (z)
Hình 4.7. Độ cao z
Đồ thị hình 4.7 biểu diễn độ cao z trong luật điều khiển U1 (Luật điều khiển độ cao). Trong thời gian đầu tín hiệu có dao động nhưng sau khoảng 3s tín hiệu đã hồn tồn ổn định.
Trang 56
❖ Đáp ứng độ cao z (10m)
Hình 4.8. Đáp ứng độ cao z
Đồ thị hình 4.8 biểu diễn đáp ứng độ cao z của Quadrotor trong không gian. Trong thời gian đầu tín hiệu điều khiển có dao động nhưng sau khoảng 3s tín hiệu điều khiển bám theo tín hiệu đặt và ổn định.
❖ Tọa độ Quadrotor trong không gian
Hình 4.9. Tọa độ Quadrotor trong khơng gian
Đồ thị hình 4.9 biểu diễn đáp ứng tọa độ của Quadrotor trong không gian. Khi đạt độ cao 10m và giá trị đặt các góc Roll, Pitch, Yaw thì Quadrotor đã hồn tồn ổn định trong không gian.
Trang 57
❖ Tốc độ 4 động cơ của Quadrotor
Hình 4.10. Tốc độ đáp ứng của 4 động cơ Quadrotor
Đồ thị hình 4.10 mô tả tốc độ 4 động cơ của Quadrotor nhằm đáp ứng các
thông số mong muốn như bảng 4.1. Trong khoảng thời gian q độ thì Quadrotor có tốc độ khơng ổn định nhằm nâng Quadrotor lên đồng thời tạo góc cho phù hợp với thông số đặt. Khi Quadrotor lên đến trạng thái z = 10m, thì Quadrotor ổn định tốc độ riêng của từng động cơ (tốc độ mỗi động cơ khác nhau nhằm đáp ứng góc Roll, Pitch, Yaw), đồng thời tổng lực nâng của 4 động cơ gây nên bằng với trọng lực P của Quadrotor. Lúc này, Quadrotor nằm ở 1 độ cao cố định, nhưng thân Quadrotor sẽ trượt theo phương Ox và Oy do phụ thuộc vào các góc Roll, Pitch, Yaw.
Trang 58
❖ Đáp ứng Quadrotor theo phương Ox: Đồ thị hình 4.11 biểu diễn đáp ứng của
Quadrotor theo phương Ox trong không gian.
Hình 4.11. Đáp ứng theo phương Ox của Quadrotor.
Quadrotor sẽ di chuyển theo phương Ox phụ thuộc vào giá trị đặt các góc Roll, Pitch, Yaw
❖ Đáp ứng Quadrotor theo phương Oy: Đồ thị hình 4.12 biểu diễn đáp ứng của
Quadrotor theo phương Oy trong khơng gian.
Hình 4.12. Đáp ứng theo phương Oy của Quadrotor.
Quadrotor sẽ di chuyển theo phương Oy phụ thuộc vào giá trị đặt các góc Roll, Pitch, Yaw
Trang 59
4.2. Thông số mong muốn Quadrotor ở trạng thái lơ lửng
Bảng 4.2. Thơng số mong muốn của mơ hình Quadrotor ở trạng thái lơ lửng
❖ Đáp ứng góc nghiêng Roll (00)
Hình 4.13. Đáp ứng góc Roll
Đồ thị hình 4.13 biểu diễn đáp ứng góc nghiêng Roll của Quadrotor trong không gian. Trường hợp này Quadrotor sẽ không nghiêng trái hoặc nghiêng phải mà lơ lửng trong không gian.
Thông số Mô tả Giá trị
phi_d Góc Roll mong muốn Quadrotor đạt được 00 theta_d Góc Pitch mong muốn Quadrotor đạt được 00 psi_d Góc Yaw mong muốn Quadrotor đạt được 00 z_d Độ cao mong muốn Quadrotor đạt được 10 m
Trang 60
❖ Đáp ứng góc lật Pitch (00)
Hình 4.14. Đáp ứng góc lật Pitch
Đồ thị hình 4.14 biểu diễn đáp ứng góc lật Pitch của Quadrotor trong khơng gian. Trường hợp này Quadrotor sẽ không lật trước hoặc lật sau mà lơ lửng trong không gian.
❖ Đáp ứng góc xoay Yaw (00)
Hình 4.15. Đáp ứng góc xoay Yaw
Đồ thị hình 4.15 biểu diễn đáp ứng góc xoay Yaw của Quadrotor trong khơng gian. Trường hợp này Quadrotor sẽ không xoay trái hoặc xoay phải mà lơ lửng trong không gian.
Trang 61
❖ Đáp ứng độ cao z (10m):
Hình 4.16. Đáp ứng độ cao z
Đồ thị hình 4.16 biểu diễn đáp ứng độ cao z của Quadrotor trong không gian. Trường hợp này Quadrotor sẽ lơ lửng trong không gian.
❖ Tọa độ Quadrotor trong khơng gian
Hình 4.17. Tọa độ Quadrotor trong khơng gian
Đồ thị hình 4.17 biểu diễn đáp ứng tọa độ của Quadrotor trong không gian. Khi đạt độ cao 10m và giá trị đặt các góc Roll, Pitch, Yaw thì Quadrotor đã hồn tồn ổn định và lơ lửng trong khơng gian.
Trang 62
❖ Tốc độ 4 động cơ của Quadrotor
Hình 4.18. Tốc độ đáp ứng của 4 động cơ Quadrotor
Đồ thị hình 4.18 mơ tả tốc độ 4 động cơ của Quadrotor nhằm đáp ứng các
thông số mong muốn như bảng 4.2. Trong khoảng thời gian q độ thì Quadrotor có tốc độ khơng ổn định nhưng đảm bảo 4 tốc độ của 4 động cơ là bằng nhau nhằm nâng quadrotor đồng thời giữ cho các góc xoay Roll, Pitch, Yaw đều bằng 0 hay có nghĩa là quadrotor sẽ khơng nghiêng trái/ phải, không lật trước/ sau và cũng không xoay quay Oz. Khi Quadrotor lên đến trạng thái z = 10m, thì Quadrotor ổn định tốc độ của 4 động cơ. Ở trạng thái này, tổng lực nâng của 4 động cơ gây nên bằng với trọng lực P của Quadrotor, Quadrotor nằm ở 1 độ cao cố định hay còn gọi là trạng thái “Hover”.
❖ Đáp ứng quadrotor theo phương Ox: Đồ thị hình 4.19 biểu diễn đáp ứng của
Quadrotor theo phương Ox trong không gian.
Trang 63
Vì góc nghiêng Roll = 0, nên Quadrotor sẽ không di chuyển theo phương Ox như lý thuyết trình bày ở chương 2.
❖ Đáp ứng quadrotor theo phương Oy: Đồ thị hình 4.20 biểu diễn đáp ứng của
Quadrotor theo phương Oy trong không gian.
Hình 4.20. Đáp ứng theo phương Oy của Quadrotor
Vì góc lật Pitch = 0, nên quadrotor sẽ khơng di chuyển theo phương Oy như lý thuyết trình bày ở chương 2.
4.3. Nhận xét kết quả mô phỏng
Dựa vào đồ thị đáp ứng các ngõ ra của quadrotor, chúng ta thấy kết quả mô phỏng rất khả quan. Tín hiệu ngõ ra nhanh chóng tiến về giá trị đặt và ổn định. Trong khoảng thời gian 0 – 3s thì có hiện tượng dao động ở các ngõ ra, nhưng sau thời gian 3s thì hệ thống đã hồn tồn ổn định. Các trường hợp mô phỏng đều cho đồ thị đúng như mong muốn. Ngồi những trường hợp mơ phỏng trên, tác giả cũng xây dựng nhiều giả lặp khác về các trạng thái hoạt động của quadrotor, kết quả cũng cho thấy sự khả quan, các tín hiệu ngõ ra nhanh đạt đến giá trị đặt.
Trang 64
CHƯƠNG 5
SO SÁNH ĐIỀU KHIỂN BACKSTEPPING VỚI CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN KHÁC 5.1. Phương pháp điều khiển trượt: (Sliding Mode Control - SMC)
Điều khiển trượt còn được gọi là điều khiển cấu trúc thay đổi, là điều khiển hồi tiếp chuyển mạch tốc độ cao. Lý thuyết điều khiển dùng để điều khiển chuyển mạch hồi tiếp tốc độ cao, điều khiển trạng thái hệ thống phi tuyến trên bề mặt xác định trong không gian trạng thái. Bề mặt này được gọi là mặt trượt hay bề mặt chuyển mạch.
Hình 5.1. Sơ đồ khối bộ điều khiển trượt
5.1.1. Xây dựng luật điều khiển trượt cho mơ hình Quadrotor 5.1.1.1. Luật điều khiển góc 5.1.1.1. Luật điều khiển góc
Trong luật điều khiển góc, ta sẽ đi thiết kê bộ điều khiển cho cả 3 góc (roll, pitch, yaw) và sơ đồ khối cho bộ điều khiển góc được mơ tả như hình 5.2.
Trang 65
❖ Luật điều khiển góc nghiêng Roll
Ta có: 𝑒 = 𝜙𝑑− 𝜙 (5.1)
Mặt trượt S được định nghĩa như sau: 𝑆 = 𝑐1𝑒 + 𝑒̇ (5.2) Trong đó: 𝑐1là hằng số lớn hơn không.
Đạo hàm của mặt trượt S ta được phương trình:
𝑆̇ = 𝑐1𝑒̇ + 𝑒̈ = 𝑐1(𝜙̇𝑑− 𝜙̇) + 𝜙̈𝑑− 𝜙̈ (5.3) Xác định hàm Lyapunov: 𝑉(𝑒, 𝑠) =1
2(𝑒2+ 𝑠2) (5.4) Căn cứ vào hàm Lyapunov, luật điều khiển trượt được chọn:
𝑆̇ = −𝑘1𝑆𝑖𝑔𝑛(𝑆) − 𝑘2𝑆 (5.5)
Trong đó: 𝑆𝑖𝑔𝑛(𝑆) = {−1 nếu S < 0 1 nếu S > 0
Và hệ số 𝑘1, 𝑘2là một hằng số. Để thõa mãn điều kiện 𝑆𝑆̇ < 0, giới hạn thiết
lập các hệ số 𝑘1, 𝑘2 phải thõa 𝑘1 > 0, 𝑘2 > 0. Thay thế 𝜙̈ ở công thức (2.24) vào
công thức (5.3) ta thu được luật điều khiển U2 như sau:
𝑈2 = 1
𝑏1(𝑘1𝜙𝑆𝑖𝑔𝑛(𝑆𝜙) + 𝑘2𝜙(𝑆𝜙) − 𝑎1𝜃̇𝜓̇ + 𝑎2𝜃̇Ω𝑟 + 𝜙̈𝑑+ 𝑐1𝜙(𝜙̇𝑑− 𝜙̇) (5.6) ❖ Luật điều khiển góc lật Pitch
Tính tốn tương tự như góc nghiêng Roll, ta có luật điều khiển U3 như sau:
𝑈3 = 1
𝑏2(𝑘1𝜃𝑆𝑖𝑔𝑛(𝑆𝜃) + 𝑘2𝜃(𝑆𝜃) − 𝑎3𝜙̇𝜓̇ + 𝑎4𝜙̇Ω𝑟 + 𝜃̈𝑑 + 𝑐1𝜃(𝜃̇𝑑− 𝜃̇) (5.7) ❖ Luật điều khiển góc xoay Yaw
Tính tốn tương tự như góc nghiêng Roll và góc lật Pitch, ta có luật điều khiển U4 như sau:
𝑈4 = 1
Trang 66
5.1.1.2. Luật điều khiển độ cao
Xây dựng luật điều khiển độ cao của Quadrotor cũng tương tự như các bước xây dựng luật điều khiển góc và sơ đồ khối được mơ tả trong hình 5.3.
Hình 5.3. Sơ đồ khối luật điều khiển độ cao Quadrotor
Ta có: 𝑒 = 𝑧 − 𝑧𝑑 (5.9)
Mặt trượt S được định nghĩa như sau: 𝑆 = 𝑐1𝑒 + 𝑒̇ (5.10) Trong đó: 𝑐1là hằng số lớn hơn không.
Đạo hàm của mặt trượt S ta được phương trình:
𝑆̇ = 𝑐1𝑒̇ + 𝑒̈ = 𝑐1(𝑧̇ − 𝑧̇𝑑) + 𝑧̈ − 𝑧̈𝑑 (5.11) Thay thế 𝑧̈ ở công thức (2.24) vào (5.11) ta thu được luật điều khiển U1 như
sau:
𝑈1 = 𝑚
𝑐𝑜𝑠𝜙𝑐𝑜𝑠𝜃(𝑘1𝑧𝑆𝑖𝑔𝑛(𝑆𝑧) + 𝑘2𝑧(𝑆𝑧) + 𝑔 − 𝑧̈𝑑+ 𝑐1𝑧(𝑧̇ − 𝑧̇𝑑) (5.12)
5.1.2. Xây dựng mơ hình Quadrotor trong Matlab Simulink
Mơ hình điều khiển Quadrotor bằng phương pháp trượt (SMC) tương tự như mơ hình điều khiển Quadrotor bằng phương pháp Backstepping nhưng chỉ khác nhau luật điều khiển.
Các thông số vật lý cũng lựa chọn giống như mơ hình điều khiển Quadrotor bằng phương pháp Backstepping
Trang 67
Hình 5.4. Luật điều khiển U1, U2 , U3, U4
Khối luật điều khiển U1, U2 , U3, U4 được xây dựng trong Matlab/ Simulink từ các phương trình (5.6 đến 5.8) và (5.12) để tính tốn luật điều khiển cho mơ hình Quadrotor phương pháp điều khiển trượt (SMC).
Trang 68
5.1.3. Kết quả mô phỏng
❖ Thông số mong muốn trường hợp tổng quát
Thông số Mô tả Giá trị
phi_d Góc Roll mong muốn Quadrotor đạt được 50 theta_d Góc Pitch mong muốn Quadrotor đạt được 80
psi_d Góc Yaw mong muốn Quadrotor đạt được 100 z_d Độ cao mong muốn Quadrotor đạt được 10m
Bảng 5.1. Thơng số mong muốn của mơ hình trường hợp tổng quát
❖ Góc nghiêng Roll:
Hình 5.5 . Góc nghiêng Roll
Đồ thị hình 5.5 biểu diễn góc Roll trong luật điều khiển U2 (Luật điều khiển góc nghiêng Roll). Trong thời gian đầu tín hiệu có dao động nhưng sau khoảng 5s tín hiệu đã hoàn toàn ổn định.
Trang 69
❖ Đáp ứng góc nghiêng Roll (50)
Hình 5.6. Đáp ứng góc nghiêng Roll
Đồ thị hình 5.6 biểu diễn đáp ứng góc nghiêng Roll của Quadrotor trong không gian. Trong thời gian đầu tín hiệu điều khiển có dao động nhưng sau khoảng 5s tín hiệu điều khiển bám theo tín hiệu đặt và ổn định.
❖ Góc lật Pitch
Hình 5.7. Góc Pitch
Đồ thị hình 5.7 biểu diễn góc Pitch trong luật điều khiển U3 (Luật điều khiển góc lật Pitch). Trong thời gian đầu tín hiệu có dao động nhưng sau khoảng 5s tín hiệu đã hoàn toàn ổn định
Trang 70
❖ Đáp ứng góc lật Pitch (80)
Hình 5.8. Đáp ứng góc lật Pitch
Đồ thị hình 5.8 biểu diễn đáp ứng góc lật Pitch của Quadrotor trong khơng gian. Trong thời gian đầu tín hiệu điều khiển có dao động nhưng sau khoảng 5s tín hiệu điều khiển bám theo tín hiệu đặt và ổn định
❖ Góc xoay Yaw
Hình 5.9. Góc Yaw
Đồ thị hình 5.9 biểu diễn góc Yaw trong luật điều khiển U4 (Luật điều khiển góc lật Yaw). Trong thời gian đầu tín hiệu có dao động nhưng sau khoảng 5s tín hiệu đã hoàn toàn ổn định.
Trang 71
❖ Đáp ứng góc xoay Yaw (100)
Hình 5.10. Đáp ứng góc xoay Yaw
Đồ thị hình 5.10 biểu diễn đáp ứng góc xoay Yaw của Quadrotor trong khơng gian. Trong thời gian đầu tín hiệu điều khiển có dao động nhưng sau khoảng 5s tín hiệu điều khiển bám theo tín hiệu đặt và ổn định
❖ Độ cao z
Hình 5.11. Độ cao z
Đồ thị hình 5.11 biểu diễn độ cao z trong luật điều khiển U1 (Luật điều khiển độ cao z). Trong thời gian đầu tín hiệu có dao động nhưng sau khoảng 5s tín hiệu đã hoàn toàn ổn định.
Trang 72
❖ Đáp ứng độ cao z (10m)
Hình 5.12. Đáp ứng độ cao z
Đồ thị hình 5.12 biểu diễn đáp ứng độ cao z của Quadrotor trong không gian. Trong thời gian đầu tín hiệu điều khiển có dao động nhưng sau khoảng 3s tín hiệu điều khiển bám theo tín hiệu đặt và ổn định
❖ Tọa độ Quadrotor trong khơng gian
Hình 5.13. Tọa độ Quadrotor trong khơng gian
Đồ thị hình 5.13 biểu diễn đáp ứng tọa độ của Quadrotor trong không gian.