slam 学习之 AMCL 概念与原理分析

雨点打透心脏的1/2处 2022-12-06 15:11 400阅读 0赞

AMCL(adaptive Monte Carlo Localization)自适应蒙特卡洛定位，A也可以理解为augmented，是机器人在二维移动过程中概率定位系统，采用粒子滤波器来跟踪已经知道的地图中机器人位姿，对于大范围的局部定位问题工作良好。对机器人的定位是非常重要的，因为若无法正确定位机器人当前位置，那么基于错误的起始点来进行后面规划的到达目的地的路径必定也是错误的。

## 一. 首先了解 几个概念： ##

**1. 需要清楚的几个重要数学名词 ：**

> 状态转移概率![20200913233514386.png][]：  
> 状态转移分布描述状态如何随时间变化的特征，可能作为机器人控制的效果。  
> 测量概率![20200913233540254.png][]：  
> 测量分布描述测量如何由状态控制的特征。  
> 置信度![20200913233558387.png][]  
> 机器人的置信度是对给定所有过去的传感器测量和所有过去控制的环境状态（包括机器人的状态）的一个后验分布。

在了解上述的概念后，必须掌握的一个算法：贝叶斯滤波（Bayes filter）算法，该算法几乎是后期定位算法的一个核心思路，它的伪代码如下：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70][]

可以理解为该算法通过前一时刻对现在的预测乘上这一时刻对外界的检测等于现在这一时刻的状态，其中第三行叫作控制更新或预报，第四行叫作测量更新。  
（这段话有点过于学术化了，直白的讲现在的状态等于过去对现在预测的状态乘上测量的情况）

[AMCL 原理概述][AMCL]：

**2. 粒子滤波和蒙特卡洛**

**蒙特卡洛**：是一种思想或方法。举例：一个矩形里面有个不规则形状，怎么计算不规则形状的面积？不好算。但我们可以近似。拿一堆豆子，均匀的撒在矩形上，然后统计不规则形状里的豆子的个数和剩余地方的豆子个数。矩形面积知道的呀，所以就通过估计得到了不规则形状的面积。拿机器人定位来讲，它处在地图中的任何一个位置都有可能，这种情况我们怎么表达一个位置的置信度呢？我们也使用粒子，**哪里的粒子多**，就代表机器人在哪里的可能性高。

**粒子滤波**：粒子数代表某个东西的可能性高低。通过某种评价方法（评价这个东西的可能性），改变粒子的分布情况。比如在机器人定位中，某个粒子A，我觉得这个粒子在这个坐标（比如这个坐标就属于之前说的“这个东西”）的可能性很高，那么我给他打高分。下次重新安排所有的粒子的位置的时候，就在这个位置附近多安排一些。这样多来几轮，粒子就**都集中到可能性高的位置**去了。

**3. 重要性采样**

就像转盘抽奖一样，原本分数高（我们给它打分）的粒子，它在转盘上对应的面积就大。原本有100个粒子，那下次我就转100次，转到什么就取个对应的粒子。这样多重复几次，仍然是100个粒子，但是分数高的粒子越来越多了，它们代表的东西（比如位姿）几乎是一样的。

**4. 机器人绑架**

举例，机器人突然被抱走，放到了另外一个地方。类似这种情况。

**5. 自适应蒙特卡洛**

自适应体现在：

1.  解决了机器人绑架问题，它会在发现粒子们的平均分数突然降低了（意味着正确的粒子在某次迭代中被抛弃了）的时候，**在全局再重新的撒一些粒子**。
2.  解决了粒子数固定的问题，因为有时候当机器人定位差不多得到了的时候，比如这些粒子都集中在一块了，还要维持这么多的粒子没必要，这个时候**粒子数可以少一点**了。

**6. KLD采样**

就是为了控制上述粒子数冗余而设计的。比如在栅格地图中，看粒子占了多少栅格。占得多，说明粒子很分散，在每次迭代重采样的时候，允许粒子数量的上限高一些。占得少，说明粒子都已经集中了，那就将上限设低，采样到这个数就行了。

**7. 测距仪的似然域(likelihood field）**

主要思想是首先将传感器扫描到的终点![z\_t][z_t]映射到地图的全局坐标空间。假定三种噪声和不确定性的来源

1.测量噪声。由测量过程引起的噪声使用高斯进行建模。在x-y空间，它涉及寻找地图上最近的障碍物。

2.测量失败，

3. 无法解释的随机测量

将障碍物检测的似然描述成全局x-y坐标的函数，叫作似然域。

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zODE0NTMxNw_size_16_color_FFFFFF_t_70][]

