Система автоматического управления (САУ)

Справочник по САУ симулятора: из каких контуров она состоит, как каждый контур устроен, откуда берутся его коэффициенты и как он ведёт себя на полной модели. Модель самолёта — в physics.md и aerodynamics.md, датчики — в SENSORS.md.

Все числа ниже — для FPV-самолёта по умолчанию (AircraftParams()) на крейсерской скорости 16 м/с. Это результаты моделирования, а не эксперимента: графики посчитаны на полной нелинейной модели 6DOF с приводами рулей скриптом docs/figures/sau_plots.py, схемы нарисованы docs/figures/sau_diagrams.py. После изменения кода их можно перерисовать — числа в таблицах печатает тот же скрипт.

Содержание

Обзор
Как рассчитаны коэффициенты
Тангаж
Высота и скорость: TECS и раздельные контуры
Крен
Курс и скольжение
Защита по углу атаки
Навигация по точкам
САУ одним классом — SAU
LQR
Проверки
Ограничения и что дальше

1. Обзор

1.1 Структура

САУ — один класс control.sau.SAU (раздел 9). На каждом шаге интегрирования (100 Гц) он делает одно и то же:

Датчики (sensors) превращают истинное состояние в измерения — с шумом (NoisySensors, сценарии s8–s10), реалистичные (RealisticSensors, 3D-режимы по умолчанию) или без шума (TruthSensors, 3D с --sensors truth).
Оценка состояния (estimator) — инерциальный EKF StateEstimator при реалистичных датчиках (раздел 9.3); иначе оценка = измерения.
Наведение (nav) — в режиме маршрута навигатор выдаёт уставки курса и высоты.
Продольный внешний контур (lon) по уставкам высоты и скорости даёт команду тангажа θ_cmd и газ δt. По умолчанию это TECS (раздел 4).
Защита по углу атаки (prot) может заменить θ_cmd и газ, если α подходит к критическому (раздел 7).
Тангаж (pitch) отрабатывает θ_ref рулём высоты δe (раздел 3).
Боковой канал (lat): курс → уставка крена → элероны δa; скольжение → руль направления δr (разделы 5–6).

Команды [δe, δt, δa, δr] идут в приводы рулей (sim/actuators.py: апериодическое звено, ограничение скорости, упоры — physics.md, раздел 6). Привод — часть объекта управления, а не САУ. Управление на шаге интегрирования не меняется (дискретная САУ).

1.2 Контуры

Контур	Вход → выход	Класс (файл)	Раздел
Тангаж	θ_ref → q_ref → δe	`PitchController` (controllers.py)	3
Высота и скорость — по умолчанию	h_ref, Va_ref → θ_cmd, δt через энергию	`TECS` (tecs.py)	4.1
Высота и скорость — раздельно	h → θ_cmd (ПИ), Va → δt (ПИ)	`AltSpeedHold` = `AltitudeHold` + `SpeedController`	4.2
Крен	φ_ref → p_ref → δa (упреждение + ПИ по p)	`RollController` (controllers.py)	5
Курс	χ_ref, κ → φ_ref	`LateralController` (controllers.py)	6.1
Скольжение	β → δr, + микс δa → δr	`LateralController`	6.2
Защита по α	α → замена θ_cmd и газа	`AngleOfAttackProtector` (aua.py)	7
Навигация	маршрут, GPS → χ_ref, h_ref, κ, ḣ_ff	`WaypointNavigator` (navigation.py)	8
LQR (альтернатива продольному каскаду)	[u, w, q, θ], h → δe, δt	`LQRController` (lqr.py)	10

1.3 Режимы

Режим `Mode`	Что задаёт оператор	Что держит САУ
`PITCH`	тангаж θ, курс χ, скорость Va	θ и χ; газ держит скорость
`ALTITUDE`	высоту h, курс χ, скорость Va	h, χ, Va
`ROUTE`	маршрут (точки N, E, h), скорость Va	курс и высоту выдаёт навигатор

Скорость держится во всех режимах.

1.4 Где используется

Программа	Как подключена САУ
3D-тренажёр `GameScenario3D`	`SAU` с истинными датчиками; режимы и уставки — клавиши, маршрут — окно станции
стенд `Bench3D`	подкласс `BenchSAU`: + защита по α и режим «Срыв»; параметры — ползунками на лету
конструктор `BlockLab3D`	часть каналов ведёт схема из блоков (`control/blocks.py`), остальные — `SAU`
сценарии s8–s10	`scenarios/lateral_common.FullSAU` — обёртка `SAU` с псевдодатчиками
сценарии s1–s6	регуляторы напрямую: `PitchController`, `SpeedController`, П-контур высоты, крен — `RollHold`
сценарий s7	`LQRController`

1.5 Знаки рулей

Знаки рулей — как в physics.md, раздел 2. Для САУ важны два:

δe > 0 — нос вниз. Поэтому в контуре тангажа стоит «−Kq»: чтобы поднять нос, руль отклоняют в минус.
δr > 0 — нос влево. Чтобы убрать скольжение β > 0 (поток справа), нос доворачивают вправо: δr = −K·β.

2. Как рассчитаны коэффициенты

Коэффициенты САУ не подбираются вручную — они рассчитываются по модели самолёта (control/tuning.py, решение РЕШ-21). Умолчания классов регуляторов (PitchControlParams, TECSParams, RollControlParams и т. д.) — результат этого расчёта для FPV-самолёта на 16 м/с; checks/check_sau.py проверяет, что они совпадают.

2.1 Метод: последовательное замыкание контуров

Метод — из учебника Beard & McLain (B&M), гл. 6. Каждый контур рассчитывается по упрощённой линейной модели как звено второго (реже первого) порядка с заданными собственной частотой ωn и демпфированием ζ. Внешний контур делается медленнее внутреннего, чтобы для него внутренний контур выглядел почти идеальным звеном «выход = уставка». Тогда контуры можно рассчитывать по очереди, изнутри наружу.

Как разнесены полосы:

Продольный канал. Тангаж (ωn = 15 рад/с) работает на частоте короткопериодического движения ЛА (16 рад/с) — он его демпфирует. Темпы энергии TECS (2 рад/с) в 7 раз медленнее тангажа, высота и скорость (0.5 1/с) — ещё в 4 раза медленнее.
Боковой канал. Скорость крена (полюс 75 рад/с) чуть быстрее собственного движения крена ЛА (56 1/с). Угол крена (4 рад/с) — в 19 раз медленнее. Курс (двойной полюс 2 рад/с) всего в 2 раза медленнее крена: инерция крена учтена в расчёте курса явно (раздел 6.1), поэтому большой разнос не нужен.
Навигация (выход на линию пути, ~0.1 рад/с) — самая медленная.

2.2 Упрощённые модели объекта

Модели — линеаризация около горизонтального полёта на скорости Va (B&M гл. 5). Коэффициенты считает plant_coeffs(aircraft, Va); Γ — комбинации моментов инерции (physics.md, раздел 3.2).

Канал	Модель	Коэффициенты	FPV, 16 м/с
крен	φ̈ = −a_φ1·φ̇ + a_φ2·δa	a_φ1 = −q̄Sb·C_p,p·b/(2Va), a_φ2 = q̄Sb·C_p,δa, где C_p,x = Γ3·Croll_x + Γ4·Cn_x	56.0 1/с; 593 1/с²
скольжение	β̇ = −a_β1·β + a_β2·δr	a_β1 = −ρVaS·CY_β/(2m), a_β2 = ρVaS·CY_δr/(2m)	0.427; 0.319
тангаж	θ̈ = −a_θ1·θ̇ − a_θ2·θ + a_θ3·δe	a_θ1 = −q̄c·Cmq·c/(2Va·Jy), a_θ2 = −q̄c·Cmα/Jy, a_θ3 = q̄c·Cmδe/Jy	15.5; 86.7; −224
скорость	V̇a = −a_V1·Va + a_V2·δt	из m·V̇a = T(δt, Va) − D(Va); производные тяги — численно	0.379 1/с; 11.5 м/с²
курс	χ̇ = (g/Vg)·tg φ	координированный разворот	—
высота	ḣ = Va·sin γ	—	—

Здесь q̄ = ½ρVa² — скоростной напор. Модель тяги винта не дифференцируется в явном виде, поэтому ∂T/∂δt и ∂T/∂Va берутся конечными разностями.

2.3 Требования и результат

Требования задаются структурой SAUSpecs, результат — SAUDesign.

