Алгоритмы беспилотного автомобиля.Практическое занятие.

Андрей Антонов | robotosha.ru

10.10.2014

1 Цель работыЗнакомство с алгоритмами вероятностной робототехники и их практическая

реализация на языке программирования Python.

2 Вероятностная локализация

2.1 Равномерное распределение

Рассмотрим ситуацию. Пусть мы имеем одномерное пространство, определя-ющее местоположение робота. Каждую возможную позицию изобразим в видеячейки. Для простоты, предположим, что у нас есть 5 возможных позиций.

Рис. 1: Возможные местоположения робота

Будем считать, что вероятности нахождения робота в любой из ячеек 𝑥𝑖 равны.В этом случае мы имеем дело с равномерным распределением. Этот вид распреде-ления отражает максимальную неопределенность.

Полная вероятность равна 1, поэтому вероятность каждой позиции, для нашегоробота в случае равномерного распределения:

𝑝(𝑥𝑖) =1

5= 0.2

На языке Python это распределение можно задать следующим образом:

1

Рис. 2: Равномерное распределение

> p=[0.2, 0.2, 0.2, 0.2, 0.2]> print p

Мы просто задали вектор значений для вероятностей. В общем случае, для про-извольного числа элеменов, это можно сделать следующим образом:

p = [] # пустой списокn = 5 # число элементов

for i in range(n):p.append(1./n) # точка после единицы важна!

print p # отображаем результат на экране

2.2 Измерения

Теперь предположим, что ячейки, в которых может находится наш робот двухтипов - красные и зеленые. Как и ранее, мы задаемся равномерным распределе-нием. Робот имеет датчик цвета и может определить в ячейке какого цвета оннаходится. Предположим, что он определил, что цвет красный. Как это повляетна начальное предположение о местоположении?

Рис. 3: Ячейки различных цветов. Робот определяет, что находится в красной.

2

Очевидно, что для красных ячеек 𝑥2 и 𝑥3 вероятность должна вырасти, дляячеек 𝑥1, 𝑥4 и 𝑥5 - уменьшится. Для того, чтобы внести измерение в исходноепредположение, мы применяем умножение. Для ячеек, где цвет совпадает и изме-ренным, мы умножим на относительно большое число, скажем на 0.6, где отлича-ется - умножим на относительно маленькое число 0.2. Таким образом, все зеленыеячейки мы умножим на 0.2, красные - на 0.6. Наши множители отличаются в трираза ( 0.6

0.2= 3), то есть у нас в 3 раза больше шансов находится в красной ячейке,

чем в зеленой.После умножения, наше предположение будет выглядеть следующим образом:

Рис. 4: Предположение о местонахождении после проведения измерения

Сумма всех полученных значений 0.04×3+0.12×2 = 0.36. Для того, чтобы по-лучить полную вероятность равную единице, необходимо произвести нормировку.Для этого разделим каждое из значений на их сумму (0.36). В итоге получим:

Рис. 5: Нормализация

Каждую из этих вероятностей записывают следующим образом:

𝑝(𝑥𝑖|𝑍)

Эта вероятность называется апостериорной вероятностью нахождения в ячейке𝑥𝑖, после (или, при условии) получения измерения 𝑍.

Давайте реализуем это на языке Python.

p=[0.2, 0.2, 0.2, 0.2, 0.2] # априорная вероятностьpHit = 0.6 # ячейка краснаяpMiss = 0.2 # ячейка зеленая

p[0]=p[0]*pMiss # апостериорная вероятность для 1-й ячейкиp[1]=p[1]*pHit # апостериорная вероятность для 2-й ячейкиp[2]=p[2]*pHit # апостериорная вероятность для 3-й ячейкиp[3]=p[3]*pMiss # апостериорная вероятность для 4-й ячейкиp[4]=p[4]*pMiss # апостериорная вероятность для 5-й ячейки

print p

3

Мы получили ненормализованные значения.Для получения суммы, мы можем воспользоваться командой:

print sum(p)

Запишем более элегантное решение:

p=[0.2, 0.2, 0.2, 0.2, 0.2] # априорная вероятность для каждой ячейкиworld=[’green’, ’red’, ’red’, ’green’, ’green’] # цвета ячеекZ = ’red’ # полученное измерениеpHit=0.6 # множитель для ячеек, совпадающих с измерениемpMiss=0.2 # множитель для ячеек, не совпадающих с измерением