## 二 . AMCL 简介 ##

### 首先介绍粒子滤波 ###

粒子滤波（particle filter）是贝叶斯滤波的一种非参数体现，主要思想是用一系列从后验得到的随机状态采样表示后验，我的理解是相当于起初会有有限个不同的样本点，假定他们的位置是是服从其状态更新或预测概率的，计算每个样本在当前时刻的测量概率，根据每个点的测量概率，从中在挑选一定数量的样本点，通过展现这一群点的分布情况来描述其当前时刻的置信度，其算法伪代码如下：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 1][]

其中![20200914000924423.png][],**状态预测部分简单的说是第一个for循环，而测量更新部分是第二个for循环**，算法输入的部分包括前一个时刻的粒子集、当前的控制及测量，输出是当前时刻的粒子集。其推导过程书上有些，程序第四行![20200914000951436.png][]，这里![20200914001008944.png][]，而函数g相当于时机器人自身的控制函数。

AMCL(adaptive Monte Carlo Localization)自适应蒙特卡洛定位 ，源于MCL算法的一种升华，一种提高。那么为什么要从MCL上升至AMCL呢？首先应该了解MCL的算法原理 ：

### 蒙特卡洛定位算法 ###

蒙特卡洛定位适用于局部定位和全局定位两类问题，尽管它相对的年轻，但是已经成为定位领域中的主流算法，如下所示为蒙特卡洛定位算法：                          
![基于粒子滤波的蒙特卡洛定位算法][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMzNzQyMTQ3_size_16_color_FFFFFF_t_70_pic_center]  
      通过把合适的概率运动和感知模型代入到粒子滤波算法中得到，使用M个粒子的集合 ![20200913231745711.png][]\\表示置信度![20200913231818754.png][]，初始置信度由先验分布随机产生的M个这样的粒子得到。算法第4行使用运动模型采样，以当前置信度为起点使用粒子，第5行使用测量模型以确定粒子的重要性权值（这两个模型介绍见后面）通过增加粒子总数M能提高定位的近似精度。

MCL以目前的形式解决了全局定位问题，但无法从机器人绑架或全局定位失败中恢复过来。当机器人位置被获取时，其他地方的不正确粒子会逐渐消失。在某种程度上，粒子只能“幸存”在一个单一的姿势附近，如果这个姿势恰好不正确，算法就无法恢复。

这个问题是非常重要的，实际上任何随机算法，就像蒙特卡洛算法，在重采样步骤中可能意外的丢弃所有正确位姿附近的粒子。当粒子数很小（M=50）。并且，当粒子扩散到整个比较大的体积（如全局定位过程）时，这个问题的重要性就能充分的显示。当然这个问题已经解决了，AMCL的提出正是对这个问题的一个解决方法。AMCL算法在机器人遭到绑架的时候，会随机的注入粒子（injection of random particles）其思想是增加随机粒子到粒子集合，从而在运动模型中产生一些随机状态。相对MCL算法，AMCL算法正是多了一个“A”” ,那么这个"A"是什么？“”A“”是adaptive 自适应的缩写，但是个人觉得将"A"理解为Augmented（增加的）更为合适。

amcl重实际的处理流程跟上面的伪代码不太一样。稍微解释下上面的伪代码。  
函数输入机器人上一个状态，运动，测量和粒子数。利用运动采样模型生成M个服从给定分布的随机粒子X，利用测量模型为每个粒子评估产生测量zt的概率Pzt，并把X和Pzt组合到一起存放。  
第8行开始筛选粒子，筛选的规则是，在上面产生的M个粒子中选出M个粒子，每次从中按概率Pzt去选择一个粒子。将产生的M的新粒子作为结果输出。  
实际的流程不太一样，但是思想基本一致。流程如下：  
1、获取机器人当前位姿，计算移动增量delta，判断机器人移动调节是否满足  
2、根据上一个位姿Xt-1和移动增量delta，用运动采样模型产生（在原有粒子基础上按给定分布生成）若干数量（sample-count个）的粒子，也就是生成若干个当前状态的可能位姿。  
3、将激光雷达数据从激光坐标系换算到baselink坐标系，使用激光传感器模型为每一个粒子计算在其状态下获得测量Zt的概率，并计算总概率totalweight。  
4、上面已经获得了伪代码中的粒子和概率组合，就进行采样（可设定若干次数据更新采样一次）。采样后粒子数量不变，并重置所有粒子的概率为平均值。（1/sample-count）  
5、对粒子进行聚类，计算每个cluster的位姿均值、概率（权重）均值、协方差等参数 （实际中此步骤和上一步是同时进行的）  
6、获取平均概率最大的那一个聚类，认为是粒子滤波所得的最终机器人位姿。根据此位姿（机器人在MAP坐标系下的位置），修正ODOM坐标系下机器人的位姿（编码器累计值计算得到的机器人位姿）。完成一个PF滤波周期。