Контур	Требование (`SAUSpecs`)	Формула	Коэффициенты
скорость крена	полюс λ_p = 75 рад/с	FF = a_φ1/a_φ2, kp_p = (λ_p − a_φ1)/a_φ2, ki_p = kp_p/τ_φ	FF = 0.094 с, kp_p = 0.032 с, ki_p = 0.128
угол крена	τ_φ = 0.25 с, p_max = 120 °/с	p_ref = (φ_ref − φ)/τ_φ	—
курс	ζ_χ = 1	K_χ = 1/(4ζ²τ_φ)	K_χ = 1.0 1/с
микс δa → δr	гасить момент рыскания элеронов	K = −N_δa/N_δr, N_x = Γ4·Croll_x + Γ8·Cn_x	−0.654
скольжение	kp_β = 0.5 (шум зонда), ζ_β = 0.7	ki_β = ((a_β1 + a_β2·kp_β)/(2ζ_β))²/a_β2	ki_β = 0.55 1/с
тангаж	ωn_θ = 15 рад/с, ζ_θ = 0.8	kp_θ = (ωn² − a_θ2)/a_θ3, kd_θ = (2ζωn − a_θ1)/a_θ3	Kθ = 16.2 1/с, Kq = 0.038 с
TECS: высота, скорость	K_h = K_V = 0.5 1/с	—	—
TECS: контуры Ė и Ḃ	ω_T = ω_B = 2 рад/с	K_E = g/a_V2, ki_T = ω_T·K_E, ki_B = ω_B/2	K_E = 0.852, ki_T = 1.70, ki_B = 1.0
раздельные: высота (ПИ)	ω_h = ωn_θ/30 = 0.5 рад/с, ζ_h = 1.5	kp_h = 2ζ_h·ω_h/Va, ki_h = ω_h²/Va	0.094 рад/м, 0.016 рад/(м·с)
раздельные: скорость (ПИ)	ωn_V = 1 рад/с, ζ_V = 0.8	kp_V = (2ζωn − a_V1)/a_V2, ki_V = ωn²/a_V2	0.106, 0.087
высота (П, сценарии s5–s6)	ω_h = 0.5 рад/с	KH = ω_h/Va	0.031 рад/м

2.4 Пересчёт для другого ЛА или другой скорости

from control.tuning import design_sau, SAUSpecs, print_design
from control.sau import SAU, SAUParams

d = design_sau(aircraft, Va=20.0, specs=SAUSpecs(wn_theta=12.0))
print_design(d)                                   # коэффициенты объекта и регуляторов
params = SAUParams(tecs=d.tecs, pitch=d.pitch, lateral=d.lateral,
                   speed=d.speed, altitude=d.altitude)

python -m control.tuning печатает расчёт для AircraftParams() на 16 м/с.

2.5 Где полная модель разошлась с учебником

Расчёт проверялся на полной модели (checks/check_sau.py, 3D-полёты). Найденные отличия учтены в tuning.py:

Коэффициент передачи тангажа. В модели B&M угол атаки жёстко связан с тангажом (α ≡ θ), и замкнутый контур тангажа даёт K = 0.61. В полной модели α возвращается к балансировочному, и θ отрабатывает уставку полностью. Для внешних контуров принято K = 1.
Предел q_ref каскада тангажа. При пределе 60 °/с руль высоты ограничивался Kq·q_max ≈ 2°, и в полёте 2026-10-07 ЛА «застрял» в наборе на θ = 13.5° — ушёл с 200 на 500 м. Теперь q_max = Kθ·40°: предел не срабатывает, пока |θ_ref − θ| ≤ 40°.
Высота — ПИ, а не П. На скорости, отличной от точки балансировки, П-контур имеет статическую ошибку. ζ_h = 1.5: при 0.9 нуль ПИ давал перерегулирование 18 %.
Крен и курс — до 2026-10-08 были ПИД крена и ПИ курса: ступенька курса 10° давала элероны почти в упор и перерегулирование 38 %. Заменены каскадом по схеме PX4 (разделы 5–6, РЕШ-22).
Предел интеграла скорости = 1/ki — весь диапазон газа (при 0.5 оставалась ошибка 0.13 м/с).

3. Тангаж

3.1 Структура

Класс PitchController. Каскад из двух П-звеньев:

$$ q_{ref} = K_\theta,(\theta_{ref} - \theta), \quad |q_{ref}| \le q_{max}; \qquad \delta_e = \delta_e^* - s_e,K_q,(q_{ref} - q), \quad s_e = \left(\frac{V_{a0}}{V_a}\right)^2 $$

θ — от ИНС, q — от гироскопа, Va — от ПВД.
δe* — балансировочное отклонение руля (set_trim_elevator): регулятор работает с отклонением от балансировки, как в линейной модели, и не «проседает» на старте.
Масштаб по скорости gain_scheduling: эффективность руля растёт как Va², поэтому выход контура умножается на (Va₀/Va)², Va₀ = 16 м/с, в пределах 0.25…9.
Знак. δe > 0 опускает нос, поэтому перед Kq стоит минус.

Без насыщений каскад тождествен ПД-закону B&M δe = δe* + kp_θ·(θ_ref − θ) − kd_θ·q с Kθ = kp_θ/kd_θ, Kq = −kd_θ. Каскад удобнее: в нём есть сигнал q_ref, который можно ограничить и показать на стенде.

Ограничения: уставка θ_ref — ±30° (set_pitch_setpoint; TECS и ПИ высоты ограничивают её ±15°), q_ref — ±648 °/с, δe — упоры ±25°.

3.2 Переходный процесс

Ступенька уставки тангажа +3°, полная модель против линейной теории (левый столбец). В средней и правой колонке — крен и курс (разделы 5–6). Внизу — команда САУ и положение руля после привода: привод запаздывает на сотые доли секунды.

Ступенька	Перерегулирование, модель / теория	t_уст 5 %, модель / теория
тангаж θ_ref +3°	0 / 1.5 %	0.61 / 0.22 с

Начальный участок совпадает с теорией, но полная модель подходит к уставке медленнее: в ней на тангаж влияет изменение угла атаки и скорости, которого в модели B&M нет. Руль высоты отклоняется не больше чем на 1.9° от балансировки.

4. Высота и скорость: TECS и раздельные контуры

Продольный внешний контур выбирается полем SAUParams.lon: "tecs" — по умолчанию (тренажёр, стенд, s8–s10), "split" — раздельные контуры, для сравнения. У обоих один интерфейс: step(refs, est, dt, hold_alt) → (θ_cmd, δt).

4.1 TECS — управление полной энергией

TECS (Total Energy Control System, Lambregts, 1983) управляет высотой и скоростью совместно. Так же устроены продольные контуры автопилотов PX4 (FW_T_*) и ArduPilot (TECS_*). Реализация — control/tecs.py, РЕШ-27.

Идея. Полная механическая энергия ЛА на единицу веса E = h + V²/(2g) складывается из потенциальной (высота) и кинетической (скорость). TECS работает с её производной по времени, отнесённой к скорости, — она безразмерна (рад):

$$ \dot E \equiv \frac{1}{V_a}\frac{dE}{dt} = \gamma + \frac{\dot V}{g}, \qquad \gamma = \frac{\dot h}{V_a} $$

Далее Ė — эта производная полной энергии (в литературе по TECS — specific energy rate). Её смысл — угол набора, эквивалентный приросту энергии: Ė = 0.05 рад — это набор под углом 2.9° на постоянной скорости или разгон с ускорением 0.05·g ≈ 0.5 м/с² в горизонте.

Из уравнения движения вдоль траектории m·V̇ = T − D − m·g·sin γ следует

$$ \dot E = \frac{T - D}{m g}, $$

то есть производная полной энергии зависит только от тяги: газ добавляет или убирает энергию. Тангаж энергию не меняет — он перераспределяет её между высотой и скоростью. Это распределение описывает производная баланса энергии

$$ \dot B = w,\gamma - (2 - w),\frac{\dot V}{g} = 2\gamma - (2 - w),\dot E, $$

где вес w ∈ [0, 2] задаёт, что важнее: w = 1 — поровну, w = 0 — тангаж держит только скорость. Отсюда разделение: газ ← Ė, тангаж ← Ḃ.

Раздельные контуры поступают иначе: тангаж держит высоту, газ — скорость. Но тангаж меняет и скорость (набор за счёт скорости), а газ — и высоту. Поэтому при наборе скорость проседает, а при снижении растёт; TECS эту связь развязывает.

Структура.

Внешние контуры превращают ошибки высоты и скорости в требуемые производные:

$$ \dot h_{ref} = \dot h_{ff} + K_h,(h_{ref} - h), \quad -V_a\gamma_{sink}(V_a) \le \dot h_{ref} \le V_a\gamma_{climb}(V_a), \quad \gamma_{ref} = \dot h_{ref}/V_a $$

$$ \dot V_{ref} = K_V,(V_{ref} - V_a), \quad |\dot V_{ref}| \le a_{max} $$

$$ \dot E_{ref} = \gamma_{ref} + \dot V_{ref}/g, \qquad \dot B_{ref} = w,\gamma_{ref} - (2 - w),\dot V_{ref}/g $$

Внутренние контуры — интегральные (интеграл ошибки по Ė — это ошибка самой энергии) с упреждением:

$$ \delta_t = \delta_t^*(V_{ref}) + K_E,\dot E_{ref} + k_{i,T}!\int(\dot E_{ref} - \dot E),dt $$

$$ \theta_{cmd} = \alpha^*(V_a) + \gamma_{ff} + k_{i,B}!\int(\dot B_{ref} - \dot B),dt, \qquad \gamma_{ff} = \tfrac{1}{2}\big(\dot B_{ref} + (2 - w),\dot E_{ref}\big) $$