# функция, вычисляющая нормализованное# апостериорное распределениеdef sense(p, Z):

q=[]# перебираем все ячейки, определяем совпадает ли цвет# с измеренным и вычисляем вероятностьfor i in range(len(p)):

hit=(Z==world[i])q.append(p[i]*(hit * pHit + (1-hit) *pMiss))

s = sum(q) # сумма значений# производим нормализациюfor i in range(len(p)):

q[i] = q[i] / sreturn q

print sense(p,Z) # выводим результат

Рис. 6: Уменьшение неопределенности, используя измерение

4

Давайте посмотрим, что мы сделали. У нас было равномерное распределение,обладающее максимальной неопределенностью. Произведя измерение, мы мы ис-пользовали его для снижения неопределенности. В этом состоит основная идеялокализации.

Задание: поменяйте измеренное значение с red на green и посмотрите как из-менится результат.

Теперь, давайте произведем серию измерений. Для этого, заменим переменнуюZ, хранящую наше измерение на вектор measurements:

measurements=[’red’, ’green’]

и вместо оператора print sense(p,Z) запишем:

for k in range(len(measurements)):p = sense(p, measurements[k])

print p

Выполнив, получившийся код, мы увидим, что в результате опять получаемравномерное распределение. Происходит это из-за того, что каждую ячейку мыумножаем на 0.6, а затем на 0.2. Домножение на константу не меняет вида рас-пределения.

2.3 Точное движение робота

Опять вернемся к распределению, показанному на Рис. 5. Допустим, мы не зна-ем где находится робот, но знаем, что он движется слева-направо. Предположим,что мир является циклическим, то есть, находясь в крайнем правом положении идвигаясь направо, робот попадает в начало.

В этом случае, все наши значения вероятностей при сдвиге робота вправо наодин шаг, также сдвигаются вправо, а крайнее правое значение становится первым.

Это проиллюстрировано ниже.

Рис. 7: Определенное движение робота направо в циклическом мире

Реализуем это в коде на Python.

5

# Функция движения - возвращает вероятности после перемещения# p - исходная вероятность# U - кол-во ячеек, на которое робот перемещается влево или вправо# отрицательные значения U - движение влевоdef move(p, U):

q=[]for i in range(len(p)):

q.append(p[(i-U) % len(p)])return q

Задание: Закоментируйте строки, в которых мы производили измерения:

# for k in range(len(measurements)):# p = sense(p, measurements[k])## print p

Измените априорную вероятность на очень простую:

p=[0, 1, 0, 0, 0]

и реализуйте движение робота:

print move(p, 1)

2.4 Неточное движение робота

Пусть робот совершает свое перемещение с высокой вероятностью точно, ска-жем, пусть эта величина равна 0.8, с вероятностью 0.1 проскакивает требуемуюячейку и с вероятностью 0.1 не доезжает до нее. Ниже приведены примеры пере-мещения робота на 2 ячейки (U=2) и на одну ячейку (U=1).

Рис. 8: Неточное движение робота

6

Наш робот старается двигаться точно, но иногда он проскакивает свою цель,иногда не доезжает до нее. Эта ситуация является примером реального движенияробота, которое всегда неопределенно. Учет этой неопределенности является оченьважным и именно наличие неопределенности делает задачу локализации трудной.

Представим это математически.

Для случая U=2:𝑝(𝑋𝑖+2|𝑋𝑖) = 0.8

𝑝(𝑋𝑖+1|𝑋𝑖) = 0.1

𝑝(𝑋𝑖+3|𝑋𝑖) = 0.1

Если априорное распределение выглядело:

Тогда, после перемещения оно станет:

Рассмотрим более сложный случай. Пусть наше начальное распределение:

Запишем вероятности для движения из двух возможных ячеек, учитывая, чтомы рассматриваем «циклический» мир:

Мы видим, что в пятой ячейке у нас есть вероятность очутиться двумя раз-личными способами: «недоехав» до цели из четвертой ячейки, или «проскочив»цель, начав движение из второй. Общая вероятность для пятой ячейки являетсясуммой этих значений: 0.05 + 0.05 = 0.1.

Рассмотрим теперь случай равномерного распределения априорной вероятно-сти.

7

Возьмем четвертую ячейку. В нее мы можем попасть тремя различными возмож-ными путями: точно переместившись на две ячейки из второй, «недоехав» из тре-тьей, или «переехав», двигаясь из первой. Возможные варианты вероятностей:0.2× 0.80.2× 0.10.2× 0.1Просуммировав эти значения, мы опять получим общую вероятность равную 0.2.Аналогично и для других ячеек. То есть движение не изменит равномерное рас-пределение вероятностей.