其实，代码中采用的重采样方法很简单，原理依然是，按照每个粒子的权重对粒子进行采样。在重采样过程中，我们还需要提起机器人绑架问题。由于粒子聚集后，万一机器人定位是错误的，或者机器人被搬动，粒子无法聚集到正确的范围内。所以还需要在定位效果不好的情况下全局随机的粒子，来模拟初始化时的情形。

稍后我看再看MCL和AMCL的区别，我们先看一下AMCL具体是怎么执行的：

### 蒙特卡洛定位算法自适应变种 ###

AMCL算法在机器人遭到绑架的时候，会随机注入粒子（injection of random particles），增加粒子的方法引起两个问题，一是每次算法迭代中应该增加多少粒子，二是从那种分布产生这些粒子。  
解决第一个问题可通过监控传感器测量的概率来评估增加粒子，即式(1)

![2020091323234396.png][]

并将其与平均测量概率联系起来，在粒子滤波中这个数量的近似容易根据重要性因子获取，因为重要性权重是这个概率的随机估计，其平均值为式

![20200913232418185.png][]

这个接近上式中的期望概率。  
解决第二个问题可以根据均匀分布在位姿空间产生粒子，用当前观测值加权得到这些粒子。如下给出增加随机粒子的蒙特卡洛定位算法自适应变种（AMCL）：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 2][]

与MCL相比，这个算法跟踪式(1)的似然值的短期与长期均值，整体框架与MCL相同，但在第八行中给出了经验测量似然，并在第10、11行维持短期和长期似然平均，算法要求![20200913233005108.png][]，参数![20200913233021467.png][]和![20200913233032432.png][]分别估计长期和短期平均的指数滤波器的衰减率。算法的关键在第13行，重采样过程中，随机采样以以下式 概率增加

![20200913233101810.png][]

否则重采样以MCL相同的方式进行，即根据式( 1.3 ) (1.3)(1.3)如果短期似然优于长期似然，则算法将判断不增加随机采样，否则的话则按两者之比的比例增加随机采样，以这种方式可抵消瞬时传感器噪声带来的定位误差。

（1）MCL算法和AMCL算法的区别：

AMCL算法增加了短期和长期的指数滤波器衰减率αslow，αfast，换句话说MCL中αslow，αfast为0，AMCL中的不为0。

![Center][]

（2）四个参数的含义

![Center 1][]

粒子X产生测量Zt的概率的含义是：在X的位姿下，能够产生实际测量Zt的概率，其实也就是反演模型产生的虚拟测量与实际测量的吻合程度。显然，定位越不正确，概率越小，定位越准确，概率越大。因此，可以监控全局平均概率，如果概率变小，说明定位越来越不准确了。  
![在这里插入图片描述][2019120315405435.png]  
Alfa（slow）要远远小于Alfa（fast），Wavg是本次滤波得到的全局平均概率。显然Wfast会很显著地反应滤波效果，而Wslow则变化比较慢，更能反应滤波的长期效果。  
![在这里插入图片描述][20191203154435552.png]  
如上图公式（称为滤波评价值），当滤波效果越差，Wfast就变得越来越小于Wslow，那么返回值就会越大。反之，滤波效果较好时候，或者比较稳定的时候，Wfast和Wslow比较接近，返回值就接近0。

（3）Xt代表M个粒子的集合，第5行利用运动模型从旧粒子采样获取新位姿，第6行它的重要性权重依据测量模型设置。

![Center 2][]

（4）AMCL中最重要的地方就是随机采样概率。（看图说话）,sample\_motion\_model()函数也就是粒子滤波中提到的g()函数，

