第12章：3D感知与场景理解

本章深入探讨机器人3D感知技术，从传感器原理到高级场景理解算法。我们将学习如何从原始传感器数据构建完整的3D环境模型，实现物体识别、位姿估计和语义理解。这些技术是机器人自主操作和导航的基础，直接影响任务执行的成功率。

12.1 点云获取：LiDAR vs ToF vs 结构光

12.1.1 LiDAR（激光雷达）

LiDAR通过测量激光脉冲的飞行时间(Time of Flight)获取距离信息。机械式LiDAR通过旋转镜面扫描环境，固态LiDAR使用相控阵或MEMS技术实现电子扫描。

工作原理： $$d = \frac{c \cdot \Delta t}{2}$$ 其中$c$为光速，$\Delta t$为激光往返时间。

关键参数：

  • 角分辨率：水平0.1°-0.4°，垂直0.2°-2°
  • 测距精度：±2-5cm（30m处）
  • 最大测程：100-300m（反射率>10%）
  • 点云密度：300k-2M点/秒
  • 视场角：水平360°，垂直30°-40°

优势：

  • 长测程，高精度
  • 不受环境光影响
  • 可获取反射强度信息

劣势：

  • 成本高（机械式$5k-50k）
  • 机械部件易损（MTBF ~5000小时）
  • 雨雾天性能下降
  • 对透明/高反射表面失效

12.1.2 ToF相机（飞行时间相机）

ToF相机使用调制光源和相位检测原理，通过测量反射光的相位差计算距离。

连续波调制原理： $$\phi = 2 \pi f_m \cdot \frac{2d}{c}$$ $$d = \frac{c \cdot \phi}{4 \pi f_m}$$ 其中$f_m$为调制频率（通常20-100MHz），$\phi$为相位差。

关键参数：

  • 分辨率：QVGA(320×240) - VGA(640×480)
  • 帧率：30-60 FPS
  • 测距精度：1%距离（标准偏差）
  • 测程：0.5-10m（室内），可扩展至30m
  • 视场角：60°-90°

多路径干扰问题： 当光线经多次反射返回时，测量相位为多条路径的加权和： $$I_{measured} = \sum_i A_i \cdot e^{j\phi_i}$$ 这导致凹角处产生"飞点"(flying pixels)。

12.1.3 结构光

结构光通过投影已知图案并分析变形来计算深度。

三角测量原理： $$Z = \frac{f \cdot b}{d}$$ 其中$f$为焦距，$b$为基线长度，$d$为视差。

编码策略：

  1. 时间编码：格雷码、相移法
  2. 空间编码：De Bruijn序列、伪随机点阵
  3. 混合编码：结合时间和空间信息

散斑结构光（如Kinect v1、iPhone FaceID）：

  • 投射伪随机红外点阵（~30k点）
  • 通过块匹配计算视差
  • 深度分辨率：$\delta Z = \frac{Z^2}{f \cdot b} \cdot \delta d$

关键参数对比：

| 特性 | LiDAR | ToF相机 | 结构光 |

特性 LiDAR ToF相机 结构光
测程 100-300m 0.5-10m 0.3-5m
精度 ±2-5cm 1%距离 0.1%距离
分辨率 稀疏点云 VGA级别 VGA-1080p
帧率 10-20Hz 30-60Hz 30Hz
室外性能 优秀 差 极差
成本 高($5k+) 中($500) 低($50)

12.1.4 传感器标定与同步

内参标定（ToF/结构光相机）： $$\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X/Z \\ Y/Z \\ 1 \end{bmatrix}$$ 外参标定（多传感器）： 使用标定板或自然特征点，最小化重投影误差： $$E = \sum_i |p_i - \pi(T \cdot P_i)|^2$$ 时间同步：

  • 硬件触发：精度<1μs
  • PTP协议：精度<1ms
  • 软件时间戳：精度~10ms（需要补偿）

12.2 点云处理：滤波、配准、分割

12.2.1 点云滤波

体素格栅滤波（Voxel Grid Filter）： 将空间划分为立方体网格，每个体素保留一个代表点（质心或随机）。

对于体素大小 r：

1. 计算每个点的体素索引：(⌊x/r⌋, ⌊y/r⌋, ⌊z/r⌋)
2. 对同一体素内的点求质心
3. 时间复杂度：O(n)，空间复杂度：O(m)，m为体素数

统计滤波（Statistical Outlier Removal）： 基于邻域点距离的统计分布去除离群点。

