Quadrotor / multirotor UAV — нелинейная 6-DoF динамика¶
Модель квадрокоптера с жёстким каркасом (rigid-body) в полной 6-DoF формулировке: 12 состояний (положение, скорость, ориентация, угловые скорости), 4 управляющих входа (общая тяга + три момента в связанной СК). Реализована на чистом Python/NumPy с интеграторами Эйлера и RK4. Подходит для PID-стабилизации, MPC слежения по траектории, и для адаптивных RL-критиков (iADP, AIDI, AA-INDI) — особенно для сценариев с отказами роторов.
-
Быстрый старт
Прогоните 10-секундный hover в три строки.
-
API модели
NonlinearQuadrotor— Python-класс 6-DoF динамики. -
Математика
Уравнения движения, конвенции NED / связанной СК.
-
Тесты-примеры
Hover, свободное падение, гироскопическая связь.
Параметры эталонной модели (справочно)¶
Эталон — типовой исследовательский квадрокоптер класса AscTec
Hummingbird / Pelican, ≈1.5 кг, плечо 22.5 см. Эти числа — дефолт
в QuadrotorParameters.default_parameters(); их можно переопределить
для других платформ (DJI F450, Crazyflie, X4-frame и т.д.).
| Параметр | Значение |
|---|---|
| Масса \(m\), кг | 1.5 |
| Момент инерции \(J_x = J_y\), кг·м² | 0.0211 |
| Момент инерции \(J_z\), кг·м² | 0.0366 |
| Длина плеча, м | 0.225 |
| Линейный body-drag \(k_{dx}=k_{dy}\), Н·с/м | 0.10 |
| Линейный body-drag \(k_{dz}\), Н·с/м | 0.20 |
| Максимальная общая тяга, Н | 30 (≈ 2:1 thrust-to-weight) |
| Максимальный момент по оси, Н·м | 1.5 |
Системы координат и состояние¶
В модели используются две правые ортогональные системы координат:
- Связанная (body-fixed): \(x\) — вперёд, \(y\) — вправо, \(z\) — вниз. Это совпадает с NED при горизонтальной ориентации.
- Земная (NED, north-east-down): \(x\) — север, \(y\) — восток, \(z\) — вниз. Гравитация направлена по \(+z\).
Преобразование «связанная → земная» — стандартная матрица поворота с последовательностью углов Эйлера ZYX (321: рысканье → тангаж → крен).
Вектор состояния и управляющее воздействие:
- \(x_e, y_e, z_e\) — положение в земной СК (NED), м.
- \(u_b, v_b, w_b\) — скорость в связанной СК, м/с.
- \(\phi, \theta, \psi\) — углы крена, тангажа, рысканья (Эйлер ZYX), рад.
- \(p, q, r\) — угловые скорости в связанной СК, рад/с.
- \(T\) — общая тяга вдоль \(-z\) связанной СК (вверх при горизонтали), Н.
- \(\tau_x, \tau_y, \tau_z\) — моменты в связанной СК (крен, тангаж, рыскание), Н·м.
Управляющий вектор \(\mathbf{u}\) — это уже выходы allocation-блока (motor mixing). Маппинг 4 оборотов роторов \(\omega_i\) в \((T, \tau)\) зависит от конфигурации (X / +) и не входит в эту модель — он реализуется отдельным хелпером (или внутри RL-агента).
Сингулярность Эйлера
При \(|\theta| = \pi/2\) возникает gimbal lock: множитель \(1/\cos\theta\) в кинематике углов уходит в бесконечность. Для манёвров с большим тангажом используйте кватернионную форму (в данной модели не реализована — задача будущего расширения).
Математическая модель¶
Объединённая система ОДУ имеет четыре блока: кинематика положения, динамика скорости, кинематика углов Эйлера, динамика угловых скоростей.
1. Кинематика положения¶
где \(R_{eb}\) — матрица поворота из связанной в земную СК (ZYX 321):
(сокращения \(c_\bullet = \cos\bullet\), \(s_\bullet = \sin\bullet\)).
2. Динамика скорости (связанная СК)¶
Уравнение Ньютона в связанной СК с учётом вращения, силы тяжести, тяги и линейного аэродинамического сопротивления:
где:
- \(\mathbf{F}_{\text{grav},e} = [0, 0, m g]^\top\) — сила тяжести в NED (\(+z\) — вниз),
- \(\mathbf{F}_{\text{thrust},b} = [0, 0, -T]^\top\) — тяга вдоль \(-z\) связанной,
- \(D = \mathrm{diag}(k_{dx}, k_{dy}, k_{dz})\) — диагональная матрица drag,
- \(\boldsymbol\omega = [p, q, r]^\top\).
3. Кинематика углов Эйлера (ZYX 321)¶
где \(t_\theta = \tan\theta\).
4. Динамика угловых скоростей (Newton-Euler)¶
С диагональным тензором инерции \(\mathbf{J} = \mathrm{diag}(J_x, J_y, J_z)\):
Слагаемые \((J_i - J_j)\,\omega_i\,\omega_j\) — гироскопическая связь осей (Эйлеровы перекрёстные члены).
Быстрый старт¶
import numpy as np
from tensoraerospace.aerospacemodel.quadrotor.nonlinear import NonlinearQuadrotor
# Hover на месте при нулевой начальной точке
m = NonlinearQuadrotor(
x0=np.zeros(12),
dt=0.01,
integrator="rk4",
)
# T = m·g удерживает аппарат в равновесии
u_hover = np.array([m.hover_thrust, 0.0, 0.0, 0.0])
for _ in range(1000): # 10 секунд
m.run_step(u_hover)
print(f"Финальное состояние (max |x|): {np.max(np.abs(m.current_state)):.2e}")
# → ≈ 0 (точное равновесие при RK4)
Hover с возмущением и маленьким моментом крена¶
import numpy as np
from tensoraerospace.aerospacemodel.quadrotor.nonlinear import (
NonlinearQuadrotor,
set_initial_state,
)
# Стартуем с лёгким наклоном в 3°
m = NonlinearQuadrotor(
x0=set_initial_state(phi=np.deg2rad(3.0)),
dt=0.005,
integrator="rk4",
)
# Тяга баланса + небольшая отрицательная крен-команда (компенсация)
T_hover = m.hover_thrust
for k in range(2000):
tau_x = -0.05 * m.current_state[6] # P-обратная связь по phi
u = np.array([T_hover, tau_x, 0.0, 0.0])
m.run_step(u)
phi_final = np.rad2deg(m.current_state[6])
print(f"Финальный угол крена: {phi_final:.4f}°")
Хелпер для смены начального состояния¶
from tensoraerospace.aerospacemodel.quadrotor.nonlinear import set_initial_state
# Стартовое положение: 5 м над землёй (NED z = -5), наклон по тангажу 5°
x0 = set_initial_state(z_e=-5.0, theta=np.deg2rad(5.0))
Валидация¶
Корректность ОДУ подтверждена пятью аналитическими тестами в
tests/aerospacemodel/quadrotor_test.py:
| Тест | Проверка | Допуск |
|---|---|---|
| Hover-равновесие | \(T = m\,g\), нулевые моменты → состояние не меняется за 10 с | \(\max\|x\| < 10^{-9}\) |
| Свободное падение без drag | \(z_e(t) = \tfrac{1}{2}g\,t^2\) | \(\delta < 10^{-3}\) |
| Свободное падение с drag | \(z(t) = v_\infty t + v_\infty\tau(e^{-t/\tau} - 1)\) | \(\delta < 10^{-3}\) |
| Single-step roll-torque | \(p \approx \tau_x \cdot dt / J_x\) | \(\delta < 10^{-9}\) |
| Гироскопическая связь | \(\dot p = (J_y-J_z)\,q\,r / J_x\) при \(q=r=1\) | \(\delta < 10^{-12}\) |
Плюс 6 sanity-проверок: размерности входов, accumulation истории, поддержка обоих интеграторов (Euler + RK4), валидация ошибочного имени интегратора.
Allocation (mixer): связь между виртуальным управлением и роторами¶
Базовая модель NonlinearQuadrotor принимает виртуальный вектор
\((T, \tau_x, \tau_y, \tau_z)\) — это уровень, на котором работают
PID/MPC/RL-контроллеры. Для воспроизведения отказов на уровне
ротора (что необходимо для FTC-сценариев из Lu 2019, Wang 2019,
Lanzon 2015) есть XConfigAllocator — двунаправленный mixer:
где \(a = L/\sqrt 2\), \(L\) — длина плеча, \(k_T\) — thrust coefficient, \(k_M\) — yaw-torque coefficient. Матрица full-rank → инверсия существует.
from tensoraerospace.aerospacemodel.quadrotor import default_allocator
alloc = default_allocator() # k_T=7.5e-6, k_M≈0.016·k_T, arm=0.225
v = alloc.mix(omega_squared) # 4 ω² → [T, τ]
omega2 = alloc.unmix(v) # [T, τ] → 4 ω² (может быть < 0 для нереализуемых v)
omega2 = alloc.saturate(omega2, 0, 1000) # клип до физических ограничений
Среда NonlinearQuadrotor-v0 поддерживает два режима подачи действия:
# Режим "virtual" (default): action = [T, τ_x, τ_y, τ_z]
env = gym.make("NonlinearQuadrotor-v0", initial_state=np.zeros(12),
number_time_steps=1000, action_space="virtual")
# Режим "rotor": action = [ω₁², ω₂², ω₃², ω₄²], env применяет allocator
env = gym.make("NonlinearQuadrotor-v0", initial_state=np.zeros(12),
number_time_steps=1000, action_space="rotor")
Без damage_profile оба режима эквивалентны (round-trip mix↔unmix).
Подсистема повреждений (rotor-level events)¶
Для воспроизведения канонических FTC-сценариев из литературы доступна event-driven система отказов на уровне ротора. Каждый ротор имеет коэффициент эффективности \(\mu_i \in [0, 1]\), и среда применяет \(\omega^2_{i,\text{eff}} = \mu_i \cdot \omega^2_{i,\text{cmd}}\) перед mixing'ом обратно в \((T, \tau)\).
Три типа событий:
| Событие | Семантика | Источник |
|---|---|---|
RotorDamageEvent(rotor_id, mu) |
Мгновенная потеря эффективности на ротор \(i\) | Lu et al. 2019 |
RotorLossEvent(rotor_id) |
Полный отказ (\(\mu = 0\)) | Lanzon et al. 2015 |
MotorEfficiencyDecay(rotor_id, tau, mu_floor) |
Экспоненциальный износ \(\dot\mu = -(1/\tau)(\mu - \mu_\text{floor})\) | gradual wear |
Три готовых пресета: LANZON_M1_LOSS, LU_M1_50PCT_LOSS,
WEAR_DEGRADATION_M3. Импорт из
tensoraerospace.aerospacemodel.quadrotor.damage.
from tensoraerospace.aerospacemodel.quadrotor.damage import LANZON_M1_LOSS
env = gym.make("NonlinearQuadrotor-v0", initial_state=np.zeros(12),
number_time_steps=2000, damage_profile=LANZON_M1_LOSS)
obs, _ = env.reset()
T_hover = 1.5 * 9.81
for k in range(2000):
obs, r, term, trunc, info = env.step(np.array([T_hover, 0, 0, 0]))
if "damage_events_triggered" in info:
print(f"t={k*0.01}s: {info['damage_events_triggered']}")
Без damage_profile среда бит-в-бит идентична baseline (rotor-effectiveness
\(\mu = 1\) для всех 4 моторов).
Ограничения текущей версии¶
- Эйлеровы углы (ZYX 321) — модель страдает gimbal-lock'ом при \(|\theta| = \pi/2\). Для акробатических манёвров (loops, flips) нужна кватернионная форма — задача будущего расширения.
- Линейный drag только — нет квадратичного по скорости (значимо на крейсерских >10 м/с) и нет blade-flap aerodynamics.
- Симметричная X-конфигурация в allocator'е — для несимметричных рам (Y, H, гексакоптер) потребуется отдельный allocator.
- Простой column-clip saturation — нет thrust-priority allocation (Faessler 2017) для случаев насыщения, когда нужно сохранить приоритет одного канала над другим.
Источники¶
Динамика и системы координат¶
- Stevens, B. L., Lewis, F. L., Johnson, E. N. (2015). Aircraft Control and Simulation, 3rd ed. Wiley. — основная методологическая база для 6-DoF rigid-body уравнений в связанной СК (NED frame, ZYX Euler convention).
- PX4 Airframe Reference: Quadrotor X
— конвенция нумерации моторов и направлений вращения,
использованная в
XConfigAllocator.
Параметры эталонной платформы¶
- AscTec Hummingbird / Pelican research-class quadrotors — типовые значения \(m \approx 1.5\) кг, \(J_x = J_y \approx 0.021\) кг·м², \(J_z \approx 0.037\) кг·м², плечо 22.5 см. Платформы широко используются в академических FTC-работах по UAV.
FTC-сценарии (источники для damage subsystem и пресетов)¶
- Lu, P., Yu, B., van Kampen, E.-J., Chu, Q. P. (2019).
Quadrotor Fault Tolerant Incremental Sliding Mode Control driven by
Sliding Mode Disturbance Observers. Aerospace Science and Technology,
87:417–430. DOI: 10.1016/j.ast.2019.03.001
— multiplicative effectiveness loss на роторах, основа для пресета
LU_M1_50PCT_LOSSи событияRotorDamageEvent. 129 цитирований. - Wang, X., van Kampen, E.-J., Chu, Q. P. (2019). Quadrotor fault-tolerant incremental nonsingular terminal sliding mode control. Aerospace Science and Technology, 95:105514. DOI: 10.1016/j.ast.2019.105514 — параллельная работа с terminal sliding mode; та же модель отказов.
- Lanzon, A., Freddi, A., Longhi, S. (2015). Active fault-tolerant
control for quadrotors subjected to a complete rotor failure.
IEEE/RSJ IROS 2015. DOI: 10.1109/IROS.2015.7354046
— экстремальный сценарий полной потери ротора (spin-mode
recovery), основа для
LANZON_M1_LOSSиRotorLossEvent.
Связанные топики в TensorAeroSpace¶
- Моделирование повреждений ЛА (F-16) — более развитая damage-подсистема для самолётов с strip-theory и пересчётом тензора инерции через теорему Гюйгенса-Штейнера.
- AIDI — адаптивная INDI-подобная архитектура, применимая к данной модели quadrotor для отказоустойчивого управления.
- iADP, IM-GDHP — онлайн- адаптивные критики, восстанавливающиеся после изменений в плант- модели за десятки миллисекунд.
Python API¶
NonlinearQuadrotor(x0, selected_state_output=None, t0=0, dt=0.01, integrator='rk4')
¶
Bases: ModelBase
Pure-numpy 6-DoF rigid-body quadrotor in NED frame.
Action: [T, tau_x, tau_y, tau_z] — collective thrust (N) and
body-frame torques (N·m). The model is "abstract" in the actuation:
motor mixing / RPM allocation is not part of this class. Use the
helper in utils.py (TODO) or the env layer for that.
State: see module docstring of __init__.py for the 12-element
layout.
QuadrotorParameters(m=1.5, g=9.81, Jx=0.0211, Jy=0.0211, Jz=0.0366, arm_length=0.225, kdx=0.1, kdy=0.1, kdz=0.2, thrust_max=30.0, thrust_min=0.0, torque_max=1.5)
dataclass
¶
Inertial + geometric parameters of a single rigid-body quadrotor.
quadrotor_ode_6dof(x, u, t, params)
¶
Right-hand side of the 6-DoF quadrotor ODE.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
x
|
ndarray
|
state vector (12,) — see module docstring for layout. |
required |
u
|
ndarray
|
control vector (4,) — |
required |
t
|
float
|
current time, s (unused — model is autonomous). |
required |
params
|
QuadrotorParameters
|
inertial / geometric parameters. |
required |
Returns:
| Type | Description |
|---|---|
ndarray
|
|
XConfigAllocator(k_T=7.5e-06, k_M=1.2e-07, arm_length=0.225)
dataclass
¶
Bidirectional X-quad rotor↔virtual allocator.
The allocation matrix :math:M is defined by
.. math::
\begin{bmatrix} T \\ \tau_x \\ \tau_y \\ \tau_z \end{bmatrix}
= M \begin{bmatrix} \omega_1^2 \\ \omega_2^2 \\ \omega_3^2 \\ \omega_4^2 \end{bmatrix},
where (with :math:a = L/\sqrt 2)
.. math::
M = \begin{bmatrix}
k_T & k_T & k_T & k_T \\
-k_T a & k_T a& k_T a& -k_T a \\
k_T a & -k_T a& k_T a& -k_T a \\
k_M & k_M & -k_M & -k_M
\end{bmatrix}.
The matrix is full-rank for non-degenerate parameters, so the
inverse :func:unmix is well-defined.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
k_T
|
float
|
thrust coefficient (N per :math: |
7.5e-06
|
k_M
|
float
|
yaw-torque coefficient (N·m per :math: |
1.2e-07
|
arm_length
|
float
|
distance from CG to each rotor, m. |
0.225
|
matrix
property
¶
Read-only 4×4 forward allocation matrix M.
matrix_inverse
property
¶
Read-only 4×4 inverse matrix M⁻¹.
mix(omega_squared)
¶
Map :math:(\omega_1^2, \ldots, \omega_4^2) → :math:(T, \tau_x, \tau_y, \tau_z).
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
omega_squared
|
ndarray
|
shape |
required |
Returns:
| Type | Description |
|---|---|
ndarray
|
Virtual control vector |
unmix(virtual)
¶
Map :math:(T, \tau_x, \tau_y, \tau_z) → :math:(\omega_1^2, \ldots, \omega_4^2).
Note: the result may contain negative entries when the
commanded virtual triple is not realisable by 4 unidirectional
rotors (e.g. requesting :math:T = 0 with non-zero pitch torque).
The caller should pass the result through :func:saturate before
feeding it into :func:mix.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
virtual
|
ndarray
|
shape |
required |
Returns:
| Type | Description |
|---|---|
ndarray
|
Rotor speeds-squared |
ndarray
|
Possibly negative — pass through :func: |
saturate(omega_squared, omega_min=0.0, omega_max=1000.0)
¶
Clip rotor speeds-squared into :math:[\omega_\min^2, \omega_\max^2].
Element-wise clip — does not preserve any virtual quantity. For thrust-priority allocation strategies, see the literature (e.g. Faessler 2017); this method is the simplest sane default.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
omega_squared
|
ndarray
|
shape |
required |
omega_min
|
float
|
lower rotor-speed bound, rad/s. Default 0 (perpetual zero allowed — useful for rotor-loss scenarios). Must be non-negative. |
0.0
|
omega_max
|
float
|
upper rotor-speed bound, rad/s. Default 1000 (≈ 9550 RPM). |
1000.0
|
Returns:
| Type | Description |
|---|---|
ndarray
|
Clipped |
RotorDamageEvent(trigger_time, rotor_id, label=None, mu=1.0)
dataclass
¶
Bases: DamageEvent
Instantaneous multiplicative effectiveness loss on rotor rotor_id.
mu is the surviving fraction (1.0 = healthy, 0.0 = total loss).
Mirrors the convention in Lu 2019 (ISMC FTC):
:math:\omega^2_{i,\text{eff}} = \mu \cdot \omega^2_{i,\text{cmd}}.
RotorLossEvent(trigger_time, rotor_id, label=None)
dataclass
¶
Bases: DamageEvent
Complete rotor stop — mu = 0 (Lanzon 2015 catastrophic case).
MotorEfficiencyDecay(trigger_time, rotor_id, label=None, tau=1.0, mu_floor=0.0)
dataclass
¶
Bases: DamageEvent
Exponential effectiveness decay starting from trigger_time.
From the trigger onward, mu[i] evolves as
.. math:: \dot \mu_i = -(1/\tau)(\mu_i - \mu_\text{floor}),
so mu exponentially approaches mu_floor with time-constant
tau. Applies a one-shot setter at trigger time; the actual
decay is integrated by :meth:RotorDamageState.step_decay on each
env step.
RotorDamageManager(profile=None)
¶
Owns :class:RotorDamageState and replays events over the episode.
Used by the env layer: on every integrator tick the env calls
:meth:update with the current and previous timestamps; events
that fall in that window are applied to the state. Time-decay
rotors are advanced one Euler step.
reset(*, seed=None)
¶
Clear all damage and re-baseline (called by env.reset).
seed is accepted for compatibility with gym.Env.reset.
inject_event(event)
¶
Add a one-shot event for this episode (single-fire).
update(t_current, t_previous, dt)
¶
Apply events in (t_previous, t_current] and advance decay.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
t_current
|
float
|
time at the END of the integrator step. |
required |
t_previous
|
float
|
time at the START of the integrator step. |
required |
dt
|
float
|
integrator step size (used by |
required |
Returns:
| Type | Description |
|---|---|
list[DamageEvent]
|
List of events that fired during this step (for logging). |
NonlinearQuadrotorEnv(initial_state, number_time_steps, *, dt=0.01, integrator='rk4', action_space='virtual', allocator=None, damage_profile=None, damage_event_callback=None, omega_min=0.0, omega_max=1000.0)
¶
Bases: Env
Gymnasium env over the pure-numpy nonlinear 6-DoF quadrotor.
Parameters:
| Name | Type | Description | Default |
|---|---|---|---|
initial_state
|
ndarray
|
12-element initial state. See
:mod: |
required |
number_time_steps
|
int
|
Episode length cap (steps). |
required |
dt
|
float
|
Discretisation step (s). Default 0.01. |
0.01
|
integrator
|
Literal['euler', 'rk4']
|
|
'rk4'
|
action_space
|
Literal['virtual', 'rotor']
|
|
'virtual'
|
allocator
|
Optional[XConfigAllocator]
|
X-config allocator to use when |
None
|
damage_profile
|
Optional[DamageProfile]
|
Optional :class: |
None
|
damage_event_callback
|
Optional[Callable[[Any, Any], None]]
|
Optional |
None
|
omega_min
|
float
|
Lower rotor-speed bound, rad/s. Used in the env's saturation step before mixing. Default 0. |
0.0
|
omega_max
|
float
|
Upper rotor-speed bound, rad/s. Default 1000. |
1000.0
|
