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

Оптимальная фильтрация

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

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

Фильтр частиц является одним из самых популярных методов оптимальной фильтрации. Этот метод использовался на автономном автомобиле Stanford Junior, который занял второе место на DARPA Challenge в 2007 году.

Фильтр частиц позволяет получить оценку (приближенное значение) параметров системы или объекта (обозначим их как параметры А) которые нельзя измерить напрямую. Для построения этой оценки фильтр использует измерения других параметров (параметры Б) связанных с первыми. Покажем это на схеме:



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

Таким образом из множества гипотез в конце концов останутся только те, которые наиболее близки к истинному значению параметров А.

В данной статье рассмотрен один из простейших вариантов применения фильтра частиц. Робот движется в двумерном пространстве и может измерять дальность до определенных объектов в этом пространстве (ориентиров). Задача — определить местоположение (координаты) робота.

При этом робот может совершать развороты с точностью до ±5 градусов (погрешность разворота), перемещаться с точностью до ±20 метров (погрешность движения) и измерять дальность до ориентиров с точностью до ±15 метров.

Алгоритм фильтра частиц разделим на две части: инициализация и основной цикл фильтрации.

Инициализация

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

Основной параметр фильтра частиц — число этих самых частиц (обозначим это число N). Тут все просто: чем больше частиц — тем точнее фильтр, и тем больше вычислений нужно проводить на каждой итерации основного цикла.

Начальное распределение частиц зависит от априорной информации. Например, если нам известно, что робот находится в клетке с координатами (0, 0), то все частицы должны быть случайным образом распределены внутри этой клетки.


(Нажмите на картинку для увеличения.)

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

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



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

При этом, не зависимо от наличия априорной информации, вес всех частиц в начальный момент времени должен быть одинаковым и равным 1/N.

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

Если у вас есть карта области, то на этой карте обозначены ориентиры относительно которых мы будем проводить измерения. Эти ориентиры так же нужно добавить в фильтр частиц. (Возможно применение фильтра без известной карты области и расположения ориентиров, но это уже тема для другой статьи.)

В рассматриваемом примере используются четыре ориентира, расположенных по углам карты (на рисунках они не обозначены).

Основной цикл фильтрации

Основной цикл фильтрации разделен на три фазы:

Движение (Motion update)

На этом этапе робот совершает движение и, так как движение робота происходит с погрешностями, теряет информацию о своем местоположении.

Объясню про потерю информации:

Допустим вы стоите на месте и точно знаете свои географические координаты. Затем вы делаете 100 шагов вперед. Так как вы не робот, и не можете сделать 100 шагов одинаковой длины, то вы не можете определить дистанцию которую вы пройдете за эти 100 шагов (впрочем, даже если бы вы были роботом, у вас все равно была бы погрешность движения). Поэтому когда вы снова остановитесь вы будете знать свои координаты лишь приблизительно.

Таким образом, совершив движение, вы перешли от «Я точно знаю где я нахожусь» к «Я примерно знаю где я нахожусь» — то есть потеряли информацию.

Так как каждая частица представляет собою модель робота, то и двигаться она должна как робот. Например, если дать роботу команду «проехать вперед 200 метров», то все частицы должны получить точно такую же команду. При этом, так как ориентация у всех частиц разная, «вперед» у них тоже разное.

Расчет новых координат робота довольно прост:

Сначала повернем робота на заданный угол, просто добавив значение этого угла к текущей ориентации робота:

Orientation = Orientation + angle

Затем рассчитаем новые координаты из чистой тригонометрии:

newX = oldX - distance * Sin(Orientation)
newY = oldY + distance * Cos(Orientation)

В данном примере нулевой угол поворота робота соответствует направлению вниз, а угол в девяносто градусов — направлению влево.



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

Так же отметим, что из-за погрешности разворота робот проехал не вертикально вниз, а отклонился на 3.7 градуса. Погрешность движения привела к тому, что робот проехал не 200 метров, а 185.

Не забываем, что каждая частица — модель робота, и её движение тоже происходит с погрешностью. При этом, не смотря на то, что модель погрешностей частиц точно такая же как модель погрешностей робота, фактическая погрешность (погрешность полученная на выполненном движении, в приведенном примере 3.7 градуса и 15 метров соответственно) движения частицы не зависит от фактической погрешности робота.

Измерение (Measurement update)

На этом этапе робот совершает измерения и получает новую информацию о своем местоположении.

Дополним предыдущий пример, когда мы сделали 100 шагов и потеряли информацию:

Измерив свои координаты в новой точке, вы переходите от «Я примерно знаю где я нахожусь» к «Я снова точно знаю где я нахожусь» — то есть получаете информацию.

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

В рассматриваемом примере дальность до ориентира измеряется с погрешностью измерений (±15 метров). Пока все просто: делаем измерения для робота и для всех частиц.

Формула расчета дальности до ориентира:

measurement = Sqrt((orientierX - robotX)^2 + (orientierY - robotY)^2)

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

Для сравнения измерений будем использовать формулу нормального распределения:

,
где ?^2- среднеквадратическое отклонение (погрешность измерения); ?0 — математическое ожидание (измерение робота); ? — значение величины (измерение частицы); f(?) — вероятность получения значения ? при заданных ?0 и ?^2.

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

particleList[i].weight = f(measurement[i][0]) * f(measurement[i][1]) * f(measurement[i][2]) * f(measurement[i][3])

Отлично! Мы обработали полученную информацию и рассчитали вес каждой частицы. Но теперь суммарный вес всех частиц превышает единицу, а значит его нужно нормализовать (напомню, что суммарный вес частиц должен равняться 1).

S = Sum(weight)
for i in range(N):
	weight[i] = weight[i]/S

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

Отсев (Resampling)

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

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

Пример:

Если вес частицы равен 0.0001, а число частиц N = 1000, тогда вероятность того, что эта частица переживет отсев равна:

1 - (1 - 0.0001)^1000 = 0.095 = 9.5 %

Если же вес частицы равен 0.005 (напомню, что начальный вес всех частиц был 1/N = 0.001), тогда вероятность того, что она переживет отсев равна:

1 - (1 - 0.005)^1000 = 0.993 = 99.3 %

Есть множество способов применить такой алгоритм отсева, но я расскажу про один из самых простых и эффективных — колесо отсева (Resampling wheel).

Рассмотрим этот алгоритм на примере. Возьмем массив из семи частиц (N = 7), и представим их вес следующим образом:



Алгоритм начинает работу со случайной частицы, поэтому возьмем случайную величину:

index = random.randint(0, 6) # index0

И введем переменную ?, равную нулю.

betta = 0

Теперь мы добавляем к ? случайное число от нуля до удвоенного максимального веса (в данном случае максимальный вес равен 0.25):

betta = betta + random.uniform(0, 2*max(weight)) # betta0

После этого мы сравниваем значение ? со значением веса текущей частицы. Если ? больше веса частицы, то мы вычитаем из ? вес частицы и увеличиваем index на 1:

if betta > weight[index]:
	betta = betta - weight[index]
	index = index + 1

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

newParticleList.append(particleList[index])
betta = betta + random.uniform(0, 2*max(weight)) # betta1

Полный код алгоритма отсева:

index = random.randint(0, 6)
betta = 0
for i in range(N):
	betta = betta + random.uniform(0, 2*max(weight))
	while betta > weight[index]:
		betta = betta - weight[index]
		index = (index + 1)%N # индекс изменяется в цикле от 0 до N
	newParticleList.append(particleList[index])
particleList = newParticleList

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

S = Sum(weight)
for i in range(N):
	weight[i] = weight[i]/S

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

После первого отсева распределение частиц в приведенном примере будет выглядеть так:

Получение оценки состояния объекта

Получить оценку состояния объекта от фильтра частиц вы можете не зависимо от того в какой фазе он сейчас находится. Для этого достаточно просуммировать значение параметров всех частиц, помноженных на их вес:

estimateX = 0
estimateY = 0
for i in range(N):
	estimateX = estimateX + particleList[i].X*particleList[i].weight
	estimateY = estimateY + particleList[i].Y*particleList[i].weight

Результат работы фильтра частиц:

Заключение

Путем применения фильтра частиц в приведенном примере, нам удалось всего за 8 итераций основного цикла получить координаты робота с точностью 12.9 метров, при погрешности движения 20 метров и погрешности измерения 15 метров.

Ознакомившись с основными принципами построения фильтра частиц, вы теперь можете применить его для решения своих собственных задач!

Ссылки по теме

Курс лекций по автономным системам навигации
Еще один пример реализации фильтра частиц
Stanford Junior