10 / 19
Алгоритм навигации беспилотного летательного аппарата на основе улучшенного алгоритма…
ISSN 0236-3933. Вестник МГТУ им. Н.Э. Баумана. Сер. Приборостроение. 2017. № 3
85
карте есть несколько ориентиров, совместимых с наблюдаемым, в качестве ас-
социированного ориентира выбирается ориентир с минимальным расстоянием.
Учитывая ошибки измерений, за расстояние от
i
-го наблюдаемого ориенти-
ра до
j
-го ориентира на локальной карте целесообразно использовать расстоя-
ние Махаланобиса [23]:
т 1
,
ij
ij
ij
M
−
=
y J y
где
ˆ
ij
i
j
= −
y y y
— вектор разности координат по результатам измерений;
i
y
—
вектор измерений координат
i
-го наблюдаемого ориентира;
ˆ
j
y
— вектор коор-
динат
j
-го ориентира в локальной карте;
J
— ковариационная матрица ошибок
измерений.
Если
,
ij
M Gate
≤
то полагаем
i
-й наблюдаемый ориентир совпадающим с
j
-м
существующим.
Если более чем один ориентир из присутствующих на локальной карте удо-
влетворяет условиям
,
ij
M Gate
≤
то в качестве ассоциированного ориентира вы-
бираем ориентир с минимальным нормированным расстоянием
k
N
=
( )
(
)
т 1
min
ln .
ij
ij
−
=
+
y J y
J
В соответствии с проведенным анализом, этот простой алгоритм ассоциации
данных не учитывает взаимосвязь между ориентирами, поэтому вероятность не-
правильной ассоциации у алгоритма высока. В настоящей работе для решения
указанной проблемы использован алгоритм локальной ассоциации данных SLAM
на основе улучшенного муравьиного алгоритма, который был изложен в работе
[24]. Этот алгоритм можно разделить на два этапа: на первом этапе определяются
ориентиры в локальном пространстве совпадений и наблюдаемые ориентиры,
имеющие возможности ассоциации по критерию индивидуальной совместимости
(Individual Compatibility, IC) [23]; на втором этапе находят совпадающие ориен-
тиры и координаты совпадающих наблюдаемых ориентиров на множестве состо-
яний с помощью улучшенного муравьиного алгоритма.
Алгоритм EKF–SLAM с адаптацией диапазона наблюдений.
С учетом из-
ложенного, размерность вектора состояния системы 6 3 .
n
+
В большой и слож-
ной среде, одновременно с приращением числа наблюдаемых ориентиров, будет
также постоянно увеличиваться размер вектора состояния системы. Вычисле-
ние ковариационной матрицы и матрицы Якоби будет резко усложняться,
ошибки вычисления матриц Якоби — увеличиваться, что приведет к снижению
эффективности работы алгоритма и точности позиционирования. Чтобы избе-
жать этого, предложен алгоритм EKF–SLAM с адаптацией зоны наблюдения —
AR-EKF–SLAM (Adaptive Observation Range–EKF–SLAM). Суть алгоритма со-
стоит в использовании локальной круговой карты для текущей оценки коорди-
нат аппарата и локализации зоны используемых ориентиров в глобальной си-
стеме координат, с одновременным обновлением глобальной карты. Принцип
локализации зоны наблюдения показан на рис. 4.