Реализуем теперь неточное движение на Python. Программный код будет вы-глядеть следующим образом:

p = [0, 1, 0, 0, 0]pExact = 0.8 # вероятность точного перемещенияpOvershoot = 0.1 # вероятность "перелета" при движенииpUndershoot = 0.1 # вероятность "недолета" при движении

def move(p,U):q = []for i in range (len(p)):

s = pExact * p[(i-U) % len(p)]s = s + pOvershoot * p[(i-U-1) % len(p)]s = s + pUndershoot * p[(i-U+1) % len(p)]q.append(s)

print move(p, 2)

Обратите внимание на изменения в функции move()Выполнив этот код, мы получим результат:

[0.0, 0.0, 0.1, 0.8, 0.1]

2.5 Непрерывное движение

Пусть начальное распределение вероятностей выглядит следующим образом:

Мир опять же циклический и робот начинает постоянное движение (постоянно пе-ремещается, U = 1). В этом случае, распределение будет стремиться к предельному(стационарному) распределению, являющемуся равномерным распределением.

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

Попробуйте реализовать движение дважды и посмотреть на результирующеераспределение:

8

p=move(p,1)p=move(p,1)print p

Посмотрите чему равно максимальное значение вероятности.Теперь реализуйте 1000 шагов и посмотрите на результат.

for k in range(1000):p = move(p,1)

print p

Должно получиться равномерное распределение.

2.6 Движение и измерение

Объединим теперь движение с измерением.

Рис. 9: Циклический процесс движение-измерения

Каждый раз, двигаясь робот теряет информацию о своем местонахождении, по-тому что его движение неточно. Когда же он производит измерения собственнымидатчиками, робот получает информацию. После того как движение произведено,распределение вероятностей становится немного более плоским и «размазанным»(увеличивается дисперсия). После же проведения измерений, распределение сужа-ется и становится более высоким.

Существует информационный показатель - энтропия. Один из вариантов еематематической формулировки:

−∑︁

𝑝(𝑋𝑖) log 𝑝(𝑋𝑖)

Не вдаваясь в детали, энтропия показывает каким количеством информации об-ладает распределение. При обновлении движения энтропия уменьшается, при об-новлении измерения энтропия, напротив растет.

Реализуем цикл измерение-движение в коде на Python:

p = [0.2, 0.2, 0.2, 0.2, 0.2] # равномерное распределениеworld = [’green’, ’red’, ’red’, ’green’, ’green’]measurements = [’red’, ’green’]pHit = 0.6pMiss = 0.2

9

pExact = 0.8pOvershoot = 0.1pUndershoot = 0.1motions = [1, 1] # последовательность перемещений: шаг вправо, шаг вправо

def sense(p, Z):q=[]for i in range(len(p)):

hit=(Z==world[i])q.append(p[i]*(hit * pHit + (1-hit) *pMiss))

s = sum(q)for i in range(len(p)):

q[i] = q[i] /sreturn q

def move(p,U):q = []for i in range (len(p)):

s = pExact * p[(i-U) % len(p)]s = s + pOvershoot * p[(i-U-1) % len(p)]s = s + pUndershoot * p[(i-U+1) % len(p)]q.append(s)

for k in range(len(measurements)):p = sense(p, measurements[k])p = move(p, motions[k])

print p

Результатом работы программы являются значения:

[0.21157894736842103, 0.1515789473684211, 0.08105263157894739,0.16842105263157897, 0.3873684210526316]

Если посмотреть на измеренные значения red, green, то можно сделать вывод, чторобот начал свое движение, находясь в третьей ячейке и сделал два перемещения,оказавшись в пятой ячейке. Это отлично согласуется с полученным результатом— наибольшее значение вероятности у пятой ячейки (0.3873684210526316).

Задание: измените значение измерений на [’red’, ’red’]. Робот видит красный,движется, видит красный и вновь перемещается. Выполните программу. Результатпоказывает, что робот, скорее всего, находится в четвертой ячейке.

[0.07882352941176471, 0.07529411764705884, 0.22470588235294123,0.43294117647058822, 0.18823529411764706]

Это опять же отлично согласуется с интуитивной проверкой.

10

2.7 Как это работает в беспилотном автомобиле

