4. 点云数据获取与预处理

3D无序抓取系统的第一道“数据关口”不是抓取算法，而是点云质量。相机输出的原始点云通常包含大量与目标无关的信息：料框底面、侧壁、支撑结构、环境背景、传感器噪声、反光造成的飞点，以及因遮挡产生的空洞。如果直接把这些数据送入位姿估计或抓取规划，算法会把大量时间消耗在无效区域，甚至把料框边缘误判为工件。因此，点云预处理的目标非常明确：把“相机看到的原始点云”变成“机器人能用的场景点云”——只保留工件候选区域，并让点云与机器人坐标系建立可复现的对应关系。

本节围绕一条完整的预处理链展开：点云滤波 → 降采样 → 离群点去除 → 平面分割 → 坐标系对齐。每一步都有明确的输入、输出和判断依据，顺序不能随意颠倒。例如，先降采样再滤波会丢失细小特征；先做平面分割再去除离群点，可能把料框底面的噪声误当成平面的一部分。下面按实际处理顺序说明关键操作与参数选择。

一、点云滤波：先划定“只看哪里”

滤波的第一步不是去噪，而是限定感兴趣区域（ROI）。以常见的料框抓取工位为例，相机安装在料框上方约 0.8 m 处，料框尺寸约 600 mm × 400 mm × 200 mm。原始点云中会包含地面、周边设备、操作人员等大量无关点。此时使用直通滤波（PassThrough）沿 Z 轴保留 0.3~1.2 m 范围，再沿 X、Y 轴裁剪到料框外沿以内 20 mm 的区域，可以一次性去掉 70% 以上的无效点。这一步的判据很直观：可视化后，点云应只剩料框内部和上沿，地面与背景完全消失。

如果相机安装在机械臂末端（Eye-in-Hand），ROI 需要随末端位姿动态计算，不能写死为固定坐标。此时通常把点云先变换到工具坐标系，再在工具坐标系下做直通滤波，这样无论机械臂走到哪里，ROI 都稳定覆盖料框区域。

二、降采样：让点云“变轻”但不丢形状

结构光或 ToF 相机单帧点云可达 30 万~200 万点。直接送入后续算法，单帧耗时可能超过 1 s，无法满足抓取节拍。降采样最常用的方法是体素栅格滤波（VoxelGrid）：把空间划分为边长 l 的立方体体素，每个体素内所有点用其质心代替。体素边长 l 的选择需要权衡：l 过大，工件边缘和圆角被抹平，位姿估计精度下降；l 过小，降采样效果不明显。

一个可操作的取值方法是：先测量工件最小特征尺寸，例如某工件最小圆角半径为 5 mm，则体素边长取 2~3 mm，保证每个特征上仍有 2~3 个体素跨越。对于 100 万点的原始点云，体素边长 3 mm 通常可降至 8 万~15 万点，降采样后点云仍能清晰看出工件轮廓。判断降采样是否合格，可以观察工件边缘是否连续、平面是否出现明显锯齿；若边缘断裂，说明体素过大。

三、离群点去除：剔除“不该存在”的点

反光、多路径反射和传感器噪声会产生飞点。这些点通常远离主体点云，表现为稀疏的孤立簇。常用的统计离群点去除（Statistical Outlier Removal）对每个点计算它到最近 K 个邻居的平均距离，假设该距离服从高斯分布，若某点的平均距离超过全局均值加 n 倍标准差，则判为离群点并删除。典型参数为 K=30、标准差倍数 1.5。若飞点密度较高，可改用半径滤波：规定半径 r=3 mm 内邻居数少于 5 的点删除。

需要注意：离群点去除不能过度。如果工件本身存在细小凸起或薄壁结构，过大的 K 值或过小的标准差倍数会把真实特征一并删掉。建议先用统计滤波处理，再可视化检查工件边缘是否完整；若发现工件局部被削掉，应放宽阈值或改用半径滤波。

四、平面分割：把料框“拿掉”

料框底面和侧壁是最大的干扰平面。平面分割的目标是识别并剔除这些平面，只保留工件候选点。最常用的方法是RANSAC 平面拟合：随机选取三个点构成平面，统计距离该平面小于阈值 d 的内点数，迭代若干次后取内点最多的平面作为最优平面。典型参数为距离阈值 2~5 mm、迭代 200 次。对于料框，通常需要分割两次：第一次分割底面，第二次分割侧壁；也可以按法向量方向区分，底面法向量近似竖直，侧壁法向量近似水平。

分割后应检查剩余点云是否只包含工件。如果料框底面有残留，说明距离阈值偏小或迭代次数不足；如果工件底部被误删，说明阈值偏大。一个实用的验证方法是：把分割后的点云按高度着色，工件区域应呈现连续的高度变化，而底面区域应基本消失。

五、坐标系对齐：让点云“说机器人的语言”

经过上述步骤，点云已经干净，但它仍处于相机坐标系。机器人无法直接使用相机坐标下的位姿，必须通过手眼标定把点云变换到机器人基坐标系。标定结果是一个 4×4 齐次变换矩阵 T，包含旋转 R 和平移 t。对于 Eye-to-Hand 结构，相机固定在支架上，T 是常量；对于 Eye-in-Hand 结构，T 随机械臂位姿变化，需要实时计算。

对齐后的验证方法：在料框内放置一个已知尺寸的标准球或标定板，用机器人末端去触碰其表面，比较机器人坐标与点云坐标的偏差。若重投影误差小于 2 mm，通常认为对齐合格。此外，还要确认 Z 轴方向与抓取方向一致——多数系统约定机器人基坐标系 Z 轴竖直向上，点云中工件顶面应位于料框底面之上，若方向相反，需要在变换矩阵中修正。

六、形成可用场景点云的判据

经过上述五步，最终输出的场景点云应满足以下条件：第一，点云仅包含料框内工件候选区域，底面与侧壁已剔除；第二，点密度均匀，无大面积空洞或飞点簇；第三，点云坐标已变换到机器人基坐标系，Z 轴方向与抓取方向一致；第四，单帧处理耗时满足节拍要求，通常控制在 200 ms 以内。只有同时满足这四条，后续的位姿估计和抓取规划才有稳定的输入基础。

实际调试中，建议把每一步的中间结果保存为 PCD 文件，便于对比不同参数的效果。例如，体素边长从 2 mm 改为 5 mm 后，点云数量从 15 万降到 3 万，但工件边缘是否仍然清晰，需要回看中间结果才能判断。预处理不是“一次调好就永远不变”的环节，更换工件、调整相机位置或环境光照变化后，都需要重新检查关键参数。