δt*(V), α*(V) — балансировка горизонтального полёта, таблица на 10…28 м/с (compute_trim). Упреждение работает на любой скорости, а не только на 16 м/с.
K_E = g/a_V2 — сколько газа нужно на единицу Ė.
ḣ_ff — упреждение от навигатора: уклон участка маршрута × путевая скорость вдоль него. Без него на наклонной прямой высота отстаёт на ḣ_ff/K_h.
Измерения: ḣ — вертикальная скорость GPS (оценка EKF). Va и V̇ — фильтр воздушной скорости, как TECSAirspeedFilter в PX4: вход — показание ПВД (не оценка EKF) и, если есть акселерометр (RealisticSensors, 3D), продольная удельная сила f_x. Комплементарный фильтр 2-го порядка (РЕШ-33):

$$ \dot V_f = f_x - g\sin\theta + b,\quad \dot{\hat V}a = \dot V_f + 2\zeta_V\omega_V e,\quad \dot b = \omega_V^2 e,\quad e = V{a,\text{ПВД}} - \hat V_a $$

ω_V = 1 рад/с (вдвое выше K_V), ζ_V = 0.9. V̇ берётся от акселерометра: белый шум ПВД в него не проходит, b убирает смещение акселерометра и ошибку приближения V̇a ≈ f_x − g·sin θ (точно V̇a = â·(f + Rᵀg), â — орт воздушной скорости в связанной СК). Без акселерометра (s8–s10) — прежняя схема: V̇ — производная Va после фильтра 1-го порядка τ_f = 0.15 с, контуры получают Va без фильтра.
Пределы угла траектории γ_climb(V), γ_sink(V) — 0.7 располагаемой производной энергии на газе 1 и 0 при этой скорости, но не круче 15°. Поэтому TECS не просит набора, который ЛА не может выполнить без потери скорости.
Защита от потери скорости. При Va < Va_min = 1.25·Va_stall = 9.4 м/с коэффициент k растёт от 0 до 1 на 1.5 м/с: вес высоты w → 0 (тангаж держит только скорость), набор запрещается, газ → 1.
Режим PITCH: γ_ref = γ, θ_cmd = уставка оператора; газ держит скорость.
Анти-виндап обоих интегралов: интеграл не копится, пока выход в упоре.

Расчёт (design_sau → d.tecs, FPV, 16 м/с):

Величина	Формула	Значение
K_h, K_V	требование	0.5 1/с
K_E	g/a_V2	0.852
ki_T	ω_T·K_E, ω_T = 2 рад/с	1.70
ki_B	ω_B/2 (∂Ḃ/∂θ ≈ 2), ω_B = 2 рад/с	1.0
γ_climb(V)	0.7·min(Ė_max(V), sin 15°)	10.4° (на 28 м/с — 8.1°)
γ_sink(V)	0.7·min(−Ė_min(V), sin 15°)	4.9° на 10 м/с … 10.4° с 19 м/с
a_max	0.7·(T(δt = 1) − D)/m	5.4 м/с²

Внутренние контуры — чистые интеграторы. Объект по газу с учётом фильтра — (a_V2/g)/(τ_f·s + 1), разомкнутый контур ω_T/(s·(τ_f·s + 1)): частота среза ω_T, запас по фазе 90° − atan(ω_T·τ_f) = 73°. Пропорциональную часть (как FW_T_THR_DAMP в PX4) не берём: шум ПВД 0.2 м/с проходит через дифференцирование V̇ с усилением ω_T·K_E·σ_V/g, которое от τ_f не зависит (σ газа 0.049 с П-частью против 0.019 без неё). А интеграл V̇ — это сама отфильтрованная скорость, почти без шума.

4.2 Раздельные контуры

Классический вариант B&M гл. 6 (AltitudeHold + SpeedController):

$$ \theta_{cmd} = \alpha^* + k_{p,h}(h_{ref} - h) + k_{i,h}!\int(h_{ref} - h),dt, \qquad |\theta_{cmd}| \le 15° $$

$$ \delta_t = \delta_t^* + k_{p,V}(V_{ref} - V_a) + k_{i,V}!\int(V_{ref} - V_a),dt, \qquad 0 \le \delta_t \le 1 $$

В сценариях s5–s6 вместо ПИ высоты — П-контур θ_ref = α* + KH·(h_ref − h), KH = 0.031 рад/м.

4.3 Малые ступеньки: проверка расчёта

TECS по каждой из величин ведёт себя как звено первого порядка K/(s + K) (теория — пунктир). Раздельные контуры — второго порядка с нулём ПИ.

Контур, ступенька	Перерегулирование, мод. / теор.	t_уст 5 %, мод. / теор.	Влияние на другую величину
TECS, h +5 м	0 / 0 %	6.1 / 6.0 с	\|ΔVa\| ≤ 0.29 м/с
TECS, Va 16 → 18 м/с	0 / 0 %	6.8 / 6.0 с	\|Δh\| ≤ 0.22 м
раздельные, h +5 м	5.4 / 7.6 %	3.3 / 6.4 с	\|ΔVa\| ≤ 0.95 м/с
раздельные, Va 16 → 18 м/с	8.7 / 8.7 %	4.0 / 4.0 с	\|Δh\| ≤ 0.21 м

На малых ступеньках раздельные контуры быстрее (у них выше полоса), но при наборе 5 м скорость проседает втрое сильнее.

4.4 Большие манёвры: TECS против раздельных контуров

Разница видна, когда манёвр упирается в ограничения.

Манёвр	Контуры	h: t_уст ±1 м	Va в манёвре, м/с	α max	газ в нуле
h +50 м	раздельные	12.3 с	15.0…16.9	4.3°	0 %
	TECS	18.8 с	15.9…16.4	3.1°	0 %
h −50 м	раздельные	11.9 с	12.3…18.0	2.4°	28 %
	TECS	23.6 с	15.8…16.1	0.4°	0 %
Va 16 → 11 м/с, h = const	раздельные	\|Δh\| ≤ 1.1 м	провал до 8.8	7.7°	16 %
	TECS	\|Δh\| ≤ 0.3 м	11.0 (без провала)	3.6°	0 %

Раздельные контуры набирают высоту за счёт скорости и снижаются с разгоном и газом в нуле. При торможении до 11 м/с скорость проваливается до 8.8 м/с — ниже Va_min, а угол атаки доходит до 7.7° (порог предупреждения — 9°).
TECS меняет высоту не быстрее, чем позволяет располагаемая производная энергии (избыток тяги), поэтому скорость почти не меняется. Платой — более медленный набор и снижение.
С шумом датчиков (установившийся полёт): σ газа 0.018 у TECS против 0.021, σ δe 0.25° против 0.68°.

4.5 TECS на стенде

Вкладка стенда «Высота и скорость (TECS)» показывает схему TECS с живыми значениями на каждом звене; производные энергии Ė, Ḃ там умножены на Va и выражены в м/с набора (Ė·Va = 1 м/с — энергии прибавляется столько же, сколько при наборе 1 м/с на постоянной скорости). Ползунки: K_h, K_V, пределы угла набора и снижения, w, Kp и Ki газа и тангажа (Kp = 0 по умолчанию — можно включить и увидеть шум газа от ПВД), предел θ, Va_min, τ_f. Режим «Срыв» на вкладке защиты обходит TECS; при выходе из него интегралы TECS сбрасываются. Подробно — USER_GUIDE.md.

5. Крен

5.1 Структура

Класс RollController (общий для LateralController и RollHold), схема как в PX4 и ArduPilot (РЕШ-22):

$$ p_{ref} = \frac{\varphi_{ref} - \varphi}{\tau_\varphi}, \quad |p_{ref}| \le p_{max} $$

$$ \delta_a = s\cdot FF\cdot p_{ref} + s^2\Big(k_{p,p},e_p + k_{i,p}!\int e_p,dt\Big), \qquad e_p = p_{ref} - p, \quad s = \frac{V_{a0}}{V_a} \in [0.5,,2] $$

Упреждение FF = a_φ1/a_φ2 — отклонение элеронов, при котором установившаяся скорость крена равна p_ref. Оно делает основную работу без участия обратной связи.
ПИ по скорости крена только поправляет: П — против возмущений, И — против постоянного реактивного момента винта. При p = 0 интеграл копит (φ_ref − φ)/τ_φ, то есть снимает и статическую ошибку угла крена.
Масштаб s: a_φ1 ∝ Va, a_φ2 ∝ Va², поэтому FF ∝ 1/Va, kp и ki ∝ 1/Va².
Анти-виндап: интеграл не копится, пока элероны в упоре. Предел интеграла — вклад в δa не больше 3.4° (моменту винта нужно ≲ 2°).

Аналоги параметров в автопилотах: τ_φ — PX4 FW_R_TC (0.5 с), ArduPilot RLL2SRV_TCONST (0.45 с); p_max — FW_R_RMAX, RLL2SRV_RMAX; FF, kp, ki — FW_RR_FF/P/I, RLL_RATE_FF/P/I. FPV-самолёт резвее типичного, поэтому τ_φ = 0.25 с.