Не намного более сложный алгоритм реализован в беспилотных автомобиляхдля решения задачи локализации. Хотя дорожное покрытие и не имеет красных изеленых отметок, однако имеются линии дорожной разметки, разделяющие поло-сы движения и отделяющие обочину. Изображение, получаемое с камеры в реаль-ном времени, сравнивается с заранее записанными кадрами дорожного полотна,тем самым определяется местоположение автомобиля на дороге.

В общих чертах, процесс локализации выглядит следующим образом:

∙ Пространство разбивается на возможные местоположения (количество и раз-мер позиций зависит от требуемой точности). Предположение о том, что ав-томобиль находится в данной точке выражается вероятностью.

∙ Производится измерение и точность полученного измерения (также выра-женная вероятностью) умножается на априорную вероятностью. Посколь-ку после умножения сумма всех вероятностей может не равнятся единице -производится нормализация.

∙ Движение математически выражается сверткой. Это означает, что мы бе-рем все возможные вероятности нахождения в данном местоположении искладываем их.

То есть, фактически, задача локализации решается операциями умножения и сло-жения вероятностей и это является основой автономного движения.

2.8 Формальное определение локализации

Пусть мы имеем функцию вероятности

0 6 𝑃 (𝑋) 6 1

которая определена для множества значений 𝑋.Для простоты предположим, что у нас есть всего два значения 𝑋1 и 𝑋2. Если

вероятность 𝑃 (𝑋1) = 0.2, то вероятность 𝑃 (𝑋2) = 1− 0.2 = 0.8, т.к.∑︁𝑃 (𝑋𝑖) = 1

Математическое выражение измерения приводит нас к правилу Байеса. Это фун-даментальное выражение в вероятностом выводе, которое, в действительности,достаточно простое.

Пусть 𝑋 - ячейка, 𝑍 - измерение. Тогда обновление измерения вычисляет функ-цию правдоподобия для ячейки при поступлении измерения:

𝑃 (𝑋𝑖|𝑍) =𝑃 (𝑍|𝑋𝑖)𝑃 (𝑋𝑖)

𝑃 (𝑍)

𝑃 (𝑋𝑖|𝑍) - апостериорное распределение 𝑃 (𝑋𝑖) - априорное распределение (изна-чальная вероятность нахождения в той или иной ячейке)𝑃 (𝑍|𝑋𝑖) - правдоподобие (вероятность измерения, шанс встретить красный или

11

зеленый цвет для каждой возможной позиции)𝑃 (𝑍) - вероятность получения измерения (без какой-либо информации о местона-хождении)

𝑃 (𝑍) в знаменателе выполняет функцию нормализации

𝑃 (𝑍) =∑︁

𝑃 (𝑍|𝑋𝑖)𝑃 (𝑋𝑖)

Выразим теперь полную вероятность для движения робота для момента вре-мени 𝑡.

𝑃 (𝑋 𝑡𝑖 ) =

∑︁𝑗

𝑃 (𝑋 𝑡−1𝑖 )|𝑃 (𝑋𝑖|𝑋𝑗)

Эту формулу в учебниках часто можно встретить в виде:

𝑃 (𝐴) =∑︁𝐵

𝑃 (𝐴|𝐵)𝑃 (𝐵)

Данное выражение называют теоремой полной вероятности. Операцию взве-шенного суммирования переменных, называют также сверткой.

Это лежит в основе вероятностных методов, которые обычно называют «филь-тры». Далее мы рассмотрим алгоритмы частичного фильтра и фильтра Калмана,которые используются в беспилотных автомобилях для поиска других автомоби-лей и предсказания их местоположения.

3 Фильтр КалманаФильтр Калмана является методом оценки состояния системы. Он очень похож

на локализацию Монте-Карло, которую мы рассмотрели выше. Основное отличиев том, что фильтр Калмана оценивает непрерывное состояние, в то время как влокализации Монте-Карло мы разбивали окружающий мир на отдельные состав-ляющие.

Фильтр Калмана дает унимодальное распределение, метод Монте-Карло даетмультимодальное распределение.

Оба эти метода применимы к задачам локализации робота и отслеживаниютранспортных средств. Еще одним способом решения этих же задач являютсячастичный фильтр (непрерывный и мультимодальный).

Фильтр Калмана позволяет оценить местоположение в будущем, используяданные о местоположениях объекта в прошлом и настоящем. В беспилотных ав-томобилях этот метод используется для оценки траектории движения различныхобъектов на основе радиолокационных данных и данных, полученных с лазерныхдальномеров.

