Модель (формулировка задачи)
- Пространство свободного движения: $\mathcal{F}={\mathbb{R}}^3\setminus \mathcal{O}$ (объекты $\mathcal{O}$ — замкнутые множества). Для учёта размеров аппарата используем надувание препятствий: $\mathcal{F}\leftarrow \mathcal{F}\setminus (\mathcal{O}\oplus B_r)$.
- Пусть траектория — гладкая кривая $\gamma :[0,1]\to \mathcal{F}$. Минимизируем длину (или обобщённую стоимость):
$$\underset{\gamma }{\min }J(\gamma )={\int }_0^1\Vert {\gamma }^{\prime }(t)\Vert dt$$ при ограничениях:
$$\gamma (0)=p_0,\gamma (1)=p_1,\frac{{\gamma }^{\prime }(0)}{\Vert {\gamma }^{\prime }(0)\Vert }=v_0,\frac{{\gamma }^{\prime }(1)}{\Vert {\gamma }^{\prime }(1)\Vert }=v_1.$$
- Ограничение на кривизну (максимальная центростремительная кривизна ${\kappa }_{\max }$):
$$\kappa (t)=\frac{\Vert {\gamma }^{\prime }(t)\times {\gamma }^{\prime \prime }(t)\Vert }{\Vert {\gamma }^{\prime }(t){\Vert }^3}\le {\kappa }_{\max }\forall t.$$ (В 3D при необходимости добавляют ограничение на кручение/горизонтальный уклон: $|\arctan (⟨{\gamma }^{\prime }(t),e_z⟩/\Vert {\gamma }_{xy}^{\prime }(t)\Vert )|\le {\alpha }_{\max }$.)
- Коллизии:
$$\gamma (t)\in \mathcal{F}\forall t\in [0,1].$$
- Часто добавляют сглаживающий член (комфорт/энергия) — эластика:
$$J(\gamma )={\int }_0^1(1+\lambda \kappa (t{)}^2)\Vert {\gamma }^{\prime }(t)\Vert dt.$$
Почему такая модель. Это минимизация длины с нелинейным неравенством на кривизну и ограничением коллизий — даёт оптимальную по длине и реализуемую траекторию с учётом маневренности дрона и безопасности.
Практические геометрические методы и обоснование
1) Dubins / Reeds–Shepp (2D, секвенции дуг/прямых)
- Что: аналитические оптимальные пути для автомашин с ограничением на кривизну в плоскости (Dubins: без заднего хода; Reeds–Shepp: с задним ходом).
- Почему: дают быстрые точные решения в 2D и хороши как семена для локальной оптимизации; используют минимальное число сегментов (CSC/CCC).
- Ограничение: не учитывают 3D, гладкость кривизны (для комфортного полёта лучше clothoid/сплайны).
2) Клотхоиды / сплайн-аппроксимации (Hermite, Bézier, B‑spline, quintic)
- Что: параметрические кривые с контролируемой гладкостью; клотхоиды обеспечивают линейную вариацию кривизны (плавная смена радиуса поворота).
- Почему: обеспечивают непрерывную кривизну (важно для динамики дрона), удобно накладывать ограничения на максимальную кривизну и наклон; позволяют получить оптимизацию по контрольным точкам (многочисленные численные solvers).
- Практика: строят коридор свободного пространства, затем оптимизируют контрольные точки сплайна с ограничениями $\kappa (t)\le {\kappa }_{\max }$ (дискретизация/коллокейшн).
3) Эластика / вариационные методы (геодезические в Finsler/Riemannian метрике)
- Что: минимизация функционалов вида $\int (1+\lambda {\kappa }^2)ds$ — общая теория геодезических в метрике, зависящей от направления/кривизны.
- Почему: естественно вводит штраф за кривизну, превращая задачу в поиск геодезической в анизотропной метрике; можно задавать «стоимость» близко к препятствиям (метрика, взрывающаяся у границ).
- Инструменты: Euler–Lagrange уравнения, численные интеграторы, Fast Marching на анизотропных метриках для глобальной аппроксимации расстояния.
4) Fast Marching / Dijkstra в конфигурационном пространстве с Finsler‑метрикой
- Что: строят поле расстояний в расширенном пространстве положения+направления (или с штрафом за повороты).
- Почему: быстро даёт глобальные минимумы в дискретной аппроксимации и учтёт препятствия; результат служит хорошим инициалом для непрерывной оптимизации.
5) Sampling‑based (kinodynamic RRT*, PRM) с локальным планировщиком, учитывающим кривизну
- Что: строят граф в конфигурационном пространстве; локальные ребра — Dubins/клотхоидные сегменты.
- Почему: хорошо работает в сложных топологиях (много огибаний вокруг зданий), обеспечивает вероятностную сходимость к оптимуму (RRT*). Нужно комбинировать с локальным сглаживанием.
6) Последовательная выпуклая оптимизация (SCP / convexification)
- Что: пошагово линеаризуют ограничения столкновений/кривизны, решают выпуклые подзадачи (QP/SOCP).
- Почему: позволяет использовать мощные выпуклые солверы для реального времени; хорошо для финальной оптимизации по сплайн‑параметрам внутри найденного коридора.
7) Коридорное разложение / гомотопический анализ
- Что: делят свободное пространство на связные коридоры (выпуклые полигоны), рассматривают разные гомотопические классы.
- Почему: гарантирует, что оптимизация не застрянет в «неправильном» классе; внутри каждого коридора можно сделать выпуклую оптимизацию траектории с ограничениями на кривизну.
Рекомендованная практическая схема (баланс глобальности, гладкости и вычислительной эффективности)
1. Построить конфигурационное пространство (надув препятствий для учёта размеров).
2. Быстрый глобальный поиск: Fast Marching в анизотропной метрике или sampling‑graph с Dubins/клотхоидным локальным планировщиком для получения начальной траектории и набора гомотопий.
3. Коридор/выпуклая аппроксимация вдоль пути.
4. Локальная оптимизация сплайнами (B‑spline/quintic/clothoid) с ограничением $\kappa (t)\le {\kappa }_{\max }$, ограничением наклона и коллизий (SCP + проверка по дискретизации).
5. При необходимости — повторная глобальная корректировка (RRT*/пересчёт метрики) и реоптимизация.
Краткие замечания по выбору метода
- Для простых 2D задач и жёсткой кривизны — Dubins/RS удобно и аналитически; для комфорта/плавности — клотхоиды/сплайны лучше.
- Для городского «лабиринта» с множеством гомотопий — sampling‑approaches (RRT*, PRM) или Fast Marching в расширенном пространстве + гомотопический учёт.
- Для реального времени — строим коридор и применяем SCP по сплайнам; для оффлайна/оптимального планирования — вариационные методы/элastica.
Итого: модель — задача минимизации длины (или длины+штрафа на кривизну) для кривой $\gamma$ с ограничением $\kappa (t)\le {\kappa }_{\max }$ и $\gamma (t)\in \mathcal{F}$. Практическая комбинация: глобальный поиск (Fast Marching / sampling + Dubins/clothoid local) → коридор → локальная сплайн‑оптимизация с контролем кривизны и коллизий.