7 / 19
Кэ Кэ Гэн, Н.А. Чулин
82
ISSN 0236-3933. Вестник МГТУ им. Н.Э. Баумана. Сер. Приборостроение. 2017. № 3
ошибка матрицы
B
V
L
увеличивается со временем. Для решения этой проблемы
координаты БПЛА получены с помощью алгоритма SLAM, а не визуальной одо-
метрией, т. е. координаты характерных точек можно найти из уравнения:
,
,
,
,
.
f
f
v
B
b k
b k
V k w k
= +
X X R X
Расширенный фильтр Калмана (EKF) в алгоритме SLAM.
Для алгоритма
планирования полета БПЛА целесообразно использовать модель, описываю-
щую только кинематику траекторного движения. При этом можно принять, что
на каждом шаге алгоритма скорость полета не меняется, а изменения скорости
от шага к шагу ограничены располагаемыми ускорениями.
Вектор состояния модели движения камеры на БПЛА и матрицу координат
ориентиров в неподвижной системе координат можно записать следующим об-
разом:
т
,
;
v v
V
b b
=
X X V
т
,1
,2
,
,
,
, ...,
, ...,
,
f
f
f i
f n
M b
b
b
b
=
X X X X X
где
т
,
,
v
v v v
x y z
b
b b b
V v v v
=
— скорость камеры в неподвижной системе координат;
т
,
,
,
,
,
,
f i
f i
f i
f i
b
b b b
x y z
=
X
— координаты
i
-го ориентира в неподвижной системе
координат;
n
—
число наблюдаемых ориентиров.
Расширенный вектор состояния камеры на БПЛА и ориентиров карты в не-
подвижной системе координат имеет вид
[
]
т
,
.
V M
=
X X X
Рис. 3.
Системы координат для момента времени
:
k
B B B B
O X Y Z
— базовая неподвижная система координат;
,
,
,
,
V k V k V k V k
O X Y Z
— связанная система
координат БПЛА в момент
k
;
,
,
,
,
W k W k W k W k
O X Y Z
— система координат камеры в момент
k
;
1,
1,
1,
1,
k
k k
k
C C C C
O X Y Z
— система координат камеры для первого кадра изображения в момент
k
;
2,
2,
2,
2,
k
k
k
k
C C C C
O X Y Z
— система координат камеры для второго кадра изображения в момент
k