3.1 Нормальное распределение

Фильтр Калмана дает нормальное распределение вероятностей. Гауссово (нор-мальное) распределение определяется функцией

12

Рис. 10: Распределение Гаусса

𝑓(𝑥) =1√2𝜋𝜎2

𝑒−12

(𝑥−𝜇)2

𝜎2

где 𝜇 - среднее (математическое ожидание), 𝜎2 - дисперсияРаспределение Гаусса является унимодальным (один пик), симметричным рас-

пределением. Дисперсия является мерой неопределенности. Чем больше дисперсия- тем больше неопределенность.

Реализуем функцию, описывающую нормальное распределение на языке Python:

# подключаем математическую библиотекуfrom math import *

def f(mu, sigma2, x):return 1/sqrt(2.*pi*sigma2) * exp(-.5 * (x-mu)**2 / sigma2)

Проверим результат ее работы:

print f(10., 4., 8.)

У вас должно получиться 0.12098536226.

3.2 Измерение и движение

Напомним, что после измерения использовалось произведение вероятностей(правило Байеса), обновление движения использовало свертку (полная вероят-ность).

Процесс оценки, используя фильтр Калмана также состоит из двух этапов:

∙ Обновление измерения

∙ Предсказание

13

Рис. 11: Фильтр Калмана

3.2.1 Шаг обновления измерения

Предположим, что у нас есть априорное распределение, описывающее веро-ятное местонахождение другого автомобиля имеющее достаточно большую дис-персию, то есть высокую степень неопределенности (черная кривая на рисункениже). Среднее для этого распределения 𝜇. Мы получаем измерение (синяя кри-вая), имеющее небольшую дисперсию и среднее 𝜈, которое приносит нам какую-тоинформацию о локализации автомобиля.

Рис. 12: Распределения вероятностей

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

Предположим, что априорное распределение имело среднее 𝜇 и дисперсию 𝜎2,вероятность измерения среднюю 𝜈 и дисперсию 𝑟2. Тогда новое распределениепосле обновления будет иметь среднее

𝜇′ =𝑟2𝜇+ 𝜎2𝜈

𝑟2 + 𝜎2

и дисперсию

𝜎′2 =1

1𝑟2

+ 1𝜎2

Новое среднее будет ближе к 𝜈, чем к 𝜇Реализуем это на Python:

14

def update(mean1, var1, mean2, var2):new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2)new_var = 1 / (1/var1 + 1/var2)return [new_mean, new_var]

Проверим результат, выполнив команду:

print update(10., 8., 13., 2.)

Результатом должен быть вектор [12.4, 1.6].

3.2.2 Шаг предсказания

Шаг предсказания также называют обновление движения. Предположим, чторобот совершает перемещение на расстояние 𝑢. Перемещение не точно и имеетдисперсию 𝑟′2 Новые параметры распределения вычисляются на этом шаге следу-ющим образом:

𝜇′ = 𝜇+ 𝑢

𝜎′2 = 𝜎2 + 𝑟2

Реализация этого шага на Python:

def predict(mean1, var1, mean2, var2):new_mean = mean1 + mean2new_var = var1 + var2return [new_mean, new_var]

Для проверки, выполняем:

print predict(10., 4., 12., 4.)

В результате должно получиться [22.0, 8.0].Теперь запишем основную программу, которая берет функции update() и predict()

и реализует последовательность перемещений и измерений.

measurements = [5., 6., 7., 9., 10.] # последовательность измеренийmotion = [1., 1., 2., 1., 1.] # последовательность перемещенийmeasurement_sig = 4. # дисперсия измеренияmotion_sig = 2. # неопределенность (дисперсия) движенияmu = 0. # среднее априорного распределенияsig = 10000. # большая начальная неопределенность

for n in range(len(measurements)):[mu, sig] = update(mu, sig, measurements[n], measurement_sig)print ’update: ’, [mu, sig][mu, sig] = predict(mu, sig, motion[n], motion_sig)print ’predict: ’, [mu, sig]

В результате запуска кода, должно получиться следующее:

15

