Численные методы нелинейной фильтрации для оценки состояния квадрокоптера с поворотными роторами

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

В случае нелинейных систем наибольшую популярность получили методы нелинейной калмановской фильтрации [1], которые успешно применяются для решения задач оценки состояния летательных аппаратов [11-16] и других объектов [9,10,17].

Классическим алгоритмом является расширенный фильтр Калмана (ЕКҒ), в основе которого лежит линеаризация правой части стохастической модели в окрестности оценки его состояния для оценки математического ожидания неизвестного вектора состояния и матрицы ковариации. Среди более современных решений можно отметить сигма-точечный (UKF) квадратурный (QKF) и кубатурный (СКҒ) фильтры Калмана, реализация которых не подразумевает вычисление матриц Якоби/Гессе модели.

Выбор конкретного метода нелинейной фильтрации для оценки состояния объекта управления может быть обусловлен большим количеством факторов, основным из которых является точность оценки состояния при приемлемой вычислительной сложности. Алгоритм квадратурного фильтра Калмана практически невозможно эффективно применять для систем, размерность которых превышает 5 из-за экспоненциального роста времени вычислений [2], поэтому в работе рассматриваются три алгоритма: EKF, UKF и СКҒ. На примере управляемой динамики квадрокоптера с поворотными роторами, интерес к которым значительно возрос в последние годы из-за их повышенной маневренности и способности независимо от направления движения ориентировать корпус [3], будет продемонстрирована производительность алгоритмов фильтрации и обоснован выбор наиболее подходящего фильтра для оценки состояния подобного рода систем.

2.    Расширенный фильтр Калмана

Расширенный фильтр Калмана использует модель стохастической непрерывной системы

X = f ( x, t ) + W                                      (1)

и дискретные измерения

Z k = h k (x(t k )) + У к ,

где X - вектор состояния системы, w - шум системы, У к - шум измерений.

Задача фильтрации — найти являющуюся функцией измерений Z k оценку вектора состояния системы Xk, минимизирующую дисперсию ошибки X k — x(t k )• Пусть в момент времени t k - i получена оценка вектора состояния X k -i- На основании этой оценки строится прогноз оценки вектора состояний X^- (оценка априори), затем проводятся измерения Z k и коррекция оценки априори на основании результатов измерений X+ (оценка апостериори). Оценку априори вектора состояния X^- вычисляют интегрированием модельного уравнения:

dX dt

= ^f(X,t)

с начальными условиями X ( t ) = Xk-i, X (0) = xq. Оценку априори для ковариационной матрицы ошибки для линеаризованных уравнений в приращениях Р— вычисляют как