![Center 3][]

（5）motion\_model用的是《概率机器人》这本书第5章的sample\_motion\_model\_velocity 也就是状态转移模型，amcl重使用的是下面的里程计模型

![Center 4][]

（6）各个参数的作用、

![Center 5][]

（7）measurement\_model用的是《概率机器人》这本书的第6章landmark\_model\_known\_correspondece

也就是测量模型。measurement\_model()函数计算的就是![20200914001227940.png][],基本符合了粒子滤波算法的形式，并且也展现了贝叶斯滤波的思想

AMCL算法中默认的采用测距仪的似然域模型，另外还给出了测距仪的波束波形（beam）和likelihood\_field\_prob

### ![Center 6][] ###

（8）算法各部分作用，以及算法的输入输出

借用《Probablistic Robotics》书中一个例子，AMCL算法的全局定位的一个过程：

![Center 7][]

![Center 8][]

*白色小圆点代表：真实的机器人位置，红色圈圈代表离自己均值（请仔细对照图，分析过程，收获颇丰）*

> 在进行第一次标记检测时，几乎所有粒子根据这个检测抽取，如图b所示。该步骤对应测量概率的短期平均小于测量概率的长期平均的情况。多次检测后，粒子紧紧环绕早真实的机器人周围，就像d所示，并且短期和长期测量似然平均都将增加。在这个定位阶段，机器人只是跟踪其位置，观察似然相当高，并且只偶尔增加小数量的随机粒子。
> 
> 当将机器人放置在其他位置时，测量概率下降。在这个新的位置，第一次标记检测还没有触发任何附加粒子，因为平滑估计Wfast仍然很高（e），在新位置进行了几次标记检测后，Wfast比Wslow下降的快，并有更多的随机粒子被加进来（f、g）。最后机器人定位成功。

## 具体模型分析 ##

### （1）里程计运动模型 ###

在第一章MCL和AMCL算法中均涉及运动模型采样，本章将介绍所使用的里程计运动模型。里程通常可通过整合轮子的编码信息来得到，许多商业机器人在固定的时间间隔产生这样的积分位姿估计，即里程计运动模型通过距离测量来估计运动。

解决机器人定位问题主要是解决从机器人内部里程计使用的坐标到物理世界坐标之间的变换问题。里程计模型使用相对运动信息，由内部里程计测量，在时间间隔![2020091422201110.png][]内，机器人从位姿![20200914222033416.png][]运动到![20200914222055161.png][],里程计反馈了从![20200914222148748.png][]

的相对前进，这里的‘-’代表其是基于机器人内部坐标的，该坐标系与全局世界坐标的关系是未知的。在状态估计中利用![20200914222247622.png][]和之![20200914222314165.png][]间的相对差是真是位姿xt-1和xt之间差异的一个很好的估计器，因此运动信息ut由下式( 2.1 )给出：

![20200914222333794.png][]

为了提取相对距离，![20200914222708611.png][]被转变成三个步骤：旋转![20200914222726239.png][]、直线平移![20200914222739354.png][]和另一个旋转![20200914222755518.png][]，如下图所示：

![测距模型][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMzNzQyMTQ3_size_16_color_FFFFFF_t_70_pic_center 1]

这三个参数足以组成由历程及编码的相对运动的统计量。由于AMCL是基于粒子滤波的定位方法，因此希望有一个![2020091422343853.png][]的采样算法，运动模型采样算法如下所示：   ![里程计模型][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMzNzQyMTQ3_size_16_color_FFFFFF_t_70_pic_center 2]

从算法2. 1中我们可以看到，四个α \\alphaα参数分别对旋转和平移方向的撒点角度δ \\deltaδ产生影响，使用采样算法可直接通过控制得到位姿估计xt的值。

## （2）测距仪模型 ##

与里程计运动模型相同，在第一章MCL和AMCL算法中均涉及了使用测量模型确定离子的重要性权值，测距仪是时下最流行的机器人传感器，因此使用测距仪作为测量模型的近似物理模型来测量附近物体的距离。

### （2.1） 波束模型 ###

波束模型采用四类测量误差，包括小的测量噪声、意外对象引起的误差、由于未检测到对象引起的误差和随机意外噪声。因此期望模型是![20200913233213573.png][]四个密度的混合，每一种密度都与一个特定类型的误差有关，四种密度如下所示：