update: [4.998000799680128, 3.9984006397441023]predict: [5.998000799680128, 5.9984006397441023]update: [5.999200191953932, 2.399744061425258]predict: [6.999200191953932, 4.399744061425258]update: [6.999619127420922, 2.0951800575117594]predict:[8.999619127420921, 4.09518005751176]update: [8.99811802788143, 2.0235152416216957]predict: [9.99811802788143, 4.0235152416216957]update: [9.999906177177365, 2.0058615808441944]predict: [10.999906177177365, 4.005861580844194]

Этот код реализует алгоритм одномерного фильтра Калмана.

Задание: установите очень маленькую величину начальной неопределенности(sig = 0.0000000001) и посмотрите как изменится результат.

Для реализации многомерного случая, необходимо перейти к многомернымгауссианам.

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

∙ наблюдаемые состояния

∙ скрытые состояния

В нашем случае наблюдаемым состоянием является координата (которая из-меряется дальномерами), скрытым состоянием является скорость. Из-за того, чтоэти переменные взаимосвязаны, последующие наблюдения наблюдаемых перемен-ных дают информацию о скрытых параметрах, поэтому мы можем оценить этискрытые переменные. Так, наблюдая за объектом в нескольких точках, мы можемоценить насколько быстро он движется.

Для многомерного случая уравнения фильтра Калмана записываются в мат-ричной форме:

𝑋 - матрица оцененных значений𝑃 - ковариационная матрица неопределенности𝐹 - матрица перехода из одного состояния в другое𝑈 - вектор движения𝑍 - значения измерений𝐻 - функция измерения (распределение)𝑅 - шум измерения𝐾 - усиление Калмана𝐼 - единичная матрица

Уравнения предсказания запишутся:

𝑋 ′ = 𝐹𝑋 + 𝑈

𝑃 ′ = 𝐹 · 𝑃 · 𝐹 𝑇

16

Шаг обновления измерения:

𝑌 = 𝑍 −𝐻 ·𝑋

𝑆 = 𝐻 · 𝑃 ·𝐻𝑇 +𝑅

𝐾 = 𝑃 ·𝐻𝑇 · 𝑆−1

Обновляем оценку и неопределенность:

𝑋 ′ = 𝑋 + (𝐾 · 𝑌 )

𝑃 = (𝐼 −𝐾 ·𝐻) · 𝑃

Более простым в реализации и очень мощным является алгоритм частичногофильтра.

4 Частичный фильтрЕсли мы изучим вычислительную эффективность уже рассмотренных алгорит-

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

∙ работает с непрерывным пространством состояний;

∙ допускает мультимодальные распределения;

∙ как и другие фильтры, дает приближенную оценку, а не точное значение;

∙ в некоторых областях (слежение) может быть достаточно эффективным сточки зрения вычислительных затрат;

∙ легко реализуем.

Запишем часть кода для класса robot на Python:

from math import *import random

landmarks=[[20.0, 20.0],[80.0, 80.0],[20.0, 80.0],[80.0, 20.0]]

world_size = 100.0

class robot:

17

def __init__(self):self.x = random.random() * world_sizeself.y = random.random() * world_sizeself.orientation = random.random() * 2.0 * piself.forward_noise = 0.0;self.turn_noise = 0.0;self.sense_noise = 0.0;

def set(self, new_x, new_y, new_orientation):if new_x < 0 or new_x >= world_size:

raise ValueError, ‘X coordinate out of bound’if new_y < 0 or new_y >= world_size:

raise ValueError, ‘Y coordinate out of bound’if new_orientation <0 or new_orientation >= 2 * pi:

raise ValueError, ‘Orientation must be in [0..2pi]’self.x = float(new_x)self.y = float(new_y)self.orientation = float(new_orientation)

# позволяет изменить параметры шума# часто полезно для фильтров частиц

def set_noise(self, new_f_noise, new_t_noise, new_s_noise):self.forward_noise = float(new_f_noise);self.turn_noise = float(new_t_noise);self.sense_noise = float(new_s_noise);

def sense(self):Z=[]for i in range(len(landmarks)):

dist = sqrt((self.x - landmarks[i][0]) ** 2 +(self.y - landmarks[i][1]))

dist += random.gauss(0.0, self.sense_noise)Z.append(dist)

return Z

def move(self, turn, forward):if forward < 0:

raise ValueError, ‘Robot cant move backwards’

# поворачиваемся и добавляем случайность к команде поворотаorientation = self.orientation + float(turn) +random.gauss(0.0, self.turn_noise)

orientation %= 2*pi

# перемещаемся и добавляем случайность в команду движения

18