5.2 Расчёт

Объект по скорости крена ṗ = −a_φ1·p + a_φ2·δa. С упреждением и П-частью ошибка p затухает с полюсом a_φ1 + a_φ2·kp_p = λ_p = 75 рад/с (собственный — 56). Нуль ПИ — на 1/τ_φ. При быстром контуре p угол крена отрабатывает уставку как апериодическое звено φ/φ_ref ≈ 1/(τ_φ·s + 1).

5.3 Переходный процесс

Средний столбец рисунка в разделе 3.2 — выход из крена 20° в горизонт (RollHold). Перерегулирование 0.3 % (теория 0), время установления 0.69 с (теория 0.71 с), элероны до 10°. Модель и теория практически совпадают.

5.4 Крен в продольных сценариях

Реактивный момент винта кренит ЛА, спиральная мода неустойчива: без элеронов ЛА уходит в спираль за ~25 с. Поэтому в продольных сценариях s1–s7 крен держит RollHold (φ_ref = 0, тот же RollController), подключаемый обёрткой with_roll_hold(controls_fn, aircraft, sp, dt). У него свой генератор шума, чтобы не сдвигать шум продольных датчиков сценария. |φ| ≲ 0.5°, δa ≈ 0.4°.

6. Курс и скольжение

Класс LateralController: курс → уставка крена → контур крена (раздел 5) → δa; скольжение и микс элеронов → δr.

6.1 Курс

Держится путевой угол χ (направление земной скорости, GPS), а не курс ψ. При боковом ветре САУ сама выставляет угол упреждения сноса, и линия пути не сносится.

$$ \dot\chi_{ref} = V_g,\kappa + K_\chi,\mathrm{wrap}(\chi_{ref} - \chi), \qquad \varphi_{ref} = \operatorname{arctg}\frac{V_g,\dot\chi_{ref}}{g}, \quad |\varphi_{ref}| \le 30° $$

Затем уставка крена проходит ограничитель скорости 90 °/с.

Почему арктангенс. В координированном развороте χ̇ = (g/Vg)·tg φ. Если задать φ_ref = atan(Vg·χ̇_ref/g), то при установившемся крене тангенс и арктангенс сокращаются, и χ̇ = χ̇_ref точно, на любой скорости и при любом крене.
wrap приводит ошибку к [−180°, 180°): разворот идёт кратчайшим путём (с 170° на −170° — вправо на 20°, а не влево на 340°).
Кривизна κ заданной траектории приходит от навигатора (1/R на дугах и кружении, 0 на прямых). Крен atan(Vg²/(g·R)), нужный для дуги радиуса R, задаётся сразу упреждением, а не набирается ошибкой курса — как центростремительный член в L1/NPFG. Без него на дугах ЛА отставал: в ветре |d − R| был 12.8 м вместо 4.3 м.
Интеграла по курсу нет. На прямой φ = 0 при любом χ, поэтому статической ошибки нет даже в ветре (установившаяся ошибка 0.02°).

Расчёт. С учётом инерции крена φ/φ_ref ≈ 1/(τ_φ·s + 1) контур курса χ/χ_ref = K/(τ_φ·s² + s + K), демпфирование ζ = 1/(2√(K·τ_φ)). Для ζ = 1: K_χ = 1/(4τ_φ) = 1.0 1/с, двойной полюс на 1/(2τ_φ) = 2 рад/с.

Ступеньки курса 10°, 30° и 90° (полная САУ, датчики без шума):

Условия	Перерегулирование	t_уст 5 %, с	φ max, °	δa max, °	p max, °/с	β max, °
16 м/с, штиль	0 / 0 / 0 %	2.3 / 2.5 / 4.9	12.8 / 29.5 / 30.2	4.7 / 7.0 / 7.0	46 / 68 / 68	1.7 / 2.8 / 2.8
25 м/с, штиль	0 / 0 / 0 %	2.3 / 3.1 / 7.2	19.8 / 29.9 / 30.0	3.9 / 4.4 / 4.4	59 / 67 / 67	1.2 / 1.3 / 1.3
12 м/с, штиль	0 / 0 / 0 %	2.3 / 2.3 / 3.9	9.6 / 26.8 / 30.6	5.3 / 9.4 / 9.4	38 / 69 / 69	2.0 / 4.7 / 4.7
16 м/с, ветер 5 м/с на восток	0 / 0 / 0 %	2.2 / 2.6 / 5.7	13.7 / 29.4 / 30.5	3.8 / 8.0 / 8.0	28 / 61 / 61	—

Перерегулирования нет ни на одной скорости. Скорость крена ограничивает не p_max (120 °/с), а ограничитель скорости уставки крена. Высота при развороте 90° меняется не больше чем на 0.33 м. В ветре β max не приведено: его определяет не разворот, а порыв в начале прогона (старт с балансировки без ветра).

Сравнение со ступенькой +20° на линейной теории — правый столбец рисунка в разделе 3.2: время установления 2.31 с против 2.38 с по теории.

6.2 Скольжение

$$ \delta_r = -\Big(k_{p,\beta},\beta + k_{i,\beta}!\int\beta,dt\Big) + K_{mix},\delta_a $$

β измеряет зонд и пропускает через апериодическое звено 1/(τ_β·s + 1), τ_β = 0.1 с (beta_tau; дискретизация как у звена Aperiodic в blocks.py). kp_β = 0.5 ограничен шумом зонда (σ 0.6°): при kp_β = 2 руль дрожал на ~1.6° за такт. При beta_hold = False руль направления в нейтрали, микс тоже выключен.

Фильтр β — временная мера (РЕШ-33): в PX4 и ArduPilot датчика β нет, рыскание демпфируется по r, координация — упреждением r_ref = g·sin φ/Va, скольжение — по боковому ускорению a_y (ArduPilot YAW2SRV_SLIP). Перевод контура на r и a_y — в DEV_PLAN.

6.3 Координация разворота (РЕШ-23)

После перехода на каскад крена на входе в разворот остался рывок носа против разворота. Причина — обратное рыскание элеронов (Cn_δa < 0): опущенный элерон сильнее тормозит своё крыло. Уставка крена при ступеньке курса прыгала за один шаг, упреждение давало δa ≈ 15° скачком, и нос дёргался против разворота (r = −38 °/с). Контур β отвечает только после роста β и рывок не убирает.

Два средства, оба штатные в автопилотах:

Ограничение скорости уставки крена 90 °/с (PX4 FW_PN_R_SLEW_MAX): φ_ref нарастает рампой, элероны не скачут. При включении САУ ограничитель стартует с текущего крена.
Микс элеронов в руль направления δr += K_mix·δa (ArduPilot KFF_RDDRMIX): момент рыскания от элеронов гасится сразу, не дожидаясь β. K_mix = −N_δa/N_δr, где N_x = Γ4·Croll_x + Γ8·Cn_x — вклад руля в ṙ (B&M 3.17); для FPV K_mix = −0.654.

Ступенька курса 30° на 16 м/с:

Вариант	Рывок носа, r min	max \|ṙ\|	β max	t_уст 5 %
каскад крена без средств	−38 °/с	536 °/с²	7.8°	2.4 с
+ ограничение скорости φ_ref	−20	269	6.3	2.5
+ микс δa → δr	−13	186	3.5	2.4
оба (по умолчанию)	−6	131	2.8	2.5

Cn_δa оценён на крейсерском CL = 0.28 и по модулю растёт с CL. На малой скорости микс недокомпенсирует, остаток добирает контур β (β max 4.7° на 12 м/с).

7. Защита по углу атаки

Класс AngleOfAttackProtector (control/aua.py) — надзорный уровень над тангажом. Стоит между внешним продольным контуром и тангажом: получает θ_cmd и балансировочный газ и может их заменить. Нужен датчик α — зонд (TruthSensors его дают, NoisySensors — с флагом alpha_probe=True). Включена на стенде, в остальных программах prot = None.

7.1 Автомат состояний

Состояние	Когда	Что делает
НОРМ	α < α_пред	пропускает θ_cmd и газ без изменений
ПРЕД	α_пред ≤ α < α_крит	опускает уставку тангажа на 3°
КРИТ	α ≥ α_крит	перехват: θ_ref = −7°, газ = 1
ВОССТ	после КРИТ, пока α ≥ α_выход	держит перехват; при α ≥ α_крит — снова КРИТ

Гистерезис (α_выход < α_пред) нужен, чтобы автомат не возвращался в НОРМ внутри зоны предупреждения и не «дребезжал».

Пороги. Стенд берёт их из параметров ЛА (default_aua): α_пред = 9°, α_крит = 12.2° (максимум CL), α_выход = α_пред − (α_крит − α_пред) = 5.8°. Умолчания самого AUAParams (15° / 20° / 10°) остались от прежнего ЛА (Aerosonde) — для FPV их нужно задавать явно, как на стенде.

7.2 Демонстрация: режим «Срыв»

Режим «Срыв» стенда отключает внешний продольный контур: газ 0, θ_ref = +15°. Скорость падает, угол атаки растёт.

