建设银行网站转账必须u盾吗,elgg与wordpress对比,中英文的网站开发,wordpress上传录音自动驾驶定位算法-粒子滤波(Particle Filter)
自动驾驶对定位的精度的要求在厘米级的#xff0c;如何实现厘米级的高精度定位呢#xff1f;一种众所周知的定位方法是利用全球定位系统(GPS)#xff0c;利用多颗卫星的测量结果#xff0c;通过三角测量(Triangulation)机制确…自动驾驶定位算法-粒子滤波(Particle Filter)
自动驾驶对定位的精度的要求在厘米级的如何实现厘米级的高精度定位呢一种众所周知的定位方法是利用全球定位系统(GPS)利用多颗卫星的测量结果通过三角测量(Triangulation)机制确定目标的位置GPS定位的原理见https://blog.csdn.net/liuphahaha/article/details/137609080?spm1001.2014.3001.5501 附赠自动驾驶学习资料和量产经验链接
但是GPS并不总是提供高精度定位数据在GPS信号强的情况下定位精度在13m范围内在GPS信号弱的情况下定位精度下降到1050m范围内。虽然依赖于RTK可以将卫星定位的精度提高到厘米级但是在GPS信号弱的场景下定位精度仍然不能满足应用需求。所以仅仅使用GPS不能实现高可靠的高精度定位的。
为了达到厘米级(3-10cm)的定位精度除了GPS之外还需要借助于其它传感器如激光传感器(LIDAR)、径向距离和角度传感器(Radial distance and angle sensorRADAR)、视觉传感器(Camera)等然后利用特定的算法将这些信息融合起来。粒子滤波(Particle Filter)就是利用这些传感器产生的数据进行高精定位的一种常用方法。 Particle Filters From Udacity Lecture
1.粒子滤波(Particle Filter)的算法思想
相对之前提到的标准卡尔曼滤波粒子滤波(Particle Filter)没有线性高斯分布的假设相对于直方图滤波粒子滤波(Particle Filter)不需要对状态空间进行区间划分。粒子滤波算法采用很多粒子对置信度 进行近似,每个粒子都是对t时刻机器人实际状态的一个猜测。 越接近t时刻的Ground Truth状态描述的粒子权重越高。
粒子更新的过程类似于达尔文的自然选择(Natural Selection)机制与当前Sensor测量状态越匹配的粒子有更大的机会生存下来与Sensor测量结果不符的粒子会被淘汰掉最终粒子都会集中在正确的状态附近。
2、粒子滤波算法在自动驾驶定位中的应用
以放置在封闭环境(Close Environment)中的自动驾驶车辆(Kidnapped Vehicle)为例看它是如何通过粒子滤波(Particle Filter)确定自己所在的位置的。 2.1 先看看自动驾驶车辆有什么
1)地图(Map)。每一辆自动驾驶汽车都配备了一幅高精度的地图地图中包含了大量的地标(Landmark)的位置信息。地标(Landmark)是特定区域内的稳定特征它们能够在相当长的一段时间内都保持不变比如路边建筑物、杆状物等。
2)GPS。使用安装在车辆内部的GPS传感器设备可以提供车辆在地图中的大概的位置范围。基于此局部性范围仅选择全局地图的一部分用以匹配计算从而避免大量的冗余计算提升计算的时效性从而达到实时定位的目的。
3)Lidar/Radar等传感器。安装在车辆上的Lidar/Radar传感器将测量其与地图路标(Landmark)之间的距离从而进一步确定车辆的位置。但是Lidar/Radar的测量结果也不准确存在测量噪声。
2.2 将所有传感器信息组合起来
通过粒子滤波(Particle Filter)将这些所有信息组合在一起用于实现实时的高精度定位。 图片来源:https://github.com/sohonisaurabh/CarND-Kidnapped-Vehicle-Project
粒子滤波(Particle Filter)的主要步骤如下:
1Initialisation Step在初始化步骤中根据GPS坐标输入估算位置估算位置是存在噪声的但是可以提供一个范围约束。
2Prediction Step在Prediction过程中对所有粒子(Particles)增加车辆的控制输入(速度、角速度等)预测所有粒子的下一步位置。
3Update Step在Update过程中根据地图中的Landmark位置和对应的测量距离来更新所有粒子(Particles)的权重。
4Resample Step根据粒子(Particles)的权重对所有粒子(Particles)进行重采样权重越高的粒子有更大的概率生存下来权重越小的例子生存下来的概率就越低从而达到优胜劣汰的目的。
2.3 粒子滤波代码实现
1Initialisation Step
初始化阶段车辆接收到来自GPS的带噪声的测量值并将其用于初始化车辆的位置。GPS的测量值包括车辆的位置P(x,y)和朝向θ并且假设测量结果的噪声服从正态分布。
我们创建100个粒子并用GPS的测量值初始化这些粒子的位置和朝向。粒子的个数是一个可调参数可根据实际效果和实际需求调整。初始化时所有粒子的权重相同。
void ParticleFilter::init(double x, double y, double theta, double std[]) {// TODO: Set the number of particles. Initialize all particles to first// position (based on estimates of// x, y, theta and their uncertainties from GPS) and all weights to 1.// Add random Gaussian noise to each particle.// NOTE: Consult particle_filter.h for more information about this method (and// others in this file).// TODO Complete/*Number is particles are chosen in order to run the algorithm in almost realtime and introduce lowest possible error in localization. This is a tunableparameter.*/num_particles 20;default_random_engine gen;normal_distributiondouble dist_x(x, std[0]);normal_distributiondouble dist_y(y, std[1]);normal_distributiondouble dist_theta(theta, std[2]);int i;for (i 0; i num_particles; i) {Particle current_particle;current_particle.id i;current_particle.x dist_x(gen);current_particle.y dist_y(gen);current_particle.theta dist_theta(gen);current_particle.weight 1.0;particles.push_back(current_particle);weights.push_back(current_particle.weight);}is_initialized true;
}2Prediction Step
初始化完成之后对所有粒子执行车辆运动模型预测每个粒子下一步出现的位置。粒子的位置更新是通过以下公式完成: 图片来源:https://github.com/sohonisaurabh/CarND-Kidnapped-Vehicle-Project
车辆运动过程同样存在噪声前面的文章提到到比如车辆发出控制指令velocity10m/s但是由于设备测量误差、车轮打滑等原因实际的运动速度可能大于10m/s也可能小于10m/s我们同样假设车辆运动噪声服从正态分布。
void ParticleFilter::prediction(double delta_t, double std_pos[],double velocity, double yaw_rate) {// TODO: Add measurements to each particle and add random Gaussian noise.// NOTE: When adding noise you may find std::normal_distribution and// std::default_random_engine useful.// http://en.cppreference.com/w/cpp/numeric/random/normal_distribution// http://www.cplusplus.com/reference/random/default_random_engine/// TODO completedefault_random_engine gen;int i;for (i 0; i num_particles; i) {double particle_x particles[i].x;double particle_y particles[i].y;double particle_theta particles[i].theta;double pred_x;double pred_y;double pred_theta;// Instead of a hard check of 0, adding a check for very low value of// yaw_rateif (fabs(yaw_rate) 0.0001) {pred_x particle_x velocity * cos(particle_theta) * delta_t;pred_y particle_y velocity * sin(particle_theta) * delta_t;pred_theta particle_theta;} else {pred_x particle_x (velocity / yaw_rate) *(sin(particle_theta (yaw_rate * delta_t)) -sin(particle_theta));pred_y particle_y (velocity / yaw_rate) *(cos(particle_theta) -cos(particle_theta (yaw_rate * delta_t)));pred_theta particle_theta (yaw_rate * delta_t);}normal_distributiondouble dist_x(pred_x, std_pos[0]);normal_distributiondouble dist_y(pred_y, std_pos[1]);normal_distributiondouble dist_theta(pred_theta, std_pos[2]);particles[i].x dist_x(gen);particles[i].y dist_y(gen);particles[i].theta dist_theta(gen);}
}3Update Step
在预测过程中我们将车辆的速度和角速度合并到粒子滤波器中在更新过程中我们将激光雷达(Lidar)/Radar对于Landmark的测量结果集成到粒子滤波(Particle Filter)中用于更新所有粒子的权重。
Update Step包含三个主要步骤: aTransformationbAssociationc) Update Weights。
Transformation(坐标变换)Lidar/Radar对Landmark的测量都是相对于车辆自身坐标系统的需要先转换到地图坐标系。传感器设备都安装在车上所以传感器的测量结果都是以车辆为坐标原点X轴沿车辆方向Y轴沿垂直于X轴的左侧方向。 图片来源:https://github.com/sohonisaurabh/CarND-Kidnapped-Vehicle-Project 写成非矩阵形式: 通过Transformation我们已经将观测值的坐标转换到地图的坐标空间下一步是将每个转换后的观测值与Map中的LandMark相关联。Associations主要将LandMark的测量结果匹配到Map中的LandMark。
如下图所示L1L2…L5是地图(Map)中的LandmarkOBS1、OBS2、OBS3是车辆的Observation。红色方块是车辆的GroundTruth位置蓝色方块是车辆的预测位置。 我们可以看到地图有5个LandMark它们分别被标识为L1、L2、L3、L4、L5并且每个LandMark都有已知的地图位置。我们需要将每个转换后的观测值TOBS1、TOBS2、TOBS3与这5个标识符中的一个相关联。其中一个直观的做法就是每个转换后的观测LandMark坐标与最近的Map LandMark相关联。 void ParticleFilter::dataAssociation(std::vectorLandmarkObs predicted,std::vectorLandmarkObs observations,double sensor_range) {// TODO: Find the predicted measurement that is closest to each observed// measurement and assign the// observed measurement to this particular landmark.// NOTE: this method will NOT be called by the grading code. But you will// probably find it useful to// implement this method and use it as a helper during the updateWeights// phase.// TODO complete/*Associate observations in map co-ordinates to predicted landmarks using
nearest neighbor algorithm. Here, the number of observations may be less than
the total number of landmarks as some of the landmarks may be outside the range
of vehicles sensor.*/int i, j;for (i 0; i observations.size(); i) {// Maximum distance can be square root of 2 times the range of sensor.double lowest_dist sensor_range * sqrt(2);int closest_landmark_id -1;double obs_x observations[i].x;double obs_y observations[i].y;for (j 0; j predicted.size(); j) {double pred_x predicted[j].x;double pred_y predicted[j].y;int pred_id predicted[j].id;double current_dist dist(obs_x, obs_y, pred_x, pred_y);if (current_dist lowest_dist) {lowest_dist current_dist;closest_landmark_id pred_id;}}observations[i].id closest_landmark_id;}
}Update Weights完成观测LandMark坐标转换之后和地图匹配之后就可以更新粒子的权重了。由于粒子(Particle)对所有LandMark的观测都是独立的所以粒子的总权重是所有观测LandMark的多元高斯概率密度的乘积。 void ParticleFilter::updateWeights(double sensor_range, double std_landmark[],std::vectorLandmarkObs observations,Map map_landmarks) {// TODO: Update the weights of each particle using a mult-variate Gaussian// distribution. You can read// more about this distribution here:// https://en.wikipedia.org/wiki/Multivariate_normal_distribution// NOTE: The observations are given in the VEHICLES coordinate system. Your// particles are located// according to the MAPS coordinate system. You will need to transform// between the two systems. Keep in mind that this transformation requires// both rotation AND translation (but no scaling). The following is a good// resource for the theory:// https://www.willamette.edu/~gorr/classes/GeneralGraphics/Transforms/transforms2d.htm// and the following is a good resource for the actual equation to implement// (look at equation 3.33 http://planning.cs.uiuc.edu/node99.html// TODO completeint i, j;/*This variable is used for normalizing weights of all particles and bringthem in the range of [0, 1]*/double weight_normalizer 0.0;for (i 0; i num_particles; i) {double particle_x particles[i].x;double particle_y particles[i].y;double particle_theta particles[i].theta;/*Step 1: Transform observations from vehicle co-ordinates to map* co-ordinates.*/// Vector containing observations transformed to map co-ordinates w.r.t.// current particle.vectorLandmarkObs transformed_observations;// Transform observations from vehicles co-ordinates to map co-ordinates.for (j 0; j observations.size(); j) {LandmarkObs transformed_obs;transformed_obs.id j;transformed_obs.x particle_x (cos(particle_theta) * observations[j].x) -(sin(particle_theta) * observations[j].y);transformed_obs.y particle_y (sin(particle_theta) * observations[j].x) (cos(particle_theta) * observations[j].y);transformed_observations.push_back(transformed_obs);}/*Step 2: Filter map landmarks to keep only those which are in thesensor_range of current particle. Push them to predictions vector.*/vectorLandmarkObs predicted_landmarks;for (j 0; j map_landmarks.landmark_list.size(); j) {Map::single_landmark_s current_landmark map_landmarks.landmark_list[j];if ((fabs((particle_x - current_landmark.x_f)) sensor_range) (fabs((particle_y - current_landmark.y_f)) sensor_range)) {predicted_landmarks.push_back(LandmarkObs{current_landmark.id_i, current_landmark.x_f, current_landmark.y_f});}}/*Step 3: Associate observations to lpredicted andmarks using nearest* neighbor algorithm.*/// Associate observations with predicted landmarksdataAssociation(predicted_landmarks, transformed_observations,sensor_range);/*Step 4: Calculate the weight of each particle using Multivariate Gaussian* distribution.*/// Reset the weight of particle to 1.0particles[i].weight 1.0;double sigma_x std_landmark[0];double sigma_y std_landmark[1];double sigma_x_2 pow(sigma_x, 2);double sigma_y_2 pow(sigma_y, 2);double normalizer (1.0 / (2.0 * M_PI * sigma_x * sigma_y));int k, l;/*Calculate the weight of particle based on the multivariate Gaussian* probability function*/for (k 0; k transformed_observations.size(); k) {double trans_obs_x transformed_observations[k].x;double trans_obs_y transformed_observations[k].y;double trans_obs_id transformed_observations[k].id;double multi_prob 1.0;for (l 0; l predicted_landmarks.size(); l) {double pred_landmark_x predicted_landmarks[l].x;double pred_landmark_y predicted_landmarks[l].y;double pred_landmark_id predicted_landmarks[l].id;if (trans_obs_id pred_landmark_id) {multi_prob normalizer *exp(-1.0 * ((pow((trans_obs_x - pred_landmark_x), 2) /(2.0 * sigma_x_2)) (pow((trans_obs_y - pred_landmark_y), 2) /(2.0 * sigma_y_2))));particles[i].weight * multi_prob;}}}weight_normalizer particles[i].weight;}/*Step 5: Normalize the weights of all particles since resmapling using* probabilistic approach.*/for (int i 0; i particles.size(); i) {particles[i].weight / weight_normalizer;weights[i] particles[i].weight;}
}4Resample Step
重采样(Resample)是从旧粒子(Old Particles)中随机抽取新粒子(New Particles)并根据其权重进行替换的过程。重采样后具有更高权重的粒子保留的概率越大概率越小的粒子消亡的概率越大。
常用的Resample技术是Resampling Wheel。Resampling Wheel的算法思想如下如下图所示的圆环圆环的弧长正比与它的权重(即权重越大弧长越长)。 下一步就是利用类似于轮盘赌(Roulette Wheel)的方式将圆环安装固定步长(Resampling Wheel中将圆环切分为2*max(weights))进行等分选定一个方向进行n次随机旋转每次旋转获得一个位置这个位置就是被选中的粒子的索引。 Python代码如下:
p3 []
index int(random.random()*N)
beta 0.0
mw max(w)
for i in range(N):beta random.random()*2*mwwhile w[index] beta:beta beta - w[index]index index 1p3.append(p[index]) Resample的C代码如下
void ParticleFilter::resample() {// TODO: Resample particles with replacement with probability proportional to// their weight. NOTE: You may find std::discrete_distribution helpful here.// http://en.cppreference.com/w/cpp/numeric/random/discrete_distribution// TODO completevectorParticle resampled_particles;// Create a generator to be used for generating random particle index and beta// valuedefault_random_engine gen;// Generate random particle indexuniform_int_distributionint particle_index(0, num_particles - 1);int current_index particle_index(gen);double beta 0.0;double max_weight_2 2.0 * *max_element(weights.begin(), weights.end());for (int i 0; i particles.size(); i) {uniform_real_distributiondouble random_weight(0.0, max_weight_2);beta random_weight(gen);while (beta weights[current_index]) {beta - weights[current_index];current_index (current_index 1) % num_particles;}resampled_particles.push_back(particles[current_index]);}particles resampled_particles;
}最后把这些代码串起来实现车辆定位的功能。
// Set up parameters here
double delta_t 0.1; // Time elapsed between measurements [sec]
double sensor_range 50; // Sensor range [m]double sigma_pos[3] {0.3, 0.3, 0.01}; // GPS measurement uncertainty [x [m], y [m], theta [rad]]
double sigma_landmark[2] {0.3, 0.3}; // Landmark measurement uncertainty [x [m], y [m]]// Read map data
Map map;
if (!read_map_data(../data/map_data.txt, map)) {cout Error: Could not open map file endl;return -1;
}// Create particle filter
ParticleFilter pf;while (循环接收车辆的控制信息和Sense信息) {if (!pf.initialized()) {// Sense noisy position data from the simulatordouble sense_x std::stod(j[1][sense_x].getstd::string());double sense_y std::stod(j[1][sense_y].getstd::string());double sense_theta std::stod(j[1][sense_theta].getstd::string());pf.init(sense_x, sense_y, sense_theta, sigma_pos);} else {// Predict the vehicles next state from previous (noiseless// control) data.double previous_velocity std::stod(j[1][previous_velocity].getstd::string());double previous_yawrate std::stod(j[1][previous_yawrate].getstd::string());pf.prediction(delta_t, sigma_pos, previous_velocity, previous_yawrate);}// receive noisy observation data from the simulator// sense_observations in JSON format// [{obs_x,obs_y},{obs_x,obs_y},...{obs_x,obs_y}]vectorLandmarkObs noisy_observations;string sense_observations_x j[1][sense_observations_x];string sense_observations_y j[1][sense_observations_y];std::vectorfloat x_sense;std::istringstream iss_x(sense_observations_x);std::copy(std::istream_iteratorfloat(iss_x), std::istream_iteratorfloat(),std::back_inserter(x_sense));std::vectorfloat y_sense;std::istringstream iss_y(sense_observations_y);std::copy(std::istream_iteratorfloat(iss_y), std::istream_iteratorfloat(),std::back_inserter(y_sense));for (int i 0; i x_sense.size(); i) {LandmarkObs obs;obs.x x_sense[i];obs.y y_sense[i];noisy_observations.push_back(obs);}// Update the weights and resamplepf.updateWeights(sensor_range, sigma_landmark, noisy_observations, map);pf.resample();// Calculate and output the average weighted error of the particle// filter over all time steps so far.vectorParticle particles pf.particles;int num_particles particles.size();double highest_weight -1.0;Particle best_particle;double weight_sum 0.0;for (int i 0; i num_particles; i) {if (particles[i].weight highest_weight) {highest_weight particles[i].weight;best_particle particles[i];}weight_sum particles[i].weight;}
}在Udacity Simulator上的演示效果如下:
