Санкт-Петербургский государственный университет
Кафедра компьютерных технологий и систем
Алиев Айдын Эльдар оглы
Выпускная квалификационная работа бакалавра
Разработка противоаварийных алгоритмов
управления квадрокоптером
010400
Прикладная математика и информатика
Научный руководитель,
кандидат физ.-мат. наук,
доцент
Жабко Н.А.
Санкт-Петербург
2016
Содержание
Введение . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Обзор литературы . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Глава 1. Математическое описание. Постановка задачи. . . . . . . . .
1.1 Математическая модель движения квадрокоптера. . . . . . .
1.2 Постановка задачи. . . . . . . . . . . . . . . . . . . . . . . . .
Глава 2. Обнаружение и локализация отказов приводов винта квадрокоптера с помощью метода главных компонент . . . . . . . . . . .
2.1. Метод главных компонент. . . . . . . . . . . . . . . . . . . .
2.2 Обнаружение и локализация отказов приводов винтов квадрокоптера. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Глава 3. Управление полетом квадрокоптера при отказе одного привода.
3.1 ПИД регулятор. . . . . . . . . . . . . . . . . . . . . . . . . . .
3.2 ПД регулятор для полета квадрокоптера с 4мя винтами. . .
3.3 Управление квадрокоптером при полном отказе одного винта.
3.4 Общая схема решения задачи. . . . . . . . . . . . . . . . . . . . .
Глава 4. Эксперимент и результаты. . . . . . . . . . . . . . . . . . . .
Выводы. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Заключение. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Список литературы . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2
3
4
5
5
8
10
10
14
16
16
18
21
23
24
27
28
29
Введение
В работе рассматривается задача обнаружения аварийной ситуации
для управляемого движения квадрокоптера. Целью работы является выявление отказа винта на основе полученных с датчиков квадрокоптера данных о значениях измеряемых переменных состояния. Данная задача является актуальной, ибо ни одна система не может обеспечить бесперебойную
работу аппарата. В случае непредвиденного отказа какой-либо системы или
её части важно как можно быстрее восстановить её работоспособность, пока последствия не стали критичными, а для этого необходимо максимально
быстро идентифицировать и локализовать саму проблему и принять соответствующие меры. Для решения данной задачи используется подход, базирующийся на методе главных компонент (Principal Component Analysis —
PCA) и не требующий точного знания модели объекта. Поскольку для рассматриваемого объекта, как правило, параметры объекта точно неизвестны, для решения задачи выбран именно подход, основанный на PCA. Для
управления движением квадрокоптера выполнен синтез пропорциональнодифференциальных регуляторов по управляемым переменным. Моделирование и проведение экспериментов для сбора данных, используемых для
выявления аварийной ситуации, выполнено в среде MATLAB для конкретной модели квадрокоптера.
3
Обзор литературы
Математическая модель движения квадрокоптера была представлена
в [2]. Метод главных компонент и тонкости данного метода описан в [3].
Применение данного метода для подводной автономной системы изложено
в [4]. Задача управления квадрокоптером с тремя винтами рассмотрена в
[1]. Подробно ПИД регулятор был изложен в [5]. Алгоритмы стабилизации
для управления движением квадрокоптера расписаны в [6].
4
Глава 1. Математическое описание. Постановка
задачи.
1.1 Математическая модель движения
квадрокоптера.
Квадрокоптер — это летательный аппарат с 4 двигателями, расположенными на концах двух, обычно перпендикулярных, осей. Двигатели
квадрокоптера, расположенные на разных осях, вращаются в противоположных направлениях для компенсации крутящего момента. Квадрокоптер снаряжен следующими датчиками: трехосевой акселерометр, трехосевой гироскоп, трехосевой магнитометр, датчик температуры, барометр,
4 инфракрасных датчика препятствий в горизонтальной плоскости, ультразвуковой дальномер, направленный вниз.
Система координат и силы, действующие на квадрокоптер, представлены на рис. (1). Инерциальная система координат A определяется осями
a1 , a2 , a3 из начала координат O. Связанная система координат B прикреплена к центру масс квадрокоптера C с b1 , указывающим основное направление, и b3 , перпендикулярным к плоскости приводов винтов в условиях идеального парения. Эти векторы параллельны главным осям. L — расстояние
от привода винта до центра масс квадрокоптера. Fi — сила тяги, создаваемая каждым двигателем. Для управления квадрокоптером используются
Рис. 1: Модель квадрокоптера с инерциальной и связанной системами координат
углы Эйлера (φ, θ, ψ — углы крена, тангажа и рысканья соответственно).
Матрица перехода R из системы координат B в A выглядит следую5
щим образом:
cψcθ − sφsψsθ −cφsψ cψsθ + cθsφsψ
R = cθsψ + cψsφsθ cφcψ sψsθ − cψcθsφ ,
−cφsθ
sφ
cφcθ
где c — обозначают cos соответствующего угла, s — обозначают sin. Обозначим компоненты угловой скорости тела в связанной системе координат
ω B = (p, q, r).
Нелинейная модель, определяющая в любой момент времени ускорение центра масс квадрокоптера, которая была описана в [2], задается
уравнениями:
0
0
,
0
(1)
mr̈ = 0 + R
F1 + F2 + F3 + F4
−mg
где
m — масса квадрокоптера, g — ускорение свободного падения, r =
x, y, z — вектор, задающий положение объекта в пространстве.
Квадрокоптер состоит из корпуса и четырех винтов. Матрица тензора
инерции в связанной системе координат выглядит следующим образом:
B
Ixx 0 0
B
0 .
I B = 0 Iyy
B
0 0 Izz
Момент инерции двигателя, выраженный в неподвижной системе координат квадрокоптера, не зависит от ориентации, и представляется в виде:
P
Ixx 0 0
P
0 .
I P = 0 Iyy
P
0 0 Izz
Суммарный момент инерции:
B
P
Ixx + 4Ixx
0
0
B
P
.
0
Iyy
+ 4Iyy
0
I=
B
P
0
0
Izz
+ 4Izz
Каждый двигатель имеет угловую скорость ωi и создает силу тяги
Fi :
Fi = kF ωi2
Двигатели также создают момент:
Mi = kM ωi2
6
Экспериментально имеем kF ≈ 6.41 · 10−6 и kM ≈ 1.5 · 10−9 .
Угловое ускорение, определяемое углами Эйлера:
p
L(F2 − F4 )
p
ṗ
− q × I q ,
L(F3 − F1 )
I q̇ =
r
M1 − M2 + M3 − M4
r
ṙ
(2)
Запишем уравнение (2) в виде:
F1
p
p
0
L 0 −L
ṗ
F 2
I q̇ = −L 0 L 0 − q × I q ,
F3
r
r
γ −γ γ −γ
ṙ
F4
(3)
где γ = kkMF — взаимосвязь между подъемной силой и силой сопротивления.
Система датчиков квадрокоптера измеряет все переменные состояния
X = [x, y, z, ẋ, ẏ, ż, φ, θ, ψ, φ̇, θ̇, ψ̇].
Переменные ωi измерить напрямую нельзя, так как отсутствует такой датчик.
После получения значений переменных с датчиков квадрокоптера
необходимо применить поправку к мощностям двигателей, для устранения возможного отклонения. Для этой цели обычно применяется ПД регулятор. Для синтеза ПД регулятора используется линейная модель системы (1). Линеаризуем систему (1) в окрестности положения равновесия для
получения взаимосвязи между желаемыми ускорениями и углами тангажа
и крена:
r̈1 = g(∆θ cos ψ0 + ∆φ sin ψ0 ),
r̈2 = g(∆θ sin ψ0 + ∆φ cos ψ0 ),
1
r̈3 = u1 − g.
m
Аналогично линеаризуя систему (3), имеем:
F1
0
L 0 −L
ṗ
q̇ = I −1 −L 0 L 0 F 2 ,
F 3
ṙ
γ −γ γ −γ
F4
(4)
(5)
Для осуществления управляемого движения выбираются четыре управляющих переменных u1 , u2 , которые позволяют обеспечить стабилизацию
7
переменных по заданным значениям:
u1 =
4
X
Fi ,
i=1
F1
0
L 0 −L
F 2
u2 = −L 0 L 0
F 3 ,
γ −γ γ −γ
F4
где u1 — управление, обеспечивающее парение, а u2 — управление, регулирующее ориентацию квадрокоптера в пространстве.
Законы управления сформулированы таким образом, чтобы обеспечить движение из начальной точки (x0 , y0 , z0 ) в пространстве с координатами OXYZ в конечную точку (x1 , y1 , z1 ).
1.2 Постановка задачи.
Модель (1), (3) используется для проведения экспериментов при моделировании движения объекта. В случае непредвиденного отказа какойлибо системы или её части важно как можно быстрее восстановить её работоспособность, пока последствия не стали критичными. В качестве ситуаций, которые могут быть характерными для квадрокоптера, можно рассматривать полный или частичный отказ одного или нескольких винтов.
В случае полного отказа винта соответствующая переменная ωi принимает
нулевое значение, то есть ωi = 0, и, следовательно, Fi = 0. Данная ситуация является аварийной. Для предотвращения падения квадрокоптера при
возникновении указанной ситуации необходимо зафиксировать возникновение данного события и обеспечить посадку или управляемое движение
в изменившихся условиях. Таким образом, проблема построения противоаварийных законов управления предполагает решение следующих задач:
1. На основе измеренных данных о переменных состояния объекта
X = [x, y, z, ẋ, ẏ, ż, φ, θ, ψ, φ̇, θ̇, ψ̇]
в режиме его нормального функционирования, а также аналогичным
данным, поступающих при отказе винта зафиксировать такой отказ
для конкретного винта (задача обнаружения и локализации отказа).
2. Обеспечить управляемое движение квадрокоптера при отказе одного
из винтов.
Нормальным режимом функционирования будем считать управляемое движение квадрокоптера с четырьмя рабочими винтами при перемещении че8
рез заданные точки (xi , yi , zi ), i = 1, n.
9
Глава 2. Обнаружение и локализация отказов
приводов винта квадрокоптера с помощью метода
главных компонент
2.1. Метод главных компонент.
Метод главных компонент (Principal component analysis — PCA), описанный в [3], является одним из основных методов уменьшения размерности данных, потеряв наименьшее количества информации. Основная идея
метода состоит в уменьшении размерности набора данных с учетом большого количества взаимосвязанных переменных при сохранении стольких
вариаций, присутствующих в исходном наборе данных, сколько возможно.
Это достигается за счет трансформации измеренных данных в новый набор
некоррелированных переменных, называемых главными компонентами.
Рис. 2: Иллюстрация метода главных компонент для систем управления
Предполагается, что X = x1 , . . . , xn — (N × n)-матрица, составленная из N n-мерных наблюдений состояния системы. Далее, ищется сокращенный набор значительно меньшего числа r < n переменных, который сохраняет большую часть информации, содержащейся в дисперсиях
и ковариациях между переменными состояния. Это достигается с помощью набора ортогональных векторов в направлениях, где данные варьируются большего всего. Тогда, для описания дисперсии данных достаточно
несколько главных компонент.
Для иллюстрации рассмотрим N = 8 измерений для двух перемен-
10
ных x1 (k), x2 (k), см. рис. 3. Измерения представлены в виде двух векторов
x1 = x1 (1), . . . , x1 (8)
x2 = x2 (1), . . . , x2 (8) .
(6)
X = x1 x2 .
(7)
А матрица данных
содержит все измеренные данные в системе координат (x2 |x1 ). Как показано на рис. 3, измеренные данные колеблются в обоих направлениях координат x1 и x2 .
Теперь, нужно перейти к новой системе координат (t2 |t1 ), для которой дисперсии данных максимальны в направлении t1 , затем — t2 . Таким
образом формируются первая и вторая главные компоненты. Также накладывается условие, что (t2 |t1 ) образуют ортогональную систему.
Рис. 3: График 8 измерений для двух переменных
Рис. 4: График 8 измерений для двух переменных с главными компонентами
11
На рис. 4 показаны результаты. Таким образом, матрица данных X
переходит в новую матрицу данных
T = t1 t2 .
(8)
Дисперсия данных очень велика в направлении t1 и очень мала в направлении t2 , поэтому главная компонента t1 достаточно аппроксимирует дисперсию данных и второй главной компонентой t2 можно пренебречь.
Основная задача заключается в том, чтобы трансформировать матрицу с n переменными xi (k)
X = x1 , . . . , xn
по N измерений в новую матрицу
T = t1 , . . . , tr
по N измерений, но с меньшим числом переменных r < n. Это можно
достичь с помощью матрицы перехода P
T(N ×r) = X(N ×n) P(n×r) .
(9)
P = p1 , . . . , p r .
(10)
Так как эта матрица перехода является ортонормальной, то P T P = I.
Тогда верно следующее
X = TPT.
(11)
В терминах многомерной статистики матрица T называется “score matrix”,
а P –– “loading matrix”. Формулу (11) можно записать в виде
X=
t1 pT1
+
t2 pT2
+ ... +
tr pTr
=
r
X
tj pTj
j=1
Для того, чтобы найти элементы pj матрицы P , которая ведет к максимальной дисперсии, нужно решить задачу пошаговой оптимизации. Для
каждого шага j с tj = Xpj максимальная дисперсия tj имеет вид
maxtTj tj = max(Xpj )T (Xpj ) = max(pTj X T Xpj )
(12)
с ограничением (10)
pTj pj = 1
что означает ортогональность компонент.
Стандартным подходом к этой задаче оптимизации является исполь12
зование метода множителей Лагранжа. Если нужно максимизировать функцию f (pj ) при условии g = pTj pj − 1 = 0, функция потерь принимает вид
V = f (pj ) − λj g(pj )
где λj — множитель Лагранжа. Это приводит к
V = pTj X T Xpj − λj (pTj pj − 1)
и
dV
= 2X T Xpj − 2λj pj = 0
dpj
или
T
X X − λj I pj = 0.
С
A = XT X
получаем
A − λj I pj = 0.
(13)
Получаем классическую задачу поиска собственных чисел. Матрица пропорциональна корреляционной или ковариационной матрице с нулевыми
средними значениями переменных, λj — собственные числа а pj — собственные векторы. Из (13) следует
pTj Apj = pTj λj pj
и подставляя в (12) имеем наибольшую дисперсию
max tTj tj = max pTj λj pj
Тогда максимальные собственные числа λj характеризуют максимальную
дисперсию для переменных tj .
При обратном преобразовании данных в исходную систему координат
получаются только те исходные данные, у которых значения дисперсии
максимальны, то есть незначимые шумы удаляются.
Далее рассчитывается квадрат ошибки прогноза, представляющий
собой квадрат нормы разности исходного и восстановленного сигналов и
используемый для выявления аномальных
2ситуаций, принято обозначать
e
SPE (Square Prediction Error): SP E =
X
.
Процесс считается нормальным, если
SP E < σ 2 ,
где σ 2 является границей доверительного интервала или порогом для SPE
13
и является достаточно малым числом.
2.2 Обнаружение и локализация отказов приводов
винтов квадрокоптера.
Для выявления отказа с помощью метода PCA, в первую очередь,
необходимо выделить главные компоненты, которые характеризуют динамику квадрокоптера. Это выполняется по данным для режима нормального функционирования, главные компоненты выбираются так, чтобы ошибка SPE была достаточно мала в этом режиме. Если в системе управления
происходит какой-то сбой, главные компоненты уже недостаточно точно
характеризуют динамику, и ошибка SPE выходит за допустимые пределы, это и позволяет выявить сбой. При исследовании сбойных ситуаций
важными вопросами, которые решаются индивидуально в зависимости от
объекта и рассматриваемой ситуации, являются определение количества
главных компонент, допустимого порога, превышение которого означает,
что ошибка SPE становится недопустимой, и произошел сбой, а также используемых для оценок данных об измеряемых переменных. Аналогичное
же применение данного метода используется в [4].
В данной работе исследуется ситуация, связанная с полным отказом
винта. При этом варьируется набор использованных измеряемых переменных и количество главных компонент. Изначально производится предварительный сбор данных в режиме нормального функционирования квадрокоптера.
• Формируется матрица из измерений состояния квадрокоптера с четырьмя рабочими винтами
X = [X, x, y, z, ẋ, ẏ, ż, φ, θ, ψ, φ̇, θ̇, ψ̇].
• Вычисляется ковариационная матрица A = X T X с нулевым математическим ожиданием E(xi (k)) = 0 и E(x2i (k)) = 1.
• Вычисляются собственные числа λj матрицы A и собственные векторы
pj , j = 1, m.
• Составляется матрица P = p1 , . . . , pr , выбрав максимальные собственные числа λj и соответствующие им собственные векторы pj , j =
1, r.
• Выполняется линейное преобразование данных T = XP .
b = T P T = XP P T , где X = X
b + X,
e
• Переход обратно к n переменным: X
e = X(E − P P T ) — остаток.
X
• Анализируя ошибку SPE выбирается порог.
14
После этого осуществляется моделирование полета с отказом привода
винта в заранее неизвестный момент времени:
Формируется матрица из измерений состояния квадрокоптера с четырьмя
рабочими винтами
X = [x, y, z, ẋ, ẏ, ż, φ, θ, ψ, φ̇, θ̇, ψ̇].
Выбирается количество главных компонент таким образом, чтобы ошибка
SPE была достаточно маленькой.
Выполняется линейное преобразование T = XP .
b = TPT.
Выполняется переход обратно к n переменным: X
Таким образом ошибка SPE сравнивается с заранее выбранным пороговым значением. Если ошибка больше этого значения, то наблюдается
отказ привода винта.
15
Глава 3. Управление полетом квадрокоптера при
отказе одного привода.
3.1 ПИД регулятор.
Простейшая система автоматического регулирования с обратной связью представлена на рис. 5
Рис. 5: Схема регулятора
где R — регулятор, P — объект регулирования, r — управляющее воздействие, e — сигнал ошибки, u — выходная величина регулятора, y —
регулируемая величина.
Если выходная переменная u описывается выражением
Z t
de(t)
(14)
u(t) = Ke(t) + Ki
e(t)dt + Kd
dt
0
где K, Ki , Kd — коэффициенты усиления пропорциональной, интегрирующей и дифференцирующей составляющих регулятора соответственно, t —
время, то такой регулятор называют ПИД — регулятором, который был
изложен в [5].
Рис. 6: Схема ПИД регулятора
Пропорциональная составляющая вырабатывает выходной сигнал,
противодействующий отклонению регулируемой величины от заданного
16
значения, наблюдаемому в данный момент времени. Пропорциональная составляющая представляется в виде P = Kp e(t). В этом режиме контроллер берет отклонение, умножает его на константу и выдает его в качестве
управляющего воздействия. Чем больше отклонение, тем больше выходной сигнал. Если входной сигнал равен заданному значению, то выходной
равен нулю.
Интегрирующая составляющая пропорциональна интегралу по времени от отклонения регулируемой величины. Её используют для устранения статической ошибки. Она позволяет регулятору со временем учесть
статическую
представляется в виR t ошибку. Интегрирующая составляющая
1
де I = Ki 0 e(τ )dτ . Коэффициент Ki равен Ki = Ti , где Ti — постоянная
интегрирования.
Дифференцирующая составляющая пропорциональна темпу изменения отклонения регулируемой величины и предназначена для противодействия отклонениям от целевого значения, которые прогнозируются в будущем. Отклонения могут быть вызваны внешними возмущениями или
запаздыванием воздействия регулятора на систему. Дифференцирующая
составляющая представляется в виде D = Kd de(t)
dt . Коэффициент Kd равен
Kd = Td , где Td — постоянная дифференцирования.
В зависимости от поставленных задач пропорциональная, интегральная или дифференциальная компоненты могут отсутствовать. Рассмотрим
регулятор, в котором отсутствует интегральная компонента. Если в уравнении (14) положить Ti = ∞, то получим
Td
R(s) = K + Td s = K 1 + s .
(15)
K
Выражение (15) называется ПД регулятором.
17
3.2 ПД регулятор для полета квадрокоптера с 4мя
винтами.
Для регулирования величин будем использовать ПД — регуляторы,
так как П — регулятор сделает систему управления углами колебательной,
при использовании ПИ — регулятора в системе управления углами также
будут два комплексных корня, а при ПИД — регуляторе порядок системы
возрастает до третьего.
Рис. 7: Схема управления
Регуляторы получены линеаризацией уравнений модели движения
(1) в точке, соответствующей состоянию парения r = r0 , θ = φ = 0, ψ = ψ0 ,
ṙ = 0, и φ̇ = θ̇ = ψ̇ = 0, где углы собственного вращения и нутации малы
(cφ ≈ 1, cθ ≈ 1, sφ ≈ φ, sθ ≈ θ).
В режиме парения номинальная тяга должна соответствовать:
Fi,0 =
mg
.
4
Номинальные значения входных данных для парения: u1,0 = mg, u2,0 = 0.
Если предположить, что квадрокоптер симметричен, то есть Ixx =
Iyy , то получаем:
L
u2,x
=
(F2 − F4 )
ṗ =
Ixx
Ixx
u2,y
L
q̇ =
=
(F3 − F1 )
(16)
Iyy
Iyy
u2,z
γ
ṙ =
=
(F1 − F2 + F3 − F4 )
Izz
Izz
Иначе говоря, уравнения движения не зависят друг от друга с точки зрения угловых ускорений. Каждая компонента углового ускорения зависит
только от соответствующей компоненты u2 .
Задача управления состоит в определении четырех входов, {u1 , u2 },
необходимых для парения или следования желаемой траектории, zdes . Как
показано на рис. (7), мы будем использовать отклонения от положения
квадрокоптера, чтобы получить из (4) регулятор положения, который напрямую определят u1 . Из (4) также можно получить нужную ориентацию.
18
Регулятор управления ориентацией вытекает из (5). Данный регулятор использует встроенные акселерометры и гироскопы для управления по крену,
тангажу и рысканью.
Так как ПД регулятор основывается на линеаризованных уравнениях
движения, ориентация должна быть близка к номинальному состоянию
парения, где углы крена и тангажа малы. В состоянии, близком к парению,
управление ПД регулятором принимает следующий вид:
kp,φ (φdes − φ) + kd,φ (pdes − p)
(17)
u2 = kp,θ (θdes − θ) + kd,θ (qdes − q)
kp,ψ (ψdes − ψ) + kd,ψ (rdes − r)
Для парения нужно rT (t) = r0 и ψT (t) = ψ0 . Ускорения r̈i,des вычисляются с помощью ПД регулятора. Определение ошибки положения с
помощью компонент с использованием стандартных триад ai :
ei = (ri,T − ri )
Чтобы гарантировать, что эти ошибки экспоненциально стремятся к нулю,
потребуем выполнение следующего условия:
(r̈i,T − r̈i,des ) + kd,i (ṙi,T − ṙi ) + kp,i (ri,T − ri ) = 0,
(18)
где ṙi,T = r̈i,T = 0 для парения.
Из (4) можно получить зависимость между требуемыми ускорениями
и углами крена и тангажа. Учитывая, что ∆θ = θ − θ0 = θ и ∆φ = φ − φ0 =
φ, можно записать:
r̈1,des = g(θdes cos ψT + φdes sin ψT ),
r̈2,des = g(θdes sin ψT + φdes cos ψT ),
1
u1 − g.
m
Для парения, последнее уравнение приводит к:
r̈3,des =
u1 = mg + mr̈3,des = mg − m(kd,3 ṙ3 + kp,3 (r3 − r3,0 ))
Другие два уравнения можно использовать для подсчета требуемых углов
крена и тангажа для регулятора ориентации:
1
φdes = (r̈1,des sin ψT − r̈2,des cos ψT )
g
1
θdes = (r̈1,des cos ψT − r̈2,des sin ψT )
g
19
Скорости по крену и тангажу принимаются равными нулю.
pdes = 0, qdes = 0, ψdes = ψT (t), rdes = ψ̇T (t)
Эти уравнения обеспечивают заданные значения для регулятора ориентации (17). Этот закон управления имеет наивысшее быстродействие. ПД регулятор реагирует не только на величину отклонения e(t), но, что наиболее
важно, на скорость ее изменения.
20
3.3 Управление квадрокоптером при полном отказе
одного винта.
Вводится вектор состояния s = (p, q, nx , ny ) для описания положения устройства и условий, при которых управляемость изменяется в зависимости от количества потерянных пропеллеров. Учитывая, что общая
тяга летательного аппарата может быть определена, и одновременно можно контролировать направление тяги и его ускорение (по крайней мере,
квази-статически) и тем самым изменять свою позицию, подробнее изложена в работе [1]. Отклонение отношения от равновесия записывается в
виде se = s − s̄, и будет развертываться как:
ė
s = Ase + Bu
0
ā
0
=
∂s s=s̄ 0 −n̄z 0
n̄z
0 −r̄
(19)
0
0
r̄
0
где n = (nx , ny , nz ) — вектор координат в неподвижной системе, n =
(nx , ny , nz ) — вектор желаемых координат, s — вектор состояний в момент
парения, r — а константу ā можно посчитать по следующей формуле:
T
T
P
Ixx
− Izz
Izz
ā =
r̄ − B (ω̄1 + ω̄2 + ω̄3 + ω̄4 )
B
Ixx
Ixx
С тремя оставшимися двигателями вектор u = (u1 , u2 ) вводится в
зависимости от отклонений фактических сил двигателя от равновесия, в
единицах силы:
u1 = (F3 − F 3 ) − (F1 − F 1 ),
u2 = (F2 − F 2 ),
где F i — производная угловой скорости при парении. Остальные степени
свободы путём указания того, что общая тяга соответствуют желаемой:
F1 + F 2 + F3 = F 1 + F 2 + F 3 .
Расширим систему (19):
ė
s = Ase + Bu
0 1
L 1 0
B= B
Ixx 0 0
0 0
Исследование ранга матрицы управляемости C =
21
(20)
B AB A2 B A3 B
легко показывает, что система (20) управляема при L 6= 0 и n̄z 6= 0
Затем, для системы (20) ищется LQR, стабилизирующий ее. Полученное управление подставляется в систему (5).
22
3.4 Общая схема решения задачи.
Таким образом разработка алгоритма противоаварийного управления может быть выполнена в соответствии со следующей схемой:
1. Записывается набор данных, поступающих с датчиков о переменных
состояния, в режиме нормального движения квадрокоптера при четырех работающих двигателях, а также наборы данных для различных
вариантов отказов винтов.
2. Для различных выборок из исходного набора применяется метод PCA,
в результате чего определяется набор данных, позволяющих идентифицировать конкретный отказ, оценивается допустимый порог SPE и
необходимое количество главных компонент.
3. Формируется алгоритм управления, обеспечивающий периодическое управление движение в соответствиях с п.3.3 или посадку.
23
Глава 4. Эксперимент и результаты.
В качестве иллюстрации рассмотрим конкретную ситуацию полета
квадрокоптера из точки (5, 4, 1) в (6, 9, 7). В момент времени t = 15 происходит отказ привода винта квадрокоптера. Возникает задача обнаружения
этого отказа и стабилизации по высоте z.
На рис. (8) и (9) показано восстановление данных в режиме нормального функционирования квадрокоптера. Количество главных компонент
выбрано равным 3 и 6 соответственно. На рис. (10) представлен график
ошибки SPE в режиме нормального функционирования. Из рис. (11) видно, что с момента времени t = 15 ошибка SPE начала расти и превысила
выбранный порог. На рис. (12) представлено поведение высоты квадрокоптера в случае непредвиденного отказа в момент времени t = 15.
Рис. 8: Восстановление переменных x, y и z по трем главным компонентам
24
Рис. 9: Восстановление переменных x, y и z по шести главным компонентам
Рис. 10: Ошибка SPE в режиме нормального функционирования
25
Рис. 11: Ошибка SPE при полном отказе одного винта
Рис. 12: Стабилизация квадрокоптера в случае непредвиденного отказа привода винта
26
Выводы.
В работе рассмотрена задача разработки противоаварийных законов
управления квадрокоптером на примере его управляемого движения при
полном отказе одного из винтов. Первым этапом при исследовании такой
проблемы является решение задачи обнаружения и локализации возникающих отказов. Для решения такой задачи выбран метод PCA, поскольку он не требует информации непосредственно о модели объекта, которая
может не требоваться для настройки законов управления и полное задание которой для конкретных объектов может отсутствовать. Метод PCA
применен в различных условиях на примере конкретной математической
модели квадрокоптера, в том числе совместно с реализованным законом
управления в ситуации с полным отказом винта, в результате чего экспериментально установлена применимость, а также узкие места и вопросы,
требующие изучения, в задаче идентификации отказов винтов квадрокоптера с целью обеспечения дальнейшего управляемого движения.
27
Заключение.
В работе получены следующие результаты:
1. Проведен анализ возможности применения метода PCA для идентификации отказов приводов винтов квадрокоптера.
2. Предложена и реализована схема построения алгоритма противоаварийного управления в ситуации полного отказа винта квадрокоптера,
основанная на применении метода PCA и обеспечения управляемого движения квадрокоптера в этой ситуации.
3. Проведены вычислительные эксперименты в среде MATLAB, в результате чего проиллюстрирована успешность применения предложенной
схемы.
28
Список литературы
[1] Mueller M.W., D’Andrea R. Stability and control of a quadrocopter despite
the complete loss of one, two, or three propellers, IEEE International
Conference on Robotics & Automation (ICRA), 2014.
[2] Michael N., Mellinger D., Lindsey Q., Kumar V. The GRASP Multiple
Micro – UAV Testbed // IEEE Robotics & Automation Magazine. 2010.
Vol. 17, No 3. P. 3–4.
[3] Isermann R. Fault – Diagnosis Systems. Berlin: Springer – Verlag Berlin
Heidelberg, 2006. 475 p.
[4] Князькин Я. В. Обнаружение и диагностика динамических сбоев в процессе стабилизации движения морского судна // Процессы управления
и устойчивость. 2014. T. 1. № 1. С. 330–335.
[5] Денисенко В. ПИД –регуляторы: принципы построения и модификации
// Современные технологии и автоматизации. 2006. № 4.
[6] Гэн К., Чулин Н.А. Алгоритмы стабилизации для автоматического
управления траекторным движением квадрокоптера // Наука и Образование МГТУ им Н.Э,Баумана. 2015. № 05 с.218-235.
29
Отзывы:
Авторизуйтесь, чтобы оставить отзыв