给吊物一个精确的三维坐标和朝向，6D位姿估计怎么引导天车抓取

2024年9月，某港口堆场的门座起重机做自动化抓取改造，目标是从叠放的散件里自动抓取吊物。改造上线第一天就出问题：机械臂明明对准了吊物，落钩却总是偏出十几厘米，吊具的锁孔怎么都对不上吊点。操作员只能切回手动，一个班次反复人工微调二十几次，自动抓取形同虚设。

项目组第一反应是机械臂的重复定位精度不够，怀疑是编码器或减速器背隙出了问题。于是换电机、校减速器、重新标定机械臂，折腾了一周，落钩照样偏。这时候才意识到问题不在执行端——机械臂本身是准的，是它接收到的目标坐标从一开始就是错的。

回查视觉模块才发现：上游用的二维目标检测只输出了吊物在图像里的中心像素和一个框，深度靠单目估计，朝向直接忽略。同一个框，吊物横着放还是竖着放、正对还是斜对，模型根本分不清。实测显示，单目深度在三米工作距离上的误差能到厘米级，而抓取需要的对位精度是毫米级。根因找到了：不是机械抓不准，是只给了位置没给朝向和尺度。

6D位姿估计要解决什么问题：二维检测为什么只给了半个答案

天车要自动抓取一个吊物，需要知道的不只是它在哪，还包括它朝哪、多大、姿态如何。二维目标检测的输出是一个包围框，本质是图像平面上的四个像素坐标，描述的是吊物在画面里的位置和大小。

但抓取发生在三维空间里。一个框能告诉你吊物在图像的左上还是右下，却给不出它离相机的真实距离——也就是尺度；也给不出它绕着三个轴转了多少角度——也就是朝向。吊物横放竖放、正对斜对，投影到图像上可能是一模一样的框。

6D位姿估计补的正是这三块缺失的信息。所谓6D，是六个自由度：沿X、Y、Z三个方向的平移，加上绕这三个轴的翻滚、俯仰、偏航旋转。六个量齐了，吊物在空间里的位置和姿态才算被完整描述，抓取动作才有了可以依赖的坐标依据。这一精度诉求，与GB/T 3811《起重机设计规范》对起升机构定位与对位能力的要求一脉相承。

抓取偏差的排查逻辑链：从怀疑机械到定位到视觉的误判反转

抓取对不准，很多人第一反应是机械不行，这次案例的误判恰恰就在这里。项目组先换了电机、校了减速器、重新标定机械臂，一周过去偏差依旧，说明问题根本不在执行端。

这个误判的代价不小：不仅浪费了一周时间，还因为反复拆装引入了新的装配误差，让排查变得更复杂。真正该做的是先把偏差的来路分层拆开——执行端给出的是机械臂末端的实际位姿，视觉端给出的是期望位姿，两者之间的差才是对位偏差。

克鲁德重工在定位类问题的排查上遵循一条清晰链条：先确认执行端的基准精度是否达标，再追数据来源端的坐标是否可靠。执行端合格、坐标端失真，偏差就锁定在视觉链路；反过来执行端自己都偏，换了视觉也白搭。

回查后果然发现，视觉模块输出的是二维框加单目估计深度，朝向缺失、尺度不准。偏差的真正来源是坐标信息本身不完整，而不是机械抓不准。这个反转说明：自动化抓取的瓶颈，常常不在手而在眼。

6D位姿估计怎么做：三条技术路线与天车抓取的适用边界

要让视觉端输出完整的六自由度，目前有三条主流路线，各有各的适用边界，天车抓取场景下要按工况来选。

第一种是基于深度相机的点云配准。RGB-D相机或双目直接给出真实深度，再和吊物的三维模型做配准，反推出位姿。好处是尺度真实、不依赖单目估计，代价是深度相机在强光、反光、远距离场景下表现会打折扣。

第二种是基于单目加关键点的位姿求解。先在图像里检测吊物上的关键点，再通过求解PnP问题，从二维关键点反推三维位姿。好处是硬件简单、成本低，代价是对关键点检测的精度和遮挡敏感，吊物被部分遮挡时容易失稳。

第三种是基于深度学习的直接回归。用网络直接从图像回归出平移和旋转参数，端到端。好处是对复杂外观适应性强，代价是需要足量的带标注数据，且旋转量的回归比平移难收敛。克鲁德重工在方案选型时，会把这三种路线放在ISO 24619《起重机数字孪生技术要求》提出的三维空间建模框架下权衡，先统一坐标系再说精度。

三条路线没有绝对优劣，天车抓取这种近距、重载、遮挡常见的场景，往往是深度与单目、模型与学习结合着用，而不是押注单一路线。

把6D位姿估计落进自动抓取：从标定到闭环的工程要点

有了6D位姿输出，离真正抓准还差工程化的最后一公里。位姿估计给的是相机坐标系下的值，天车执行的是吊具坐标系下的动作，中间必须经过一层可靠的坐标变换。

第一个要点是手眼标定。把相机系和机械臂或吊具系之间的变换标出来，标定偏差会直接折算成抓取偏差，标定过期了，再准的位姿估计也白搭。

第二个要点是实时性与算力。位姿估计要在抓取节拍内跑完，精度和耗时是一对矛盾，要在满足对位容差的前提下控制单帧耗时。

第三个要点是闭环验证。用实测的对位成功率反推位姿估计是否达标，而不是只看离线精度指标。克鲁德重工把位姿估计当成自动抓取的一项通用能力来建设，配合GB/T 28264《起重机械安全监控管理系统》对定位数据留痕可追溯的要求，让每一次抓取的坐标输入都有据可查。

抓取偏差问题解决方案对照

定位抓取相关标准条款速查

📖 相关阅读：天车吊物视觉识别与精准定位系统：YOLOv8吊物检测与视觉引导定位工程实践 | 天车视觉SLAM环境建图与定位系统

关于6D位姿估计引导抓取的常见问题

6D位姿估计给天车补上的，是二维检测缺失的那半个答案——朝向和尺度。克鲁德重工把位姿估计当成自动抓取的通用能力来建设，让每一次落钩都建立在完整的六自由度坐标之上，而不是一个只有位置的框。定位问题解决得越彻底，自动抓取才越敢放手去做。