8 / 19
Алгоритм навигации беспилотного летательного аппарата на основе улучшенного алгоритма…
ISSN 0236-3933. Вестник МГТУ им. Н.Э. Баумана. Сер. Приборостроение. 2017. № 3
83
Согласно принципу визуальной одометрии [21], уравнения состояния дви-
жения камеры на БПЛА и ориентиров карты можно записать в виде
[
]
(
)
,
,
т
1
, 1
,
,
,
,
,
,
,
v
v
b k
b k
v
k
V k
M k
b k
f
v
B
b k
V k w k
t
X
+
+
+ + Δ
=
=
+
+
X V Ω
X X X
V Ω
X R
где
Ω
— гауссовский белый шум с нулевым средним. Вектор наблюдения
,
.
f
k
w k
=
Z X
Оценка вектора состояния в алгоритме одновременной локализации и кар-
тографирования (SLAM) для рассматриваемой модели, которая является нели-
нейной, но с «гладкими» нелинейными функциями в правых частях уравнений
состояний и наблюдений, может быть получена с помощью расширенного
фильтра Калмана (Extended Kalman Filter, EKF), в котором на каждом шаге про-
водится линеаризация путем разложения в ряд Тейлора с отбрасыванием чле-
нов ряда выше первого порядка.
Если нелинейную модель БПЛА и ориентиров карты записать в виде
(
)
( )
1
,
;
,
k
k k
k
k
k
k
f
u
h
+
=
+
=
+
X X ε
S X η
где
k k
u a
=
— управляющие переменные;
,
k k
ε η
— гауссовский белый шум с ну-
левым математическим ожиданием и ковариациями
Q
и
,
R
то применение
расширенного фильтра Калмана для алгоритма SLAM можно представить в ви-
де приведенной ниже последовательности действий.
1. Прогноз («одношаговый») состояния и ковариационной матрицы оши-
бок с использованием локальной линеаризации нелинейной системы:
(
)
т
1|
т
т
1|
ˆ
ˆ ,
,
;
,
k k
k k
k
k k k k
k k
k
k
f
u
+
+
=
=
+
X
X M
P F P F G Q G
где
(
)
ˆ ,
ˆ ,
,
ˆ
k k
k k
k
k
u
f
u
∂
=
∂
X
X
F
X
(
)
ˆ ,
,
k k
k k
k
k
u
f
u
u
∂
=
∂
X
X G
— матрицы частных производ-
ных первого порядка (матрица Якоби) при разложении нелинейных функций
(
)
,
k k
f
u
X
в ряд Тейлора в окрестности оценки на
k
-м шаге
ˆ( , ):
k k
X u
(
)
(
)
(
)
(
)
ˆ ,
,
ˆ
ˆ
,
,
.
k k
k k
k k
k k
k
k
k
u
f
u
f
u f
u
∂
≈
+
−
∂
X
X
X
X
X X
X
С учетом приведенного выше анализа, ковариационная матрица может
быть записана следующим образом:
(
) (
)
(
)
(
)
(
)
(
)
6 6
6 3
3 6 3 6
.
3 6
(3 3 )
vv
vm
k
mv
mm
n
n
n
n
n n
×
×
+ × + =
×
×
P
P
P
P
P