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

Обзор фильтров частиц

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

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

Вес

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

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

Повторная выборка

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

Для повторной выборки используется техника колеса передискретизации.

Вот видео Себастьяна из Udacity, рассказывающего о колесе передискретизации.

Код для передискретизации:

p3 = []
index = int(random.random()*N)
beta = 0.0
mw = max(w)
for i in range(N):
  beta += random.random()*2*mw
  while w[index] < beta:
     beta = beta - w[index]
     index = index + 1

  p3.append(p[index])

Реализация фильтров частиц

Фильтры твердых частиц имеют четыре основных этапа:

  1. Этап инициализации: на этапе инициализации мы оцениваем нашу позицию по входным данным GPS. На последующих этапах процесса эта оценка будет уточнена для локализации нашего автомобиля.
  2. Шаг прогнозирования: на этапе прогнозирования мы добавляем управляющий вход (скорость и скорость рыскания) для всех частиц.
  3. Этап обновления: на этапе обновления мы обновляем наши веса частиц, используя положение ориентиров на карте и измерения объектов.
  4. Шаг передискретизации: во время передискретизации мы выполним передискретизацию M раз (M - диапазон от 0 до length_of_particleArray), рисуя частицу i (i - индекс частицы), пропорциональную ее весу. На этом этапе используется колесо повторной дискретизации.

1. Шаг инициализации

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

И последний код для шага инициализации:

if(is_initialized) {
	return;
} 

//Number of particles
num_particles = 100;

//SD
double std_x = std[0];
double std_y = std[1];
double std_theta = std[2];

//Normal distributions
normal_distribution<double> dist_x(x, std_x);
normal_distribution<double> dist_y(y, std_y);
normal_distribution<double> dist_theta(theta, std_theta);

//Generate particles with normal distribution with mean on GPS values.
for(int i = 0; i < num_particles; i++) {
	Particle p;
	p.id = i;
	p.x = dist_x(gen);
	p.y = dist_y(gen);
	p.theta = dist_theta(gen);
	p.weight = 1.0;
	particles.push_back(p);
}
is_initialized = true;

2. Шаг прогноза

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

И последний код для шага предсказания:

//Normal distributions for sensor noise
normal_distribution<double> dist_x(0, std_pos[0]);
normal_distribution<double> dist_y(0, std_pos[1]);
normal_distribution<double> dist_theta(0, std_pos[2]);

for(int i = 0; i < num_particles; i++) {
	if(fabs(yaw_rate) < 0.00001) {  
		particles[i].x += velocity * delta_t * cos(particles[i].theta);
		particles[i].y += velocity * delta_t * sin(particles[i].theta);
	} 
	else{
		particles[i].x += velocity / yaw_rate * (sin(particles[i].theta + yaw_rate*delta_t) - sin(particles[i].theta));
		particles[i].y += velocity / yaw_rate * (cos(particles[i].theta) - cos(particles[i].theta + yaw_rate*delta_t));
		particles[i].theta += yaw_rate * delta_t;
	}

	//Noise
	particles[i].x += dist_x(gen);
	particles[i].y += dist_y(gen);
	particles[i].theta += dist_theta(gen);
}

3. Шаг обновления

Теперь, когда мы включили в наш фильтр входные данные для измерения скорости и скорости рыскания, мы должны обновить веса частиц на основе показаний ориентиров с помощью LIDAR и RADAR.

Шаг обновления состоит из трех основных шагов:

  1. Трансформация
  2. Ассоциация
  3. Обновить веса

Преобразование

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

Наблюдения в системе координат автомобиля можно преобразовать в координаты карты (x m и y m), передав координаты наблюдения автомобиля (x c И y c), координаты частицы карты (x p и y p) и угол поворота (-90 градусов) через однородную матрицу преобразования. Эта матрица однородного преобразования, показанная ниже, выполняет вращение и перенос.

Умножение матриц приводит к:

Ассоциации

Ассоциация - это проблема сопоставления измерения ориентира с объектом в реальном мире, таким как ориентиры на карте.

