4. 视觉位姿估计方法概览

在机器人抓取、装配、码垛与视觉引导对接任务中，位姿估计要回答的核心问题是：目标物体相对于相机或机器人基座，在三维空间中的位置（x, y, z）与姿态（roll, pitch, yaw）是多少。不同方法在输入数据、先验知识、计算代价和鲁棒性上差异很大。理解四类主流方法的边界，是后续评测与部署选型的基础。

4.1 四类方法的核心原理

基于特征的方法从图像或点云中提取可重复的局部特征，如 SIFT、ORB、SURF 或点云中的 FPFH、SHOT，再与已知三维模型的特征库进行匹配，通过 2D-3D 或 3D-3D 对应关系求解 PnP 或 ICP，得到 6D 位姿。其优势是无需大量训练数据，对纹理丰富、特征明显的物体精度高；缺点是弱纹理、重复纹理或强遮挡时匹配不稳定。

基于模板的方法预先在多个视角下渲染物体模板，或采集真实模板图像，在线阶段将输入图像与模板库做相似度匹配，取最优模板对应的视角作为粗位姿，再用边缘或深度做精配准。典型代表是 Linemod 及其变体。它适合无纹理、刚性、形状区分度高的工业零件，但对视角采样密度和遮挡敏感。

基于深度学习的方法通过 CNN 或 Transformer 直接回归位姿，或预测关键点、稠密对应、语义分割后再用几何方法求解。常见路线包括 PoseCNN、PVNet、DeepIM、FoundationPose 等。它能在杂乱背景和部分遮挡下保持较好鲁棒性，但依赖训练数据分布，跨域泛化与实时性需要重点评测。

基于点云的方法直接以深度相机、激光雷达或结构光获取的三维点云为输入，通过全局配准（FPFH + RANSAC + ICP）、局部配准或基于学习的点云网络（PointNetLK、DCP）求解位姿。它天然提供尺度与几何约束，精度高，但点云质量、噪声、缺失和初始位姿对结果影响显著。

4.2 方法流程与关键参数

四类方法在工程落地时通常遵循“粗定位 + 精配准”的两阶段思路。粗定位负责缩小搜索空间，精配准负责提升位姿精度。以下是一个典型的点云精配准参数配置示例：

# Open3D 全局配准 + ICP 精配准示例
voxel_size = 0.005          # 降采样体素，5mm
radius_feature = voxel_size * 5
radius_normal = voxel_size * 2

# 1. 计算 FPFH 特征
fpfh_src = compute_fpfh(pcd_src, radius_feature)
fpfh_tgt = compute_fpfh(pcd_tgt, radius_feature)

# 2. RANSAC 全局配准
result_ransac = registration_ransac_based_on_feature_matching(
    pcd_src, pcd_tgt, fpfh_src, fpfh_tgt,
    mutual_filter=True,
    max_correspondence_distance=0.01,
    estimation_method=TransformationEstimationPointToPoint(False),
    ransac_n=4,
    checkers=[CorrespondenceCheckerBasedOnEdgeLength(0.9),
              CorrespondenceCheckerBasedOnDistance(0.01)],
    criteria=RANSACConvergenceCriteria(100000, 0.999))

# 3. ICP 精配准
result_icp = registration_icp(
    pcd_src, pcd_tgt, 0.02, result_ransac.transformation,
    TransformationEstimationPointToPlane())

关键参数包括体素大小、特征半径、最大对应距离、RANSAC 迭代次数和 ICP 收敛阈值。体素过大丢失细节，过小则计算量激增；最大对应距离需与物体尺度和点云噪声匹配，通常取物体直径的 1%～5%。

4.3 典型场景与案例

在 3C 电子装配线上，连接器、屏蔽罩等零件纹理弱、形状规则，模板法配合边缘精配准可在 30ms 内完成识别，平移误差小于 0.5mm。在物流拆垛场景中，纸箱、包裹类别多、堆叠遮挡严重，深度学习法先做实例分割与关键点预测，再用点云 ICP 精配准，可稳定输出抓取位姿。在汽车零部件检测中，工件表面有加工纹理，特征法结合双目点云可获得高精度位姿，但需注意重复纹理导致的误匹配。

4.4 常见误区与复盘

• 误区一：认为深度学习方法一定优于传统方法。在训练分布内深度学习表现好，但换光照、换相机、换物体后可能急剧下降；传统方法在受控工业场景中反而更稳定。
• 误区二：忽略点云质量对精度的影响。结构光在黑色、反光、透明表面易产生空洞，直接导致 ICP 配准失败，需先做点云补全或改用多视角融合。
• 误区三：只评测精度不评测时延。机器人抓取通常要求 10～50ms 级响应，深度学习模型需做 TensorRT 量化与剪枝，否则难以满足节拍。
• 误区四：不做坐标系标定就谈位姿精度。手眼标定误差会直接叠加到位姿结果，标定残差应控制在 0.2mm 以内再评测算法。

4.5 四类方法对比清单