![测距仪模型组成][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMzNzQyMTQ3_size_16_color_FFFFFF_t_70_pic_center 3]

1.  即使传感器正确测量了最近对象的距离，它返回的值也受到误差的影响，即测量噪声，通常由一个窄的均值为![20200914225607474.png][]、标准偏差为![20200914225621862.png][]的高斯建模，![20200914225634625.png][]表示高斯分布，如上图a)所示，实际上测距传感器的值局限于区间![20200914225505255.png][]，这里![20200914224420942.png][]表示最大的传感器距离。
2.  意外对象比如与机器人共享操作空间的人，处理这类对象的一种方法是将他们作为传感器噪声来处理，未建模对象会导致比![20200914225555383.png][]更短的距离。该情况下距离测量用指数分布描述，如上图b)，其密度在![20200914225600110.png][]范围内指数减少。
3.  有时环境中的障碍会被完全忽略如声呐传感器遇到镜面反射、激光雷达检测到黑色吸光物体等，这便是传感器检测失败误差，其典型结果是返回传感器允许的最大值![20200914224420942.png][]，这是一个离散函数，如上图c)所示。
4.  测距仪偶尔会产生完全无法解释的测量，例如超声在几面墙之间反弹，从而产生随机意外噪声，为了使之简单化，对于这样的测量，这里使用一个分布在完整传感器测量范围![20200914225505255.png][]的均匀分布来建立模型。如上图d)所示。

上述4个参数![20200914225428669.png][] , ![20200914225412825.png][],![20200914224420942.png][],![20200914225356801.png][]通过加权平均混合，且有

![20200914225045644.png][]

注意此处的![20200914224420942.png][]只是一个参数而与前文所提最大传感器距离![20200914224420942.png][]不同，所有4种密度线性组合后得到的典型密度如下图所示：

![典型混合分布的“伪密度”][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMzNzQyMTQ3_size_16_color_FFFFFF_t_70_pic_center 4]

可以看到所有4种基本模型的基本特性在这个组合密度中仍然存在，故测距仪模型可由如下波束模型算法实现：

![计算距离扫描的似然算法][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMzNzQyMTQ3_size_16_color_FFFFFF_t_70_pic_center 5]

其中算法的输入包括一个完整的距离扫描![20200914225228412.png][]、机器人姿态![20200914225247404.png][]和地图![20200914225300836.png][]，在一个循环中将各传感器波束![20200914225228412.png][]的似然相乘，第4行采用射线投射来特定的传感器测量计算无噪声距离，第5行计算了各个距离测量的似然，算法返回期望的概率![20200914225210945.png][]。  
基于波束的传感器模型具有两个主要缺点，一是缺乏光滑性，在有许多小障碍的混乱环境中分布不光滑，二是计算量过大，因为该模型需要对每一个传感器的波束都进行评估，故计算量过大需要很大内存。为了克服以上缺点，引入另一种似然域模型。

### （2.2） 似然域模型 ###

似然域模型不必计算相对于任何有意义的传感器物理生成模型的条件概率，因为该模型没有一个合理的物理解释。它的主要思想是首先将传感器扫描的终点z t 映射到地图的全局坐标空间，因此就必须知道机器人的局部坐标系在何处、传感器光束的源头和指向等信息。  
机器人t 时刻的位姿为![20200914224930842.png][]，传感器的安装位置相对于机器人的中心坐标![20200914224914442.png][],激光光束相对于机器人的朝向角度为![2020091422485752.png][],激光测量的终点相对传感器中心为![20200914224840516.png][]，激光扫描到的点投影到地图的全局坐标系坐标为![20200914224826118.png][],其坐标变化如下式( 3.1 ) 所示

![20200914224245853.png][]

这些坐标只有当传感器监测到障碍物才有意义，否则会输出最大值![20200914224420942.png][]，而在似然域模型中将会把最大距离的读数丢弃。  
与波束模型类似，似然域模型有三种噪声和不确定性来源，分别是测量噪声、测量失败和无法解释的随机测量，其中测量噪声涉及寻找地图上最近的障碍物，其高斯噪声与测量目标到地图m mm上最近物体之间的欧氏距离有关，与基于波束模型的传感器模型相同，期望概率![20200914224642232.png][]集成了所有三种分布![20200914224700930.png][]，式中使用熟悉的混合权值![20200914224718328.png][] 、![2020091422444476.png][] 和 ![20200914224420942.png][]。其算法如下所示：