对每个点$p_i$：

  1. 计算k近邻平均距离：$\bar{d}_i = \frac{1}{k}\sum_{j \in N_k(i)} |p_i - p_j|$
  2. 计算全局均值和标准差：$\mu = \frac{1}{n}\sum_i \bar{d}_i$，$\sigma = \sqrt{\frac{1}{n}\sum_i (\bar{d}_i - \mu)^2}$
  3. 剔除条件：$\bar{d}_i > \mu + \alpha \cdot \sigma$（通常$\alpha = 2$）

半径滤波（Radius Outlier Filter）： 剔除指定半径内邻居数量小于阈值的点。适用于去除稀疏噪声。

双边滤波（Bilateral Filter）： 保边平滑，同时考虑空间距离和法向量相似度： $$p'_i = \frac{\sum_{j \in N(i)} w_s(|p_i - p_j|) \cdot w_r(|n_i - n_j|) \cdot p_j}{\sum_{j \in N(i)} w_s(|p_i - p_j|) \cdot w_r(|n_i - n_j|)}$$ 其中$w_s$、$w_r$为高斯核函数。

12.2.2 点云配准

ICP（Iterative Closest Point）算法：

基本ICP迭代步骤：

  1. 对应点查找：$c_i = \arg\min_{q \in Q} |T \cdot p_i - q|$
  2. 变换估计：$T^* = \arg\min_T \sum_i |T \cdot p_i - c_i|^2$
  3. 收敛判断：$\Delta E < \epsilon$ 或 迭代次数达到上限

点到点ICP优化问题： $$E_{p2p} = \sum_{i=1}^n |R \cdot p_i + t - q_i|^2$$ 闭式解（SVD方法）：

  1. 计算质心：$\bar{p} = \frac{1}{n}\sum p_i$，$\bar{q} = \frac{1}{n}\sum q_i$
  2. 去中心化：$p'_i = p_i - \bar{p}$，$q'_i = q_i - \bar{q}$
  3. 协方差矩阵：$H = \sum p'_i \cdot q'^T_i$
  4. SVD分解：$H = U\Sigma V^T$
  5. 旋转矩阵：$R = VU^T$（处理行列式为-1的情况）
  6. 平移向量：$t = \bar{q} - R\bar{p}$

点到面ICP（Point-to-Plane）： $$E_{p2plane} = \sum_{i=1}^n ((R \cdot p_i + t - q_i) \cdot n_i)^2$$ 使用Gauss-Newton迭代求解，收敛速度更快。

GICP（Generalized ICP）： 将点云不确定性建模为协方差矩阵： $$E_{GICP} = \sum_{i=1}^n (p_i - Tq_i)^T(C_i^p + TC_i^qT^T)^{-1}(p_i - Tq_i)$$ NDT（Normal Distribution Transform）：

  1. 将空间划分为网格
  2. 每个网格内的点拟合高斯分布$N(\mu, \Sigma)$
  3. 最大化点云与NDT场的匹配概率

配准初始化方法：

  • FPFH特征：基于点的邻域几何特征
  • 4PCS算法：随机采样共面四点组
  • 语义特征：利用物体类别信息约束

12.2.3 点云分割

平面分割（RANSAC）：

输入：点云P，迭代次数K，内点阈值d

1. for k = 1 to K:
2.   随机采样3个点，拟合平面：ax + by + cz + d = 0
3.   计算所有点到平面距离
4.   统计内点数量（距离<d）
5.   保留最大内点集合
6. 最小二乘重新拟合最终平面

迭代次数估计：$K = \frac{\log(1-p)}{\log(1-w^n)}$ 其中$p$为成功概率，$w$为内点比例，$n$为最小采样数。

欧式聚类（Euclidean Clustering）： 基于点间距离的区域生长算法：

1. 构建KD-Tree加速近邻搜索
2. for 每个未访问点p:
3.   创建新簇C，将p加入C和队列Q
4.   while Q非空:
5.     取出点q，标记为已访问
6.     找出q的r-邻域内所有点
7.     将未访问的邻域点加入C和Q

区域生长（Region Growing）： 基于法向量和曲率的生长：

  1. 计算所有点的法向量和曲率
  2. 从曲率最小的点开始生长
  3. 生长条件：法向量夹角<θ_n且曲率差<θ_c

基于图的分割（Graph-based）： 构建图$G=(V,E)$，顶点为超体素，边权重为相似度。使用归一化割（Normalized Cut）优化： $$NCut(A,B) = \frac{cut(A,B)}{assoc(A,V)} + \frac{cut(A,B)}{assoc(B,V)}$$

深度学习方法：

  • PointNet++：层次化特征学习
  • DGCNN：动态图卷积
  • MinkowskiNet：稀疏3D卷积