• ^P k = ^ФР + - 1^фТ + Q,

  ғ =

  
  

  df ⁽X^,t') 0x

  
  

  , Ф = Е + F A t

  Х = Х _к- 1

  
  

  с начальными условиями Р ( t ) = Р +у , Р (0) = Ро- Здесь, Q - ковариационная матрица шума системы. Оценку апостериори для вектора состояния и ковариационной матрицы ошибки строят следующим образом:

  
  

  Х + = Х_к + К к ( z - — Н к Х_к ) , р+ = (1 - К к Н к ) Р_к.

  
  

  Н к - линеаризованная матрица чувствительности:

  
  

  dh(x,t)

  ^Н к =  Д--- ОХ

  
  

  Х = Х _к- 1

  
  

  а К к - корректирующая матрица обратной связи -

  
  

  К к = Р^- Н к \Н_к Р к Н к + R] 1 ,

  
  

  где R - ковариационная матрица шума измерений.

  
  

  3. Сигма-точечный фильтр Калмана

  Сигма-точечный фильтр Калмана также использует модель дискретных измерений (2). Задача фильтрации - найти являющуюся функцией измерений z - несмещённую оценку вектора состояния системы х^- ), минимизирующую дисперсию ошибки х - — хД - ). Априори оценка вектора состояния Х^- вычисляется как

  
  

  2 N

  ^Х- = Е «■■•/ Х) ■ i =0

  
  
  
  

  Аргументами функции / в выражении (3) являются так называемые сигма-точки, выбор которых определяется соотношениями

  
  

  ^Х к ^Х к —1,

  х = Хк— 1 + Vn + X ■ (УРУ'■ г = 1 ,...,N,

  х — = х -— 1 — .Х, • (УРУф— ^N , г = N + 1 ,..., 2 N,

  
  
  
  

  где (У Р_к \ ) обозначает г-й столбец матрицы У Р к _—1. Здесь используется разложение Холенного вида. Р = VP VP ^к , г,те VP ~ нижняя треугольная матрица. N - размерность оцениваемого вектора состояния. Весовые коэффициенты в формуле (3) вычисляются как

  
  

  о ^х          і

  ^W = X + N’

  
  

  2 ( X + N ) ’

  
  

  г = I ,..., 2 N.

  
  
  
  

Оценка матрицы ковариации может быть получена по формуле

2 N

Р к = Е W* (/ (Хк) — Х—)(/ (Хк) — Х—)^к + Q’ i =0

где Q - ковариационная матрица шума системы. При этом весовые коэффициенты в формулах (3) и (6) совпадают, за исключением коэффициента W⁰, который в формуле (6) принимает значение [1]:

W0 = x+N + 1 — °2 + 3’ где a Е [10-4,1] - параметр, определяющий разброс сигма-точек вокруг среднего. Параметр /3 позволяет учесть априорные данные о функции плотности вероятности неизвестного вектора состояния системы (для нормального распределения /3 = 2). Наконец, А = a2(N + к) — N - параметр масштабирования. Далее происходит коррекция сделанных на предыдущем этапе оценок вектора состояния и матрицы ковариации с помощью вектора и модели измерений. С помощью функции һ из уравнений (2) сигма-точки (4) отображаются в пространство измерений, где также делается оценка среднего и матрицы ковариации:

С к = һ (X) ,

2 N

^ к = £ w» • С к , » =0

2N s- = £ w» (с- — ^к )(с- — ^ )т + R,                       (7)

» =0

где R - ковариационная матрица шума измерений. Окончательные оценки для вектора состояния и матрицы ковариации получаются по формулам

Х + = X- + K - ( z - — ^ к ),

Р+ = р- — к- sk! , где

2 N

Т к = £w» ( х - — х^-)« к — ^ к )^т, » =0

К к = Т к S^-1.

Таким образом, алгоритм сигма-точечного фильтра Калмана имеет три параметра а, /3 и к, выбор которых определяет конкретную UKF-реализацию.

4.    Кубатурный фильтр Калмана

Кубатурный фильтр Калмана был разработан в 2000 году и избавлен от проблемы быстро растущей вычислительной сложности квадратурных фильтров [2]. В кубатурном фильтре используется кубатурное правило Гаусса третьего порядка для оценки математического ожидания производной вектора состояния:

2 N

_2N £/ (м + Vbg), » =1

Е[/(х)] = / /(х)Т(х,м, y)dx

R^

где Т(х,ц, У) - функция плотности вероятности нормального распределения случайного вектора х со средним ц и ковар нацией У = Vy Vy^T, С ^_ узлы кубатурной функции

J VNe»,          г = 1,...,N,

(—VNe»-N,    г = N + 1,..., 2N, где е» - г-й координатный вектор пространства RN. Нетрудно обнаружить, что выбор параметров сигма-точечного фильтра Калмана a = 1, 3 = 0, к = 0 соответствует реализации кубатурного фильтра Калмана, так как весовые коэффициенты (5) и выражения для математического ожидания и ковариации с учетом предположения о нормальности распределения вектора состояния в этом случае совпадают. Таким образом, CKF в каком-то смысле является частной реализацией UKF.

5.    Управляемая динамика квадрокоптера с поворотными роторами

Вектор состояния квадрокоптера с поворотными роторами включает в себя положение центра масс, скорость, кватернион ориентации корпуса квадрокоптера и угловую скорость:

ж = (г v q Q) ^T.

Движение аппарата описывается уравнениями [13]:

Г = v, v = q ° (^qBR. ° (-і^^кшіішіі о qBR^ о q, г=1

^q = 2 q ° ^Q,

Q = J— [- Q x JbQ + ^rB x (-1)г+1^Д|Д|е^.- г=1

• ^- ^ qBR_t ° ЬШг |Д |^eR ° ^qBR, + ^ qBR. ° (M ^шR + W^{R x J}R. W^R) ° ^qBR^ , г=1                               г=1

6.    Вычислительные эксперименты

где M - общая масса аппарата, д - ускорение свободного падения, е{4 - единичный вектор вдоль оси симметрии Тго ротора, к и b - аэродинамические коэффициенты пропеллера, шг - скорость вращения Тго пропеллера, Jb ~ тензор инерции корпуса в главных осях корпуса. гв - радиус-вектор, проведенный из центра масс квадрокоптера йму к ротору. еВ -орт вектора гВ, qBR4 ~ кватернион, определяющий ориентацию г-го ротора по отношению к корпусу аппарата, Jr. - тензор инерции г-го ротора с пропеллером, записанный в собственных главных осях, w4Ri - полная угловая скорость г-го ротора с пропеллером, записанная в собственных главных осях. Для обеспечения независимого управления по положению и ориентации используются регуляторы вида r"d(t) = ^(t) + Кгі(Г°(€) - r-(t)) + КТ25г,

Qd(t) = Q0(t) + Km(Q°(t) - Q(t)) + KQ2^q, где 6q - векторная часть кватерниона рассогласования, 6г - рассогласование текущей позиции, Г0, г0, Q0, Q0 - целевое ускорение, скорость, угловое ускорение и угловая скорость соответственно, Кг1, КГ2, Кщ, Кт - диагональные матрицы коэффициентов. Поддержание значений оборотов ш4 и ориентации qBR4 двигателей, удовлетворяющих выходам регуляторов (10) согласно модели (12), обеспечивает достижение целевых параметров движения. В данной работе не рассматривается решение задачи обратной динамики квадрокоптера и синтеза контура управления, а считается, что способ определения управляющих параметров известен.

Для определения траектории движения квадрокоптера модель (9) численно интегрируется, при этом управляющие параметры ш г,, q BR ₄ являются некоторыми функциями текущего состояния и целевой траектории. Интегрирование модели проводится с использованием пакета MATLAB методом Рунге-Кутты 4-го порядка с шагом 6 = 10^-2 с. Вектор измерений составлен аналогичным вектору состояния (8) способом:

г = (г v q Q)^T .

(И)

Измерения моделируются добавлением к результатам интегрирования уравнений движения (9) белого гауссовского шума, параметры которого выбраны таким образом, чтобы соответствовать параметрам существующих устройств [4].

Стандартные отклонения шума измерений горизонтальных компонент положения, вертикальной компоненты позиции, компонент скорости, углов тангажа, крена, рысканья и компонент угловой скорости приведены в табл. 1.

Таблица!

Параметры шума измерений

^ ТХ	1м
^ TZ	2м
сту	0.5м/с
СТ а	0.5 °
а р	0.5 °
а ^	1.5 °
а п	0.6 °/с

В качестве модели динамики летательного аппарата в алгоритмах нелинейной фильтрации используется упрощенная модель движения квадрокоптера, в которой не учтена инерция поворотных роторов с пропеллерами. Тогда выражение для углового ускорения (9) запишется как

П = J— [ - П X J b П + ^ Т ? X ( -1^ кШ г\ Ш г\ е ^ г =1

^- ^ Ч ш; . ° bw i \w i \e ^ О Q BR^ . г =1

К данному упрощению часто прибегают на практике, так как при проектировании квадрокоптера с поворотными роторами даже приблизительная идентификация основных параметров динамики исполнительных органов системы управления является трудоемким процессом и требует проведения специальных измерений [5]. Таким образом, выражения, связанные с динамикой исполнительных органов системы управления, формируют вектор w шума системы из уравнения стохастической непрерывной системы (1).

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

Длительность каждого полетного задания составляет 90 секунд, в течение этого времени квадрокоптер двигается по криволинейной траектории в пространстве, а его корпус разворачивается согласно целевым параметрам ориентации в текущий момент. Максимальная допустимая скорость аппарата ограничена значением 5 м/с, а угловая скорость - 3°/с. Все три фильтра используют одинаковые измерения и работают на одном и том же наборе траекторий.

Производительность каждого из алгоритмов фильтрации исследуется для различных интервалов наблюдения, кратных шагу интегрирования системы, то есть измерения и оценка состояния производятся с интервалами T n = N6.

Матрицы Q ковариации шума системы и R ковариации шума измерений, используемые во всех трех алгоритмах, были выбраны с учетом знаний о параметрах шума измерений и скорректированы таким образом, чтобы алгоритмы показывали высокую производительность для интервала измерений Т = 6:

Q = 10- 6 diag(1, 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 50 , 50 , 50) ,

R = diag(1, 1 , 1 , 0 . 5 , 0 . 5 , 0 . 5 , 0 . 003 , 0 . 003 , 0 . 003 , 0 . 006 , 0 . 003 , 0 . 003 , 0 . 003) .

Результаты численных экспериментов представлены в табл. 2, где приведены значения усредненных по 100 пролетам среднеквадратичных отклонений оценки состояния и количество случаев некорректной оценки. Некорректной оценкой считается случай, когда ее качество становится хуже, чем качество измерений (табл.1), такие результаты не учитывались при усреднении. Таблица не содержит у-компонент элементов вектора состояния, так как эти результаты качественно не отличаются от параметров для ж-компоненты.

Т а б л и ц а 2

Сравнение производительности алгоритмов нелинейной фильтрации

N	Метод	гж,м	г2,м	гд,м/с	гд,м/с	еж, °	ez, °	ож,°/с	oz, °/с
	EKF	0.14/0	0.13/0	0.07/0	0.02/0	0.14/0	0.11/0	0.27/0	0.23/0
1	UKF	0.10/0	0.19/0	0.06/0	0.06/0	0.18/0	0.21/0	0.30/0	0.23/0
	CKF	0.09/0	0.14/0	0.06/0	0.03/0	0.15/0	0.21/0	0.28/0	0.23/0
	EKF	0.48/0	0.23/0	0.17/0	0.04/0	0.22/0	0.17/0	0.30/0	0.23/0
2	UKF	0.16/0	0.26/0	0.09/0	0.09/0	0.27/0	0.27/0	0.36/0	0.22/0
	CKF	0.14/0	0.17/0	0.09/0	0.03/0	0.24/0	0.27/0	0.33/0	0.22/0
	EKF	0.80/53	0.63/22	0.29/0	0.16/0	0.25/0	0.22/0	0.33/0	0.22/0
3	UKF	0.19/0	0.33/0	0.10/0	0.11/0	0.33/0	0.31/0	0.40/0	0.22/0
	CKF	0.18/0	0.22/0	0.10/0	0.03/0	0.30/0	0.31/0	0.35/0	0.22/0
	EKF	0.96/99	0.82/89	0.40/27	0.31/9	0.31/7	0.29/0	0.33/0	0.25/0
4	UKF	0.23/0	0.39/0	0.13/0	0.13/0	0.43/6	0.35/0	0.40/0	0.24/0
	CKF	0.21/0	0.24/0	0.13/0	0.04/0	0.40/0	0.36/0	0.36/0	0.24/0
	EKF	то/100	то/100	0.37/67	0.37/68	0.33/21	0.29/0	0.34/0	0.25/0
5	UKF	0.23/0	0.40/0	0.12/0	0.14/0	0.44/25	0.37/0	0.40/0	0.24/0
	CKF	0.19/0	0.25/0	0.12/0	0.06/0	0.43/6	0.37/0	0.37/0	0.24/0
	EKF	то/100	то/100	0.37/73	0.40/88	0.37/19	0.32/0	0.37/0	0.23/0
6	UKF	0.29/0	0.48/0	0.15/0	0.16/0	0.44/25	0.40/0	0.42/0	0.23/0
	CKF	0.26/0	0.31/0	0.15/0	0.05/0	0.40/1	0.41/0	0.40/0	0.22/0
	EKF	то/100	то/100	0.40/76	0.42/98	0.49/99	0.58/1	0.40/0	0.28/0
7	UKF	0.27/0	0.50/0	0.15/0	0.17/0	0.47/81	0.40/0	0.40/0	0.26/0
	CKF	0.23/0	0.31/0	0.15/0	0.06/0	0.45/52	0.40/0	0.41/0	0.25/0
	EKF	0.85/99	то/100	0.38/86	0.38/85	0.46/79	0.37/0	0.38/0	0.27/0
8	UKF	0.30/0	0.53/0	0.17/0	0.16/0	0.46/71	0.44/0	0.41/0	0.28/0
	CKF	0.27/0	0.37/0	0.16/0	0.07/0	0.45/41	0.45/0	0.45/0	0.27/0
	EKF	то/100	то/100	0.44/96	0.38/88	то/100	0.69/16	0.44/4	0.31/3
9	UKF	0.30/0	0.52/0	0.15/0	0.19/0	то/100	0.45/0	0.44/0	0.28/0
	CKF	0.26/0	0.27/0	0.15/0	0.08/0	0.49/94	0.48/0	0.49/2	0.27/0

Как и ожидалось, качество оценки вектора состояния коррелирует с интервалом работы фильтров - снижение частоты измерений ведет к ухудшению параметров оценки. Наиболее низкую производительность на больших интервалах измерений показывает расширенный фильтр Калмана: уже для N > 2 ошибки оценки состояния в нескольких случаях выходят за рамки допустимых, а при N >  5 оценка положения становится некорректной. Производительность сигма-точечного и кубатурного фильтров Калмана с ростом N также падает, но не настолько заметно, при этом отказов в работе этих алгоритмов значительно меньше. Сравнительный анализ результатов UKF и СКҒ показал, что СКҒ-алгоритм производит более точную оценку состояния в большинстве рассматриваемых случаев и является более устойчивым к повышению интервала измерений.

На рис. 1 изображены графики зависимости среднеквадратичных отклонений z- компонент вектора оценки состояния от временных интервалов измерений.

а)

б)

а)

Рис. 1. Зависимость усредненного среднеквадратичного отклонения оценки вектора состояния от интервала измерений

б)

7.    Заключение

Рассмотрено применение широко распространенных алгоритмов нелинейной фильтрации для оценки состояния квадрокоптера с поворотными роторами. Проведен сравнительный анализ производительности расширенного фильтра Калмана, сигма-точечного фильтра Калмана и кубатурного фильтра Калмана для различных интервалов измерений.

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

Показатели качества сигма-точечного и кубатурного фильтра Калмана имеют схожую динамику, однако СКР показал более качественные результаты при оценке положения и скорости квадрокоптера на всех интервалах измерений, при примерно совпадающих с UKF показателях для ориентации и угловой скорости.

Превосходство UKF и СКҒ над ЕКҒ в данном эксперименте обусловлено использованием в последнем из алгоритмов линеаризованного уравнения стохастической дифференциальной системы (1), что негативно влияет на производительность метода при росте интервала измерений.

Отдельно стоит добавить, что алгоритм сигма-точечного фильтра Калмана имеет три свободных параметра для настройки, и, как следует из данной работы, их выбор влияет на качество оценки состояния. Таким образом, существует потенциальная возможность с помощью этих параметров повысить эффективность оценки состояния систем известного вида.