2.2. Mô hình hóa Quadrotor
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)