С защитой: на 4.7 с α проходит 9° (ПРЕД — уставка тангажа на 3° ниже), на 5.3 с — 12.2° (КРИТ — нос вниз, газ полный). Перехват длится всего 0.12 с: угол атаки сразу падает ниже α_выход. Угол атаки не превысил 12.2°, скорость упала до 7.1 м/с и восстановилась; стенд переводит САУ в удержание высоты, на которой сработала защита, — высота не потеряна.
Без защиты: α уходит в закритическую область до 74° и устанавливается около 65° — глубокий срыв: ЛА парашютирует со скоростью ~6.5 м/с и теряет 6.3 м высоты в секунду.

Порог КРИТ совпадает с максимумом CL, поэтому защита срабатывает уже на границе срыва. Перехват по измеренному α — это и есть место, где прямое измерение угла атаки зондом полезнее косвенной оценки (тема на паузе, archive/probe/REPORT.md).

8. Навигация по точкам

Класс WaypointNavigator (control/navigation.py), метод — B&M гл. 10–11. Навигатор только формирует уставки для существующих контуров: путевой угол χ_ref, высоту h_ref, кривизну κ (упреждение крена, раздел 6.1) и уклон участка (упреждение ḣ в TECS).

8.1 Следование по прямой и окружности

Прямая (B&M 10.1, векторное поле). Начало участка r, направление χ_q; e_py — боковое отклонение от линии (> 0 — правее):

$$ e_{py} = -\sin\chi_q,(p_N - r_N) + \cos\chi_q,(p_E - r_E), \qquad \chi_{ref} = \chi_q - \chi_\infty,\frac{2}{\pi}\operatorname{arctg}(k_{path},e_{py}) $$

Далеко от линии ЛА идёт к ней под углом χ∞ = 60°; ближе ~1/k_path = 100 м плавно ложится на неё. Угол упреждения сноса в ветре снимать не нужно — его даёт сам контур курса по путевому углу.

Окружность (B&M 10.2), центр c, радиус ρ, λ = +1 — по часовой, −1 — против:

$$ d = |p - c|, \quad \vartheta = \operatorname{arctg2}(p_E - c_E,; p_N - c_N), \qquad \chi_{ref} = \vartheta + \lambda\Big(\frac{\pi}{2} + \operatorname{arctg}\frac{k_{orbit}(d - \rho)}{\rho}\Big) $$

8.2 Менеджер маршрута

Маршрут — точки w₁…w_N (север, восток, высота). Углы маршрута скругляются дугой радиуса R_fillet (B&M 11.2, алгоритм 6). Для угла у точки w_i: q₀, q₁ — направления входящего и исходящего участков, ϱ = arccos(−q₀·q₁) — внутренний угол.

Прямая w_{i−1} → w_i — до полуплоскости через z₁ = w_i − d·q₀, где d = R/tg(ϱ/2).
Дуга с центром c = w_i − (R/sin(ϱ/2))·орт(q₀ − q₁) — до полуплоскости через z₂ = w_i + d·q₁.
Прямая w_i → w_{i+1}.

ЛА проходит не через саму точку, а по дуге, зато не проскакивает поворот. Первая версия без скругления (переключение у самой точки) проскакивала поворот 90° на ~R_min и через 15 с была всё ещё в ~95 м от новой линии. На остром угле или коротком участке вынос d ограничен половиной участка — тогда радиус дуги меньше R.

Конец маршрута. После последней точки — кружение радиусом R_orbit вокруг неё. Замкнутый маршрут (loop = True) — снова к первой точке.
Высота на прямой меняется линейно по ходу участка от h_{i−1} к h_i, на дуге — h_i.
Правка маршрута в полёте (update_route): цель — точка с тем же номером, участок к ней — от предыдущей точки нового маршрута; кружение остаётся кружением вокруг новой последней точки.

8.3 Параметры (`NavParams`)

Параметр	Значение	Смысл
`chi_inf`	60°	угол подхода к линии издалека
`k_path`	0.01 1/м	масштаб выхода на линию, ~100 м
`k_orbit`	2.0	крутизна поля у окружности
`R_fillet`	200 м	радиус скругления углов (0 — без скругления)
`R_orbit`	200 м	радиус кружения у последней точки
`orbit_cw`	True	кружение по часовой
`loop`	False	замкнутый маршрут

R_fillet и R_orbit должны быть больше минимального радиуса координированного разворота по путевой скорости R_min = Vg²/(g·tg φ_max). Для FPV при φ_max = 30°: 45 м на 16 м/с в штиль, 78 м при попутном ветре 5 м/с. Умолчания 200 м подбирались для прежнего ЛА (30 м/с, R_min ≈ 160 м) и для FPV с запасом.

8.4 Полёт по маршруту

Маршрут сценария s10: (1000, 0, 100) → (1000, 1000, 130) → (0, 1000, 130) → (600, 300, 100), старт из начала координат на север, затем кружение. Шум датчиков включён; отклонение посчитано по истинному положению.

Величина	Штиль	Ветер 5 м/с на восток
\|e_py\| max на прямых (10 с после входа отброшены)	0.1 м	0.2 м
\|d − R\| max на дугах R = 200 м	0.5 м	0.8 м
\|d − R\| max на кружении (после выхода на окружность)	0.2 м	0.5 м
\|h − h_ref\| в конце	0.05 м	0.05 м

Навигатор видит положение по GPS с шумом σ = 1 м, поэтому по его собственной оценке e_py «гуляет» до ±4.5 м — но ЛА этот шум почти не отрабатывает: истинное отклонение меньше метра. На острый угол у точки 3 навигатор даёт скачок h_ref (дуга срезает угол, а высота на прямой считается от точки) — TECS отрабатывает его снижением без потери скорости.

9. САУ одним классом — `SAU`

control/sau.py, РЕШ-26. Одна и та же САУ работает в сценариях, тренажёре и стенде; раньше она была собрана трижды.

9.1 Звенья и интерфейсы

Каждое звено — отдельный объект; его можно заменить другим с тем же методом.

Звено	Сейчас	Интерфейс	Что может встать на его место
`sensors`	`TruthSensors`, `NoisySensors`, `RealisticSensors`	`measure(t, state, Va, α, β) → meas`	дрейф барометра, ошибки ПВД от скольжения
`estimator`	`StateEstimator` (инерциальный EKF) или None — оценка = измерения	`update(meas, dt) → est`	состояния магнитного поля, UKF
`nav`	`WaypointNavigator`	`step(pn, pe, χ) → (χ_ref, h_ref)` или None; `.kappa`, `.h_slope`, `.chi_q`	L1 / NPFG, траектории Дубинса
`lon`	`TECS` или `AltSpeedHold`	`step(refs, est, dt, hold_alt) → (θ_cmd, δt)`; `reset()`, `reset_alt()`, `thr_trim`	—
`prot`	`AngleOfAttackProtector` или None	`step(α, θ_cmd, δt*, dt) → AUAOutput`	—
`pitch`	`PitchController`	`set_pitch_setpoint(θ)`, `step(t, meas, dt)`	тангаж по схеме крена (θ → q_ref → FF + ПИ) с упреждением q в развороте
`lat`	`LateralController` (`lat.roll` — `RollController`)	`set_course(χ, κ)`, `step(meas, dt) → (δa, δr)`	INDI по крену вместо `lat.roll`
весь `_control`	каскад	подкласс `SAU`	LQR / LQI, INDI, NMPC — для сравнения с каскадом

Ключи словарей meas / est: t, h, hdot, Va, q, p, theta, phi, chi, Vg, beta и по флагам alpha, pn, pe (подробно — SENSORS.md).

9.2 Использование

from runner import compute_trim
from control.sau import SAU, SAUParams, NoisySensors, TruthSensors, Mode
from control.navigation import WaypointNavigator

trim = compute_trim(aircraft, 16.0)                       # (α*, δe*, δt*)
sau = SAU(aircraft, trim, TruthSensors(), SAUParams(), nav=WaypointNavigator())
sau.engage(theta=trim[0], h=100.0, Va=16.0, chi=0.0)      # уставки = текущий режим
sau.set_mode(Mode.ALTITUDE)
sau.refs.h, sau.refs.chi = 150.0, np.radians(90)          # новые уставки

def controls_fn(t, state, Va, alpha):                     # для runner.run()
    beta = air_data(state, wind(state[H], t, wp))[2]
    return sau.step(t, state, Va, alpha, beta, dt)

engage(θ, h, Va, χ) — включение: уставки равны текущему режиму, интегралы с нуля.
set_mode(m) — смена режима: интеграл высоты с нуля; при переходе в PITCH уставка тангажа подхватывает последнюю команду внешнего контура — без скачка.
fly_route(точки, старт) — маршрут с первой точки, режим ROUTE.
После шага доступны для журнала и окна стенда: est, theta_cmd (до защиты), theta_ref (после), prot_out, controls.
observe(t, state, Va, α, β, dt) — только датчики и оценка, без управления. 3D-тренажёр вызывает его в ручном полёте: к включению САУ фильтр уже сошёлся.

