CC BY
292
Наука й Образование
МГТУ им. Н.Э. Баумана
Сетевое научное издание
ISSN 1994-040В
Наука и Образование. МГТУ им. Н.Э. Баумана. Электрон. журн. 2016. № 12. С. 182-206.
DOI: 10.7463/1216.0852517
Представлена в редакцию: 04.11.2016 Исправлена: 18.11.2016
© МГТУ им. Н.Э. Баумана
УДК 004.021
Интегрированная навигационная система для беспилотных летательных аппаратов с возможностью обнаружения и изоляции неисправностей
ГЭН К1'", ЧуЛИН Н. А.1 'J5ggigjcejgl26.com
:МГТУ им. Н.Э. Баумана, Москва, Россия
В работе представлена интегрированная навигационная система, сочетающая достоинства спутниковой навигационной системы (СНС), визуальной навигации на основе алгоритма одновременной локализации и картографирования (Simultaneous Localization And Mapping -SLAM), барометрического и радиовысотомера для обеспечения коррекции системы инерциальной навигации (ИНС). Для обнаружения возможной недостоверной навигационной информации с датчиков использован фильтр Калмана в сочетании с тестом «хи-квадрат». Предложен алгоритм слияния данных с нескольких источников информации, использующий назначение весовых коэффициентов в зависимости от дисперсии ошибок для изоляции информации с низкой достоверностью. Результаты моделирования в среде Matlab и макетирования в реальной среде показывают, что точность результатов навигации интегрированной навигационной системы может оставаться в допустимых пределах в течение всего полёта с ожидаемой вероятностью обнаружения и изоляции неисправностей.
Ключевые слова: интегрированная навигационная система; слияние данных; фильтр Калмана; тест «хи-квадрат»; обнаружение и изоляция неисправностей
Введение
Рассматривается беспилотный летательный аппарат (БПЛА) лёгкого класса, выполняющий полёт в автономном режиме или при дистанционном пилотировании в условиях недостаточности предоставляемой оператору информации о среде. В этих случаях информация с одного источника не может обеспечить необходимой точности навигации БПЛА. Например, ошибки бесплатформенной инерциальной навигационной системы (БИНС) быстро накапливаются с течением времени, прежде всего из-за ошибок чувствительных элементов, из-за чего БИНС не могут быть использованы как единственное навигационное средство для БПЛА в режиме сколь либо продолжительного полёта [1]. Спутниковая навигационная система(СНС) может предоставить информации о местоположении, путевой скорости и высоте без накопления ошибок с течением времени, но не может работать в
режиме реального времени, особенно с учётом необходимости решения задачи стабилизации аппарата [2]. Визуальная навигационная система имеет высокую точность, но из -за ограниченности диапазона визуального наблюдения и сложности вычислений для обработки изображений, тоже трудно применима в режиме реального времени [3]. Барометрический высотомер имеет большую методическую ошибку, если фактические атмосферные условия не соответствуют стандартной атмосфере. Точность измерения радиовысотомера сильно зависит от неровности земной поверхности и наклона платформы БПЛА [4-5]. Поэтому интегрированная навигационная система на основе слияния нескольких источников навигационной информации, позволяющая компенсировать недостатки каждого из источников, несомненно, необходима для БПЛА.
Интегрированная навигационная система обычно сочетает два или более двух типов навигационных систем. Таким образом, интегрированная навигационная система может в полной мере использовать преимущества взаимодополняемости различных навигационных систем и генерировать достоверную навигационную информацию. Такие системы обычно включают инерциальный измерительный блок (Inertial Measurement Units - IMU), СНС, визуальную навигационную система и т.д. Тем не менее, в практических приложениях присутствует много факторов, влияющих на точность и достоверность измеренной информации, например, ошибки передачи данных, помехи окружающей среды, методические ошибки датчиков и т. д. Поэтому функция обнаружения и изоляции неисправностей также необходима для интегрированной навигационной системы.
В последние годы для улучшения работоспособности систем навигации было проведено большое число исследований и предложено достаточно много интегрированных навигационных систем. Например, интегрированная навигационная система, сочетающая IMU и лидар на основе расширенного фильтра Калмана [6-7]; встроенная навигационная система с видеокамерой и лазерным сканером в карданном подвесе для автономного полета беспилотных летательных аппаратов [8]; интегрированная навигационная система БПЛА на основе согласования изображений [9]; комплексированная инерциально спутниковая система навигации малогабаритных беспилотных летательных аппаратов в условиях сложной среды в случае кратковременного отсутствия спутниковых сигналов [10]; использование иерархии фильтров в интегрированной навигационной системе [11-12] и другие.
В данной статье предлагается интегрированная навигационная система для БПЛА на основе преобразования режимов навигации[13], фильтра Калмана и слияния навигационной информации с нескольких источников: СНС, системы визуальной навигации на основе алгоритма одновременной локализации и картографирования (Simultaneous Localization And Mapping - SLAM), барометрического высотомера, радиовысотомера и ИНС. Подобная интегрированная навигационная система может не только обеспечить обнаружение и изоляцию навигационной информации с низкой достоверностью на основе остаточных ошибок теста «хи-квадрат» и результата теста «хи-квадрат» с двумя прогнозами состояния, но и использовать достоинства различных датчиков для повышения достоверности навига-
ционной информации. Результаты моделирования в системе Matlab и макетирования в естественной среде показывают, что предлагаемая система имеет высокую точность навигации и возможность обнаружения и изоляции неисправностей. Схема интегрированной навигационной системы показана на рисунке 1, где ZA, ZG, Zs - выходные сигналы визуальной навигационной системы, спутниковой навигационной системы GPS и БИНС с ошибками; hB,hR - выходные сигналы барометрического высотометра и радиовысотомера с ошибками; Z0 - наблюдаемая навигационная информация после слияния данных; Xr - навигационная информация с фильтра Калмана ; M - матрица обнаружения неисправности; пунктирными линиями обозначены сигналы, которые могут отсутствовать.
Рис. 1 Схема интегрированной навигационной системы
1. Преобразователь режимов навигации
Рассматриваемая в данной статье интегрированная навигационная система включает три источника навигационной информации: СНС (GPS), SLAM (аббревиатура визуальной навигационной системы на основе алгоритма SLAM) и ИНС. Кроме того, для повышения точности информации о высоте, используются барометрический высотомер (Barometric Altimeter - BA) и радиовысотомер (Radio Altimeter - RA). Каждый из них имеет свои недостатки и не может самостоятельно обеспечивать точность навигационной информации в режиме реального времени в различных условиях полета достаточно долго. Для решения этой проблемы, в данной работе предлагается метод автоматического выбора разных режимов навигации в соответствии с условиями полета. Известно, что во многих случаях БПЛА не может получать достоверный сигнал GPS, например в помещениях, в горной местности, при числе наблюдаемых спутников менее четырёх, и т. д. Поэтому в случае отсутствия достоверного сигнала GPS БПЛА выбирает режим навигации БИНС/SLAM/BA/RA. Но визуальная навигационная система на основе алгоритма SLAM не может обеспечить навигационную информацию в режиме реального времени из-за
сложности алгоритмов обработки изображений. Поэтому визуальная навигационная система используется только в случае отсутствия сигнала GPS или выполнения миссий полета, требующих высокой точности например, посадка в указанном месте или полёт в среде с препятствиями. В этих случаях выбирается режим навигации БИНС/SLAM/BA/RA или БИНС/GPS/SLAM/BA/RA. В остальных ситуациях выбирается режим навигации БИНС/GPS/BA/RA.
На рисунке 2 показана диаграмма преобразований режимов интегрированной навигационной системы. Множество режимов полета
Q = {ft 4 ,4э},
где 4 - интегрированная навигационная система БИНС/GPS/BA/RA; q2 - интегрированная навигационная система БИНС/SLAM/BA/RA; q3 - интегрированная навигационная система БИНС/GPS/SLAM/BA/RA; SG- сигнал GPS (SG=1- сигнал GPS существует; SG=-1-сигнал GPS не существует); DG- расстояние БПЛА до целевой точкой; RG- порог диапазона преобразования режимов навигации.
f <u Y
SCi 1 J
iDG HC. /
А. /
/ / w
Г ч* \
( SG = 1 }
\DG S RCj/ч i»1 Jt
Рис. 2 Преобразователь режимов навигации
2. Разработка фильтра Калмана с возможностью обнаружения
неисправности
Уравнение состояний интегрированной навигационной системы имеет вид [14]:
где Х0 - вектор состояния; Ж - матрица перехода; С - матрица влияния шумов системы;
W - вектор шумов системы (предполагается - «белый» шум). Вектор состояния X включает в себя компоненты:
Хо = [[^¿^¿^¿у^адЛЭД ех,еу,е2,Ух,Уу,У2 J (2)
где буквами E, N и U обозначены направления на восток, север и вверх; фЕ, и % -ошибки определения углов ориентации в инерциальной системе координат; д\Е , и дуи - ошибки определения скорости БПЛА в инерциальной системе координат; дЬ, дХ и дк -
ошибки определения широты, долготы и высоты; ех ,е и ег - случайные погрешности датчиков угловой скорости (ДУС) (предполагается - марковский процесс первого порядка) в связанной системе координат; Ух и - случайные погрешности акселерометров
(предполагается - марковский процесс первого порядка). Переходная матрица системы Ж имеет вид:
К Ж
Ж =
0,
(3)
15x15
Ж =
,- матрица с размерностью 9x9 , элементы которой (согласно [10,14]):
0 Wie ^ Ь соэ Ь 0 0 0 0 0 0
- ^ вш Ь 0 0 0 0 0 ^^^^ вш Ь 0 0
^ соэ Ь 0 0 0 0 0 ^ соэ Ь 0 0
0 -/и /ы 0 эт Ь -2wie соэ Ь 2w1e соэ Ь +уи эт Ь) 0 0
/и 0 -/е эт Ь 0 0 -2^^ соэ Ь 0 0
-/м /е 0 -2wI.e соэ Ь 0 0 -2^е ^ Ь 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 1 0 0 0
(4)
Здесь уб , и уи - фактические скорости БПЛА в инерциальной системе координат; /Е , /м и / -удельные силы в инерциальной системе координат; L, Я и h - фактические широта, долгота и высота БПЛА; - угловая скорость вращения Земли. Матрица К - блочная матрица с размерностью 6*6, имеет вид:
К =
к: о
о,
3x3
и:
(5)
где к: - матрица ориентации.
Матрица - матрица размерностью 6*6, в которой главная диагональ имеет вид:
К =diag[-1/Т„ ,-1/Ту,-1/Т„ ,-1/Тх -11 Ту,-1/Та] (6)
где Тге, Т^ и Тгг - интервалы корреляции ошибок ДУС; Тж , ^ и Тж - интервалы корреляции ошибок акселерометров.
Вектор шумов с дисперсией Q измерительной системы имеет вид:
W = Гw ,w ,w ,w ,w ,w ,1 (7)
гх гу Г2 ах ау а2 V/
где wn., и wrz - внешние возмущения, действующие на ДУС («белый» шум); wax, ^ и - внешние возмущения, действующие на акселерометр («белый» шум). Матрица влияния шумов системы имеет вид:
п
" °9х 3 09х3 "
G = ^Эх 3 0зх 3 (8)
_ 0 ЭХЭ 13х3 _ 15х6
Информация состояния БПЛА по показаниям БИНС имеет вид [15]:
Ls = L + SL As = А +SA ; < hs = h + Sh
VSE = VE +SVE VSN = VN +SVN VSU = VU +Svu
(9)
где ^ = [, Х5 ,к5, , , ] - состояния БПЛА по показаниям БИНС с ошибками. Информация состояния БПЛА по показаниям СНС имеет вид [15]:
' Lg = L -VLg VGE = VE - VV v VE _ G
ag = A-VAG ; VGN = VN -VVN _ G (10)
hG = h -VhG VGU = VU -VVU G
гдеZG = [Lg,Xg,hG,VGE,VGN,VGU] - состояния БПЛА по показаниям СНС с ошибками; Vve g, Vv„ g и Vvu g - ошибки измерения скорости БПЛА по показаниям СНС; VLG , VAg и VhG - ошибки определения местоположения БПЛА по показаниям СНС.
Информация состояния БПЛА по показаниям визуальной системы навигации на основе алгоритма SLAM имеет вид:
' La II L A^ VAE = VE - VV v VE _ A
Aa = A-VAA ; - VAN = VN -VVN _ A (11)
hA = h -VhA VAU = VU -VVu _ A
где ZA = [ La, XA,hA
'VAE'VAN'VAU
] - состояния БПЛА по показаниям SLAM с ошибками;
Vv, А, Vv„ А и Vvu А - ошибки измерения скорости БПЛА по показаниям SLAM; VL , VA и VhA - ошибки определения местоположения БПЛА по показаниям SLAM.
Высота полета БПЛА по показанием барометрического высотомера и радиовысотомера с ошибками:
где и - ошибки определения высоты БПЛА по показаниям барометрического высотомера и радиовысотомера.
Видно, что навигационная информация о высоте может быть получена от нескольких датчиков. Чтобы получить достоверную информацию, в данной работе была разработана схема обработки информации с возможностью обнаружения недостоверной навигационной информации при помощи фильтров Калмана. В качестве примера на рисунке 3 показана схема обработки информации о высоте.
Рис. 3 Схема обработки навигационной информации о высоте с возможностью обнаружения недостоверной
информации
Как видно из рисунка, обработки навигационной информации с возможностью обнаружения недостоверной навигационной информации о высоте состоит из основного фильтра Калмана, суб-фильтров Калмана, обнаружителей неисправности и прогноза состояния.
Уравнение измерений основного фильтра Калмана «О» можно записать в виде:
v
л8 -а, 6л + У Лл
ь -к Зк+
vSE
УзУ
5ъи +Уг,Сг0
н^ох^о+^со
(13)
где 20 = [Ьа, А0 М0,У0Е,Уж,У0и ] - измеренная информация после слияния, которую можно записать следующим образом:
' К N К о3 уОЕ II V У УЕ _ О
= Х-УХ0 ' < Уоы = ^ -Уум _ О (14)
К = К-Ук0 Уои = Уи -УУи о
где Ууе 0, У 0 и Ууи 0 - ошибки измерения скорости БПЛА после слияния; УЗ0 , У Д0 и УЛ0 - ошибки определения местоположения и высоты БПЛА после слияния. где V - вектор шумов наблюдения; Н - матрица измерений, равная:
0 эх 6 diag [1,1,1] 0
0 эх 3 diag [1,1,1] 0
Н
'Эх 6 >3х 9
(15)
(16)
Уравнения измерений для суб-фильтров Калмана « A », « G »,« B » и « R » можно записать в виде:
'Zah (t) = hs - Ha -SH + VHA - Hah (t)X(t) + Vah (t) Zgh (t) = hs - Hg -SH + VHg = Hgh (t)X(t) + Vh (t)
<
Zbh (t) = Hs - Hb -SH + VHb - Hbh (t)X(t) + Vbh (t) ZA(t) = Hs -HR -SH + VHr - Hrh(t)X(t) + Vrh(t)
где Vah, V h, VM и Vrh - вектор шумов наблюдения высоты СНС, SLAM, барометрического
высотомера и радиовысотомера с дисперсиями Кай, R А, Rbh и Rr;j ; Hah, H
матрицы измерений, которые имеют вид:
gh '
Hbh и H h -
" °5x5 05x1 05x9 "
Hah H gh Hbh Hrh 5 1 01x 9 (17)
_ 09X5 09x1 09x9 _
Уравнения состояния БПЛА и измерений можно записать в следующем виде:
где г = "О"," ак"," gh"," ЪИ"," тк" соответствуют основному фильтру Калмана и субфильтрам Калмана.
Для решения рассматриваемой задачи на борту БПЛА с использованием микропроцессорных вычислительных устройств необходимо преобразовать уравнения к дискретному виду:
X k = фк,k+iXi k+1 + rk-iWk-i
Z
i k
' Hi k,k+1Xi k+1 + Vi k+1
(19)
где Фкк+! и Гк_ - переходные матрицы состояний и шума системы в дискретной форме. Уравнения фильтра Калмана можно записать в виде:
V -Д) Y
Xi_k|k-1 фk,k-1Xi_k-1
фкp k-lФk + Qk
Ki_k - Pi k|k-1Hi_k ( Hi_kPi klk-1Hz_k + Ri_k
P
i kk-1
Г
(20)
Xi_k - Xi_k|k-1 + Ki_k (Zi_k
"Hi_kXi klk-1
Рг _к = (1 _ Кг _кНг _к ) Рг _к\к_1
Тест «хи-квадрат» (х2, chi-square), с помощью которого проверяют соответствие между наблюдаемыми и эталонными сигналами, широко используется для обнаружения неисправностей в стохастических динамических системах [16]. Этот метод можно разделить на три вида: (1) - тест х2 по остаточной ошибке; (2) - тест х2 по состоянию с одно-
<
кратным прогнозом состояния; (3) - тест х2 по состоянию с двойным прогнозом состояния.
1) Если статистика испытаний вычисляются с использованием остаточной ошибки наблюдаемого сигнала, то практически невозможно обнаружить неисправность в процессе прогноза состояния калмановской фильтрацией, хотя неисправность датчика может быть легко обнаружена непосредственно [17].
2) Если статистика испытаний вычисляются с использованием вектора состояния, то можно обнаружить неисправность в процессе прогноза состояния калмановской фильтрацией и оценить неисправность датчика косвенно. Если для исправления используется только один прогноз состояния, то ошибка накапливается и может расходится с увеличением времени [18].
3) Если использовать два прогноза состояния и попеременно сбрасывать результаты прогноза, то можно избежать накопление ошибки. Если используется М субфильтров, то в таком типе теста должны быть использованы 2М прогнозов состояния. Это не только усложняет структуру алгоритма, но и влияет на скорость вычислений [19].
Видно, что все три вида теста имеют свои преимущества и недостатки. Чтобы повысить вероятность обнаружения неисправности, предложен обнаружитель неисправности со структурой, параллельно реализующей тест х2 по остаточной ошибке и тест х2 по состоянию. Тест с двойным прогнозом используется только для основного фильтра Калмана (см. рис. 3), что позволяет упростить структуру алгоритма и повысить скорость вычислений.
В качестве примера на рис. 4 приведёна схема обнаружителя неисправности G с рисунка 3.
Рис. 4 Обнаружитель неисправности информации о высоте, получаемой с помощью GPS
Прогноз состояния можно записать в виде: |Х, (к) = ф (к,к-^Х, (к-1)
р (к )=(к, к-1) р (к-1) ф^ (к, к-1)+г (к-1) о (к-1) г^ (к-1)
где « ^ » - номер прогнозного состояния.
Состояния с двойным прогнозом не обновляются по результатам измерений с датчиков, а сбрасываются с помощью выхода фильтра Калмана альтернативно с любым вре-
менным интервалом. Для того, чтобы вывести метод х2 обнаружения неисправностей с двойным прогнозом состояния, определим вектор состояния ошибки:
к (к)=х(к)_ ^ (к) (22)
[е, (к) = X(к)_XX, (к)
где X (к) -истинный вектор состояния; X й (к) - предсказанный вектор состояния субфильтра Калмана G; (к) -предсказанный вектор состояния я -го прогноза состояния;
Для примера рассмотрим обнаружение неисправности в суб-фильтре Калмана G. Разность ошибок прогноза с общего фильтра и суб-фильтра:
рф (к) = е^ (к) _ е (к) = XИ (к) _ X, (к) (23)
Его дисперсия:
Ти (к) = Е{Ри (кР (к)}
= Е{е^ (к)етк (к)_egh (к)е? (к)_ех (к)(к) + ех (к)< (к)} (24)
= Pgк (к)_ Ри (к)_ Р.и (к) + Р (к)
где Р & (к) - перекрестная ковариация суб-фильтра Калмана G и я -го прогноза состояния.
Если начальные состояния одинаковые, то есть Р & (к) = Р А (к) = Р к (к), то дисперсия может быть записана в упрощенном виде:
Tgk (к) = Ри (к)_ pgh (к) (25)
Определим функцию обнаружения неисправности:
Л (к) = РТИЬ (к) Т4_и1 (кР (к) (26)
Критерий решения о неисправности:
Л (к) > , есть неисправности IЛ (к) < , нет неисправности где порог вводится для исключения ложных тревог.
В данной работе используем тест х2 вектора состояния с двумя прогнозами состояния, принцип которого состоит в следующем: во время ^ существует неисправность и переключатель К находится в позиции «1», переключатель К2 находится в позиции «2», прогноз 1 «загрязняется» из-за этой неисправности, но прогноз с выхода 2 получен с помощью предыдущего правильного состояния, то есть этот выход является правильным и может быть использован для исправления неисправности. Во время , ошибки в прогнозе 1 корректируются с помощью выхода фильтра Калмана. Через время А^ переключатель К находится в позиции «2», переключатель К2 — в позиции «1», и прогноз состояния 1 используется для исправления неисправности состояния.
Тест х2 остаточной ошибки:
Предсказанный вектор может записать в виде:
[XX, (к ) = (к, к-1) XX, (к-1) , (к ) = н, (к) XX, (к)
Остаточная ошибка суб-фильтра Калмана О:
^ ( к ) = Х я* ( к )- Я, _ * Ковариация остаточной ошибки суб-фильтра Калмана О:
8 я* (к) = ня* (к) Р* (к) Н* (к) + Кя* (к )
Определим функцию обнаружения неисправности:
/я* (к )=<* (к) 8-; (к) ^ (к)
Критерий решения неисправности:
(к) > е^*, есть неисправности \угк (к) < , нет неисправности где - порог исправления неисправности.
Аналогичные фильтры Калмана применены и для других источников навигационной информации, как показано на рисунках 5 и 6.
(29)
(30)
(31)
(32)
Рис. 5 Схема фильтра Калмана для обнаружения недостоверной информации о местоположением
Рис. 6 Схема фильтра Калмана для обнаружения недостоверной информации о проекции скорости
3. Слияние нескольких источников навигационной информации и
изоляция неисправностей
Если используем множество датчиков для измерения одного параметра, то можно слить выходы всех датчиков с использованием назначения весов для выхода каждого датчика.
Результат слияния измеренной информации можно записать следующем образом:
где У - результат слияния; "\У = ■■-,>•'„] - матрица весов; X = [х1 = л1 = -- ■ =лсл] -выходы
датчиков: п -количество датчиков данного вида информации; М - матрица обнаружения
Принцип метода назначения весов для выхода каждого датчика можно записать следующем образом [20-21]:
1 п
", = —-• I 1 (34)
1/а} 1
I
где с7; и о. ^дисперсия ошиоки выходов 1-ого - ого датчики; г = \.2._ ■■ -,п.
В реальной ситуации истинные значения неизвестны, Чтобы получить ошибку измеренных сигналов, в работе определяется среднее значение выходов разных датчиков, которое принимается в качестве истинного значения для текущего момента. Для рассматриваемой ситуации такой вариант не подходит, так как разные типы датчиков в разных условиях могут давать заведомо низкую точность или даже отказ, из-за чего среднее значение может иметь большую разницу с истинным значением.
Если в моменте к, датчик , выдаёт измеренное значение Т (к), то:
где АТ (к) -ошибка измерения , -ого датчика в моменте к; АТ}. -среднее значение , -ого датчика в моменте к; а. (к) - дисперсия ошибки выхода , -ого датчика в моменте к ; Т\ (к)
- прогностическая оценка навигационной информации, полученная с помощью фильтра Калмана; N - число измерений с каждого датчика.
Поскольку процесс фильтрации включает в себя изоляцию недостоверных источников информации и исправление ошибок инерциальной навигационной системы, то можем приблизительно считать оценочную информацию в качестве истинной.
4. Моделирование и макетирование навигационной системы
Информация о высоте полета может быть получена из нескольких источников, причём точность измерения радиовысотомера сильно зависит от наклона платформы БПЛА и поверхности. Для того, чтобы проверить правильность обнаружения и изоляции неисправностей предлагаемой системой, проведено моделирование полета БПЛА по трехмерной карте с большой неровностью («волнообразностью») земной поверхности, как показана на Рис. 7.
(а) (б)
Рис. 7 (а) Трехмерная карта; (б) проекция трехмерной карты на горизонтальной плоскости
Рисунок 8 показывает траекторию полета БПЛА от точки (0,0,8) до точки (100,100,8), то есть, высота полета поддерживается на уровне около 8 метров с помощью интегрированной навигационной системы БИНС/ОPS/BA/RA.
Рис. 8 Траектория полета БПЛА
Рисунок 9 показывает результаты измерения высоты датчиком GPS, барометрическим и радиовысотомером. Измеренная высота радиовысотомера отражает вертикальное расстояние от БПЛА до земли и имеет большую разницу с истиной. В данном моделировании считаем эта разница в качестве неисправности.
10 9 8
7
>
\
h leight of GPS leight of BA leight of RA
1 h
0 5 10 15 20 25 30 35 40 45 50
x, m
Рис. 9 Измеренная высота GPS, барометрического высотомера и радиовысотомера
После слияния навигационной информации о высоте можно значительно уменьшить влияние неисправности радиовысотомера, но ошибка остаётся большой, как показано на рис. 10 (а) и (б).
8.4 8.2 8 7.8 7.6
1-74
7.2 7 6.8 6.6 6.4
0
25 t,s
-Чл*"
25
t,s
(а) (б)
Рис.10 (а) - измеренная высота после слияния; (б) - ошибка высоты
-0.6
-0.8
10
5
20
30
35
40
45
50
0
15
20
30
35
40
45
50
На рис. 11 (а) и (б) видно, что статистики измерений высоты теста «хи-квадрат» по остаточным ошибкам и теста «хи-квадрата» по состояния у радиовысотомера гораздо больше, чем у GPS и барометрического высотомера, а изогнутая форма соответствует линии измеренной высоты радиовысотомером, следовательно такого вида неисправность датчика можно обнаружить с помощью теста «хи-квадрат».
Рис. 11 (а) - статистики теста «хи-квадрат» по состоянию: GPS-синяя линия; BA-красная линия; RA-зеленая линия; (б) - статистики теста «хи-квадрат» по остаточным ошибкам : GPS-синяя линия; BA-красная линия;
RA-зеленая линия
Рис. 11 (а) и (б) показывают значения высоты и ошибки по высоте в результате слияния после изоляции неисправности. Сравнивая рис.10 и рис.12 можно увидеть, что после изоляции неисправности информация о высоте в результате слияния имеет меньшую погрешность, что позволяет повысить точность навигации.
8.1
8.05 8
7.95 Е 7.9 7.85 7.8 7.75 7.7
7.6
0
0.08 0.06 0.04 5 0.02 § 0 -0.02 -0.04
25 t,s
30 35 40 45
(а)
25
t,s
(б)
Рис.12 (а) -информации о высоте после слияния и изоляции неисправности; (б) - ошибка измерения высоты
после слияния и изоляции неисправности
0.06
0 15
20
50
Для проверки работоспособности предлагаемых решений были проведены макетные испытания и разработан пользовательский интерфейс. При макетировании в данной работе использованы следующие датчики и модуль сопряжения: инерциальный измерительный блок ВМ1160 (включает трехосный гироскоп и трехосный акселерометр); веб-камера SCB-1100N BA59-02898A для визуальной навигационной системы; блютуз GPS-приемник, барометрический высотомер ВМР085; радиовысотомер индикатор PN 1040170-00; модуль сопряжения А^шпоВоаМ101.
(г) (д) (е)
Рис.13 Основные датчики имодуль сопряжения
Основные характеристики датчиков:
- Характеристики акселерометра: диапазон измерений - ± 2g; смещение нуля -±25mg; нулевое смещения температурного дрейфа - ±1.0 mg/K; ошибка выравнивания - ±0.50; скорость передачи выходных данных - 1600Hz;
- Характеристики гироскопа: диапазон измерений - ± 250%; смещение нуля -±30/с;нулевое смещения температурного дрейфа - ±0.050/с/К; выходной шум -
0.07 / s /л/и!; скорость передачи выходных данных - 3200Hz;
- Характеристики GPS: точность -<10да ;высота над уровнем моря - < 18000да ;скорость - <515да / c; скорость передачи выходных данных - 10Hz;
- Параметры веб-камеры, полученные с помощью «Набора инструментов калибровки камеры» для системы Matlab: Фокусное расстояние: fc = [ 1015.78212 1012.65800 ] ±[ 7.15609 7.22421 ]; Центральная точка: cc = [ 320.03704 240.40334 ] ± [ 3.41690 4.77241 ]; Искажение: kc = [ 0.09333 -0.05262 -0.01156 -0.00424 0.00000 ] ±[ 0.02113 0.11162 0.00246 0.00249 0.00000 ];
- Электрические характеристики барометрического высотомера: диапазон измерений - 300~1100hPa (+9000— 500да ); выходной шум - 0.01hPa ( 0.1да ); разрешение выходных данных- 0.01hPa ( 0.1да );точность абсолютного давления - ± 1hPa; точности относительного давления - ± 0.2 hPa;
- Электрические характеристики радиовысотомера: диапазон измерений - 2500feet ( 762m );точность высоты - ± 2%~5% ;разрешение обнаружения угла 0.250 ;скорость передачи выходных данных - 4300Hz;
На рис. 14 показан приборный состав макета для отработки навигационной информации.
Проведена обработка нескольких источников навигационной информации и отображение результатов навигации в компьютере с помощью MATLAB и интегрированных навигационных алгоритмов.
Для удобства проверки правильности алгоритма разработан интерфейс интегрированной навигационной системы с использованием инструментария MATLAB GUI. Интерфейс может быть использован и для наблюдения полета БПЛА и состояния навигационных датчиков на наземной станции в режиме реального времени. Интерфейс включает несколько основных частей: информацию навигационной системы GPS; информацию датчиков инерциального измерительного блока; настройки последовательных портов для навигационных датчиков; информации визуальной навигационной системы на основе алгоритма SLAM; информацию системы БИНС, БИНС/GPS и БИНС/GPS/SLAM, как показаны на рис 15.
Рис. 15 Интерфейс информации интегрированной навигационной системы
Рисунок 15 показывает информацию интегрированной навигационной системы во внешней среде в режиме навигации БИНС/GPS/SLAM/BA/RA. В области визуальной навигационной системы SLAM показывает результаты навигации 487-ого и 488-ого кадров изображения, характерные точки (зеленые) двух изображений, ориентиры (красные) двух изображений, трехмерные координаты ориентиров в системы координат камеры и траекторию движения в земной системе координат. В области интегрированной навигационной системы показывает траекторию движения на спутниковой карте (красная линия), полученную из интегрированной навигационной системы.
На рис. 16 показаны выходы датчиков.
широтаGPS/SLAM
55.669
55.668
& 55.667
К
55.666
долгота GPS/SLAM
55.665
У
ч л ч
'У
L —
lg -la
0
100
200 300 время,с
400
37.784 37.782 37.78 i 37.778 37.776 37.774
-\g -^a
500
0
100
200 300 время,с
400
500
150 140
сЗ
о 130
о и
120 110
высота над уровнем моря
0 50 100 150 200 250 300 350 400
время,с
A _
Hb
-Hr^:
450-500
о 1.5
Ö 1
о &
§
u 0.5
значение скорости GPS
400
300
I 200
100
100
200 300 время,с
скорости GPS
400
0
направление скорости GPS
I"/*■
L
A
lA fW U
500 0
100
200 300 время,с
скорости SLAM
100
200 300 время,с
400
500 0
100
200 300 время,с
400
400
500
500
2
0
0
100 50 3 0 -50 -100
2 1.5
\о о
о 05 00 0.5
ON
0
0 100 200 300 400
время,с
-0.5
500 0 Рис. 16 Выходы датчиков
100 200 300 400 время,с
500
1
Исходная навигационная информация датчиков показывает:
1. Предсказанные изменения широты и долготы визуальной навигационной системы в основном совпадает с измеренными данными спутниковой навигационной системы GPS, хотя есть небольшая разница. Это ещё раз доказывает целесообразность применения визуальной навигационной системы.
2. По сравнению с другими показаниями, кривая изменения измеренной высоты над уровнем моря от СНС имеет значительно большие изменения величины и ошибки.
3. Кроме того, СНС может определить величину и направление скорости подвижных объектов только в горизонтальной плоскости, но не может определить скорость в вертикальном направлении, а может лишь приблизительно вычислить изменения скорости в данном направлении в зависимости от изменения высоты, что даёт большие ошибки.
4. Поэтому, в целях повышения точности навигации необходимо использовать дополнительные датчики и навигационные системы, в частности, как в данной работе, визуальную навигационную систему, барометрический высотомер и радиовысотомер.
На рис. 17 показаны выходы фильтра Калмана и траектория движения на горизонтальной плоскости.
Рис. 17 Результаты интегрированной навигационной системы
Результаты макетирования показывают правильность и работоспособность предлагаемой интегрированной навигационной системы.
Заключение
В данной работе были получены следующие основные научные результаты, имеющие важное практическое значение.
1. Разработана структура и математическая модель интегрированной навигации системы для БПЛА.
2. Предложен эффективный фильтр Калмана с использованием тестов х2 по остаточной ошибке и по состоянию для обнаружения недостоверной информации от датчиков.
3. Предложен простой и легко реализуемый алгоритм слияния нескольких источников навигационной информации при назначении весов выходов датчиков на основе ошибок дисперсий. Кроме того, этот алгоритм имеет возможность изоляции недостоверной навигационной информации с помощью матрицы обнаружения неисправности.
4. Проведено моделирование полета БПЛА в трехмерной карте с большой неровностью земной поверхности, результаты моделирования показывают работоспособность функции обнаружения и изоляции неисправностей системы.
5. Результаты макетирования, которое было проведено в наружной среде, показывают, что предлагаемая интегрированная навигационная система может обеспечить высокую точность навигации для БПЛА.
Список литературы
1. Upadhyay T N, Cotterill S, Deaton A W. Autonomous GPS/INS navigation experiment for space transfer vehicle // IEEE Transactions on Aerospace and Electronic Systems, 1993. -№ 29(3) . P. 772-785.
2. Juanjuan C, Jiancheng F, Wei S, et al. Study and Application of Low-cost Multi-sensor Integrated Navigation for Small UAV Autonomous Flight // Acta Aeronautica Et Astronautica Sinica, 2009. - № 10: 021.
3. Xiong Z, Hao Y, Wei J, et al. Fuzzy adaptive Kalman filter for marine INS/GPS navigation // Mechatronics and Automation, 2005 IEEE International Conference. IEEE, 2005. - № 2. P. 747-751.
4. Brown D L, Munoz J P, Gay R. Determination of Barometric Altimeter Errors for the Orion Exploration Flight Test-1 Entry // AIAA Guidance, Navigation, and Control Conference, 2012 . P. 4993.
5. Hajiyev C, Saltoglu R. Robust integrated INS/radar altimeter accounting faults at the measurement channels // Proceedings of the 23rd ICAS (The International Council of the Aeronautical Sciences) Congress, Toronto, Canada, 2002 . P. 2002-6.8.
6. Hang Y, Liu J, Li R, et al. Optimization method of MEMS IMU/LADAR integrated navigation system based on Compressed-EKF // 2014 IEEE/ION Position, Location and Navigation Symposium-PLANS 2014. IEEE, 2014 . P. 115-120.
7. Li R, Liu J, Zhang L, et al. LIDAR/MEMS IMU integrated navigation (SLAM) method for a small UAV in indoor environments // 2014 DGON Inertial Sensors and Systems (ISS). IEEE, 2014 . P. 1-15.
8. Huh S, Shim D H, Kim J. Integrated navigation system using camera and gimbaled laser scanner for indoor and outdoor autonomous flight of UAVs // 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 201 . P. 3158-3163.
9. Conte G, Doherty P. An integrated UAV navigation system based on aerial image matching // Aerospace Conference, 2008 IEEE. IEEE, 2008 . P. 1-10.
10. Фомичев А.В, Тань Л. Разработка алгоритма быстрой компенсации погрешностей комплексированной инерциально-спутникой системы навигации малогабаритных беспилотных летательных аппаратов в условиях сложной среды //Наука и образование. МГТУ им. Н.Э. Баумана. Электрон. журн. 2015. №10. Режим доступа: http://technomag.bmstu.ru/doc/821641.html (дата обращения 18.06.2016).
11. Yuan G, Yuan K, Zhang H. A Variable Proportion Adaptive Federal Kalman Filter for INS/ESGM/GPS/DVL Integrated Navigation System // Computational Sciences and Optimization (CSO), 2011 Fourth International Joint Conference on. IEEE, 2011. P. 978-981.
12. Hua B, Chen W, Kang G, et al. Federal adaptive filtering of restraining consecutive outliers algorithm in multi-source information integrated navigation system // 2013 10th IEEE International Conference on Control and Automation (ICCA). IEEE, 2013. P. 908-912.
13. Салычев О. С., Григорьев В. Е., Макаров Н. Н. Интегрированная бесплатформенная система навигации средней точности для беспилотного летательного аппара-та//патента RU 2539140,публикация патента:10.01.2015.
14. Алешин Б.С., Афонин А.А., Веремеенко К.К., Кошелев Б.В., Плеханов В.Е., Тихонов В.А., Тювин А.В., Федосеев Е.П. Ориентация и навигация подвижных объектов: со-
временные информационные технологии / под ред. Б.С. Алешина, К.К. Веремеенко, А.И. Черноморского. М.: Физматлит, 2006. 422 с.
15. Qin Yongyuan, Zhang Hongqian, Wang Shuhua. Theory of Kalman Filter and Integrated Navigation // Northwestern Polytechnic University Press, 2012. 386 p.
16. Da R. Failure detection of dynamical systems with the state chi-square test // Journal of guidance, control, and dynamics, 1994. - № 1017(2) . P. 271-277.
17. Segars A H, Grover V. Re-examining perceived ease of use and usefulness: A confirmatory factor analysis // MIS quarterly, 1993. P. 517-525.
18. Brumback B D, Srinath M D. A chi-square test for fault-detection in Kalman filters // Automatic Control, IEEE Transactions on, 1987. - № 32(6) . P. 552-554.
19. Yu J, Lee J G, Park C G. Fault Detection Using Propagator for Kalman Filter and Its Application to SDINS // ICCAS, 2003. P. 978-983.
20. Linben L, Zigang L I, Chaoying C. Optimal weight distribution principle used in the fusion of multi-sensor data// Journal of Chinese Inertial Technology, 2000. - № 8(2) . P. 36-39.
21. Chong-quan Z, Xi-lu D, Li-yong Z, et al. Variance estimation in multisensor fusion algorithm// IFAC, 2003. P. 7-12.
Science ¿Education
of the Baumail MSTU
Science and Education of the Bauman MSTU, 2016, no. 10, pp. 182-206.
DOI: 10.7463/1216.0852517
Received: 04.11.2016
Revised: 18.11.2016
© Bauman Moscow State Technical Unversity
An Integrated Unmanned Aerial Vehicle Navigation System Capable of Fault Detection and Isolation
KeKe Gen1*, N.A. Chulin1
* ' j s £engke@ 12 6 .com :Bauman Moscow State Technical University, Moscow, Russia
Keywords: integrated navigation system; multi-information fusion; Kalman filter; Chi-Square test;
fault detection and isolation
The article discusses an integrated navigation system development for unmanned aerial vehicles (UAVs) to provide fault detection and isolation. In case an UAV operates flights in independent mode or in remote pilot control under conditions when an operator has a lack of information on the environment, the single-source information cannot ensure appropriate precision to navigate the UAV. Therefore, the UAV, certainly, needs an integrated navigation system based on the merger of navigation information from the several sources to enable compensation for the shortcomings of each of the sources.
The integrated navigation system typically combines two or more than two types of navigation systems. Thus, an integrated navigation system can take full advantage of the complementarity of various navigation systems and generate accurate navigation information. Such systems typically include an inertial measurement unit (Inertial Measurement Units - IMU), SNA, visual navigation system, etc. However, in practical applications, there are many factors that affect the accuracy and reliability of the measured data, such as data transmission errors, ambient noise, and systematic errors of sensors, etc. Therefore, the fault detection and isolation feature is also necessary for the integrated navigation system.
The article offers an integrated navigation system for UAV based on conversion of navigation modes, Kalman filter, and merge of navigation information from the several sources, namely a SNA system, visual navigation system based on the algorithm of simultaneous localization and mapping (Simultaneous Localization And Mapping - SLAM), barometric altimeter, altimeter and inertial navigation system (INS). Such an integrated navigation system is capable not only to provide detection and isolation of low reliable navigation information on the basis of the residual errors of the test "chi-squared" and the result of the test "chi-squared" with two state predictions, but also to use the advantages of various sensors to improve reliability of navigation data. The results of simulation in Matlab and system layout in a natural environment show that the proposed system has high accuracy of navigation and the capability to provide fault detection and isolation.
References
1. Upadhyay T.N., Cotterill S., Deaton A.W. Autonomous GPS/INS navigation experiment for space transfer vehicle. IEEE Transactions on Aerospace and Electronic Systems, 1993. No. 29(3). P. 772-785.
2. Juanjuan C., Jiancheng F., Wei S., et al. Study and Application of Low-cost Multi-sensor Integrated Navigation for Small UAV Autonomous Flight. Acta Aeronautica Et Astronautica Sinica, 2009. No. 10: 021.
3. Xiong Z., Hao Y., Wei J., et al. Fuzzy adaptive Kalman filter for marine INS/GPS navigation. Mechatronics and Automation, 2005 IEEE International Conference. IEEE, 2005. No. 2. P. 747-751.
4. Brown D.L., Munoz J.P., Gay R. Determination of Barometric Altimeter Errors for the Orion Exploration Flight Test-1 Entry. AIAA Guidance, Navigation, and Control Conference, 2012. P. 4993.
5. Hajiyev C., Saltoglu R. Robust integrated INS/radar altimeter accounting faults at the measurement channels. Proceedings of the 23rd ICAS (The International Council of the Aeronautical Sciences) Congress, Toronto, Canada, 2002. P. 2002-6.8.
6. Hang Y., Liu J., Li R., et al. Optimization method of MEMS IMU/LADAR integrated navigation system based on Compressed-EKF. 2014 IEEE/ION Position, Location and Navigation Symposium-PLANS 2014. IEEE, 2014. P. 115-120.
7. Li R., Liu J., Zhang L., et al. LIDAR/MEMS IMU integrated navigation (SLAM) method for a small UAV in indoor environments. 2014 DGON Inertial Sensors and Systems (ISS). IEEE, 2014. P. 1-15.
8. Huh S., Shim D.H., Kim J. Integrated navigation system using camera and gimbaled laser scanner for indoor and outdoor autonomous flight of UAVs. 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 201. P. 3158-3163.
9. Conte G, Doherty P. An integrated UAV navigation system based on aerial image matching. Aerospace Conference, 2008 IEEE. IEEE, 2008. P. 1-10.
10. Fomichev A.V, Tan' L. Razrabotka algoritma bystroi kompensatsii pogreshnostei kompleksirovannoi inertsial'no-sputnikoi sistemy navigatsii malogabaritnykh bespilotnykh letatel'nykh apparatov v usloviiakh slozhnoi sredy. Nauka I obrazovanie = Science and education. Electronic scientific and technical publication. 2015. No. 10. Available at: http://technomag.bmstu.ru/doc/821641.html, accessed 18.06.2016.
11. Yuan G., Yuan K., Zhang H. A Variable Proportion Adaptive Federal Kalman Filter for INS/ESGM/GPS/DVL Integrated Navigation System. Computational Sciences and Optimization (CSO), 2011 Fourth International Joint Conference on. IEEE, 2011. P. 978-981.
12. Hua B., Chen W., Kang G., et al. Federal adaptive filtering of restraining consecutive outliers algorithm in multi-source information integrated navigation system. 2013 10th IEEE International Conference on Control and Automation (ICCA). IEEE, 2013. P. 908-912.
13. Salychev O.S., Grigor'ev V.E., Makarov N.N. Integrirovannaia besplatformennaia sistema navigatsii srednei tochnosti dlia bespilotnogo letatel'nogo apparata. Patent No. 2539140, patent publication: 10.01.2015. Available at:
http://www.findpatent.ru/patent/253/2539140.html, accessed 18.12.2016.
14. Aleshin B.S., Afonin A.A., Veremeenko K.K., Koshelev B.V., Plekhanov V.E., Tikhonov V.A., Tiuvin A.V., Fedoseev E.P. Orientatsiia i navigatsiia podvizhnykh ob"ektov: sovremennye informatsionnye tekhnologii, under ed. by B.S. Aleshin, K.K. Veremeenko, A.I. Chernomorskii. Fizmatlit. Moscow, 2006. 422 p. [In Russian]
15. Qin Yongyuan, Zhang Hongqian, Wang Shuhua. Theory of Kalman Filter and Integrated Navigation. Northwestern Polytechnic University Press, 2012. 386 p.
16. Da R. Failure detection of dynamical systems with the state chi-square test. Journal of guidance, control, and dynamics, 1994. No. 1017(2). P. 271-277.
17. Segars A.H., Grover V. Re-examining perceived ease of use and usefulness: A confirmatory factor analysis. MIS quarterly, 1993. P. 517-525.
18. Brumback B.D., Srinath M.D. A chi-square test for fault-detection in Kalman filters. Automatic Control, IEEE Transactions on, 1987. No. 32(6). P. 552-554.
19. Yu J., Lee J.G., Park C.G. Fault Detection Using Propagator for Kalman Filter and Its Application to SDINS. ICCAS, 2003. P. 978-983.
20. Linben L., Zigang L.I., Chaoying C. Optimal weight distribution principle used in the fusion of multi-sensor data. Journal of Chinese Inertial Technology, 2000. No. 8(2). P. 36-39.
21. Chongquan Z., Xilu D., Liyong Z., et al. Variance estimation in multisensor fusion algorithm. IFAC, 2003. P. 7-12.