![利用与最近物体的欧氏距离计算测距仪似然的算法][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMzNzQyMTQ3_size_16_color_FFFFFF_t_70_pic_center 6]

与波束算法类似，该算法通过循环将各个![20200914224359613.png][]的值相乘，并假定不同传感器波束的噪声相互独立，第4行判断传感器读数是否为最大距离读数，如果是则舍弃，否则进行5~8行运算——通过将一个正态分布和一个均匀分布混合得到似然结果，注意该算法第8行中的![20200914224420942.png][]并非随机测量带来的误差，而是传感器的最大测量距离![20200914224428123.png][]！（此处需要对比源码查看）故基于似然模型的算法只使用了![2020091422444476.png][]和![20200914224456632.png][]参数，要保证![20200914224514557.png][]。  
以上，基于AMCL定位方法的算法和模型分析结束，从上述所有模型和算法设计到的参数来看，需要重点调试的是AMCL中撒点粒子数、有效粒子数、里程计采样算法中的四个α 参数、似然域模型中的误差参数![20200914224547264.png][]和![20200914224559915.png][]、波束模型的四个误差参数以及最大测量距离![20200914224616271.png][]。

## 重采样小技巧 ##

在前面提供的伪代码中，都有一句draw i with probability的伪代码，也就是根据粒子的权重，去挑选粒子。实际如何做呢？  
我们知道，所有粒子的权重归一化后求和为1。如果把所有粒子排列成一排，权重越大，占用位置越大，而他们占用的总位置求和是1。那么我们获取一个（0,1）的随机数，显然，这个随机数落在权重大的粒子上的可能性也更大啦。算法：  
c\[0\] = 0.0;  
for(i=0;i<sample\_count;i++)  
c\[i+1\] = c\[i\]+weight\[i\]; //用c来储存权重的累加值  
foreach(p in all particles)  
\{  
double r;  
r = drand48();  
for(i=0;i<sample\_count;i++)  
\{  
if((c\[i\] <= r) && (r < c\[i+1\])) //权重越大的粒子，区间就越大，随机数r落在其区间的可能性也越大  
break;  
\}  
//TODO: Select particle i  
\}

## 总结 ##

粒子滤波的思想很简单，是基于穷举法的一种高效方法。若使用穷举法，那么就是对位姿空间中的每个可能去遍历，当然从计算量上来说这是不可能的。所以粒子滤波就是足够多的随机粒子（位姿假设）放到整个位姿空间中，然后用本文的方法去选出定位效果好的粒子，不停迭代，最终所有的粒子收敛到正确的位姿附近。  
优点：能够得到机器人位姿的较优解（蒙特拉罗的），大大降低了穷举法的计算量，能够一定程度上解决机器人绑架问题  
缺点：必须要机器人移动了一定距离才能执行一次滤波，机器人不运动时无法定位；粒子需要多次迭代才能收敛；计算量随着粒子数量的增加而直线上升。

## 参考链接： ##

[amcl算法问题][amcl]

[amcl算法原理][amcl 1]

[AMCL的粒子滤波][AMCL 1]

[AMCL定位算法学习个人总结1][AMCL_1]

[AMCL算法原理讲解][AMCL 2]

