slam学习笔记2024.11.15

一、环境地图模型

二、基于ROS的栅格地图构建方法分析

      在基于ROS下的二维SLAM中，有三种应用最广的建图算法，分别是基于 图优化的Cartographer算法、基于扫描匹配的HectorSlam算法、以及基于粒子 滤波的Gmapping算法。其中，前者建图效果最好，但对传感器要求也很高。Hector Slam算法在高线束激光雷达下有很好的建图效果，但是容易在建图的过程中出 现重影。而基于粒子滤波的Gmapping算法原理简单，发展相对成熟，也已经集 成在ROS功能包下，相对于前两种建图算法，后者对传感器要求并不高，建立 的地图也能保持一定的精度，但是建图过程中因为没有回环检测，在复杂场景下 的建图效果不如Cartographer算法。

2.1基于粒子滤波的Gmapping算法

      Gmapping算法另辟蹊径，他将 地图构建与无人车位姿估计看成两个问题分开求解。Gmapping算法是由GrisettiG等人提出的完成二维栅格地图构建的SLAM算法，该算法是一个开源的建图工具，可以直接在ROS上调用，并且可以很方便对周围环境进行地图构建。为了解决SLAM问题中同步定位与建图的难点， Gmapping算法采用RBPF方法将地图构建与无人车位姿估计看成两个问题分开 求解，先解决无人车的定位问题，在解决环境地图构建问题。

三、RBPF算法

**RBPF**（Rao-Blackwellized Particle Filter，拉奥-布莱克韦尔粒子滤波器）是一种用于解决非线性、非高斯状态估计问题的递归贝叶斯滤波方法，广泛应用于机器人定位、追踪、SLAM（同步定位与地图构建）等领域。

### 背景

在传统的粒子滤波（Particle Filter, PF）中，系统状态是通过粒子集的权重来表示的，每个粒子代表一个可能的系统状态。粒子滤波非常灵活，可以处理高维度和复杂的非线性系统，但其主要缺点是粒子数目需要很大才能保证估计的准确性，尤其是在状态空间较大时，计算量非常大。

**RBPF**方法通过结合**拉奥-布莱克韦尔化**（Rao-Blackwellization）技术来克服传统粒子滤波的这一局限性，显著提高了效率，特别是在存在部分线性状态方程时。

### 拉奥-布莱克韦尔化

拉奥-布莱克韦尔化是通过将一些状态变量“分解”为已知的和未知的部分，利用条件概率分布将复杂问题简化。RBPF通过这一技术，将状态空间分解为两部分：一部分是容易推断的（通常是线性部分），而另一部分是非线性或难以推断的（通常是高维部分）。这种分解使得在某些情况下能够在粒子滤波框架下使用更少的粒子来实现高效的估计。

### RBPF的核心思想

RBPF的核心思想是在粒子滤波的框架中，**将非线性状态模型分解为两部分**：一部分可以通过粒子滤波估计，另一部分则通过解析的条件推断或卡尔曼滤波进行估计。

1. **状态空间分解**：
   假设系统的状态可以分解为两部分：
   - **\( x_t \)**：容易计算的部分（例如线性动态系统或已知的部分）。
   - **\( z_t \)**：难以计算的部分（通常是非线性或高维部分）。
   
   假设我们可以在已知部分 \( x_t \) 上执行解析的推断，而对于 \( z_t \)，则使用粒子滤波来估计。

2. **拉奥-布莱克韦尔化**：  
   利用拉奥-布莱克韦尔定理，将状态估计通过两部分条件分解：
   \[
   p(x_t, z_t | y_{1:t}) = p(x_t | y_{1:t}) \cdot p(z_t | x_t, y_{1:t})
   \]
   这里：
   - \( p(x_t | y_{1:t}) \) 是通过粒子滤波估计的分布，表示已知部分的状态。
   - \( p(z_t | x_t, y_{1:t}) \) 是基于 \( x_t \) 和观测 \( y_{1:t} \) 的条件分布，通常通过卡尔曼滤波等方法来估计。

3. **粒子滤波**：  
   粒子滤波用于估计\( x_t \)的分布。对于每个粒子\( x_t^{(i)} \)，我们使用卡尔曼滤波或其他合适的方法来估计对应的\( z_t \)的条件分布。

### RBPF的流程

RBPF方法的核心流程包括：
1. **初始化粒子**：为每个粒子初始化状态\( x_0^{(i)} \)。
2. **粒子传播**：通过运动模型更新粒子集合，得到每个粒子对应的新的\( x_t^{(i)} \)。
3. **拉奥-布莱克韦尔化估计**：
   - 对于每个粒子\( x_t^{(i)} \)，使用卡尔曼滤波（或其他方法）来估计\( z_t \)的条件分布。
