Алгоритм автоматического программного управления полётом дрона-квадрокоптера до цели и обратно

Математическая модель. Рассматривается конкретный тип квадрокоптера (рис. 1):

Расчетная схема управляемого движения указанного квадрокоптера, согласно [1-3, 9] имеет вид (рис. 2):

Рис. 2. Расчетная схема квадрокоптера

Согласно [3, 9] математическая управляемая динамическая модель движения квадрокоптера имеет вид:

t_нач ≤  t≤ t_кон, (1)

где x – шестимерный вектор:

где, в соответствии с общепринятым, точка над буквой обозначает производную по времени t.

В (1) управление u – трёхмерный вектор в декартовой системе координат {x,y,z}:

v – вектор помехи (ветер)

g – ускорение силы тяжести, v – помеха – сила ветра, А, В, С, D – матрицы вид:

Здесь матрица В выбирается постоянной, т.е. мы полагаем углы Эйлера ϕ,ψ,θ (рис. 2) постоянными и заданными для рассматриваемого полёта. В инерциальной системе отсчета, центробежная сила обнуляется. Таким образом, только гравитационная сила G = mg, величина и направление сил тяги u и помеха v (ветер) участвуют в ускорении квадрокоптера.

Задача. Будем рассматривать случай, когда помеха v (ветер) является постоянной во всё время полёта:

t_н=t_нач ≤ t ≤ t_к=t_кон,

где t_н и t_к заданы и зафиксированы. Содержательно они определяются временным ресурсом полёта квадрокоптера, который определяется, например, для рассматриваемого квадрокоптера (рис. 1) ёмкостью аккумулятора. Так как время полета достаточно мало (например, у указанного типа квадрокоптера оно составляет порядка 10 минут – время полной разрядки аккумулятора) можно, с достаточной степенью вероятности, предположить, что помеха – будет постоянной и детерминированной.

Рассмотрим следующую конкретную задачу. Из состояния покоя в начальный момент времени t_н (рис. 3) квадрокоптер стартует и поднимается на заданную высоту h за заданное время t_н ≤ t ≤ t₁. После этого всё дальнейшее движение квадрокоптера до цели происходит в горизонтальной плоскости. Конструируется программное [4, 5] управление {u(t), t₁ ≤ t ≤ t₂}, обеспечивающее его подлёт до заданной цели. После этого квадрокоптер зависает над целью на время t₂ ≤ t ≤ t₃, например, для осуществления, видеосъёмки и возвращается назад за время t₃ ≤ t ≤ t₄, также совершая горизонтальный полёт на той же высоте h. Выйдя на вертикальную ось, перпендикулярную гнезду, осуществляется спуск и его посадка в «гнездо» за время  t₄ ≤ t ≤ t₅ (рис. 3).

В связи с этим возникает задача о построении такого управления, которое на каждом интервале полёта обеспечивает его оптимальность, т.е. минимум затрат энергии на выработку управляющих воздействий, обеспечивающих решение поставленной задачи.

Известно, что для двигателей малой тяги критерий качества процесса управления [5, 8] для решения такой задачи имеет вид:

 (2)

Алгоритм оптимального управления. В соответствии с теорией оптимального управления [5, 8], разработанной в Свердловской (ныне Екатеринбургской) школе по оптимальному управлению Н.Н. Красовского, управляющие воздействия сконструируем следующим образом.

На первом этапе подъёма квадрокоптера на заданную высоту h за заданное время t_н ≤ t ≤ t₁, будем использовать следующие начальные и конечные условия:

t_н, x(t_н) = x_н,x5(t₁) = h, ϕ = ϕ*, ψ = ψ*, ϑ = ϑ*,

где h – высота подъёма квадрокоптера. Естественно, предполагается, что эта высота больше высоты цели. Будем полагать, что t_н=0 и x_н=0, где x – вектор (2). Тогда вектор x(t₁) (2) имеет все компоненты равными нулю, кроме компоненты x₅=h. При этом на рис. 3 условно траектория подъёма квадрокоптера изображена в виде вертикальной прямой, но благодаря помехе (ветру) она может иметь и криволинейную форму.

