蒙特卡罗定位

考虑一个内置环境地图的机器人。当机器人四处移动时，它需要知道它在这张地图上的位置。通过使用传感器观测值来确定它的位置和旋转(更一般地说，姿态)被称为机器人定位。

因为机器人可能并不总是以一种完全可预测的方式运行，所以它会随机估计下一步会在哪里。这些估计被称为粒子。每个粒子都包含对未来可能状态的完整描述。当机器人对环境进行观测时，它会丢弃与观测值结果不一致的粒子，并产生更多与观测值接近一致的粒子。最后，预期上大多数粒子会汇聚到机器人实际所在的位置。

机器人的状态表述取决于应用与设计。例如，典型2D机器人的状态可以由元组   组成，   表示位置，  表示方向。对于具有10个关节的机械臂，状态可以是包含每个关节角度的元组  。

机器人对其当前状态的概率估计，是分布在状态空间中的概率密度函数。^[1][4] 在MCL算法中，一次估计由一组  个粒子表示  。^[4] 每个粒子都包含一个状态，因此可以被认为是机器人状态的假设。状态空间中有许多粒子的区域对应于机器人以更大概率出现——而粒子较少的区域则不太可能是机器人在的地方。

该算法假设了马尔可夫性，即当前状态的概率分布仅取决于前一状态(而不是之前的任何状态)，   仅取决于  。^[4] 只有当环境是静态的并且不随时间变化时，这种假设才有效。^[4] 通常，在启动时，机器人没有其当前姿态的信息，因此粒子在配置空间中均匀分布。^[4]

给定环境地图，MCL算法的目标是让机器人确定其在环境中的姿态。

在每个时刻   该算法将先前的状态估计  、控制量  与从传感器接收的数据  作为输入；算法输出新的状态估计。^[4]

   算法MCL
   (d ):
       
   
       为 
    到 
   (d ):
           
    运动更新(_ u)
   
           
    传感器_更新
   
           
   
       endfor
       为 
    到 
   (d ):
           画 
    从 
    有可能 
   
           
   
       endfor
       返回 
   

3.1 1D机器人例子

机器人沿着一维走廊行进，装备有一个传感器，它只能分辨场景有门(左)还是没有门(右)。

考虑一个机器人有三个相同的门的一维圆形走廊里，其使用一个传感器，该传感器根据门是否存在而返回真或假的测量值。

在三次迭代结束时，大部分粒子根据预期收敛到机器人的实际位置。

在运动更新期间，机器人基于给定的控制命令将模拟运动应用于每个粒子，以预测其新的位置。^[1] 例如，如果一个机器人向前移动，所有的粒子都会朝着自己的方向前进，不管它们指向哪个方向。如果机器人顺时针旋转90度，所有粒子都会顺时针旋转90度，不管它们在哪个地方。然而，在现实世界中，没有一个执行器是完美的:它们可能会实现超过或低于期望的运动量。当机器人试图直线行驶时，由于车轮半径的微小差异，路径不可避免地会向一边或另一边弯曲。^[1] 因此，运动模型必须补偿噪声。因此，运动模型必须补偿噪声。所以，在运动更新过程中，粒子不可避免地会发散。这是意料之中的，如果机器人在没有感知环境的情况下盲目移动，它会逐渐无法确定自己的位置。

当机器人观测环境时，它会更新粒子，以更准确地反映自身位置。对于每一个粒子，机器人都会计算出，如果它处于粒子状态，它会以何概率感测到传感器的实际感测值。并为每个粒子分配一个权重   ，该权重按比例描述了该概率。然后，它随机抽取   个来自先前估计的新粒子，它们的概率与  成比例。与传感器读数一致的粒子更有可能被选择(可能不止一次)，与传感器读数不一致的粒子则较少被选择。因此，粒子会收敛到对机器人状态的更好估计上。这是意料之中的，机器人在感知环境时会逐渐确定自己的位置。

6.1 非参数性