Взлёт, посадка, возврат домой — это новые значения Mode и ветки в _guidance / _outer.

Проверка перехода на SAU: s8–s10 — текстовый вывод и все массивы .flightlog совпали с прежними до бита; тренажёр и стенд — прежний код против SAU на истинных измерениях, 120 с со всеми режимами, маршрутом, защитой и «Срывом»: расхождение 0.0.

9.3 Оценка состояния — `StateEstimator`

control/estimator.py (РЕШ-32) — инерциальный EKF по схеме PX4 EKF2 для реалистичных датчиков (SENSORS.md, раздел 7): гироскоп и акселерометр 100 Гц, магнитометр 50 Гц, барометр и ПВД 20 Гц, GPS 5 Гц с запаздыванием 0.2 с.

Принцип. IMU — не измерение, а вход прогноза (инерциальное счисление): по гироскопу интегрируется ориентация, по акселерометру — скорость и положение. Ошибки счисления исправляют GPS (скорость и положение), барометр, магнитометр, воздушная скорость и синтетическое скольжение β = 0. Поэтому вираж, скольжение и разгон для фильтра — просто движение, а не ошибка модели (в отличие от схемы B&M гл. 8, где акселерометр — измерение «вертикали» и верен только в координированном полёте).

Состояние. Номинальное: кватернион $q$ (связанная СК → NED), скорость $v$ и положение $p$ в NED, смещения гироскопа $b_g$ и акселерометра $b_a$, ветер $w = (w_N, w_E)$. Фильтр — с вектором ошибок (error-state EKF, как нынешний EKF2): $\delta x = [\delta\theta,\ \delta v,\ \delta p,\ \delta b_g,\ \delta b_a,\ \delta w]$, 17 компонент; ошибка ориентации — вектор поворота в осях NED: $R_{ист} = \mathrm{Exp}(\delta\theta),R$. Кватернион — только внутри фильтра: на выходе он пересчитывается в φ, θ, ψ, и вырождения при θ → 90° нет (петля в check_estimator.py).

Прогноз на шаге IMU Δt ($\omega = \omega_{изм} - b_g$, $f = f_{изм} - b_a$, $g = (0, 0, g)$):

$$ q \leftarrow q \otimes \mathrm{Exp}(\omega,\Delta t),\qquad v \leftarrow v + (R f + g),\Delta t,\qquad p \leftarrow p + v,\Delta t . $$

Уравнения ошибок:

$$ \delta\dot\theta = -R,\delta b_g - R,n_g,\quad \delta\dot v = -[R f]_\times,\delta\theta - R,\delta b_a - R,n_a,\quad \delta\dot p = \delta v,\quad \delta\dot b_g = n_{bg},\ \delta\dot b_a = n_{ba},\ \delta\dot w = n_w ; $$

$P \leftarrow F P F^\mathsf T + Q$, $F = I + A,\Delta t$. Шумы процесса — из характеристик датчиков, а не подбором: $(\sigma_g\Delta t)^2$ и $(\sigma_a\Delta t)^2$ на шаг (шум гироскопа и акселерометра), интенсивность дрейфа гироскопа $2\sigma_b^2/\tau_b$ (марковский процесс датчика). Подобраны только два числа: блуждание смещения акселерометра (10⁻⁶ (м/с²)²/с) и ветра (0.5 (м/с)²/с — турбулентность), оба — по NEES.

Коррекция (по группам, ковариация — в форме Джозефа; модели и якобианы H — функции meas_mag, meas_airspeed, meas_sideslip, проверены конечными разностями):

Измерение	Модель h(x)	R
GPS-скорость	$v$	$\sigma_{V}^2$ = 0.1² (м/с)²
GPS-положение N, E	$p_N, p_E$	(2 м)² — как точность приёмника (hAcc)
барометр	$-p_D$	0.5² м²
магнитометр, 3 оси	$R^\mathsf T m_n$ ($m_n$ — поле Земли, известно)	0.003² Гс²
воздушная скорость	$\lvert v - w\rvert$	0.2² (м/с)²
синтетическое β = 0	$a_y / a_x$, $a = R^\mathsf T (v - w)$	0.3² рад² (EKF2_BETA_NOISE)

Магнитометр — по трём осям, а не «курс по магнитометру»: курс, посчитанный через оценённый наклон, на нашей широте (наклонение поля 73°) усиливает ошибку наклона в tg 73° ≈ 3.3 раза и коррелирован с ней — с ним NEES ориентации был 35 вместо 1.4.

Отбраковка и сбросы (как EKF2_*_GATE, EKF2_NOAID_TOUT):

отношение $e_i^2/(\text{gate}^2 S_{ii}) > 1$ хотя бы по одной составляющей — группа не используется; пороги 5σ (GPS, барометр, магнитометр, β) и 3σ (воздушная скорость);
защита от блокировки: группа отбраковывается дольше 2 с (магнитометр) или 5 с (барометр, ПВД) — следующее измерение принимается без проверки;
GPS: и скорость, и положение отбраковываются дольше 5 с (фильтр без коррекции) — сброс обоих к GPS; только положение — это выброс GPS, сброс положения лишь через 20 с.

Запаздывание (как в PX4): фильтр работает на задержанном горизонте t − 0.25 с; отсчёты IMU копятся в буфере, каждое измерение используется в момент своего отсчёта (отметки времени t_gps, t_h, t_Va, t_mag).

Выходной предиктор (как в PX4, output_predictor.cpp; EstimatorParams.output = "predictor", по умолчанию). Оценку на текущем времени t даёт отдельное выходное состояние $(q_o, v_o, p_o)$ — инерциальное счисление: на каждом отсчёте IMU один шаг того же прогноза, что у фильтра, со смещениями $b_g, b_a$ из фильтра. Выходные состояния хранятся в буфере на длину задержки. После шага фильтра на горизонте $t_f$ выход в тот же момент сравнивается с фильтром, и поправка работает как комплементарный фильтр:

ориентация: $q_e = q^{-1} \otimes q_o(t_f)$, $\delta\alpha = \mp 2,\mathrm{vec}(q_e)$ (знак — по знаку скалярной части); к приращению угла следующего шага IMU прибавляется $k_{att},\delta\alpha$, $k_{att} = 0.5,\Delta t / (t - t_f)$. В контуре — запаздывание $t - t_f$, коэффициент выбран под него; постоянная времени ≈ $2(t - t_f)$ = 0.5 с;
скорость и положение: $e_v = v - v_o(t_f)$, $e_p = p - p_o(t_f)$, $\Delta v = k_v e_v + 0.1,k_v^2 \sum e_v$, $\Delta p = k_p e_p + 0.1,k_p^2 \sum e_p$, $k = \Delta t/\tau$, $\tau_v = \tau_p$ = 0.25 с (EKF2_TAU_VEL, EKF2_TAU_POS). Поправка прибавляется ко всему буферу — в контуре нет запаздывания; малая интегральная составляющая убирает статическую ошибку;
скачок фильтра при сбросе к GPS переносится в выход сразу, без сглаживания.

Выход повторяет фильтр с запаздыванием ≈ τ и сглаживает скачки его оценки от коррекций. Прежний способ — повторный прогноз номинального состояния по буферу IMU от $t_f$ до t (output = "replay", ≈ 25 шагов прогноза на шаг САУ): выход повторяет фильтр без сглаживания; оставлен для сравнения.

Выход. φ, θ, ψ из кватерниона; p, q, r — гироскоп минус оценка смещения; h = −p_D, ḣ = −v_D; pn, pe; Vg и χ по скорости; Va = |v − w|; ветер; смещения. α, β — без изменений (зонд).

Результат (checks/check_estimator.py, моделирование: FPV, 16 м/с, ветер 5 м/с, слабая турбулентность, разворот 90° и набор 30 м). Разомкнутый контур (САУ по истине, EKF наблюдает), 6 прогонов с разными seed × 120 с:

СКО ошибки	φ	θ	ψ	h	ḣ	Va	χ	Vg
сырые измерения	—	—	—	0.50 м	0.51 м/с	0.21 м/с	1.12°	0.27 м/с
EKF	0.08°	0.07°	0.29°	0.06 м	0.02 м/с	0.30 м/с	0.09°	0.03 м/с

Согласованность — средний NEES, делённый на размерность группы (≈ 1, если ковариация P честная; > 2 — фильтр самоуверен): ориентация 1.42, скорость 1.02, смещения гироскопа 0.93, смещения акселерометра 0.52, ветер 0.50, положение 0.67 (на прогоне без ухода GPS — см. ограничения). Отбраковано: воздушная скорость 0.1 %, остальные группы — 0.

Устойчивость: петля (θ до 83°, поворот на 377°) и бочка — ошибка ориентации ≤ 2.5°; выброс GPS +50 м на 10 с отбракован целиком (ошибка положения ≤ 3.8 м, сбросов нет); умеренная турбулентность — ошибка ориентации 0.3° СКО; скачок бокового ветра при t = 0 (скольжение 14°) — выставка с ошибкой 48°, сходится за ~10 с.

Замкнутый контур (180 с, рули — СКО приращения за шаг 0.01 с):