Относительно помехи (силы ветра) предполагаем, что на всём отрезке времени полёта (3) она остаётся постоянной v(t)=v*=const и неизменной. Более того, предполагаем, что сила ветра расположена в горизонтальной плоскости, что соответствует реальным условиям полёта квадрокоптера.

Согласно [4, 5] оптимальное по критерию качества (2) управляющие воздействия (программное управление) имеет вид:

t_н ≤ t ≤ t₁, 

где

  фундаментальная матрица [5] решений однородного дифференциального уравнения:

x=Ax.

где верхний индекс штрих означает транспонирование.

На втором этапе горизонтального полёта до цели (рис. 3) решается задача о переводе квадрокоптера из точки над «гнездом» в точку над целью. Граничные условия имеют следующий вид:

t₁, x(t₁), t₂,x(t₂), v(t) = v*, ϕ = ϕ*, ψ = ψ*, ϑ = ϑ*.

В соответствии с вышеизложенным, оптимальное управление по критерию качества (2), решающее эту задачу, имеет вид:

t₁ ≤ t ≤ t_2.

На третьем этапе решается задача о зависании квадрокоптера над объектом (целью) (рис. 3) на отрезке времени  t₂ ≤ t ≤ t₃,   при этом граничные условия имеют вид:

t₂, x(t₂), t₃, x(t₃) = x(t₂), v(t) = v*, ϕ=ϕ*, ψ = ψ*, ϑ = ϑ*.

Управление, решающее эту задачу, имеет вид:

t₂ ≤ t ≤ t₃.

На четвёртом этапе решается задача об управлении, обеспечивающем полёт квадрокоптера от цели в точку, лежащую на вертикали, перпендикулярную плоскости «гнезда». При этом предполагается, что квадрокоптер некоторое время зависает над целью, например для видеосъёмки. Тогда перелёт на этом этапе начинается в момент времени t3 и заканчивается в момент времени t4 (рис. 3). При этом граничные условия для решения задачи управления имеют следующий вид:

t₃, x(t₃), t₄, x(t₄) = x(t₁), v(t) = v*, ϕ=ϕ*, ψ = ψ*, ϑ = ϑ*.

Управление, решающее эту задачу, имеет вид:

t₃ ≤ t ≤ t₄.

На последнем пятом этапе, на отрезке времени t₄ ≤ t ≤ t₅, квадрокоптер спускается в «гнездо». В этом случае граничные условия имеют вид:

t₄, x(t₄), t₅, x(t₅) = x_н,v(t) = v*, ϕ=ϕ*, ψ = ψ*, ϑ = ϑ*.

Управление, решающее эту задачу, имеет вид:

t₄ ≤ t ≤ t₅.

Итак, мы построили программное управление, складывающееся из пяти управлений (3) и т.д., решающих соответствующие задачи. Известно [4, 5], что так построенное управление на каждом отрезке времени  t_j ≤ t ≤ t_j+1, j=0,...,4, t₀=t_н (рис. 3) является оптимальным по рассматриваемому критерию качества:

характеризующим затраты энергии на выработку управляющих воздействий.

Хотя работа носит теоретический характер, предлагается способ посадки квадрокоптера в «гнездо», позаимствованный из космической отрасли, а именно стыковки космических аппаратов [7]. Условно схема такой посадки изображена на рис. 4.

Рис. 4. Гарантированная посадка квадрокоптера в «гнездо»

Попадание квадрокоптера для зарядки – посадка в «гнездо», представляет определённую трудность, в связи с необходимостью большой точности приземления. Хотя квадрокоптер снабжен видеокамерами, это не упрощает решение такой задачи. Известно [7], что при стыковке двух космических аппаратов используется трос, который их соединяет и движение происходит благодаря втягиванию троса в один из аппаратов. Это обеспечивает абсолютную точность стыковки. Можно трактовать трос, как некоторый «якорь» или «гарпун», который выбрасывает один из объектов (стыкующийся) и попадает в стыковочное отверстие второго аппарата. Это используется автором статьи для обоснования возможности решения рассматриваемой задачи на практике.