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 quán 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 qn tính trái đất.
- Góc Pitch là góc xoay quanh trục Oy trong hệ quy chiếu qn tính trái đất.
Trang 30
- Góc Yaw là góc xoay quanh trục Oz trong hệ quy chiếu qn 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.
- FB [N]: Vector lự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.
- Xoay quanh trục xE một góc 𝜙(roll) ta được ma trận xoay R(𝜙, x).
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)