4. **权重更新**：根据观测信息\( y_t \)，计算每个粒子的权重，并更新粒子集合。
5. **重采样**：根据权重对粒子集合进行重采样，生成新的粒子集合。
6. **估计输出**：根据粒子集合的加权平均值，得出最终的状态估计。

### RBPF的优势

1. **高效性**：通过拉奥-布莱克韦尔化，RBPF减少了粒子数目的需求，尤其在某些部分状态模型线性或条件可计算时，计算效率大大提高。
   
2. **适用性广泛**：RBPF能够处理复杂的非线性和非高斯问题，特别是当状态空间有一部分可以通过简单的解析方法处理时。

3. **精度提升**：通过将状态分解为线性和非线性部分，RBPF能够在非线性系统中提供比标准粒子滤波更准确的估计。

### 应用领域

- **SLAM（同步定位与地图构建）**：RBPF被广泛应用于SLAM问题，尤其是在机器人导航中，当机器人同时需要估计位置和构建地图时，RBPF能够有效地处理机器人状态和地图状态的非线性动态。
- **目标跟踪**：在目标跟踪问题中，RBPF可以用于估计目标的状态，如位置和速度，特别适用于复杂、非线性的动态模型。
- **导航与定位**：RBPF在GPS信号不稳定或无法获得精确测量的环境下，能够结合多种传感器数据进行精确定位。

### 总结

RBPF（拉奥-布莱克韦尔粒子滤波器）是一种基于粒子滤波和拉奥-布莱克韦尔化的算法，通过将状态空间分解为易于处理和难以处理的部分来提高粒子滤波的效率和精度，广泛应用于机器人定位、追踪和SLAM等非线性、非高斯问题的状态估计中。

四、粒子滤波思想

粒子滤波（Particle Filter，简称PF）是一种基于蒙特卡洛方法的递推估计算法，主要用于动态系统的状态估计，尤其适用于非线性、非高斯系统的状态估计问题。它的核心思想是通过一组随机样本（粒子）来表示系统的状态分布，并通过重要性采样和重采样等操作，不断地逼近系统状态的真实分布。

### 粒子滤波的基本思想
粒子滤波的基本思想可以概括为以下几个步骤：

1. **初始化粒子集**：首先，给定初始的系统状态分布，通过随机生成一组粒子来表示系统的初始状态。这些粒子通过一定的概率分布进行初始化，每个粒子表示系统状态的一个可能值。

2. **预测步骤（Propagation）**：根据系统的动态模型（通常是通过一个状态转移方程描述的），使用前一时刻的粒子来预测当前时刻的粒子位置。即每个粒子根据上一时刻的状态和控制输入（如果有）进行传播，得到预测状态。

3. **更新步骤（Correction）**：根据新的观测值，使用观测模型对每个粒子的权重进行更新。每个粒子的权重反映了该粒子所代表的状态与当前观测的匹配程度。常用的方法是计算观测模型在该粒子状态下的似然值（如高斯分布），然后对粒子的权重进行调整。

4. **重采样步骤（Resampling）**：由于权重更新后，有些粒子的权重可能非常小，几乎不起作用，而有些粒子的权重可能较大。为了避免粒子贫化（即很多粒子的权重非常小，只有少数粒子对结果有贡献），需要通过重采样来“复制”权重大的粒子，并丢弃权重小的粒子。这一步的目的是让粒子的分布更加集中在当前的最佳估计位置上。

5. **估计步骤**：在每次迭代后，可以根据粒子集中的粒子和其权重来估计系统的状态。例如，可以通过加权平均来得到当前时刻的状态估计。

### 粒子滤波的优点
- **适用性强**：粒子滤波能够处理非线性、非高斯的系统，适用于传统卡尔曼滤波无法处理的情况。
- **灵活性**：可以处理多种复杂的动态系统模型，尤其适用于复杂的状态转移和观测模型。
- **全局优化**：由于粒子滤波采用蒙特卡洛方法，它可以通过样本表示状态的全局分布，而不仅仅是局部线性化，因此能更好地适应复杂的系统。

### 粒子滤波的缺点
- **计算开销大**：粒子滤波通常需要大量的粒子来保证估计精度，因此计算资源消耗较大，尤其在高维状态空间中，粒子的数量需要成指数级增长。
- **重采样带来的问题**：重采样虽然能避免粒子贫化，但也可能导致粒子分布过于集中，丧失了多样性。为此，可能需要额外的技术来改善重采样的效果。

### 应用
粒子滤波广泛应用于许多需要状态估计的领域，如：
- **机器人定位与导航**（如SLAM问题）
- **目标跟踪**
- **金融市场分析**
- **信号处理**
- **天气预报**

总的来说，粒子滤波通过将状态分布表示为一组粒子，并根据观测信息进行更新，是一种非常强大且灵活的状态估计方法。

