FastSLAM 1.0

摘要

基于特征的使用粒子滤波器的即时定位与建图
FastSLAM – Feature-based SLAM with Particle Filters

关键词：

• Rao-Blackwellization Particle Filter

介绍

我们注意到粒子滤波器在一些最有效的机器人算法中占据着核心的地位。 这就产生了一个问题，粒子滤波器是否可以用于SLAM问题。不幸的是，粒子滤波器受限于空间维度，高斯类算法状态估计问题的维度还在线性和二次方之间，而粒子滤波器则是指数级的。 直接将粒子滤波器应用于SLAM问题注定会，因为描述地图时所需的大量变量而失效。

本章中算法建立在SLAM问题的一个重要特性上，到目前为止我们还没有讨论它。特别的，在已知关联度的完全SLAM问题中，给定机器人位姿，在地图中任意两个不相交的特征集合之间， 存在着条件独立性。换句话说，如果有条神谕告诉我们真正的机器人路径，我们就可以不依赖于其它特征，来估计所有的特征位置。只有当不确定机器人位姿的时候，这些估计的依赖性才存在。 这一结构性的观察，将使得我们有可能使用一种称为Rao-Blackwellized particle filter(简称为RB滤波器)的粒子滤波器来解决SLAM问题。RB滤波器使用粒子集合来表示一些变量的后验概率， 同时用高斯或者其它参数化的概率密度来表示剩下的所有变量。

• Proposed by Montemerlo et al. in 2002
• Each landmark is represented by a 2x2 EKF
• Each particle therefore has to maintain N individual EKFs

地标(Landmark)和占用栅格(Occupancy Grids)

• An occupancy grid makes no assumption about types of features.
• An occupancy grid (typically) has fixed resolution.
• An occupancy grid takes space and time the size of the environment to be mapped.

步骤

• Extend the path posterior by sampling a new pose for each sample
• Compute particle weight
• Update belief of observed landmarks (EKF update rule)
• Resample

示例

首先，根据算法需要定义一个particle类：

class Particle:

    def __init__(self, N_LM):
        self.w = 1.0 / N_PARTICLE
        self.x = 0.0
        self.y = 0.0
        self.yaw = 0.0
        # landmark x-y positions
        self.lm = np.zeros((N_LM, LM_SIZE))
        # landmark position covariance
        self.lmP = np.zeros((N_LM * LM_SIZE, LM_SIZE))

particles = predict_particles(particles, u)

particles = update_with_observation(particles, z)

particles = resampling(particles)

参考

1. FastSLAM: An Efficient Solution to the Simultaneous Localization And Mapping Problem with Unknown Data Association
2. https://gaoyichao.com/Xiaotu/?book=probabilistic_robotics&title=pr_chapter13
3. http://ais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/slam10-fastslam-4.pdf

算法 | FastSLAM 1.0

原文：https://www.cnblogs.com/casperwin/p/12558377.html