4.2 KiB
3.2 Модель позиционирования
Модель позиционирования описывает положение и ориентацию беспилотного летательного аппарата в трёхмерном пространстве. Класс Position (файл position.py) инкапсулирует все параметры состояния БПЛА и операции над ними.
\section{Параметры позиции}
Позиция БПЛА характеризуется следующими параметрами:
\begin{itemize}
\item x, y — координаты в горизонтальной плоскости (пиксели карты)
\item z — масштаб изображения (уровень приближения)
\item yaw — угол рыскания (поворот вокруг вертикальной оси)
\item pitch — угол тангажа (поворот вокруг поперечной оси)
\item roll — угол крена (поворот вокруг продольной оси)
\end{itemize}
\section{Матрица гомографии}
Матрица гомографии связывает координаты точек на двух изображениях одной и той же плоскости. Для БПЛА гомография описывает преобразование между последовательными кадрами видеопоследовательности.
Матрица гомографии вычисляется как:
\begin{equation} H = K_{out} \cdot R \cdot T \cdot K_{in}^{-1} \end{equation}
где K_{in} и K_{out} — матрицы внутренних параметров камеры, R — матрица вращения, T — матрица трансляции.
\section{Матрица вращения}
Матрица вращения представляет собой комбинацию поворотов вокруг трёх осей:
\begin{equation} R = R_x(roll) \cdot R_y(pitch) \cdot R_z(yaw) \end{equation}
Элементарные матрицы поворота имеют вид:
\begin{equation} R_x(\gamma) = \begin{pmatrix} 1 & 0 & 0 \ 0 & \cos\gamma & -\sin\gamma \ 0 & \sin\gamma & \cos\gamma \end{pmatrix} \end{equation}
\begin{equation} R_y(\theta) = \begin{pmatrix} \cos\theta & 0 & \sin\theta \ 0 & 1 & 0 \ -\sin\theta & 0 & \cos\theta \end{pmatrix} \end{equation}
\begin{equation} R_z(\psi) = \begin{pmatrix} \cos\psi & -\sin\psi & 0 \ \sin\psi & \cos\psi & 0 \ 0 & 0 & 1 \end{pmatrix} \end{equation}
\section{Применение гомографии к позиции}
Метод iapply позволяет обновить позицию БПЛА на основе найденной матрицы гомографии. Алгоритм включает:
\begin{enumerate}
\item Декомпозицию гомографии на компоненты R и t
\item Вычисление оптимальной трансформации
\item Обновление координат и углов ориентации
\end{enumerate}
Трансформация координат выполняется с учётом фокусного расстояния камеры:
\begin{equation} x_{new} = x_{old} - T_x \cdot K_{focus} \end{equation} \begin{equation} y_{new} = y_{old} + T_y \cdot K_{focus} \end{equation}
\section{Операции над позицией}
Класс Position поддерживает арифметические операции:
\begin{itemize} \item Умножение на скаляр — масштабирование координат \item Деление на скаляр — обратное масштабирование \item Копирование — создание полной копии состояния \end{itemize}
Эти операции используются при интерполяции траекторий и преобразовании координат.