标签：Monte noise 粒子 Localization double landmarks Robot AMCL myrobot
什么是Monte Carlo Localization呢，中文名叫蒙特卡罗定位，权威阐述见《概率机器人》第8章，移动机器人定位:栅格与蒙特卡罗。是基于粒子滤波的定位算法。
1.粒子滤波算法与蒙特卡洛定位算法
那啥是粒子滤波(particle filter)?
参考:
https://web.mit.edu/16.412j/www/html/Advanced%20lectures/Slides/Hsaio_plinval_miller_ParticleFiltersPrint.pdfweb.mit.edu/16.412j/www/html/Advanced%20lectures/Slides/Hsaio_plinval_miller_ParticleFiltersPrint.pdf粒子滤波算法:
引自《概率机器人》程序4.3 粒子滤波算法(基于重要性采样的贝叶斯滤波的一个变种)
引自《概率机器人》程序8.2 蒙特卡罗定位蒙特卡罗算法(基于粒子滤波的定位算法)
2.蒙特卡洛定位算法代码实现
论文参考：
http://robots.stanford.edu/papers/fox.aaai99.pdfrobots.stanford.edu/papers/fox.aaai99.pdf代码引用另一位作者的:
https://github.com/udacity/RoboND-MCL-Lab/blob/master/solution.cppgithub.com/udacity/RoboND-MCL-Lab/blob/master/solution.cpp注意运行该solution.cpp时格式如下:
g++ solution.cpp -o app -std=c++11 -I/usr/include/python2.7 -lpython2.7测试结果:
5000个粒子；面积 500*500m；迭代步骤 20 steps;
迭代20步之后:
3.蒙特卡洛定位算法逻辑分析
逻辑梳理:
1.创建一个Robot对象的类，在类里规定它的一些行为，特性，当我们谈论Robot对象时，就不仅仅是myrobot了，也可以是粒子，粒子集；
Robot（) //构造函数
//Robot对象的位姿（随机给定:x,y,orient),噪声(前进噪声，转向噪声，传感器感知的噪声)；
void set(double new_x, double new_y, double new_orient)
//外部可调用此函数来给Robot对象重新设置其自身的位姿(x,y,orient)；
void set_noise(double new_forward_noise, double new_turn_noise, double new_sense_noise)
//外部可调用此函数来给Robot对象重新设置其自身的噪声(forward_noise,turn_noise,sense_noise);
vector<double> sense()
//该函数用于计算Robot对象与障碍物点(landwarks)点之间的距离，并加上传感器的测量噪声；
Robot move(double turn, double forward)
//这个函数 告诉你，这个机器人是可以动的，这里规定它的方向如何可以改变，前进的距离如何可以改变；无论如何，既然运动，那么就得加上运动噪声；
// 既然方向变了，前进距离也变了，那么位姿也跟着变了，自然地得到x,y的值；
string show_pose()
//这个函数用到to_string将double型转换成string字符串；方便你打印出Robot对象的当前时刻位姿；
string read_sensors()
//这个函数跟上一个相似，方便你打印出Robot对象当前时刻测到所有障碍物的距离集合；
double measurement_prob(vector<double> measurement)
//这个函数是核心，这里返回值是一个概率；这个概率来源于三个数据:Robot对象(粒子)与障碍物点(landmarks)的距离，传感器的测量噪声，唯一的机器人myrobot本身与障碍物点(landmarks)的距离
double x, y, orient; //robot object poses
double forward_noise, turn_noise, sense_noise; //robot object noises
private:
double gen_gauss_random(double mean, double variance)
{}
double gaussian(double mu, double sigma, double x)
{}
};2.粒子如何获得位姿:
// Create a set of particles
int n = 5000; //粒子数5000
Robot p[n];//构造一个粒子集，这个粒子集是Robot对象的一个实例化集合，具有Robot的所有行为；
for (int i = 0; i < n; i++)
{
p[i].set_noise(0.05, 0.05, 5.0);
//cout << p[i].show_pose() << endl;
}3.迭代流程,设置steps为20:
a.myrobot 运动以及感知;是的myrobot不会待在原地，它一边运动，也一边感知到周围的障碍物点(landmarks);
b.粒子群p[n]开始运动；复制myrobot的运动行为进行运动；此时粒子群p[n]经过运动之后，
位姿发生改变，我们可以换另一个粒子群对象p2[n]来暂时存放p[n]运动后的位姿，然后再令p[n]=p2[n];无它，p[n]就是个粒子集的壳；官方(《概率机器人》)操作获得粒子的位姿:
3.1 运动采样模型算法
至于这篇文章给出的代码示例，关于这部分，只是稍微做了一点变形；
Robot move(double turn, double forward)
{
if (forward < 0)
throw std::invalid_argument("Robot cannot move backward");
// turn, and add randomness to the turning command
orient = orient + turn + gen_gauss_random(0.0, turn_noise);
orient = mod(orient, 2 * M_PI);
// move, and add randomness to the motion command
double dist = forward + gen_gauss_random(0.0, forward_noise);
x = x + (cos(orient) * dist);
y = y + (sin(orient) * dist);
// cyclic truncate
x = mod(x, world_size);
y = mod(y, world_size);
// set particle
Robot res;
res.set(x, y, orient);
res.set_noise(forward_noise, turn_noise, sense_noise);
return res;
}c.粒子群p[n]到了新的地方，这时每个粒子的位置离障碍物点(landmarks)的距离肯定跟当初的距离不一样了；myrobot也运动，它的新位置离同一批障碍物点(landmarks)也不一样了；这时我们给这群可怜的粒子群p[n]一一赋予对应的粒子一个权重；该权重高的判定，取决于这个粒子是否测到障碍物群的距离与myrobot测到障碍物群的距离相似程度高。那这些粒子怎么拥有权重的具体赋值呢，来自代码里的这句 w[i]=p[i].measurement_prob(z);这个z来源，自然是来源于myrobot自己感知到的障碍物点(landmarks)了。所以，z= myrobot.sense()。至于measurement_prob（）就太值得大书特书了。官方(《概率机器人》)版本是这样计算:
3.2 观测打分模型算法
至于这篇文章给出的代码示例，关于这部分，只是稍微做了一点变形；
z的来源:
vector<double> sense()
{
// Measure the distances from the robot toward the landmarks
vector<double> z(sizeof(landmarks) / sizeof(landmarks[0]));
double dist;
for (int i = 0; i < sizeof(landmarks) / sizeof(landmarks[0]); i++)
{
dist = sqrt(pow((x - landmarks[i][0]), 2) + pow((y - landmarks[i][1]), 2));
dist += gen_gauss_random(0.0, sense_noise);
z[i] = dist;
}
return z;
}真正计算权重(其实是个概率):
double measurement_prob(vector<double> measurement)
{
// Calculates how likely a measurement should be
double prob = 1.0;
double dist;
for (int i = 0; i < sizeof(landmarks) / sizeof(landmarks[0]); i++)
{
dist = sqrt(pow((x - landmarks[i][0]), 2) + pow((y - landmarks[i][1]), 2));
prob *= gaussian(dist, sense_noise, measurement[i]);
}
return prob;
}d.在c步粒子群每个粒子都有了自己的权重了；那么在这群粒子中一定有一个粒子权重最大的代表，我们把它挑选出来，进入重采样程序里，好好用用。官方《概率机器人》介绍的低方差采样如下:
3.3 低方差采样算法
至于这篇文章给出的代码示例，关于这部分，只是稍微做了一点变形；其实我们自己也可以稍微变一变；
//Resample the particles with a sample probability proportional to the importance weight
Robot p3[n];
int index = gen_real_random() * n;
cout << "index: " << index << endl;
double beta = 0.0;
double mw = max(w, n);//瞧，我们选出的那个lucky粒子，它具有最大的权重；
cout << "mv:" << mw << endl;
//注意对比看上面的低方差采样 第6行: for m=1 to M do ;
for (int i = 0; i < n; i++)
{
beta += gen_real_random() * 2.0 * mw;//
//注意对比看上面的低方差采样 第8行：while U > c;
//而U是第7行 : U = r +(m-1).M^-1;这里我们对应beta,把beta的赋值改得稍微有点不一样；
//不过还是依旧带有随机性并且携带lucky粒子的权重；
while (beta > w[index])
{
beta -= w[index];
index = mod((index + 1), n);
}
p3[i] = p[index];
//cout << "changed i:" << i << " p3[i].show_pose() with index: " << index << p3[i].show_pose() << endl;
}
for (int k = 0; k < n; k++)
{
p[k] = p3[k];
//cout << p[k].show_pose() << endl;
}呵，p3[n]也只是个暂时存放p[n]经过重采样后的位姿的壳，存放完，然后又赋值给p[n];
至此，迭代流程就到这里了；
4.这群可怜的粒子 跟着myrobot一起，带着噪声运动，带着噪声感知，它们之间还要进行权重的pk，从中选出一个权重最大的lucky粒子，我们参考这个lucky粒子的权重，选择某种具有随机性质的采样方法，尽可能地用这个lucky粒子所带有的权重，再把整个粒子集再刷新一遍它们的位姿，其实也就是挪个位置继续待着，等着myrobot继续蹦迪运动，它们也老老实实跟着myrobot继续蹦迪，嘿~位置越靠近myrobot的那就是myrobot的brothers!myrobot想，既然是来了的都是brother,我们最后势必要紧紧地抱在一起....
4.总结
这篇文章是一个很基础的粒子滤波用于定位的原理+代码讲解版本。要看这群粒子跟着机器人继续蹦迪，更深入的版本，进入实用阶段，那就得换个真实场景的地图，障碍物也变了，桌子腿，柜子什么都有，我们需要通过激光扫描障碍物来告诉大家障碍物的位置，但是激光来源于激光传感器，它有时靠谱，有时也不靠谱；粒子数那么多，一个数组也存不下了，那就换一个更好的数据结构-KDtree来存；重采样也不能这么随意简单了，得换plus版。但是万变不离宗，总之就是一群粒子跟着机器人蹦迪，离机器人越近，那就是机器人的兄弟，我们会紧紧拥抱在一起，不分离。
转自：2.Monte Carlo Localization | 基础原理篇+配备代码讲解 - 知乎 (zhihu.com)
标签：Monte,noise,粒子,Localization,double,landmarks,Robot,AMCL,myrobot 来源： https://www.cnblogs.com/tdyizhen1314/p/16692773.html
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。