Теперь, когда наблюдения преобразованы в координатное пространство карты, следующий шаг - связать каждое преобразованное наблюдение с идентификатором наземного ориентира. В приведенном выше упражнении с картой у нас есть 5 общих ориентиров, каждый из которых обозначен как L1, L2, L3, L4, L5, и каждый с известным местоположением на карте. Нам нужно связать каждое преобразованное наблюдение TOBS1, TOBS2, TOBS3 с одним из этих 5 идентификаторов. Для этого мы должны связать ближайший ориентир с каждым преобразованным наблюдением. Рассмотрим пример ниже, чтобы объяснить проблему ассоциации данных.

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

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

В этом методе мы берем самое близкое измерение как правильное.

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

Обновить веса

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

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

И последний код для шага обновления:

//Each particle for loop
for(int i = 0; i < num_particles; i++) {
	double paricle_x = particles[i].x;
	double paricle_y = particles[i].y;
	double paricle_theta = particles[i].theta;

	//Create a vector to hold the map landmark locations predicted to be within sensor range of the particle
	vector<LandmarkObs> predictions;

	//Each map landmark for loop
	for(unsigned int j = 0; j < map_landmarks.landmark_list.size(); j++) {

		//Get id and x,y coordinates
		float lm_x = map_landmarks.landmark_list[j].x_f;
		float lm_y = map_landmarks.landmark_list[j].y_f;
		int lm_id = map_landmarks.landmark_list[j].id_i;

		//Only consider landmarks within sensor range of the particle (rather than using the "dist" method considering a circular region around the particle, this considers a rectangular region but is computationally faster)
		if(fabs(lm_x - paric
le_x) <= sensor_range && fabs(lm_y - paricle_y) <= sensor_range) {
			predictions.push_back(LandmarkObs{ lm_id, lm_x, lm_y });
		}
	}

	//Create and populate a copy of the list of observations transformed from vehicle coordinates to map coordinates
	vector<LandmarkObs> trans_os;
	for(unsigned int j = 0; j < observations.size(); j++) {
		double t_x = cos(paricle_theta)*observations[j].x - sin(paricle_theta)*observations[j].y + paricle_x;
		double t_y = sin(paricle_theta)*observations[j].x + cos(paricle_theta)*observations[j].y + paricle_y;
		trans_os.push_back(LandmarkObs{ observations[j].id, t_x, t_y });
	}

	//Data association for the predictions and transformed observations on current particle
	dataAssociation(predictions, trans_os);
	particles[i].weight = 1.0;
	for(unsigned int j = 0; j < trans_os.size(); j++) {
		double o_x, o_y, pr_x, pr_y;
		o_x = trans_os[j].x;
		o_y = trans_os[j].y;
		int asso_prediction = trans_os[j].id;

		//x,y coordinates of the prediction associated with the current observation
		for(unsigned int k = 0; k < predictions.size(); k++) {
			if(predictions[k].id == asso_prediction) {
				pr_x = predictions[k].x;
				pr_y = predictions[k].y;
			}
		}

		//Weight for this observation with multivariate Gaussian
		double s_x = std_landmark[0];
		double s_y = std_landmark[1];
		double obs_w = ( 1/(2*M_PI*s_x*s_y)) * exp( -( pow(pr_x-o_x,2)/(2*pow(s_x, 2)) + (pow(pr_y-o_y,2)/(2*pow(s_y, 2))) ) );

		//Product of this obersvation weight with total observations weight
		particles[i].weight *= obs_w;
	}
}

4. Шаг повторной выборки

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

Вот код для шага Resample:

//Get weights and max weight.
vector<double> weights;
double maxWeight = numeric_limits<double>::min();
for(int i = 0; i < num_particles; i++) {
	weights.push_back(particles[i].weight);
	if(particles[i].weight > maxWeight) {
		maxWeight = particles[i].weight;
	}
}

uniform_real_distribution<double> distDouble(0.0, maxWeight);
uniform_int_distribution<int> distInt(0, num_particles - 1);
int index = distInt(gen);
double beta = 0.0;
vector<Particle> resampledParticles;
for(int i = 0; i < num_particles; i++) {
	beta += distDouble(gen) * 2.0;
	while(beta > weights[index]) {
		beta -= weights[index];
		index = (index + 1) % num_particles;
	}
	resampledParticles.push_back(particles[index]);
}

particles = resampledParticles;

Если вы хотите увидеть полный код, посетите мой проект в репозитории github.