[20200913233514386.png]: /images/20221123/4f626a37e4694a029000f95cb8c3ad4c.png
[20200913233540254.png]: /images/20221123/70d22a105a1e44b3ba52d39e34cff011.png
[20200913233558387.png]: /images/20221123/1afc0d8d493d4461811dc00ca6e4ae7d.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70]: /images/20221123/9a5cc3f9924c421db3427cbf8a824d7c.png
[AMCL]: https://blog.csdn.net/ethan_guo/article/details/81809054
[z_t]: https://private.codecogs.com/gif.latex?z_t
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zODE0NTMxNw_size_16_color_FFFFFF_t_70]: /images/20221123/e4bf967c4527434a87f1a90246616f7f.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 1]: /images/20221123/1f6d7d5183524b2e87c25cdf7bcb7170.png
[20200914000924423.png]: https://img-blog.csdnimg.cn/20200914000924423.png
[20200914000951436.png]: https://img-blog.csdnimg.cn/20200914000951436.png
[20200914001008944.png]: /images/20221123/dc774887465c490eb7acdbb2e89b564f.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMzNzQyMTQ3_size_16_color_FFFFFF_t_70_pic_center]: /images/20221123/bd064d74aeb149d28d01d25594007905.png
[20200913231745711.png]: https://img-blog.csdnimg.cn/20200913231745711.png
[20200913231818754.png]: /images/20221123/2719d8152aad4f1298fc38a30850e29a.png
[2020091323234396.png]: /images/20221123/59f35f9c45984fbebcb8a8808573fea1.png
[20200913232418185.png]: /images/20221123/64411dd60c224590b46ac0f70748f891.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDU_size_16_color_FFFFFF_t_70 2]: /images/20221123/f0356f63abe4448da4a1be98bafdf665.png
[20200913233005108.png]: https://img-blog.csdnimg.cn/20200913233005108.png
[20200913233021467.png]: https://img-blog.csdnimg.cn/20200913233021467.png
[20200913233032432.png]: /images/20221123/8b81e90d595440ceba2b5da4cb08241d.png
[20200913233101810.png]: /images/20221123/2cacdcee451e4a319c6e0245bc4be566.png
[Center]: /images/20221123/0bc029ef9d4c4c6997d233675e6ab582.png
[Center 1]: /images/20221123/7b1a0c5191b34977bfbbf4aa30cf1e47.png
[2019120315405435.png]: /images/20221123/5f5813b5bc334cbc946e763aef9f8291.png
[20191203154435552.png]: /images/20221123/209b36e42e4e4dedb15b270dafe9e55c.png
[Center 2]: /images/20221123/527570806499456b9abd75690dd34e2f.png
[Center 3]: /images/20221123/c79004ee704544e5ba8157483a4f67c8.png
[Center 4]: /images/20221123/d3e8f39d61ae48e088558f54cfe1e1a2.png
[Center 5]: /images/20221123/19f9e464be7f4c438d51feb3f2c72525.png
[20200914001227940.png]: /images/20221123/7c04d1afe0b6418aaab77a0cbd61b678.png
[Center 6]: /images/20221123/12e1ee81e866414199849f1932e5c1bd.png
[Center 7]: /images/20221123/7eb58a84ec374915a81bb3d52f19fdf5.png
[Center 8]: /images/20221123/d086f0821a8e4fa0b1571ba9d851b8f6.png
[2020091422201110.png]: https://img-blog.csdnimg.cn/2020091422201110.png
[20200914222033416.png]: https://img-blog.csdnimg.cn/20200914222033416.png
[20200914222055161.png]: https://img-blog.csdnimg.cn/20200914222055161.png
[20200914222148748.png]: /images/20221123/b7e3f3c568b04689886b5353ae6f83a3.png
[20200914222247622.png]: https://img-blog.csdnimg.cn/20200914222247622.png
[20200914222314165.png]: /images/20221123/616bb199e9b2435b8a7ecf1ca9d9576d.png
[20200914222333794.png]: /images/20221123/59de5106651143bebf16e58f82f79d7b.png
[20200914222708611.png]: https://img-blog.csdnimg.cn/20200914222708611.png
[20200914222726239.png]: https://img-blog.csdnimg.cn/20200914222726239.png
[20200914222739354.png]: https://img-blog.csdnimg.cn/20200914222739354.png
[20200914222755518.png]: /images/20221123/bdc60df4380b45e2ab00b783aef3928c.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMzNzQyMTQ3_size_16_color_FFFFFF_t_70_pic_center 1]: /images/20221123/6ba7432b412e4a759632946c88ab0906.png
[2020091422343853.png]: https://img-blog.csdnimg.cn/2020091422343853.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMzNzQyMTQ3_size_16_color_FFFFFF_t_70_pic_center 2]: https://img-blog.csdnimg.cn/20200413160831702.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMzNzQyMTQ3,size_16,color_FFFFFF,t_70#pic_center
[20200913233213573.png]: /images/20221123/e5e8f9eef3424b14ab1f5366e93bb8cb.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMzNzQyMTQ3_size_16_color_FFFFFF_t_70_pic_center 3]: /images/20221123/df537ee64aae48759f0113ab387a3039.png
[20200914225607474.png]: https://img-blog.csdnimg.cn/20200914225607474.png
[20200914225621862.png]: https://img-blog.csdnimg.cn/20200914225621862.png
[20200914225634625.png]: https://img-blog.csdnimg.cn/20200914225634625.png
[20200914225505255.png]: /images/20221123/6028d1f403f6476f99ad638d7955811d.png
[20200914224420942.png]: /images/20221123/47e8b0e7a10f41c38ed9dcaadba802ad.png
[20200914225555383.png]: https://img-blog.csdnimg.cn/20200914225555383.png
[20200914225600110.png]: /images/20221123/65872f35c0dc4cc0b7bcae9f1cb8a6b6.png
[20200914225428669.png]: https://img-blog.csdnimg.cn/20200914225428669.png
[20200914225412825.png]: https://img-blog.csdnimg.cn/20200914225412825.png
[20200914225356801.png]: /images/20221123/295a920cf82146f49ad5f630ed64b7b6.png
[20200914225045644.png]: /images/20221123/9688bdc7634046f7807e5046acc9bb25.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMzNzQyMTQ3_size_16_color_FFFFFF_t_70_pic_center 4]: /images/20221123/6b82fd00d5d54623a3684701f64e31d5.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMzNzQyMTQ3_size_16_color_FFFFFF_t_70_pic_center 5]: /images/20221123/03ba14b1c9e245068c219b736714449f.png
[20200914225228412.png]: https://img-blog.csdnimg.cn/20200914225228412.png
[20200914225247404.png]: https://img-blog.csdnimg.cn/20200914225247404.png
[20200914225300836.png]: https://img-blog.csdnimg.cn/20200914225300836.png
[20200914225210945.png]: /images/20221123/7afe3e880a3b45c2b934838b177f37c9.png
[20200914224930842.png]: https://img-blog.csdnimg.cn/20200914224930842.png
[20200914224914442.png]: https://img-blog.csdnimg.cn/20200914224914442.png
[2020091422485752.png]: https://img-blog.csdnimg.cn/2020091422485752.png
[20200914224840516.png]: https://img-blog.csdnimg.cn/20200914224840516.png
[20200914224826118.png]: /images/20221123/09c21780d701418289e558267f7bee52.png
[20200914224245853.png]: /images/20221123/5a3266c6071c443fb98689fde95b993e.png
[20200914224642232.png]: https://img-blog.csdnimg.cn/20200914224642232.png
[20200914224700930.png]: https://img-blog.csdnimg.cn/20200914224700930.png
[20200914224718328.png]: https://img-blog.csdnimg.cn/20200914224718328.png
[2020091422444476.png]: https://img-blog.csdnimg.cn/2020091422444476.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzMzNzQyMTQ3_size_16_color_FFFFFF_t_70_pic_center 6]: /images/20221123/d2a8bd21bc45468bbb276b888014e49e.png
[20200914224359613.png]: https://img-blog.csdnimg.cn/20200914224359613.png
[20200914224428123.png]: https://img-blog.csdnimg.cn/20200914224428123.png
[20200914224456632.png]: https://img-blog.csdnimg.cn/20200914224456632.png
[20200914224514557.png]: /images/20221123/52b1ab5bfcdc41dabd2b81d894215fae.png
[20200914224547264.png]: https://img-blog.csdnimg.cn/20200914224547264.png
[20200914224559915.png]: https://img-blog.csdnimg.cn/20200914224559915.png
[20200914224616271.png]: /images/20221123/6d48408659cb42f3992995d524566694.png
[amcl]: https://blog.csdn.net/weixin_40114832/article/details/78322034?utm_medium=distribute.pc_relevant.none-task-blog-title-5&spm=1001.2101.3001.4242
[amcl 1]: https://wenku.baidu.com/view/92a45164a9956bec0975f46527d3240c8447a1d5.html#
[AMCL 1]: https://blog.csdn.net/gadwgdsk/article/details/103364846/?utm_medium=distribute.pc_relevant.none-task-blog-title-1&spm=1001.2101.3001.4242
[AMCL_1]: https://blog.csdn.net/weixin_42958308/article/details/106397799?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-1.add_param_isCf&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-1.add_param_isCf
[AMCL 2]: https://blog.csdn.net/qq_33742147/article/details/105489516?utm_medium=distribute.pc_relevant.none-task-blog-blogcommendfrommachinelearnpai2-4.add_param_iscf&depth_1-utm_source=distribute.pc_relevant.none-task-blog-blogcommendfrommachinelearnpai2-4.add_param_iscf