САУ	перебег φ после разворота	δe	δa	δt
на истине	0.4°	0.036°	0.019°	0.003
на сырых измерениях	7.5°	0.145°	0.047°	0.011
на EKF	0.4°	0.039°	0.021°	0.004

На EKF САУ работает практически как на истине. В 3D (реалистичные датчики, слабая турбулентность, ветер 4 м/с, 25 с) — |φ| ≤ 2.2° против 2.0° на истине.

Выходной предиктор против повторного прогноза (checks/check_estimator.py, раздел 5; оба выхода на одних и тех же измерениях, то же моделирование):

СКО ошибки против истины, t > 10 с	ориентация	ψ	v_N	h	Va	χ
повторный прогноз	0.33…0.49°	0.31…0.47°	0.024…0.028 м/с	0.06 м	0.30 м/с	0.09…0.10°
предиктор	0.34…0.50°	0.31…0.48°	0.026…0.030 м/с	0.06…0.07 м	0.30 м/с	0.09…0.11°

Расхождение двух выходов: ориентация в среднем 0.03°, наибольшее 0.48° — на входе в разворот; скорость ≤ 0.06 м/с, h ≤ 0.06 м, положение ≤ 0.1 м. Замкнутый контур на предикторе: перебег крена 0.43° (на повторном прогнозе 0.45°), рули — без изменений; NEES — те же числа (фильтр не менялся). Переходный процесс выхода на скачок оценки фильтра (без измерений):

скачок	63 %	95 %	перерегулирование
p_N +5 м, v_E +1 м/с	0.23 с	0.56 с	5.4 %
ориентация 2°	0.31 с	0.56 с	нет

На выставке (первые 2 с после старта в полёте) ошибка ориентации против истины — у обоих выходов 4…6° (сходимость фильтра от выставки по акселерометру и магнитометру), у предиктора не больше: 5.0° против 6.1° в окне 0.5…1 с; χ, v, h, p в эти секунды отличаются между выходами не больше чем на 0.2 (°, м/с, м).

Время шага фильтра — 0.30…0.33 мс в обоих режимах (разница в пределах разброса замера): повторный прогноз стоил ≈ 0.05 мс, основное время — прогноз ковариации 17×17 (≈ 0.045 мс на отсчёт IMU) и коррекции (ПВД и β ≈ 0.21 мс, GPS ≈ 0.19 мс, магнитометр ≈ 0.05 мс на отсчёт). Предиктор оставлен ради схемы PX4 и сглаженного выхода, не ради быстродействия.

Ограничения.

Уход GPS-положения (марковский процесс, 5–8 м за минуты) фильтр считает белым шумом: положение подтягивается к уходу с запаздыванием, и по положению фильтр несогласован относительно истины (сам уход без другого источника положения не наблюдаем — так же у PX4). Без ухода NEES положения 0.67.
Смещение акселерометра в прямолинейном полёте неотличимо от наклона, а поворот вокруг вектора магнитного поля не виден магнитометру: до первого манёвра возможна ошибка ориентации до ~2° вдоль вектора поля (фильтр это знает — σ вдоль этого направления такого же порядка). Первый разворот её снимает.
Va в турбулентности — 0.30 м/с против 0.21 у сырого ПВД: оценка ветра сглаживает быстрые порывы (в штиле 0.13 против 0.21). Для TECS гладкая Va полезнее.
Выставка в полёте — по акселерометру и магнитометру за ~2 с (при скольжении — дольше); в 3D фильтр работает с первого кадра, к включению САУ выставка закончена.
Модель магнитометра — калиброванный датчик и точно известное поле (в симуляторе так и есть); у PX4 поле и смещения магнитометра — состояния фильтра.

Диагностика — вкладка ESTIMATOR окна станции (viz/station.py, Tab или кнопка в строке статуса). Данные: StateEstimator.status(t) — внутреннее состояние фильтра (доступно и в реальном полёте) и EstimatorMonitor — сверка с истиной (только моделирование), отчёт 10 Гц.

ESTIMATOR: отставание горизонта t − t_f, длина буферов IMU и измерений, время шага update (среднее и наибольшее), собственные числа P (min > 0 — P положительно определена), число выставок, сбросов к GPS, коррекций без проверки (они же — события журнала).
AIDING: по группам измерений — отношение невязки к порогу e²/(gate²·S) (> 1 — отбраковка; аналог «innovation test ratio» QGroundControl), средний NIS = eᵀS⁻¹e / dim (по ~50 последним коррекциям; ≈ 1 — невязки соответствуют S, без истины), принято / отбраковано, время с последней принятой коррекции и возраст последнего отсчёта датчика.
CONSISTENCY: NEES / dim по группам состояния относительно истины, сглаживание 10 с; положение — относительно «истина + уход GPS».
STATE vs TRUTH: оценка, истина, ошибка выхода (что получает САУ, момент t), ошибка состояния фильтра на горизонте t_f, 3σ и шкала err_f / σ с полосой ±3σ. σ углов Эйлера — через якобиан ∂(φ, θ, ψ)/∂δθ (euler_sigma), σ для χ, Vg, Va — через якобиан по v (и w). ЛКМ по строке — история её ошибок с полосой ±3σ за 60 с; ниже — история отношений невязки к порогу. Без EKF (--est none) — ошибка сырых измерений.

10. LQR

control/lqr.py, сценарий s7 (набор → горизонт → снижение) — альтернатива продольному каскаду для сравнения.

Модель — численная линеаризация derivatives() в точке балансировки по [u, w, q, θ] и [δe, δt], дискретизация Эйлером с шагом dt.
Усиление K — из дискретного уравнения Риккати (scipy.linalg.solve_discrete_are; scipy — согласованное исключение из правила «только numpy»).
Веса: Q = diag(1, 1, 10, 20) по [Δu, Δw, Δq, Δθ], R = diag(150, 1) по [Δδe, Δδt].
Высота — внешний П-контур θ_ref = θ* + 0.05·(h_ref − h), ±25°; уставка состояния x_ref = [Va·cos θ_ref, Va·sin θ_ref, 0, θ_ref]; закон δ = δ* − K·(x − x_ref).
Регулятор считается при запуске по текущей модели ЛА — отдельного расчёта в tuning.py не нужно. Измерения u, w в s7 восстанавливаются из Va и θ.

11. Проверки

Проверка	Что проверяет
`checks/check_sau.py`	умолчания регуляторов = расчёт `tuning.py`; ступеньки тангажа, высоты, скорости, крена, курса, TECS на полной модели против теории; большой манёвр; режимы как в тренажёре
`checks/check.py`, раздел 8	знаки и логика боковой САУ
`checks/check_blocks.py`	схемы конструктора из блоков совпадают с регуляторами
`docs/figures/sau_plots.py`	все графики и таблицы этого документа

Сводка переходных процессов (полная модель, датчики без шума, 16 м/с):

Контур, ступенька	Перерегулирование, мод. / теор.	t_уст 5 %, мод. / теор.
тангаж +3°	0 / 1.5 %	0.61 / 0.22 с
крен 20° → 0	0.3 / 0 %	0.69 / 0.71 с
курс +20°	0 / 0 %	2.31 / 2.38 с
TECS: высота +5 м	0 / 0 %	6.1 / 6.0 с
TECS: скорость 16 → 18 м/с	0 / 0 %	6.8 / 6.0 с
раздельные: высота +5 м	5.4 / 7.6 %	3.3 / 6.4 с
раздельные: скорость 16 → 18 м/с	8.7 / 8.7 %	4.0 / 4.0 с

Режимы как в 3D-тренажёре (check_sau.py): на 26 м/с высота +100 м и затем −150 м отработана точно (300.0 / 150.0 м), α ≤ 3.9°; серия разворотов +90° → −90° → −45° — |φ| ≤ 30.2° при пределе уставки 30°, |Δh| ≤ 0.5 м.

12. Ограничения и что дальше

Одна точка настройки. Коэффициенты рассчитаны на 16 м/с. Масштаб по скорости есть у руля высоты и элеронов, таблицы балансировки — у TECS; полосы внешних контуров по скорости не масштабируются. Далеко от 16 м/с — пересчитать design_sau(aircraft, Va).
Оценка состояния — только в 3D (EKF, раздел 9.3; по умолчанию). В s1–s10 САУ работает прямо по измерениям; шум барометра проходит в уставку тангажа через упреждение TECS (SENSORS.md, раздел 5).
Тангаж — каскад П-звеньев без упреждения q в развороте: в крене ЛА немного теряет высоту, остаток снимает TECS.
Защита по α отвечает только за продольный канал; в развороте с большим креном перегрузка и α растут, а уставка крена не ограничивается по α.
Навигация — векторные поля B&M; L1 / NPFG и траектории Дубинса — дорожная карта.

Дорожная карта — DEV_PLAN.md; почему сделано так — decisions.md (РЕШ-21…23, 26, 27). Результаты на прежнем ЛА (Aerosonde, 30 м/с) и прежняя структура боковой САУ — тег git diploma и scenarios_report.md.

13. Пилотаж по траектории (побочная разработка)

