9 / 19
Кэ Кэ Гэн, Н.А. Чулин
84
ISSN 0236-3933. Вестник МГТУ им. Н.Э. Баумана. Сер. Приборостроение. 2017. № 3
Здесь
vv
P
— ковариационная матрица положения и ориентации робота;
,
mv
P
vm
P
— кросс-ковариационная матрица робота и ориентиров;
mm
P
— ковариа-
ционная матрица положения ориентиров.
2. Определение ошибки прогноза и ковариационной матрицы по наблюде-
ниям на шаге
k
+ 1:
(
)
1
1
1|
ˆ
;
k
k
k k
h
+
+
+
= −
y s
X
т
1
1 1|
1
1
,
k
k k k
k
k
+
+ +
+
+
=
+
J
H P H R
где
1
k
+
H
— матрица Якоби при разложении нелинейных функций
( )
k
h
X
в ряд
Тейлора в окрестности
1|
ˆ .
k k
+
X
3. Коррекция оценок состояния
1
1|
1 1
ˆ
ˆ
k
k k
k k
+
+
+ +
= +
X X K y
и ковариационной
матрицы
(
)
1
1 1
1|
,
k
k k
k k
+
+ + +
= −
P I K H P
где
т
1
1
1|
1 1
.
k
k k k k
−
+
+
+ +
=
K P H S
4. Расширение вектора состояния и ковариационной матрицы.
Ориентиры, обнаруженные датчиками аппарата на каждом шаге, включают
в себя ориентиры, уже существующие на карте, а также новые ориентиры. Су-
ществовавшие ориентиры были использованы для оценок состояния на приве-
денной выше последовательности действий. Новые ориентиры добавляются в
вектор состояния системы через процесс инициализации. Пусть на шаге
k j
-й
наблюдаемый ориентир в векторе наблюдения с координатами
,
,
f j
w k
X
является
новым. Его координаты в неподвижной системе координат:
(
)
,
,
,
,
f j
V k k
v k
g
=
=
X X Z
,
,
,
,
.
f j
v
B
b k
V k w k
+
X R X
Новый вектор состояния системы после расширения
,
,
,
.
k
new k
f j
w k
=
X
X
X
Ковариационная матрица нового вектора состояния
(
) (
)
,
т
т
т
т
,
vv
vm
vv
new k
mv
mm
mm
vv
mm
vv
mm
g
g
g
g
g
g
g
g
∇
=
∇
∇
∇
∇ ∇ +∇ ∇
X
Z
X
Z
X
X
Z
Z
P
P
P
P
P
P
P
P
P
P
P
где
,
g
∇
X
g
∇
Z
— матрицы Якоби
(
)
,
,
,
,
f j
V k w k
g
X X
для
,
V k
X
и
.
k
Z
Ассоциация данных для алгоритма SLAM.
Во-первых, необходимо обна-
ружить, есть ли новые ориентиры в области
1
S
по алгоритму ассоциации дан-
ных. В настоящее время наиболее часто используемым алгоритмом ассоциации
данных является алгоритм ближайшего соседа [22, 23] ввиду его простоты и не-
большого объема вычислений. В этом алгоритме для ассоциации каждого
наблюдаемого ориентира проводится сравнение расстояния до существующих
ориентиров с заранее заданным порогом (
Gate
). Если расстояние от наблюдае-
мого до уже существующего на карте ориентира меньше определенного порога,
то наблюдаемый ориентир совмещается с существующим ориентиром. Когда на