本章深入探讨机器人3D感知技术，从传感器原理到高级场景理解算法。我们将学习如何从原始传感器数据构建完整的3D环境模型，实现物体识别、位姿估计和语义理解。这些技术是机器人自主操作和导航的基础，直接影响任务执行的成功率。
LiDAR通过测量激光脉冲的飞行时间(Time of Flight)获取距离信息。机械式LiDAR通过旋转镜面扫描环境，固态LiDAR使用相控阵或MEMS技术实现电子扫描。
工作原理： \(d = \frac{c \cdot \Delta t}{2}\)
其中$c$为光速，$\Delta t$为激光往返时间。
关键参数：
优势：
劣势：
ToF相机使用调制光源和相位检测原理，通过测量反射光的相位差计算距离。
连续波调制原理： \(\phi = 2 \pi f_m \cdot \frac{2d}{c}\) \(d = \frac{c \cdot \phi}{4 \pi f_m}\)
其中$f_m$为调制频率（通常20-100MHz），$\phi$为相位差。
关键参数：
多路径干扰问题： 当光线经多次反射返回时，测量相位为多条路径的加权和： \(I_{measured} = \sum_i A_i \cdot e^{j\phi_i}\)
这导致凹角处产生”飞点”(flying pixels)。
结构光通过投影已知图案并分析变形来计算深度。
三角测量原理： \(Z = \frac{f \cdot b}{d}\)
其中$f$为焦距，$b$为基线长度，$d$为视差。
编码策略：
散斑结构光（如Kinect v1、iPhone FaceID）：
关键参数对比：
| 特性 | 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) |
深度传感器的精确标定是实现高质量3D感知的前提。标定误差直接传播到后续的所有处理步骤，因此需要特别重视。
内参标定（ToF/结构光相机）：
内参矩阵描述了3D点到像素坐标的投影关系： \(\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}\)
其中$(f_x, f_y)$为焦距，$(c_x, c_y)$为主点。对于深度相机，还需要标定深度畸变模型： \(d_{corrected} = d_{raw} \cdot (1 + k_1r^2 + k_2r^4) + k_3\)
标定过程通常使用棋盘格或AprilTag，采集20-50张不同姿态的图像。Zhang’s方法通过最小化重投影误差求解参数，典型精度可达0.3像素RMS。
外参标定（多传感器融合）：
外参描述传感器间的刚体变换关系。对于LiDAR-相机标定，常用方法包括：
优化目标函数： \(E = \sum_i \|p_i - \pi(T \cdot P_i)\|^2 + \lambda \sum_j \|n_j^T(T \cdot Q_j - q_j)\|^2\)
第一项为点重投影误差，第二项为平面约束。使用Levenberg-Marquardt算法迭代优化。
时间同步：
多传感器系统的时间同步至关重要，特别是在动态场景中。不同步会导致点云畸变和配准失败。
时间偏移估计： 对于软同步系统，可通过互相关分析估计时间偏移： \(\tau^* = \arg\max_\tau \sum_t f_1(t) \cdot f_2(t + \tau)\)
其中$f_1$、$f_2$为两个传感器的特征时间序列（如检测到的特征点数量）。
根据应用场景选择合适的深度传感器是系统设计的关键决策。以下是典型应用场景的推荐：
室内导航与建图：
机械臂抓取：
室外自主驾驶：
成本约束项目：
传感器冗余设计： 在安全关键应用中，需要考虑传感器失效模式：
原始点云数据通常包含噪声、离群点和冗余信息。有效的预处理是后续算法成功的基础。本节介绍点云处理的核心算法，重点关注实时性和鲁棒性的平衡。
体素格栅滤波（Voxel Grid Filter）： 将空间划分为立方体网格，每个体素保留一个代表点（质心或随机）。
对于体素大小 r：
1. 计算每个点的体素索引：(⌊x/r⌋, ⌊y/r⌋, ⌊z/r⌋)
2. 对同一体素内的点求质心
3. 时间复杂度：O(n)，空间复杂度：O(m)，m为体素数
统计滤波（Statistical Outlier Removal）： 基于邻域点距离的统计分布去除离群点。
对每个点$p_i$：
半径滤波（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$为高斯核函数。
ICP（Iterative Closest Point）算法：
基本ICP迭代步骤：
点到点ICP优化问题： \(E_{p2p} = \sum_{i=1}^n \|R \cdot p_i + t - q_i\|^2\)
闭式解（SVD方法）：
点到面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）：
配准初始化方法：
平面分割（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）： 基于法向量和曲率的生长：
基于图的分割（Graph-based）： 构建图$G=(V,E)$，顶点为超体素，边权重为相似度。使用归一化割（Normalized Cut）优化： \(NCut(A,B) = \frac{cut(A,B)}{assoc(A,V)} + \frac{cut(A,B)}{assoc(B,V)}\)
深度学习方法：
传统方法在复杂场景下往往失效，深度学习提供了端到端的解决方案：
法向量是描述局部几何的关键特征，用于配准、分割和重建。
PCA方法： 对点$p_i$的k-邻域进行主成分分析：
法向量定向： 法向量具有二义性（±n都valid），需要一致定向：
鲁棒估计： 对于噪声数据，使用RANSAC或加权最小二乘： \(C_{robust} = \sum_{j \in N_k(i)} w_j(p_j - \bar{p})(p_j - \bar{p})^T\)
权重$w_j$基于距离或法向量一致性。
特征描述子用于点云匹配和识别，需要对旋转、噪声和遮挡鲁棒。
FPFH（Fast Point Feature Histograms）：
SHOT（Signature of Histograms of Orientations）：
学习型描述子：
描述子性能评估指标：
6D位姿估计（3D位置+3D旋转）是机器人抓取和操作的核心技术。准确的位姿估计直接决定了机械臂能否成功接触和操作目标物体。本节介绍主流的位姿估计方法及其在实际系统中的应用。
6D位姿参数化：
主要挑战：
基于模型的方法假设已知物体的CAD模型或预先扫描的3D模型。
特征匹配方法：
PPF（Point Pair Features）： 对于模型上的点对$(m_1, m_2)$，计算特征向量： \(F(m_1, m_2) = (\|d\|, \angle(n_1, d), \angle(n_2, d), \angle(n_1, n_2))\)
其中$d = m_2 - m_1$，$n_i$为法向量。离线构建哈希表，在线投票确定位姿。
模板匹配方法：
迭代优化方法： 从初始估计开始，最小化重投影误差： \(E = \sum_i \rho(\|p_i - \pi(K \cdot T \cdot M_i)\|)\)
其中$\rho$为鲁棒核函数（如Huber），$K$为相机内参，$M_i$为模型点。
使用Levenberg-Marquardt或Gauss-Newton迭代优化。关键是良好的初始化和外点剔除。
无模型方法不需要预先的CAD模型，直接从数据学习位姿估计。
类别级位姿估计： 估计同类物体（如所有杯子）的标准化位姿。
关键点检测方法：
直接回归方法：
结合颜色和深度信息提高估计精度。
DenseFusion架构：
处理传感器噪声：
对称物体的位姿估计需要特殊处理。
对称性分类：
处理策略：
对称感知损失： \(L_{sym} = \min_{S \in Sym(O)} d(T_{pred}, T_{gt} \cdot S)\) 其中$Sym(O)$为物体的对称群
级联精化：
硬件加速：
失败检测与恢复：
在复杂场景中识别和定位多个物体是机器人感知的基本能力。本节讨论3D物体检测和实例分割技术，重点关注点云和RGB-D数据的处理方法。
3D物体检测输出物体的3D边界框（位置、尺寸、朝向）和类别。
问题表述：
基于点云的方法：
VoxelNet架构：
体素特征编码： \(f_{voxel} = \max_{p_i \in V} MLP(p_i - \bar{p}, p_i)\)
PointPillars优化：
基于图像的方法：
Frustum PointNet：
MonoDepth方法：
实例分割为每个物体实例生成精确的点级mask。
自顶向下方法：
自底向上方法：
SGPN (Similarity Group Proposal Network)：
相似度学习： \(S_{ij} = MLP(f_i, f_j, p_i - p_j)\)
ASIS (Associatively Segmenting Instances)：
损失函数： \(L = L_{sem} + \lambda_1 L_{ins} + \lambda_2 L_{assoc}\)
其中$L_{assoc}$强制同实例点的特征相近。
基于投票的方法：
VoteNet：
投票生成： \(v_i = p_i + \Delta_i\) 其中$\Delta_i = MLP(f_i)$预测到中心的偏移。
Transformer架构在3D感知中展现出强大能力。
3DETR架构：
查询机制：
Group-Free 3D：
结合多种传感器模态提升检测性能。
早期融合：
晚期融合：
深度融合：
MVX-Net架构：
融合操作： \(f_{fused} = \alpha \cdot f_{point} + (1-\alpha) \cdot f_{image}\)
其中$\alpha$通过注意力机制学习。
减少对精确3D标注的依赖。
弱监督方法：
自监督预训练：
对比损失： \(L_{contrast} = -\log \frac{\exp(sim(z_i, z_j)/\tau)}{\sum_k \exp(sim(z_i, z_k)/\tau)}\)
模型加速：
系统优化：
边缘部署考虑：