Алгоритмы обхода препятствий в недетерминированной среде

1.

Лекция 4
к.т.н., доц. каф. ССОД
С. А. Алексейцев

2.

Алгоритмы обхода препятствий в недетерминированной среде
Структура системы навигации

3.

Лазерный
дальномер
–
устройство,
позволяющее
сканировать
окружающее
пространство и получать данные о находящихся в данном пространстве объектах
(препятствиях) в виде векторов расстояний;
Алгоритм SLAM (Simultaneous Localization and Mapping) - алгоритм, разработанный для
локализации мобильного робота в пространстве, а так же построения динамической
карты окружающей среды.
Трассировщик препятствий на основе имеющихся данных о текущем местоположении
робота и текущего скана карты окружающей среды строит список подвижных
препятствий и прогнозирует их местоположение в следующие моменты времени.
Алгоритм обхода препятствий – алгоритм, позволяющий построить траекторию обхода
препятствий мобильным роботом при имеющихся данных о местоположении робота в
данный момент времени и текущем скане карты окружающей среды

4.

Алгоритм SLAM
Данный алгоритм имеет две основные функции:
Локализация мобильного робота – получение информации о нахождении робота в текущий
момент времени;
Построение динамической карты окружающей среды.
Алгоритм SLAM для построения решения использует фильтр
Калмана.

5.

Фильтр Калмана
При синтезе фильтра Калмана предполагают, что полезный сигнал s(t) генерируется из
белого шума с помощью так называемого формирующего фильтра в соответствии с
алгоритмом
ds t
F t s t G t u t
dt
(1)
Где u(t) - белый шум, спектральная плотность которого равна Q(t) (шум формирования);
F(t)и G(t) - заданные коэффициенты.
Уравнение (1) называют уравнением формирующего фильтра.
Например, дифференциальное уравнение, описывающее формирующий фильтр, имеет
вид
d 2s
ds
a1
a1s b0 u
2
dt
dt
Введем обозначения
s s1
(2)
ds
s2
dt

6.

Тогда вместо уравнения (2) можно записать систему
ds1
0 s1 1 s2 0 0
dt
ds2
a0 s1 a1 s2 b0 u
dt
Перепишем эту систему в векторной форме
ds1
d t 0 1 s 0 0 0
1
ds2 a0 a1 s2 0 b0 u
dt
0
Обозначим
1
F
a
a
0 1
ds1
d s d t
,
d t ds2
0 0
dt
G
0
b
0
,
s1
s
s2
0
u
u

7.

При этом уравнение (2) преобразовалось в выражение (1) в векторной форме
ds
F s Gu
dt
(3)
В процессе распространения сигнала от передатчика к приемнику он
подвергается воздействию шума, поэтому принимаемый сигнал описывается
соотношением
x t C t s t n t
(4)
Где n(t) - белый шум, спектральная плотность которого равна R(t) (шум
измерения);
C(t)- заданный коэффициент.
Также величины n(t) и R(t) могут быть вектором и матрицей соответственно.
Уравнение (4) называют уравнением наблюдения.

8.

Уравнение фильтра Калмана
ds* t
F t s* t K t x t C t s* t
dt
(5)
Здесь s*(t) - оптимальная оценка полезного сигнала, K(t) - коэффициент
передачи (усиления) фильтра.
Коэффициента усиления K(t)
фильтра
K C T R 1
d
T 1
T
F F С R C GQG
dt
В этих уравнениях ε - матрица ошибок:
M s t s* t s t s* t T

9.

10.

11.

Принцип работы алгоритма SLAM:
1. Робот находится в некотором неизвестном месте. С помощью данных, полученных
датчиками, происходит построение видимого участка карты c данной позиции;
2. С помощью полученной на данном шаге траектории выбирается следующая позиция для
передвижения;
3. Происходит передвижение на новую позицию и сравнение текущее положение с
ожидаемым, полученным на предыдущем шаге;
4.
По полученным данным и данным с предыдущей итерации происходит обновление
карты. Далее переходим к п. 2.

12.

Построение карты окружающей среды в виде карты-сетки
Карта окружающей среды представлена в форме карты-сетки. Размер ячеек выбирается в
зависимости от необходимой точности. При этом, чем меньше размер ячеек, тем дольше
будет работать алгоритм обхода препятствий.
Полученные ячейки разделяем на два типа:
свободные - это те ячейки, через которые робот может совершать беспрепятственное
движение);
ячейки, содержащие в себе препятствия.

13.

Пример карты окружающей среды

14.

Трассировка препятствий
Условия трассировки препятствий:
Препятствие является твердым телом;
Подчиняется уравнениям движения робота;
Не может скачкообразно менять скорость;
Его можно вписать в окружность.
Каждое такое препятствие обладает следующим набором параметров: