�ݺ�ߣ

Алгоритмы беспилотного автомобиля.
Практическое занятие.
Андрей Антонов | 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-й ячейки
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) # сумма значений
# производим нормализацию
q[i] = q[i] / s
return 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=[]
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.8
0.2 × 0.1
0.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.6
pMiss = 0.2
9

pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1
motions = [1, 1] # последовательность перемещений: шаг вправо, шаг вправо
def sense(p, Z):
q=[]
hit=(Z==world[i])
q.append(p[i]*(hit * pHit + (1-hit) *pMiss))
s = sum(q)
q[i] = q[i] /s
return 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
푒−1
2
(푥−휇)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 + mean2
new_var = var1 + var2
return [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_size
self.y = random.random() * world_size
self.orientation = random.random() * 2.0 * pi
self.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;
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()
В результате выполнения программы должно получиться:
[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)
После каждого нового запуска программы мы будем получать различные зна-
чения.
Задание. Выполните программу несколько раз и посмотрите как будут изменять-
ся значения.
[31.576493398917687, 51.224390069507578, 36.539222219826833,
41.92636605763002]
[37.545542414158056, 47.024914625463332, 44.260875276378968,
39.864371439859546]
4.2 Пространство вокруг робота
Мы рассматриваем мир вокру робота как циклический, то есть доехав до право-
го края, робот появляется на следующем шаге слева. В окружающем пространстве
имеется четыре ориентира и робот может измерить расстояние до них.
Рис. 13: Мир робота
20

4.3 Фильтр частиц
Фильтр частиц работает с множеством возможных позиций, в которых может
находиться робот. Каждая из этих возможных точек представлена двумя линей-
ными координатами и углом ориентации.
Сформируем 1000 случайных позиций робота 푝:
N = 1000
p=[]
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.0
mw = max(w)
for i in range(N):
beta += random.random() * 2.0 * mw
while beta > w[index]:
beta -= w[index]
index = (index + 1) % N
p3.append(p[index])
p = p3
print 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_size
self.y = random.random() * world_size
self.orientation = random.random() * 2.0 * pi
self.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=[]
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;
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 = 1000
T = 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.0
mw = max(w)
for i in range(N):
beta += random.random() * 2.0 * mw
while beta > w[index]:
beta -= w[index]
index = (index + 1) % N
p3.append(p[index])
24

p = p3
5 Итог
В рамках практического занятия были реализованы на языке Python алго-
ритмы вероятностной локализации, использующие гистограмный фильтр, фильтр
Калмана и частичный фильтр.
25

�ݺ�ߣ

Программирование беспилотного автомобиля

More Related Content

Программирование беспилотного автомобиля