dist = float(forward) + random.gauss(0.0, self.forward_noise)x = self.x + (cos(orientation)*dist)y = self.y + (sin(orientation)*dist)

x %= world_size # округлениеy %= world_size

# устанавливаем частицуres = robot()res.set(x, y, orientation)res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)return res

def Gaussian(self, mu, sigma, x):# Вычисление одномерного нормального распределенияreturn (1.0 / sqrt(2.0 *pi*sigma**2))) * exp(-((mu-x) ** 2 /(sigma**2)))

def measurement_prob(self, measurement):prob = 1.0;for i in range(len(landmarks)):

dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y -landmarks[i][1]) ** 2)

prob *= self.Gaussian(dist, self.sense_noise, measurement[i])return prob

def __repr__(self):return ’[x= %.6s y= %.6s heading= %.6s]’ % (str(self.x),str(self.y), str(self.theta))

4.1 Движение

Попробуем запустить робота, который начинает движение в точке с коорди-натами (30, 50), повернувшись на север (угол 𝜋

2). Затем он вращается по часовой

стрелке на 𝜋2, перемещается на 15 метров После этого робот производит измерения

и отображает результат. Затем поворачивается на 𝜋2

и перемещается на 10 метров.Отображает показания датчика.

myrobot=robot()myrobot.set(30.0, 50.0, pi/2)myrobot=myrobot.move(-pi/2, 15.0)print myrobot.sense()myrobot=myrobot.move(-pi/2, 10.0)print myrobot.sense()

В результате выполнения программы должно получиться:

[39.05124837953327, 46.097722286464439, 39.05124837953327,46.097722286464439]

19

[32.015621187164243, 53.150729063673246, 47.169905660283021,40.311288741492746]

Теперь попробуем изменить шумовые параметры. Установим шум движениявперед равным 5.0, шум поворота равным 0.1 и шум сенсора 5.0. Повторим вседвижения, что и в предыдущем примере.

myrobot=robot()myrobot.set_noise(5.0, 0.1, 5.0)myrobot.set(30.0, 50.0, pi/2)myrobot=myrobot.move(-pi/2, 15.0)print myrobot.sense()myrobot=myrobot.move(-pi/2, 10.0)print myrobot.sense()

После каждого нового запуска программы мы будем получать различные зна-чения.Задание. Выполните программу несколько раз и посмотрите как будут изменять-ся значения.

[31.576493398917687, 51.224390069507578, 36.539222219826833,41.92636605763002][37.545542414158056, 47.024914625463332, 44.260875276378968,39.864371439859546]

4.2 Пространство вокруг робота

Мы рассматриваем мир вокру робота как циклический, то есть доехав до право-го края, робот появляется на следующем шаге слева. В окружающем пространствеимеется четыре ориентира и робот может измерить расстояние до них.

Рис. 13: Мир робота

20

4.3 Фильтр частиц

Фильтр частиц работает с множеством возможных позиций, в которых можетнаходиться робот. Каждая из этих возможных точек представлена двумя линей-ными координатами и углом ориентации.

Сформируем 1000 случайных позиций робота 𝑝:

N = 1000p=[]for i in range[N]:

x = robot()p.append(x)

Для каждой из этих частиц произведем движение: повернемся на 0.1 и пере-местимся на 5:

p2 = []for i in range(N):

p2.append(p[i].move(0.1, 5.0))p = p2

Пусть робот находится в позиции (𝑥1, 𝑦1, 𝜃1). Робот измеряет расстояния доориентиров. Результатом является вектор, состоящий из четырех значений [L1,L2, L3, L4]. Взяв этот вектор мы используем его в расчете апостериорной вероят-ности для другой точки, которая находится в другой позиции (𝑥2, 𝑦2, 𝑡ℎ𝑒𝑡𝑎2). Затеммы производим измерения уже из этой позиции и получаем какие-то значения рас-стояний до ориентиров. Если измеренные значения близки к значениям, получен-ным в точке (𝑥1, 𝑦1, 𝜃1), то частице с координатами (𝑥2, 𝑦2, 𝑡ℎ𝑒𝑡𝑎2) присваиваетсябольший вес, если корреляция слабая - меньший вес (так как это местоположениемаловероятно). Такая операция проводится по всем частицам.

Следующей операцией является повторная выборка. В процессе повторной вы-борки остаются только частицы, имеющие наибольшие веса (имеющие наиболь-шую корреляцию с измерением, полученным с датчика). В результате этого оста-ется кластер частиц, в которых находится робот с наибольшей вероятностью.

