上海建站网络科技有限公司网站制作视频教程下载

SuMa++是基于Surfel的SLAM算法SuMa的改进版本，通过引入语义分割信息提升动态环境下的鲁棒性和回环检测性能。以下从算法原理和公式推导两方面详细阐述：

---

一、SuMa++算法原理

1. 基础：SuMa算法

SuMa使用Surfel（表面元素）构建地图，每个Surfel包含位置、法向量、半径和时间戳。关键步骤包括：

• 帧间匹配：利用ICP（迭代最近点）算法，通过最小化当前帧点云与地图Surfel之间的点到平面距离，求解位姿变换。
• 地图管理：根据新数据更新Surfel，淘汰过时元素。
• 回环检测：基于几何一致性的场景识别和位姿图优化。

2. SuMa++的改进

• 语义分割融合：使用RangeNet++对点云进行语义分割，区分静态与动态物体（如车辆、行人）。
• 动态点剔除：在ICP优化中，为动态点分配低权重或直接剔除，减少其对位姿估计的影响。
• 语义增强的回环检测：结合语义标签与几何特征，生成更具区分度的场景描述子。

---

二、公式推导

1. 语义加权的ICP优化

目标函数：引入语义权重的最小化点到平面距离。
$$\underset{T}{\min }\sum _i{w}_i{\left[(\mathbf{T}\cdot {\mathbf{p}}_i-{\mathbf{q}}_i)\cdot {\mathbf{n}}_i\right]}^2$$

• $(\mathbf{T}\in SE(3))$：待求的位姿变换矩阵。
• $({\mathbf{p}}_i)$：当前帧点云中的点。
• $({\mathbf{q}}_i,{\mathbf{n}}_i)$：地图中对应Surfel的位置和法向量。
• $(w_i)$：语义权重（静态物体$(w_i=1)$，动态物体$(w_i=0)$）。

雅可比矩阵与优化：

• 残差项：$(r_i=w_i(\mathbf{T}\cdot {\mathbf{p}}_i-{\mathbf{q}}_i)\cdot {\mathbf{n}}_i)$
• 李代数参数化：$(\mathbf{T}=\exp ({\mathbf{\xi }}^{\wedge }))$，其中$(\mathbf{\xi }\in \mathfrak{se}(3))$
• 雅可比计算：对$(\mathbf{\xi })$求导，利用链式法则展开：
  $${\mathbf{J}}_i=\frac{\partial r_i}{\partial \mathbf{\xi }}=w_i{\mathbf{n}}_i^T\left[-\mathbf{I}(\mathbf{T}\cdot {\mathbf{p}}_i{)}^{\wedge }\right]$$
• 高斯-牛顿法求解增量$(\Delta \mathbf{\xi })$：
  $$\left(\sum _i{\mathbf{J}}_i^T{\mathbf{J}}_i\right)\Delta \mathbf{\xi }=-\sum _i{\mathbf{J}}_i^T{r}_i$$

2. 语义分割与权重分配

• RangeNet++输出：对点云投影生成的深度图像进行逐像素分类，获得语义标签。
• 权重策略：
  • 静态类别（道路、建筑）：$(w_i=1)$
  • 动态类别（车辆、行人）：$(w_i=0)$
  • 半静态类别（植被）：$(w_i\in (0,1))$（依场景调整）

3. 语义增强的回环检测

• 描述子生成：结合几何特征（如FPFH）与语义统计（如各类别占比）。
• 相似性度量：对两个区域( A, B )，计算联合相似度：
  $$S(A,B)=\lambda S_{\text{geo}}(A,B)+(1-\lambda )S_{\text{sem}}(A,B)$$
  • $(S_{\text{geo}})$：几何特征余弦相似度。
  • $(S_{\text{sem}})$：语义分布的卡方检验相似度。
  • $(\lambda )$：平衡权重（通常取0.5）。

4. 地图更新策略

• Surfel语义融合：新观测与地图中Surfel的语义标签通过贝叶斯更新：
  $$P(c|z_{1:t})\propto P(z_t|c)P(c|z_{1:t-1})$$
  • ( c )：语义类别，( z_t )：当前观测。

---

三、关键改进分析

1. 动态环境鲁棒性：通过语义加权ICP，显著降低动态物体对位姿估计的干扰。
2. 回环检测增强：语义信息提升场景辨识力，减少外观变化（如昼夜、季节）导致的误匹配。
3. 计算效率：RangeNet++的极坐标投影与轻量解码器设计，确保实时性。

---

四、小结

SuMa++通过语义分割与SLAM的深度融合，解决了动态场景下的定位与建图难题。其核心在于将语义信息嵌入到ICP优化与回环检测中，通过数学建模提升系统鲁棒性。