Luận văn gồm 6 chương với cấu trúc như sau:
Chương 1: Tổng quan
Giới thiệu tổng quan về các cơng trình nghiên cứu liên quan, xu hướng phát triển của các mơ hình tương tự như trong luận văn. Khả năng ứng dụng thực tiễn, những ưu khuyết điểm của mơ hình, mức độ áp dụng của các thuật tốn.
Chương 2: Cơ sở điều khiển
Trình bày chi tiết cấu tạo và nguyên lý hoạt động mơ hình Quadrotor. Xây dựng mơ hình tốn của Quadrotor. Trình bày cơ sở lý thuyết điều khiển Backstepping. Từ mơ hình tốn học tạo bước đệm thiết kế bộ điều khiển để tiến hành mô phỏng và thực nghiệm.
Chương 3: Xây dựng luật điều khiển mơ hình Quadrotor
Dựa vào cơ sở lý thuyết điều khiển Backstepping xây dựng thuật toán điều khiển Quadrotor. Trên cơ sở đó, tiến hành mô phỏng Quadrotor trên phần mềm Matlab/Simulink.
Chương 4: Kết quả mô phỏng Quadrotor
Chạy mô phỏng trên Matlab/Simulink và vẽ đồ thị đáp ứng ngõ ra Quadrotor, nhận xét, đánh giá kết quả.
Trang 24
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
Từ kết quả mô phỏng Quadrotor bằng phương pháp điều khiển Backstepping tiến hành so sánh với các phương pháp điều khiển khác để đánh giá tính hiệu quả của thuật toán điều khiển.
Chương 6: Kết luận và hướng phát triển đề tài.
Kết luận về các kết quả mô phỏng và các kết quả thực nghiệm, đánh giá điểm đạt được và tồn tại. Đưa ra hướng phát triển để xây dựng mơ hình thực tế Quadrotor hồn thiện hơn.
Trang 25
CHƯƠNG 2
CƠ SỞ ĐIỀU KHIỂN 2.1. Cấu tạo và nguyên lý hoạt động của Quadrotor 2.1.1. Cấu tạo Quadrotor
Quadrotor được định nghĩa là một phương tiện bay có 4 cánh quạt gắn lên phía cuối của một khung hình chữ thập. Các cánh quạt được gắn vào 4 động cơ ở 4 góc, với 2 cánh quạt xen kẽ quay ngược chiều nhau, 2 cánh quạt đối xứng quay cùng chiều nhau để chống lại moment xoắn.
Hình 2.1. Mơ hình Quadrotor.
Bộ điều khiển và cảm biến thường được đặt ở giữa trung tâm mơ hình để giữ thăng bằng và tiện lợi trong việc kết nối điều khiển tới 4 động cơ.
2.1.2. Nguyên lý hoạt động Quadrotor
Cặp cánh phía trước và cặp cánh phía sau quay thuận chiều kim đồng hồ, cặp cánh bên phải và bên trái quay ngược chiều kim đồng hồ nhằm cân bằng moment xoay được tạo ra bởi cánh quạt trên khung. Cả 4 cánh sinh ra một lực đẩy bằng nhau khi cất cánh nếu quay ở cùng một tốc độ.
Trang 26
Góc nghiêng được điều khiển khi thay đổi mối quan hệ về tốc độ giữa cánh bên phải và bên trái sao cho giữ nguyên tổng lực đẩy sinh ra bởi cặp cánh này. Tương tự góc lật cũng được điều khiển bởi mối quan hệ giữa 2 tốc độ của 2 cánh phía trước và phía sau. Góc xoay lại được thay đổi nhờ vào sự thay đổi vận tốc của cánh bên phải và bên trái liên quan tới tốc độ của cặp cánh phía trước và phía sau.
Khi đó, tổng lực đẩy sinh ra bởi các cặp cánh được giữ không đổi. Độ cao của Quadrotor thay đổi khi tăng hoặc giảm đồng thời tốc độ của các cánh quạt. Hệ trục tọa độ gắn trên quadrotor như Hình 2.3
Hình 2.3. Hệ trục tọa độ đặt trên Quadrotor.
- Trục x là vector hướng từ motor 3 tới motor 1, góc xoay quanh trục x là góc Roll.
- Trục y là vector hướng từ motor 2 tới motor 4, góc xoay quanh trục y là góc Pitch.
- Trục z là vector vng góc với mặt phẳng (Oxy) và hướng xuống dưới, góc xoay quanh trục z là góc Yaw.
2.1.2.1. Trạng thái lơ lửng (Hover)
Quadrotor ở trạng thái lơ lửng trong không trung khi tất cả các cánh quạt quay cùng một tốc độ không đổi (Ω1 = Ω2 = Ω3 = Ω4 = Ω0). Khi 4 cánh quạt quay với một tốc độ Ω0 thì sẽ cân bằng với trọng lượng của Quadrotor, nên Quadrotor sẽ cân bằng tại một vị trí.
Trang 27
Hình 2.4. Trạng thái lơ lửng (Hover).
2.1.2.2. Trạng thái bay lên xuống ( Throttle )
Quadrotor sẽ bay lên hoặc hạ xuống theo phương thẳng đứng. Để bay lên, tốc độ của 4 cánh quạt tăng lên, hạ xuống thì cả 4 cánh cùng giảm tốc độ, lúc đó sẽ sinh ra một hợp lực dọc trục theo phương z điều khiển Quadrotor bay lên hoặc bay xuống. Khi tốc độ 4 cánh quạt tăng, thì lực nâng trên thân Quadrotor sẽ lớn hơn trọng lượng của Quadrotor, lúc này Quadrotor sẽ di chuyển theo hướng lên trên. Và ngược lại, khi tốc độ 4 cánh quạt cùng giảm nhỏ hơn Ω0 lực nâng Quadrotor sẽ khơng vượt q trọng lượng của Quadrotor, vì thế Quadrotor sẽ bay xuống.
Hình 2.5. Trạng thái bay lên / xuống (Throttle ).
Trong đó: Ω0 : Là vận tốc góc của cánh quạt.
Trang 28
2.1.2.3. Trạng thái nghiêng trái / phải ( Roll )
Quadrotor sẽ có thể bay nghiêng bên phải hoặc bên trái. Để bay nghiêng bên phải (bên trái), giữ nguyên tốc độ của 2 cánh quạt trước và sau, tăng (giảm) tốc độ của cánh quạt bên trái và giảm (tăng) tốc độ cánh quạt bên phải. Khi đó moment sinh ra quay quanh trục Xb làm cho tổng lực nâng của các cánh quạt khơng cịn nằm theo phương thẳng đứng mà theo phương chuyển động.
Hình 2.6. Trạng thái nghiêng trái / phải ( Roll ).
2.1.2.4. Trạng thái lật trước / sau ( Pitch )
Quadrotor sẽ bay tới hoặc bay lùi về sau. Tương tự như khi bay nghiêng trái / phải, 2 cánh quạt trái và phải giữ một tốc độ như nhau. Để bay tới (lùi) điều khiển tăng (giảm) tốc độ của cánh quạt sau và giảm (tăng) tốc độ cánh quạt trước, sinh ra moment xoay quay quanh trục Yb.
Trang 29
2.1.2.5. Trạng thái xoay qua trái / phải (Yaw)
Quadrotor quay quanh trục Zb. Điều khiển tốc độ cánh quạt theo cách như sau: Tốc độ 2 cánh đối diện thì bằng nhau, nhưng khác với tốc độ 2 cách đối diện còn lại, để Quadrotor quay quanh trục Zb theo chiều ngược kim đồng hồ, ta giảm tốc độ cặp cánh có chiều quay ngược kim đồng hồ (chiều muốn quay) và tăng tốc độ cặp cánh quạt quay thuận chiều kim đồng hồ. Để Quadrotor quay theo chiều thuận ta làm ngược lại cách trên.
Hình 2.8. Trạng thái xoay trái / phải ( Yaw )
2.2. Mơ hình hóa Quadrotor
2.2.1. Tổng quan về hệ quy chiếu trên thân Quadrotor
Để thể hiện vị trí của mình trong khơng gian, ta dùng 6 biến (𝑥, 𝑦, 𝑧, 𝜙, 𝜃, 𝜓) để mô tả. Ba biến (𝑥, 𝑦, 𝑧) thể hiện vị trí của tâm Quadrotor trong hệ quy chiếu qn tính trái đất cố định (Hình 2.9). Trong đó:
- x: Là tọa độ của Quadrotor theo trục Ox trong hệ quy chiếu quán tính trái đất. - y: Là tọa độ của Quadrotor theo trục Oy trong hệ quy chiếu quán tính trái đất. - z: là độ cao của Quadrotor.
Ba biến (𝜙, 𝜃, 𝜓) hay còn gọi là (Roll, Pitch, Yaw) là các góc Euler thể hiện hướng của Quadrotor (Hình 2.9). Trong đó:
- Góc Roll là góc xoay quanh trục Ox trong hệ quy chiếu quán tính trái đất.
Trang 30
- Góc Yaw là góc xoay quanh trục Oz trong hệ quy chiếu quán tính trái đất.
Hình 2.9. Vị trí dài và vị trí góc của Quadrotor.
Các biến (p, q, r) là vận tốc góc và (u, v, w) vận tốc dài trong hệ quy chiếu vật thể. Để mô tả đầy đủ 12 biến trạng thái của Quadrotor ta cần 2 hệ quy chiếu: Hệ quy chiếu quán tính trái đất (hệ quy chiếu trái đất) và hệ quy chiếu vật thể (Hình 2.10).
Hệ quy chiếu mặt đất E (OE, xE, yE, zE) được chọn theo quy tắc tham chiếu theo quy tắc bàn tay phải, xE chỉ hướng Bắc, yE chỉ hướng Tây, zE chỉ hướng vng góc với mặt phẳng (xE, yE) và hướng lên, OE gốc của hệ trục tọa độ. Trong hệ quy chiếu này thể hiện các vector sau:
- ΓE= [x y z]T [m]: Vector vị trí dài của Quadrotor trong hệ trục tọa độ E.
- 𝛩𝐸 = [𝜙 𝜃 𝜓]𝑇 [𝑟𝑎𝑑]: Vector vị trí góc của Quadrotor trong hệ quy chiếu trái đất bằng 3 góc Euler Roll, Pitch, Yaw.
⇨ [𝛤𝐸 𝛩𝐸] 𝑇 = [𝑥 𝑦 𝑧 𝜙 𝜃 𝜓]𝑇: Vector vị trí tổng quát 𝜉 trong hệ tọa độ E.
Hệ quy chiếu vật thể B (OB, xB, yB, zB) là hệ quy chiếu được gắn với thân Quadrotor, xB chỉ hướng phía trước, yB chỉ hướng bên trái, zB chỉ hướng vng góc với mặt phẳng (xB, yB) và hướng lên, OB gốc của hệ trục tọa độ gắn với thân Quadrotor được đặt ở vị trí tâm. Hệ quy chiếu này tuân theo quy tắc bàn tay phải. Trong hệ quy chiếu này thể hiện các vector sau:
- 𝑉𝐵 = [𝑢 𝑣 𝑤]𝑇 [m/s]: Vector vận tốc dài.
- 𝜔𝐵 = [𝑝 𝑞 𝑟]𝑇 [rad/s]: Vector vận tốc góc.
Trang 31
- τB [Nm] : Moment xoắn của Quadrotor.
⇨ [𝑉𝐵 𝜔𝐵] 𝑇 = [𝑢 𝑣 𝑤 𝑝 𝑞 𝑟]𝑇 : Vector vận tốc tổng quát 𝜗 trong hệ tọa độ B.
Hình 2.10. Hệ quy chiếu quán tính và vật thể (Kivrak, A, 2006).
2.2.2. Động học Quadrotor
Để xác định mối liên hệ giữa các vector nhằm mô tả chuyển động của vật thể cần xác định các ma trận: Ma trận xoay RΘ, ma trận chuyển vị TΘ và ma trận tổng quát JΘ.
2.2.2.1. Ma trận xoay R
Ma trận xoay là ma trận mơ tả q trình xoay trong khơng gian của Quadrotor và ma trận này có được bằng cách nhân liên tiếp ba ma trận xoay cơ bản quanh các trục x, y, z.
Trang 32 R(𝜙, 𝑥) = [ 1 0 0 0 𝐶𝜙 −𝑆𝜙 0 𝑆𝜙 𝐶𝜙 ] (2.1)
- Xoay quanh trục yE một góc 𝜃 (pitch) ta được ma trận xoay R(𝜃, y).
R(𝜃, 𝑦) = [
𝐶𝜃 0 𝑆𝜃 0 1 0 −𝑆𝜃 0 𝐶𝜃
] (2.2)
- Xoay quanh trục zE một góc 𝜓 (yaw) ta được ma trận xoay R(𝜓, z).
R(𝜓, 𝑧) = [
𝐶𝜓 −𝑆𝜓 0 𝑆𝜓 𝐶𝜓 0
0 0 1
] (2.3)
- Quy ước kí hiệu ck = cos(k), sk = sin(k)
Nhân ba ma trận quay quanh các trục x, y, z ta được ma trận xoay: 𝑅𝛩 = [
𝑐𝜓𝑐𝜃 −𝑠𝜓𝑐𝜙 + 𝑐𝜓𝑠𝜃𝑠𝜙 𝑠𝜓𝑠𝜙 + 𝑐𝜓𝑠𝜃𝑐𝜙 𝑠𝜓𝑐𝜃 𝑐𝜓𝑐𝜙+ 𝑠𝜓𝑠𝜃𝑠𝜙 −𝑐𝜓𝑠𝜙 + 𝑠𝜓𝑠𝜃𝑐𝜙
−𝑠𝜃 𝑐𝜃𝑠𝜙 𝑐𝜃𝑐𝜓
] (2.4.)
Ma trận xoay 𝑅𝛩 là ma trận chuyển đổi từ hệ trục tọa độ mặt đất sang hệ trục tọa độ vật thể.
Trang 33
2.2.2.2. Ma trận chuyển vị 𝐓Θ
Ma trận chuyển vị 𝐓Θ là ma trận chuyển đổi vận tốc góc giữa khung tham chiếu B và E: 𝑇𝛩 = [ 1 𝑠𝜙𝑡0 𝑐𝜙𝑡0 0 𝑐𝜙 −𝑠𝜙 0 𝑠𝜙/𝑐𝜙 𝑐𝜙/𝑐𝜙 ] (2.5) 2.2.2.3. Ma trận tổng quát 𝐉Θ
Ma trận tổng quát JΘ được hình thành từ ma trận xoay RΘ và ma trận chuyển vị TΘ. Ma trận tổng quát JΘ biểu diễn cho động học Quadrotor. Ma trận JΘ được thiết lập như sau: 𝐽𝛩 = [ 𝑅𝛩 03𝑥3 03𝑥3 𝑇𝛩 ] 𝐽𝛩 = [ 𝑐𝜓𝑐𝜃 −𝑠𝜓𝑐𝜙 + 𝑐𝜓𝑠𝜃𝑠𝜙 𝑠𝜓𝑠𝜙 + 𝑐𝜓𝑠𝜃𝑐𝜙 𝑠𝜓𝑐𝜃 𝑐𝜓𝑐𝜙 + 𝑠𝜓𝑠𝜃𝑠𝜙 −𝑐𝜓𝑠𝜙 + 𝑠𝜓𝑠𝜃𝑐𝜙 −𝑠𝜃 𝑐𝜃𝑠𝜙 𝑐𝜃𝑐𝜓 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 𝑠𝜙𝑡0 𝑐𝜙𝑡0 0 𝑐𝜙 −𝑠𝜙 0 𝑠𝜙/𝑐𝜙 𝑐𝜙/𝑐𝜙] (2.6)
Trong đó: 03𝑥3: Ma trận kích thước 3x3 với các phần tử bằng 0.
2.2.2.4. Mối liên hệ giữa các vector động học Quadrotor trong các hệ tọa độ
Để chuyển đổi qua lại các vector trong hệ trục tọa độ E và hệ trục tọa độ B ta sử dụng các phương trình sau:
- Phương trình liên hệ giữa 2 vector vị trí 𝜉 trong hệ trục trục E và vector vận tốc 𝜗 trong hệ trục B thông qua ma trận tổng quát JΘ:
𝜉̇ = JΘ . 𝜗 (2.7)
- Phương trình mối quan hệ giữa vận tốc trong khung tham chiếu B với E: VE = Γ̇𝐸 = RΘ. VB (2.8)
Trang 34
- Phương trình mối quan hệ giữa vận tốc góc trong khung tham chiếu B với E thông qua ma trận chuyển vị TΘ: Θ̇𝐸 = TΘ. ωB (2.9)
2.3. Động lực học Quadrotor
Áp dụng định luật II Newton, ta tính tốn lực tác dụng và moment xoắn tác động lên thân Quadrotor.
Hình 2.11. Lực và moment tác động lên Quadrotor
Phương trình chuyển động tổng quát của một vật thể chuyển động tịnh tiến (Quy đổi từ hệ trục E sang hệ trục B) theo định luật II Newton:
𝑚. Γ̈𝐸 = 𝐹𝐸 𝑚. 𝑅̂𝛩. 𝑉̇ 𝐵 = 𝑅 𝛩. 𝐹𝐵 𝑚(𝑅𝛩. 𝑉̇𝐵 + 𝑅̇𝛩. 𝑉𝐵) = 𝑅𝛩. 𝐹𝐵 𝑚. 𝑅𝛩(𝑉̇𝐵 + 𝜔𝐵. 𝑉𝐵) = 𝑅𝛩. 𝐹𝐵 𝑚(𝑉̇𝐵+ 𝜔𝐵. 𝑉𝐵) = 𝐹𝐵 (2.10) Trong đó: m [Kg] : Khối lượng Quadrotor.
FE [N] : Vector lực trong hệ trục E.
Γ𝐸 [m/s2] : Vector gia tốc dài trong hệ trục E. 𝑉𝐵 [m/s2] : Vector gia tốc dài trong hệ trục B. 𝑅𝛩 : Ma trận xoay.
Trang 35
Phương trình mơ tả chuyển động xoay của Quadrotor (quy đổi từ hệ trục E sang hệ trục B) theo định luật II Newton:
𝐼. Θ̈𝐸 = 𝜏𝐸 𝐼. 𝑇𝛩. 𝜔̂̇𝐵𝜔𝐵 = 𝑇
𝛩. 𝜏𝐵
𝐼. 𝜔̇𝐵+ 𝜔. (𝐼. 𝜔𝐵) = 𝑇𝛩. 𝜏𝐵 (2.11) Trong đó: I [Nms2]: Ma trận quán tính trong hệ trục B.
𝛩̈𝐸 [rad/s2]: Vector gia tốc gốc trong hệ trục E. 𝜔̇𝐵 [rad/s2]: Vector gia tốc gốc trong hệ trục B.
Từ phương trình (2.10) và (2.11), ta được phương trình động lực học tổng quát cho khung cứng 6 bậc tự do nói chung cũng như Quadrotor nói riêng:
[𝑚 𝐼3𝑥3 03𝑥3 03𝑥3 𝐼3𝑥3] [𝑉 ̇𝐵 𝜔̇𝐵] + [𝜔 𝐵𝑥(𝑚 . 𝑉𝐵) 𝜔𝐵𝑥(𝐼 . 𝑉𝐵)] = [𝐹𝐵 𝜏𝐵] (2.12) Trong đó:
𝐹𝐵 và 𝜏𝐵 (Vector lực tác dụng và vector moment xoắn) đặc trưng cho nguyên nhân gây ra chuyển động của Quadrotor.
I3x3 là ma trận đơn vị kích thước 3x3 𝐼 = [ 𝐼𝑋𝑋 0 0 0 𝐼𝑌𝑌 0 0 0 𝐼𝑍𝑍 ] (2.13)
Để tiến hành mơ hình hóa hệ Quadrotor, ta đơn giản hóa mơ hình Quadrotor với giả thiết sau:
- Gốc tọa độ của hệ trục gắn với khung OB trùng với trọng tâm khối lượng của khung Quadrotor nhằm tính tốn dễ dàng hơn.
- Quy định các trục của hệ B trùng với trục quán tính chính của khung. Trong trường hợp này, ma trận quán tính I là ma trận chéo và lúc này các phương trình của khung Quadrotor trở nên dễ dàng hơn.
Trang 36
𝛬 = [𝐹𝐵 𝜏𝐵]𝑇 = [𝐹𝑥 𝐹𝑦 𝐹𝑧 𝜏𝑥 𝜏𝑦 𝜏𝑧 ]𝑇 (2.14) Ta có thể viết lại phương trình (2.14) dưới dạng ma trận:
𝑀𝐵. 𝜗̇ + 𝐶𝐵(𝜗)𝜗 = 𝛬 (2.15) Với: 𝑀𝐵 = [𝑚 𝐼0 3𝑥3 03𝑥3 3𝑥3 𝐼3𝑥3] = [ 𝑚𝐼𝑋𝑋 0 0 0 𝑚𝐼𝑌𝑌 0 0 0 𝑚𝐼𝑍𝑍 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 𝐼𝑋𝑋 0 0 0 𝐼𝑌𝑌 0 0 0 𝐼𝑍𝑍] (2.16)
Với 𝑀𝐵: Ma trận quán tính hệ thống xét theo hệ trục B.
𝐶𝐵(𝜗) = 𝜗 [03𝑥3 −𝑚. 𝑆(𝑉 𝐵) 03𝑥3 −𝑆(𝐼. 𝜔𝐵)] = [ 0 0 0 0 0 0 0 0 0 0 𝑚𝜔 −𝑚𝑣 −𝑚𝜔 0 𝑚𝑢 𝑚𝑣 −𝑚𝑢 0 0 0 0 0 0 0 0 0 0 0 𝐼𝑍𝑍𝑟 −𝐼𝑌𝑌𝑞 −𝐼𝑍𝑍𝑟 0 𝐼𝑋𝑋𝑝 𝐼𝑌𝑌𝑞 𝐼𝑋𝑋𝑝 0 ] (2.17)
Với 𝐶𝐵(𝜗): Ma trận Coriolis hướng tâm xét theo hệ trục B.
Trong đó, ma trận đối xứng lệch S được định nghĩa qua vector 3 chiều k theo phương trình: 𝑆(𝑘) = −𝑆𝑇(𝑘) = [ 0 −𝑘3 𝑘2 𝑘3 0 −𝑘1 −𝑘2 𝑘1 0 ] 𝑘 = [ 𝑘1 𝑘2 𝑘3 ] (2.18)
Phương trình (2.19) xác định tốc độ quay Ω[rad/s] của toàn thể cánh quạt
𝛺 = −𝛺1+ 𝛺2− 𝛺3+ 𝛺4 𝛺 = [ 𝛺1 𝛺2 𝛺3 𝛺4 ] (2.19)
Trong đó: 𝛺1, 𝛺2, 𝛺2, 𝛺3 lần lượt là tốc độ cánh quạt trước, phải, sau và trái. Lực và moment xoắn được trực tiếp gây ra bởi chuyển động chính của các yếu tố đầu vào. Xét theo khí động học, cả lực và moment xoắn đều tỉ lệ thuận với bình phương tốc độ cánh quạt.
Trang 37
Phương trình (2.20) cho thấy tác động của vector chuyển động lên Quadrotor:
𝑈𝐵(𝛺) = 𝐸𝐵𝛺2 = [ 0 0 𝑈1 𝑈2 𝑈3 𝑈4] = [ 0 0 𝑏(𝛺12+ 𝛺22+ 𝛺32+ 𝛺42) 𝑏𝑙(−𝛺22+ 𝛺42) 𝑏𝑙(−𝛺12+ 𝛺32) 𝑑(−𝛺12+ 𝛺22− 𝛺32+ 𝛺42)] (2.20)
Trong đó: l [m]: Khoảng cách từ tâm Quadrotor đến tâm của một cánh quạt. 𝑈1, 𝑈2, 𝑈3 và 𝑈4: Thành phần vector chuyển động.
Như đã nêu trong phương trình (2.20), ta có thể xác định ma trận hằng 𝐸𝐵 qua phương trình (2.21). 𝐸𝐵 = [ 0 0 0 0 𝑏 𝑏 0 0 0 0 𝑏 𝑏 0 −𝑏𝑙 −𝑏𝑙 0 −𝑑 𝑑 0 𝑏𝑙 𝑏𝑙 0 −𝑑 𝑑 ] (2.21)
Trình bày lại các biểu thức trên dưới dạng hệ phương trình:
{ 𝑢̇ = (𝑣𝑟 − 𝜔𝑞) + 𝑔𝑠𝜃 𝑣̇ = (𝜔𝑝 − 𝑢𝑟) − 𝑔𝑐𝜃𝑠𝜙 𝜔̇ = (𝑢𝑞 − 𝑣𝑝) − 𝑔𝑐𝜃𝑠𝜙+𝑈1 𝑚 𝑝̇ =𝐼𝑌𝑌−𝐼𝑍𝑍 𝐼𝑋𝑋 𝑞𝑟 + 𝐽𝑟 𝐼𝑋𝑋𝑞𝛺 + 𝑈2 𝐼𝑋𝑋 𝑞̇ =𝐼𝑍𝑍−𝐼𝑋𝑋 𝐼𝑌𝑌 𝑞𝑟 − 𝐽𝑟 𝐼𝑌𝑌𝑞𝛺 + 𝑈3 𝐼𝑌𝑌 𝑟̇ =𝐼𝑋𝑋−𝐼𝑌𝑌 𝐼𝑍𝑍 𝑞𝑟 + 𝑈4 𝐼𝑍𝑍 (2.22)
Trong đó, tốc độ đầu vào của các cánh quạt được cho thơng qua phương trình (2.23): { 𝑈1 = 𝑏(𝛺12+ 𝛺22+ 𝛺32+ 𝛺42) 𝑈2 = 𝑏𝑙(−𝛺22+ 𝛺42) 𝑈3 = 𝑏𝑙(−𝛺12+ 𝛺32) 𝑈4 = 𝑑(−𝛺12+ 𝛺22− 𝛺32+ 𝛺42) 𝛺 = −𝛺1+ 𝛺2− 𝛺3+ 𝛺4 (2.23)
Trang 38 { 𝑋̈ = (𝑠𝜓𝑠𝜙 + 𝑐𝜓𝑠𝜃𝑐𝜙)𝑈1 𝑚 𝑌̈ = (−𝑐𝜓𝑠𝜙 + 𝑠𝜓𝑠𝜃𝑐𝜙)𝑈1 𝑚 𝑍̈ = −𝑔 + (𝑐𝜃𝑐𝜙)𝑈1 𝑚 𝜙̈ =𝐼𝑌𝑌−𝐼𝑍𝑍 𝐼𝑋𝑋 𝑞𝑟 + 𝐽𝑟 𝐼𝑋𝑋𝑞𝛺 +𝑙𝑈2