Лекция 2. Алгоритмы локализации

Вероятностная постановка задачи локализации, рекурсивная Байесовская оценка, фильтр Калмана, фильтр частиц

Transcript

1. Моделирование колесных роботов Лекция 2. Алгоритмы локализации Олег Шипитько Москва, 2020
2. https://bit.ly/2tlZrt4
3. 1. Определение и постановка задачи локализации 2. Вероятностная постановка a. Рекурсивная байесовская оценка 3. Фильтр Калмана 4. Фильтр частиц СОДЕРЖАНИЕ ЛЕКЦИИ 3
4. ЧТО ТАКОЕ ЛОКАЛИЗАЦИЯ? Локализация (в робототехнике) обозначает процесс трекинга (отслеживания) положения робота (позы и ориентации) в пространстве (фиксированной системе координат)1,2. * Ziegler, Julius; Henning Lategahn; Markus Schreiber; Christoph G Keller; Carsten Knoppel; Jochen Hipp; Martin Haueis; and Christoph Stiller. 2014. “Video based localization for bertha.” In Intelligent Vehicles Symposium Proceedings, 2014 IEEE, 1231–1238. Локализация Пространство состояний Среда 2D: (x, y, ). 3D: (x, y, z, , , ) Внутри помещений (Indoor) На открытом пространстве (Outdoor) 4
5. ПОЧЕМУ ВАЖНА ЛОКАЛИЗАЦИЯ? Знание точного положения в пространстве является предпосылкой для точного планирования движения и его осуществления. 5
6. ФОРМАЛЬНАЯ ПОСТАНОВКА ЗАДАЧИ ЛОКАЛИЗАЦИИ — все предыдущие состояния (положение) — история применяемых сигналов управления — измерения датчиков — карта Даны: — текущее положение робота Определить: 6
7. МАРКОВСКИЕ ПРОЦЕССЫ Марковское свойство (для дискретного случая): Марковский процесс — стохастический процесс который: ❏ после любого заданного значения временного параметра t не зависит от эволюции, предшествовавшей t, при условии, что значение процесса в этот момент фиксировано ❏ «будущее» процесса не зависит от «прошлого» при известном «настоящем» ❏ удовлетворяет марковскому свойству 7
8. СКРЫТЫЕ МАРКОВСКИЕ ПРОЦЕССЫ 8 — наблюдаемый процесс — скрытый процесс
9. ЛОКАЛИЗАЦИЯ КАК СКРЫТЫЙ МАРКОВСКИЙ ПРОЦЕСС 9
10. ВЕРОЯТНОСТНАЯ ПОСТАНОВКА Поза робота Карта Измерение Вектор управляющих сигналов Thrun, S., Burgard, W. and Fox, D., 2005. Probabilistic robotics. MIT press. - изменение положения за текущий шаг времени Вероятностное распределение 10
11. ВЕРОЯТНОСТНАЯ ПОСТАНОВКА Thrun, S., Burgard, W. and Fox, D., 2005. Probabilistic robotics. MIT press. 11
12. Thrun, Sebastian. 2002. “Particle filters in robotics.” In Proceedings of the Eighteenth conference on Uncertainty in artificial intelligence, 511–518. Проблема локализации может быть сформулирована следующим образом: имея вектор всех последовательных измерений сенсоров z 1:t = z 0 ...z t , и управляющих сигналов u 1:t = u 0 ...u t , необходимо восстановить апостериорное распределение x t в любой заданный момент времени t. (Sebastian Thrun 2002) ВЕРОЯТНОСТНАЯ ПОСТАНОВКА 12
14. ТЕОРЕМА БАЙЕСА 14 Априорная вероятность Насколько вероятна была гипотеза a до получения новых данных b Частная (маргинальная) вероятность Вероятность исхода b, не зависящая от a Апостериорная вероятность Как вероятно, что наша гипотеза a правдива с учетом полученных данных b Правдоподобие Насколько вероятен исход b, при условии, что наша гипотеза a верна
15. ФОРМУЛА ПОЛНОЙ ВЕРОЯТНОСТИ 15
16. 16 Один из трех стрелков вызывается на линию огня и производит два выстрела. Вероятность попадания в мишень при одном выстреле для первого стрелка равна 0.3, для второго — 0.5, для третьего — 0.8. Мишень не поражена. Найти вероятность того, что выстрелы произведены первым стрелком. — на линию огня вызван первый стрелок — на линию огня вызван второй стрелок — на линию огня вызван третий стрелок Так как вызов на линию огня любого стрелка равновозможен, то Источник: https://www.matburo.ru/ ТЕОРЕМА БАЙЕСА ПРИМЕР
17. ТЕОРЕМА БАЙЕСА ПРИМЕР 17 В результате опыта наблюдалось событие В - после произведенных выстрелов мишень не поражена. Условные вероятности этого события при сделанных гипотезах равны: По формуле Байеса находим вероятность гипотезы А 1 после опыта:
18. РЕКУРСИВНАЯ БАЙЕСОВСКАЯ ОЦЕНКА 18
19. РЕКУРСИВНАЯ БАЙЕСОВСКАЯ ОЦЕНКА 19 Теорема Байеса
20. РЕКУРСИВНАЯ БАЙЕСОВСКАЯ ОЦЕНКА 20 Марковское свойство
21. РЕКУРСИВНАЯ БАЙЕСОВСКАЯ ОЦЕНКА 21 Формула полной вероятности
22. РЕКУРСИВНАЯ БАЙЕСОВСКАЯ ОЦЕНКА 22 Марковское свойство
23. РЕКУРСИВНАЯ БАЙЕСОВСКАЯ ОЦЕНКА 23 Марковское свойство
24. РЕКУРСИВНАЯ БАЙЕСОВСКАЯ ОЦЕНКА 24 Рекурсивный член
25. РЕКУРСИВНАЯ БАЙЕСОВСКАЯ ОЦЕНКА ПОЗЫ — предыдущее состояние системы (поза робота) — модель измерения — модель движения — коэффициент нормализации — пространство поз робота 25
26. РЕКУРСИВНАЯ БАЙЕСОВСКАЯ ОЦЕНКА ПОЗЫ — предыдущее состояние системы (поза робота) — модель измерения — модель движения — коэффициент нормализации — пространство поз робота 26
27. — предыдущее состояние системы (поза робота) — модель измерения — модель движения — коэффициент нормализации — пространство поз робота РЕКУРСИВНАЯ БАЙЕСОВСКАЯ ОЦЕНКА ПОЗЫ Предсказание Коррекция 27
28. РЕАЛИЗАЦИЯ РЕКУРСИВНОЙ БАЙЕСОВСКОЙ ОЦЕНКИ ❏ Рекурсивная байесовская оценка — фреймворк ❏ Существует множество алгоритмов-реализаций: ❏ Линейный и нелинейные модели измерения и движения ❏ Нормальные и произвольные (мультимодальные) распределения ошибок ❏ Параметрические и непараметрические алгоритмы ❏ …. ❏ Фильтр Калмана ❏ Информационный фильтр ❏ Гистограммный фильтр ❏ Фильтр частиц ❏ …. 28
29. РЕАЛИЗАЦИЯ РЕКУРСИВНОЙ БАЙЕСОВСКОЙ ОЦЕНКИ ❏ Рекурсивная байесовская оценка — фреймворк ❏ Существует множество алгоритмов-реализаций: ❏ Линейный и нелинейные модели измерения и движения ❏ Нормальные и произвольные (мультимодальные) распределения ошибок ❏ Параметрические и непараметрические алгоритмы ❏ …. 29 ❏ Фильтр Калмана ❏ Информационный фильтр ❏ Гистограммный фильтр ❏ Фильтр частиц ❏ ….
30. Доказуемо оптимален в случае: ❏ Линейных моделей движения и измерения (наблюдения) ❏ Нормально распределенных ошибок движения и измерения ФИЛЬТР КАЛМАНА 30 Линейная система X(s) Y(s)
31. Система линейна если: ❏ отклик системы на сумму воздействий равен сумме откликов на каждое воздействие ЛИНЕЙНАЯ СИСТЕМА 31 Необходимые условия линейности: ❏ Гомогенность – при изменении амплитуды входного сигнала в k раз также в k раз изменяется и амплитуда выходного сигнала ❏ Аддитивность – при суммировании входных сигналов результирующий сигнал на выходе будет равен сумме реакций от исходных сигналов ❏ Инвариантность – смещение входного сигнала во времени вызывает аналогичное смещение выходного сигнала. ❏ Статическая линейность – основные законы в системе описываются линейными уравнениями ❏ Гармоническая верность – если на вход системы подать синусоидальный сигнал, то на выходе будет сигнал той же частоты
32. ФИЛЬТР КАЛМАНА ПРИМЕР 32 p v
33. ФИЛЬТР КАЛМАНА. СОСТОЯНИЕ СИСТЕМЫ 1. Оценкой текущего вектора состояния: 2. Оценкой матрицы ковариации: Наше знание о текущем состоянии системы описывается 2 элементами: 33 Источник: https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
34. ФИЛЬТР КАЛМАНА. ПРЕДСКАЗАНИЕ Предсказание о состоянии системы в следующий момент времени делается на основе модели движения: В матричной форме: — матрица эволюции процесса / системы (в нашем случае — модель движения) из t-1 в t 34
35. ФИЛЬТР КАЛМАНА. ПРЕДСКАЗАНИЕ 35
36. ФИЛЬТР КАЛМАНА. СИГНАЛЫ УПРАВЛЕНИЯ В матричной форме: — матрица описывающая изменение управляющих сигналов из t-1 в t 36
37. ФИЛЬТР КАЛМАНА. НЕОПРЕДЕЛЕННОСТЬ ДВИЖЕНИЯ — матрица ковариации, которая описывает случайный характер эволюции системы 37
38. ФИЛЬТР КАЛМАНА. КОРРЕКЦИЯ — матрица, описывающая модель измерения: как из состояния получить измерение 38
39. ФИЛЬТР КАЛМАНА. НЕОПРЕДЕЛЕННОСТЬ ИЗМЕРЕНИЙ — матрица ковариации, которая описывает случайный характер измерений 39
40. ОТСТУПЛЕНИЕ. НОРМАЛЬНОЕ РАСПРЕДЕЛЕНИЕ 40
41. ОТСТУПЛЕНИЕ. НОРМАЛЬНОЕ РАСПРЕДЕЛЕНИЕ 41
42. ФИЛЬТР КАЛМАНА. СОБИРАЕМ ВСЕ ВМЕСТЕ 42
43. ФИЛЬТР КАЛМАНА. СОБИРАЕМ ВСЕ ВМЕСТЕ 43
44. ФИЛЬТР КАЛМАНА. СОБИРАЕМ ВСЕ ВМЕСТЕ 44
45. ФИЛЬТР КАЛМАНА Предсказание: Коррекция:
46. ФИЛЬТР КАЛМАНА. СОБИРАЕМ ВСЕ ВМЕСТЕ 46
47. ФИЛЬТР КАЛМАНА. СОБИРАЕМ ВСЕ ВМЕСТЕ 47
48. ЧТО ДЕЛАТЬ С НЕЛИНЕЙНОСТЬЮ? 48 Представим, что теперь мы хотим оценивать еще и ориентацию робота в пространстве: ❏ Это приведет к появлению тригонометрических функций в модели системы ❏ На выходе нелинейной системы больше не нормальное распределение ❏ Фильтр Калмана больше неприменим Решение: ❏ Локальная линеаризация моделей ❏ Сигма-точечная аппроксимация
49. ЧТО ДЕЛАТЬ С НЕЛИНЕЙНОСТЬЮ? 49 Источник: Thrun, Burgard, Fox Линейная система Нелинейная система VS
50. ЧТО ДЕЛАТЬ С НЕЛИНЕЙНОСТЬЮ? 50 Линейная система Нелинейная система VS
51. ЧТО ДЕЛАТЬ С НЕЛИНЕЙНОСТЬЮ? 51 Линейная система Нелинейная система VS Нелинейные функции
52. РАСШИРЕННЫЙ ФИЛЬТР КАЛМАНА (EXTENDED KALMAN FILTER) 52
53. РАСШИРЕННЫЙ ФИЛЬТР КАЛМАНА (EXTENDED KALMAN FILTER) 53 Матрицы Якоби
54. РАСШИРЕННЫЙ ФИЛЬТР КАЛМАНА (EXTENDED KALMAN FILTER) 54 Пусть задана вектор-функция: Тогда матрица Якоби определяется как: Задает ориентацию плоскости нормальной к векторной функции в заданной точке. Обобщает понятие градиента скалярной функции на многомерный случай.
55. РАСШИРЕННЫЙ ФИЛЬТР КАЛМАНА (EXTENDED KALMAN FILTER) 55 Матрицы Якоби
56. РАСШИРЕННЫЙ ФИЛЬТР КАЛМАНА (EXTENDED KALMAN FILTER) 56 Линейные функции!
57. РАСШИРЕННЫЙ ФИЛЬТР КАЛМАНА (EXTENDED KALMAN FILTER) 57
58. СИГМА-ТОЧЕЧНЫЙ ФИЛЬТР КАЛМАНА (UNSCENTED KALMAN FILTER) 58
59. СИГМА-ТОЧЕЧНЫЙ ФИЛЬТР КАЛМАНА (UNSCENTED KALMAN FILTER) 59
60. СИГМА-ТОЧЕЧНЫЙ ФИЛЬТР КАЛМАНА (UNSCENTED KALMAN FILTER) 60 Нелинейная система
61. СИГМА-ТОЧЕЧНЫЙ ФИЛЬТР КАЛМАНА (UNSCENTED KALMAN FILTER) 61 Нелинейная система
62. СИГМА-ТОЧЕЧНЫЙ ФИЛЬТР КАЛМАНА (UNSCENTED KALMAN FILTER) 62 Нелинейная система
63. 63 ФИЛЬТР ЧАСТИЦ (МНОГОЧАСТИЧНЫЙ ФИЛЬТР , ЛОКАЛИЗАЦИЯ МОНТЕ-КАРЛО) ❏ Работает с любыми (в т.ч. нелинейными) моделями движения и измерения (наблюдения) ❏ Может описывать мультимодальное распределение ❏ Может решать задачу глобальной локализации — локализации без начального приближения
64. ❏ Фильтр частиц позволяет оценить апостериорное распределение вектора состояния системы в скрытом Марковском процессе. ❏ x t — состояние Марковского процесса в момент времени t. ❏ x t зависит от предыдущего состояния x t-1 в соответствии с моделью движения p(x t |u t , x t−1 ), где u t — вектор управляющих сигналов, примененных в момент t − 1. ❏ x t описывается вектором измерения z t , генерируемым в соответствии с моделью измерения p(z t |x t ). ❏ Идея фильтра частиц — аппроксимация неизвестного апостериорного распределения набором гипотез {xn t } и соответствующих им весов {wn t } , где n = 1, ..., N – количество гипотез / частиц. ❏ В задаче локализации каждая частица соответствует позе робота: x t = (x t , y t , t )T. ФИЛЬТР ЧАСТИЦ 64
65. ФИЛЬТР ЧАСТИЦ 65
66. Motion model ФИЛЬТР ЧАСТИЦ 66
67. Measurement model ФИЛЬТР ЧАСТИЦ 67
68. Resampling Resampling ФИЛЬТР ЧАСТИЦ 68
69. Resampling Motion model Measurement model Resampling ФИЛЬТР ЧАСТИЦ 69
70. ФИЛЬТР ЧАСТИЦ МОДЕЛЬ ДВИЖЕНИЯ 70 В СЛЕДУЮЩЕЙ ЛЕКЦИИ
71. 71 ФИЛЬТР ЧАСТИЦ МОДЕЛЬ ИЗМЕРЕНИЯ В СЛЕДУЮЩЕЙ ЛЕКЦИИ
72. ФИЛЬТР ЧАСТИЦ РЕСЕМПЛИНГ частицы / гипотезы 72
73. 73 ФИЛЬТР ЧАСТИЦ РЕСЕМПЛИНГ
74. 74 ФИЛЬТР ЧАСТИЦ РЕСЕМПЛИНГ
75. 75 ФИЛЬТР ЧАСТИЦ РЕСЕМПЛИНГ
76. 76 ФИЛЬТР ЧАСТИЦ РЕСЕМПЛИНГ
77. 77 ФИЛЬТР ЧАСТИЦ РЕСЕМПЛИНГ
78. 78 ФИЛЬТР ЧАСТИЦ ПРИМЕР
79. ГЛОБАЛЬНАЯ ЛОКАЛИЗАЦИЯ И KIDNAPPED ROBOT PROBLEM 79
80. ЧТО ВЫБРАТЬ? Фильтр Калмана Фильтр частиц Тип шума Гауссов, одномодальный Любое распределение, мультимодальный Решение Оптимальное Приближение Реализация Сложная (необходим вывод физической модели) Простая Устойчивость Средняя Высокая Глобальная локализация Нет Да Скорость вычислений Быстрая Зависит от реализации (как правило, медленнее) 80
81. ЧТО ВЫБРАТЬ? Фильтр Калмана Фильтр частиц Тип шума Гауссов, одномодальный Любое распределение, мультимодальный Решение Оптимальное Приближенное Реализация Сложная (необходим вывод физической модели) Простая Устойчивость Средняя Высокая Глобальная локализация Нет Да Скорость вычислений Быстрая Зависит от реализации (как правило, медленнее) 81
82. Фильтр Калмана Фильтр частиц Тип шума Гауссов, одномодальный Любое распределение, мультимодальный Решение Оптимальное Приближенное Реализация Сложная (необходим вывод физической модели) Простая Устойчивость Средняя Высокая Глобальная локализация Нет Да Скорость вычислений Быстрая Зависит от реализации (как правило, медленнее) ЧТО ВЫБРАТЬ? 82
83. РАСШИРЕННЫЙ фильтр Калмана Фильтр частиц Тип шума Любое одномодальное Любое распределение, мультимодальный Решение Приближенное Приближенное Реализация Сложная (необходим вывод физической модели) Простая Устойчивость Средняя Высокая Глобальная локализация Нет Да Скорость вычислений Быстрая Зависит от реализации (как правило, медленнее) ЧТО ВЫБРАТЬ? 83
84. РАСШИРЕННЫЙ фильтр Калмана Фильтр частиц Тип шума Любое одномодальное Любое распределение, мультимодальный Решение Приближенное Приближенное Реализация Сложная (необходим вывод физической модели) Простая Устойчивость Средняя Высокая Глобальная локализация Нет Да Скорость вычислений Быстрая Зависит от реализации (как правило, медленнее) ЧТО ВЫБРАТЬ? 84
85. ДОПОЛНИТЕЛЬНЫЕ ИСТОЧНИКИ 85 1. How a Kalman filter works, in pictures 2. Probabilistic Robotics (в PIAZZA). Главы 1, 2, 3, 4, 7, 8. 3. Cyrill Stachniss. SLAM -Course
86. ИНФОРМАЦИЯ О ПРЕЗЕНТАЦИИ 86 Эта презентация была подготовлена Олегом Шипитько в рамках курса “Моделирование колесных роботов” кафедры когнитивных технологий Московского физико-технического института (МФТИ). Автор выражает благодарность, авторам, чьи материалы были использованы в презентации. В случае, если вы обнаружили в презентации свои материалы, свяжитесь со мной, для включения в список авторов заимствованных материалов. This presentation was prepared by Oleg Shipitko as part of the “Mobile Robotics” course at the Department of Cognitive Technologies, Moscow Institute of Physics and Technology. The author is grateful to the authors whose materials were used in the presentation. If you find your materials in a presentation, contact me to be included in the list of contributing authors.