手眼标定开始

首先控制机器人采集不同姿态下的标定数据，共采集：

29 组训练姿态和8 组验证姿态

采用 OpenCV calibrateHandEye 方法进行初始求解，并进一步加入：

MAD（Median Absolute Deviation）异常值剔除方法，减少错误数据影响；基于 SE(3) 李群空间的非线性优化，对旋转和平移参数进行进一步精修。

实现：

目标像素坐标
        ↓
结合深度信息
        ↓
转换为相机坐标系3D位置
        ↓
通过手眼标定矩阵转换
        ↓
得到机器人 base 坐标系下目标位置

在调试过程中发现并修复了多个影响标定精度的问题：

1. 优化残差计算错误

原程序中残差计算错误，导致优化过程中残差始终接近全局平均值，无法有效优化参数。

修改后采用真实位姿误差作为优化目标，提高优化有效性。

1. 优化算法兼容问题

发现 LM 优化方法不支持 soft_l1 鲁棒损失函数，因此调整为 trf 优化方法，使优化过程能够结合异常值抑制。

1. Charuco 标定板检测问题

原程序中角点数量限制过低（4个），导致部分情况下 solvePnP 求解失败。

手眼标定实验结果

经过标定优化后，获得头部相机到机器人坐标系之间的转换关系：

相机坐标系：

camera_head_link → camera_left

最终结果：

• 平移误差：

x = -7.8 mm
y = +13.0 mm
z = +14.6 mm

• 姿态误差：

roll = 0.42°
pitch = 0.73°
yaw = -2.35°

---

标定精度评价结果：

训练数据：

• 平均位置误差：
  3.74 mm
• 姿态误差：
  0.41°

验证数据：

采用8组未参与求解的新姿态进行测试，其中包含头部 yaw ±40° 的大角度旋转：

• 位置误差：
  8.8 mm
• 姿态误差：
  0.34°

相比机器人 URDF 中原始相机位置估计误差约 687 mm，优化后精度提升约 80倍。

---

端到端抓取链路验证

进一步测试完整视觉定位流程：

相机检测目标
       ↓
RGB-D获取深度
       ↓
生成相机坐标系3D点
       ↓
手眼标定转换
       ↓
机器人base坐标系目标位置

选取3组新的机器人姿态进行测试：

结果：

• 11.4 mm
• 12.9 mm
• 15.4 mm

均小于 2 cm误差要求，证明当前视觉坐标转换链路能够满足机器人抓取任务需求。

以上代码均为ai实现，自己提供思路 ，然后ai执行+规划