Код, который реализует перемещение и производит измерение:

myrobot = robot()myrobot = myrobot.move(0.1, 5.0)Z = myrobot.sense()print Z # результат измеренияprint myrobot # веса важности для данной частицы

Присваиваем веса каждой частице:

w = []for i in range(N):

w.append(p[i].measurement_prob(Z))print w

21

Как видно, большинство частиц имеют очень маленькие значения весов. Нор-мализовав веса, производится повторная выборка частиц случайным образом, приэтом остаются только имеющие наибольшие нормализованные веса.

Реализация на Python:

p3 = []index = int(random.random() * N)beta = 0.0mw = max(w)for i in range(N):

beta += random.random() * 2.0 * mwwhile beta > w[index]:

beta -= w[index]index = (index + 1) % N

p3.append(p[index])p = p3print p

Напомним, что ориентация частиц (угол 𝜃) не имеет значения для вероятности.Полный код программы, реализующей фильтр частиц:

from math import *import random

landmarks=[[20.0, 20.0,[80.0, 80.0],[20.0, 80.0],[80.0, 20.0]]

world_size = 100.0

class robot:def __init(self):

self.x = random.random() * world_sizeself.y = random.random() * world_sizeself.orientation = random.random() * 2.0 * piself.forward_noise = 0.0;self.turn_noise = 0.0;self.sense_noise = 0.0;

def set(self, new_x, new_y, new_orientation):if new_x < 0 or new_x >= world_size:

raise ValueError, ‘X coordinate out of bound’if new_y < 0 or new_y >= world_size:

raise ValueError, ‘Y coordinate out of bound’if new_orientation <0 or new_orientation >= 2 * pi:

raise ValueError, ‘Orientation must be in [0..2pi]’self.x = float(new_x)self.y = float(new_y)

22

self.orientation = float(new_orientation)

def set_noise(self, new_f_noise, new_t_noise, new_s_noise):# позволяет изменить параметры шума# часто полезно для фильтров частицself.forward_noise = float(new_f_noise);self.turn_noise = float(new_t_noise);self.sense_noise = float(new_s_noise);

def sense(self):Z=[]for i in range(len(landmarks)):

dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y -landmarks[i][1]))

dist += random.gauss(0.0, self.sense_noise)Z.append(dist)

return Z

def move(self, turn, forward):if forward < 0:

raise ValueError, ‘Robot cant move backwards’

# поворачиваемся и добавляем случайность к команде поворотаorientation = self.orientation + float(turn) + random.gauss(0.0,self.turn_noise)

orientation %= 2*pi

# перемещаемся и добавляем случайность в команду движения

dist = float(forward) + random.gauss(0.0, self.forward_noise)x = self.x + (cos(orientation)*dist)y = self.y + (sin(orientation)*dist)x %= world_size # округлениеy %= world_size

# устанавливаем частицуres = robot()res.set(x, y, orientation)res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)return res

def Gaussian(self, mu, sigma, x):# Вычисление одномерного нормального распределенияreturn (1.0 / sqrt(2.0 *pi*sigma**2))) * exp(-((mu-x) ** 2 /(sigma**2)))

23

def measurement_prob(self, measurement):prob = 1.0;for i in range(len(landmarks)):

dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y -landmarks[i][1]) ** 2)

prob *= self.Gaussian(dist, self.sense_noise, measurement[i])return prob

def __repr__(self)return ’[x= %.6s y= %.6s heading= %.6s]’ % (str(self.x),str(self.y), str(self.theta))

N = 1000T = 10

myrobot=robot()

p=[]for i in range[N]:

r = robot()r.set_noise(0.05, 0.05, 5.0)p.append(r)

for t in range(T):myrobot=myrobot.move(0.1, 5.0)

Z = myrobot.sense()

p2=[]for i in range(N):

p2.append(p[i].move(0.1, 5.0))p=p2

w=[]for i in range(N):

w.append(p[i].measurement_prob(Z))

p3 = []index = int(random.random() * N)beta = 0.0mw = max(w)for i in range(N):

beta += random.random() * 2.0 * mwwhile beta > w[index]:

beta -= w[index]index = (index + 1) % N

p3.append(p[index])

24

p = p3

5 ИтогВ рамках практического занятия были реализованы на языке Python алго-

ритмы вероятностной локализации, использующие гистограмный фильтр, фильтрКалмана и частичный фильтр.

25