以粒子滤波器为中心的MCL可以近似多种不同类型的概率分布，因为它是非参数表示的。^[4] 其他一些贝叶斯定位算法，例如卡尔曼滤波器(和变体，扩展卡尔曼滤波器和无迹卡尔曼滤波器)，假设机器人的状态概率分布接近高斯分布，并且在状态概率分布是多峰的情况下表现不好。^[4] 例如，一个机器人在一个长走廊里，有许多看起来相似的门，它可能会认为每个门都有一个峰值，但他无法分辨自己在哪个门。在这种情况下，粒子滤波器可以比参数滤波器提供更好的性能。^[4]

马尔可夫定位的另一种非参数方法是基于网格的定位，它使用直方图来表示状态概率分布。与基于网格的方法相比，蒙特卡洛定位更精确，因为样本中表示的状态没有被离散化。^[2]

6.2 计算要求

粒子滤波器的时间复杂度与粒子数量成线性关系。自然，粒子越多，精度越高，因此速度和精度之间存在折衷，并且我们希望找到  的最佳值。确定  的一种策略是连续生成额外的粒子，直到接受到下一对指令  和传感器读数  。^[4] 这样，在不妨碍机器人其余部分功能的情况下，可以获得尽可能多的粒子。因此，其自适应地使用了可用的计算资源:处理器越快，生成的粒子越多，因此算法越精确。^[4]

与基于网格的马尔可夫定位相比，蒙特卡罗定位减少了内存使用，因为内存使用仅取决于粒子的数量，不随地图的大小而缩放，^[2] 并且可以以更高的频率测量。^[2]

可以使用如下所述的KLD采样来改进该算法，该采样根据机器人对其位置的确定程度来调整要使用的粒子数量。

6.3 粒子耗尽

基础的蒙特卡洛定位的缺点出现在机器人不移动地处在一个位置并且重复感知环境的场景中。^[4]假设粒子都朝着一个错误的状态会聚，或者假设一只神秘的手拿起机器人，在粒子已经会聚之后将它移动到一个新的位置。由于远离收敛状态的粒子很少被选择用于下一次迭代，它们在每次迭代中变得更少，直到它们完全消失，此时，算法无法恢复。^[4] 这一问题更有可能发生在粒子量少，例如  时。或当粒子分布在大的状态空间上时。^[4] 事实上，任何粒子滤波算法都可能在重采样步骤中意外丢弃接近正确状态的所有粒子。^[4]

缓解这个问题的一种方法是在每次迭代中随机添加额外的粒子。^[4] 这相当于假设，在任何时间点，机器人以一种很小的概率被移动到地图中的随机位置，从而在运动模型中产生一小部分的随机状态。^[4] 通过保证地图中每个区域都存有粒子，可以使该算法对粒子耗尽具有鲁棒性。

基本的蒙特卡罗定位算法相当简单。已有几种该算法的变体被提出，这些变体有些解决了其缺点而有些使其在某些情况下更为有效。

7.1 KLD采样

蒙特卡洛定位可以通过基于库尔巴克-莱布勒散度(KLD)的误差估计以自适应方式采样粒子来改进。最初，使用一个大的   是有必要的。因为需要用均匀随机分布的粒子覆盖整个地图。然而，当粒子聚集在同一位置时，保持如此大的样本量在计算上是浪费的。 ^[6]

KLD—采样是蒙特卡洛定位的一种变体，在每次迭代中，样本大小   是经过计算的。样本大小  是经过计算的。其通过概率  计算以使真实后验近似和基于样本的近似之间的误差小于  。其中变量   和  是固定的参数。^[4]

KLD的主要思想是创建一个覆盖在状态空间上的网格(直方图)。直方图中的每个仓最初都是空的。在每次迭代中，一个新的粒子都从先前的(加权的)粒子集中抽取，其概率与其权重成正比。KLD采样算法从先前加权的粒子集中提取粒子，并在将粒子放入粒子仓之前应用运动更新和传感器更新，而不是在经典MCL中进行重采样。该算法跟踪非空仓的数量  。如果一个粒子被插入到先前为空的容器中，  的值被重新计算，这使得  基本呈线性增加。重复此操作，直到样本量   与  相同。 ^[4]

很容易看到KLD采样从粒子集中剔除粒子，只在填充新位置（仓）时增加  。实际上，KLD采样始终比经典的MCL表现更好，收敛更快。^[4]