4.1粒子是什么怎么得到的

在粒子滤波中，“粒子”代表的是对系统状态的一个假设或近似。每个粒子是一个**状态的可能实例**，并且每个粒子都有一个与其相关的权重。粒子滤波通过这些粒子的集合来表示系统状态的分布，并通过更新这些粒子的状态和权重来逼近真实的状态分布。

### 粒子的含义与作用
- **粒子**：粒子是系统状态的一个样本。系统的状态通常是高维的（例如，位置、速度、姿态等），每个粒子包含一个状态向量，代表系统的一种可能状态。
- **权重**：每个粒子都有一个与之相关的权重，表示该粒子的“重要性”。粒子的权重反映了该粒子在当前观测条件下的可能性（即该粒子与实际观测的匹配程度）。

### 粒子是如何得到的？
粒子的生成通常分为两个阶段：**初始化阶段**和**预测阶段**。

#### 1. 初始化粒子集
在粒子滤波的初始化阶段，首先需要生成一个粒子集合，通常通过从一个先验分布中采样来初始化粒子。这些粒子可以通过以下几种方式获得：

- **从先验分布采样**：如果系统的初始状态有已知的概率分布，可以根据该分布随机生成初始粒子。例如，如果系统状态是一个二维位置，可以从均匀分布、正态分布或其他适当的分布中随机抽取初始粒子的位置。
- **均匀分布初始化**：如果没有明确的先验信息，可以通过均匀分布或者其他假设分布来初始化粒子。比如，可以在一个范围内随机生成大量粒子。
- **经验初始化**：有时可以基于系统的物理特性或者先前的估计，通过某种规则初始化粒子。

初始化阶段后，每个粒子都有一个初始状态和初始权重（通常是相同的权重）。

#### 2. 预测粒子集
粒子滤波的工作流程中，粒子的状态会随着时间的推移发生变化，因此在每个时间步骤中，需要通过系统的状态转移模型来预测粒子的状态。

- **状态转移模型**：根据上一时刻的粒子状态和控制输入（如果有）来预测当前时刻粒子的状态。通常，这个模型可以通过一个概率模型或方程来描述，例如：
  \[
  x_{k} = f(x_{k-1}, u_k, \omega_k)
  \]
  其中，\(x_{k}\)表示当前时刻的状态，\(x_{k-1}\)是上一时刻的状态，\(u_k\)是控制输入（如速度或加速度），\(\omega_k\)是过程噪声。

- **粒子的预测**：每个粒子会根据上述模型，使用上一时刻的状态来“传播”到当前时刻。这通常是通过加入随机噪声来模拟系统的不确定性。

预测步骤后，每个粒子都有一个新的状态，但它们的权重暂时不变，仍然需要在下一步根据观测更新。

#### 3. 观测更新与权重调整
粒子的状态通过预测模型传播到当前时刻后，接下来要根据新获得的观测值来调整每个粒子的权重。每个粒子的权重表示该粒子与实际观测的匹配程度，通常通过以下步骤获得：

- **观测模型**：观测模型将粒子的状态与当前时刻的观测联系起来。假设观测是基于某种函数（例如测量误差模型）生成的，观测模型可以描述在给定状态下生成观测的概率：
  \[
  p(z_k | x_k)
  \]
  其中，\(z_k\)是观测数据，\(x_k\)是粒子的状态。

- **权重更新**：通过计算粒子状态下的观测似然函数，来更新粒子的权重。通常是通过计算粒子状态与实际观测之间的差异（误差），并通过概率密度函数来调整权重。常见的做法是使用似然比（例如，高斯分布的似然函数）来更新粒子的权重：
  \[
  w_k = w_{k-1} \cdot p(z_k | x_k)
  \]
  其中，\(w_k\)是当前粒子的权重，\(w_{k-1}\)是上一时刻粒子的权重。

#### 4. 重采样
当粒子的权重分布非常不均匀时（即大部分粒子的权重很小，只有少数粒子的权重大），为了避免粒子贫化（即只有少数粒子在提供信息），需要进行**重采样**。

- **重采样**：重采样的目的是生成新的粒子集，使得粒子的分布更加集中在高权重的区域。重采样过程可以通过以下步骤进行：
  1. 选择权重较大的粒子，并根据它们的权重“复制”出更多的粒子。
  2. 丢弃权重较小的粒子。
  
  这样，新的粒子集将反映出当前状态的高概率区域。

---

### 总结
粒子是通过从初始分布中采样并不断更新的动态样本，每个粒子代表系统状态的一个假设，并具有一个权重来反映其与观测数据的匹配程度。在每个时间步骤中，粒子被预测到新的状态并根据观测进行更新，经过多次迭代，粒子集能够逐步逼近系统的真实状态分布。