control/aerobatics.py — регулятор полёта по пространственной траектории фигуры: петля, бочка, квадратная петля (DEV_PLAN, «Пилотажный комплекс C-11»). Пока отдельный класс AerobaticController, вне SAU; секвенсор комплекса — следующий блок. Измерения истинные.

Траектория. Фигура — отрезки line(L, roll) (прямая, с вращением по крену на roll за отрезок) и arc(R, angle) (дуга в плоскости тангажа, > 0 — на себя). В каждой точке — опорная СК $R_p = [t, y_p, z_p]$: касательная, правое крыло, «вниз» по кабине.

Каскад.

Наведение — ПД по отклонению в плоскости, перпендикулярной $t$, плюс центростремительное ускорение дуги: $a_y = -\omega_n^2 e_y - 2\zeta\omega_n \dot e_y$, $a_z = -V^2\kappa - \omega_n^2 e_z - 2\zeta\omega_n \dot e_z$; требуемая удельная сила $f = a - g$. Кривизна $\kappa$ и скорость вращения берутся в точке на $V\tau_{lead}$ впереди ($\tau_{lead}$ = 0.1 с) — упреждение запаздывания контура ориентации на скачке кривизны (вход в дугу).
Доворот креном: подъёмная сила поворачивается к $f$ вокруг $t$ на $\mu = \operatorname{atan}(f_y/(-f_z))$, $|\mu| \le 20°$, с весом $f_z^2/(f_y^2 + f_z^2)$ (без скачка, когда $f_z$ проходит через 0 на «ноже»).
Обращение модели сил: $\alpha_c = (-m f_z/(\bar q S) - C_{L0})/C_{L\alpha}$, $\beta_c = m f_y/(\bar q S,C_{Y\beta})$ (|α_c| ≤ α_warning, |β_c| ≤ 15°).
Ориентация: $R_{des} = R_v \cdot R_{b\to w}(\alpha_c, \beta_c)$, $R_v$ — по вектору воздушной скорости. Привязка к касательной $t$ (первый вариант) превращала команду по α в наклон вектора скорости: контур положения получался с коэффициентом $V m\omega_n^2/(\bar q S C_{L\alpha}) \approx 0.1$ 1/с — после бочки отклонение 1.2 м уходило с постоянной времени ≈ 8 с. Уставки угловых скоростей $\omega_c = \omega_{ff} + \log(R^T R_{des})/\tau_{att}$, $\tau_{att}$ = 0.15 с.
Рули: упреждение — балансировка моментов (Cm, Croll, Cn) при $(\alpha_c, \beta_c, \omega_c)$; обратная связь — ПИ по p (контур крена, раздел 5), П + И по q (Kq, раздел 3), П по r (полюс 10 рад/с); коэффициенты на 16 м/с, масштаб $(16/V_a)^2$.
Газ: $\delta_t = \delta_t^* + K_E \sin\gamma_{ref} + $ ПИ$(V_{a,ref} - V_a)$.

Результаты (checks/check_aerobatics.py, FPV-самолёт, вход 25 м/с, штиль, истинные измерения — моделирование):

Фигура	max \|e\| на фигуре, м	n	Va, м/с	α, °
Петля R = 15 м	0.36	0.8…5.8	19.5…26.7	−1.9…3.8
Бочка 360° за 50 м (180 °/с)	3.5	−1.7…2.3	24.5…25.6	−5.5…−0.2
Квадратная петля, углы R = 10 м	0.66	−1.2…8.9	17.1…29.0	−5.8…6.5

Бочка ограничена самолётом, а не регулятором. На «ноже» вес держит только боковая сила; при $C_{Y\beta}$ = −0.19 и 25 м/с для этого нужно β ≈ 31°, а РН даёт ≈ 20° (в упоре 2.7 % времени). Отклонение растёт с длительностью вращения: 2.0 м при 225 °/с, 3.0 м при 180 °/с, 5.5 м при 112 °/с, 9–11 м при 75 °/с.

Комплекс C-11 (C11, programme(), сценарий scenarios/s11_aerobatics.py). Фигуры 1–8 схемы Starting Schedule C-11 (январь 2010) подряд, между ними прямые 60 м:

петля R = 15 м;
горка с разворотом — вместо поворота на вертикали (при Va → 0 РН работает только за счёт обдувки винтом, её в модели нет): подъём 45°, четверть бочки, разворот на 180° дугой в наклонной плоскости, четверть бочки обратно, выход в горизонт обратным курсом со сдвигом вбок 2R. Обратная четверть бочки — −(180° − крен) (подобрано по конечной СК);
квадратная петля (стороны 20 м, углы R = 10 м);
подъём 45° с полубочкой, 5/8 петли R = 20 м из положения вверх ногами;
бочка 360° за 50 м;
вертикаль вверх, полупетля R = 8 м, вертикаль вниз с полубочкой, выход обратным курсом;
кубинская восьмёрка R = 15 м (прямые под 45° длиной 2R с полубочкой — касательные к обеим петлям при расстоянии между центрами 2√2·R);
Иммельман R = 15 м.

Штопор (фигура 9) не выполняется — нет АДХ за срывом. Каждая фигура кончается горизонтально «на ногах» в направлении по схеме (проверка — check_aerobatics.py). Траектория — 1412 м, полёт — 59 с, выход на 25 м выше входа (Иммельман).

Результаты (вход 25 м/с, 150 м; истинные измерения; моделирование), max |e| на фигуре, м:

Фигура	штиль	ветер 4 м/с
1. Петля	0.36	0.91
2. Горка с разворотом	0.74	1.14
3. Квадратная петля	0.64	1.48
4. Подъём 45° с полубочкой, 5/8 петли	2.49	2.75
5. Бочка	3.43	3.87
6. Вертикаль — полупетля — вертикаль с полубочкой	0.70	1.94
7. Кубинская восьмёрка	1.82	2.63
8. Иммельман	1.54	2.44

Va в фигурах 15.9…29.3 м/с, α −6.0…7.9° (α_warning = 9°). Наибольшие отклонения — на фигурах с полубочками (4, 5, 7, 8): на «ноже» вес держит только боковая сила (|β| до 19°). Ветер — на юг, встречный на фигуре 1, как на схеме; траектория задана в земной СК, поэтому петли и при ветре круглые относительно земли. Ориентация, восстановленная по φ, θ, ψ лога, совпадает с кватернионной до 2e-13 и при θ = 89.994° — лог корректно показывает 3D-проигрыватель (python viz/viewer3d.py results/<файл>.flightlog).

Режим в SAU и 3D-тренажёр. Mode.AEROBATIC: SAU.fly_aerobatics(state, Va, α, β) строит программу от текущей точки; все четыре руля даёт AerobaticController.step_nav по навигационной оценке — положение, земная и воздушная скорость, матрица поворота, p/q/r, Va:

с EKF (3D по умолчанию) — из фильтра: кватернион, v, p, угловые скорости без смещений гироскопа; ветер — оценка EKF на входе в программу, постоянная до конца;
без EKF — истина (воздушная скорость — по Va, α, β и матрице поворота).

По окончании программы SAU сама переходит в ALTITUDE на текущих высоте и путевом угле. В тренажёре — клавиша K (или --aero): САУ включается, разгон до 25 м/с, проверка запаса высоты траектории над рельефом (< 30 м — набор), затем комплекс; события фигур — в журнал и .flightlog; K, P, H, W/S/A/D прерывают.

Синтетическое β = 0 в EKF в пилотаже выключено (StateEstimator.fuse_beta, ставит SAU). Это допущение полёта без скольжения (EKF2_FUSE_BETA), а на «ноже» ЛА летит с β до 20° намеренно: с β = 0 ошибка оценки ветра доходила до 7.6 м/с — направление воздушной скорости сбивалось на ~17°, и ЛА уходил с траектории на 140 м (слабая турбулентность). Без β = 0 ветер по одному ПВД в фигурах не успевает за порывами (ошибка до 5 м/с), поэтому регулятор берёт ветер, оценённый в установившемся полёте перед входом.

Комплекс через SAU (вход 25 м/с, 150 м; моделирование), max |e| по оценке САУ:

Измерения	штиль	слабая турбулентность	умеренная турбулентность
истина	3.6 м	3.6 м	3.9 м
реалистичные датчики + EKF	3.8 м	9.3 м	21.8 м

Ошибки EKF в пилотаже — ориентация ≤ 1.9°, скорость ≤ 1.1 м/с, положение ≤ 4.2 м. Разница с истиной в турбулентности — порывы: без измерения α и β регулятор не видит изменения направления потока, пока порыв не уведёт ЛА с траектории (с истинными α, β — видит сразу). В реальном времени (3D, EKF, штиль): разгон 5 с, фигуры 1–8 — 5.0…64.4 с, далее удержание высоты; FPS 75, как без пилотажа.

Не охвачено: предельная перегрузка конструкции (в углах квадратной петли n = 8.9; решение автора — оставить), поворот на вертикали (нет обдувки РН винтом — заменён горкой с разворотом).

FilesExpand file tree

control.md

Latest commit

History