记录一套 eye-on-base 手眼标定、J2 形变 IMU 修正、离线表采集与拟合、以及最终“纯离线表一步到位 + IMU 仅记录”的运动控制方案。
关键词：手眼标定、eye-on-base、IMU、J2 形变、离线表、RBF、MoveIt、一步到位

---

目录

• 1. 背景与问题
• 2. 手眼标定：eye-on-base + IMU 修正 J2
• 3. IMU 零位与 J2 形变修正链
• 4. 离线表采集：Phase A~E
• 5. 离线表拟合：从 11 参数三角函数到 RBF 残差
• 6. 运动控制方案演变：融合 → 纯离线表一步到位
• 7. 纯离线表一步到位代码要点
• 8. 调试中遇到的问题与解决
• 9. 准确度评估：离线表到底有多准
• 10. 经验总结

---

1. 背景与问题

系统是 eye-on-base 结构：相机固定在基座附近，标定板/工作平面在工具杆末端视野内。末端工具杆较长（约 0.95m），J2 在大范围运动时存在明显形变，导致：

1. 编码器角度 j2_enc 与真实物理角度 j2_phys 不一致；
2. 用 URDF FK 算出的末端位置与真实末端有偏差；
3. 手眼矩阵 T_BASE_CAM 存在标定残差；
4. 大范围运动后末端停止位置在 xy 方向有 10~15mm 系统性偏差，z 方向约 2~3mm。

最初目标是“IMU 实时融合离线表”，后来经过调试，最终选择：

大范围运动用离线表一步到位；IMU 只做记录和诊断，不参与控制。

---

2. 手眼标定：eye-on-base + IMU 修正 J2

2.1 标定板与相机

• 棋盘格：7×11 内角点，方格 30mm，固定在工具杆末端附近；
• 相机内参 K、畸变 DIST 已提前标定；
• 求解使用 cv2.calibrateHandEye，尝试 TSAI / PARK / HORAUD / ANDREFF / DANIILIDIS，用 T_base_board 一致性挑最优；
• 传 link6 位姿，不叠加 TOOL_T；PnP 的 tvec 单位 mm 要除以 1000。

2.2 采集脚本关键要求

采集时 J2 不能直接用编码器，要用 IMU 实际角度做 FK：

j2_phys = zero_phys + IMU_SIGN * (imu_filtered - imu_zero)
j2_corr = j2_phys + J2_OFF - J1J2_SIN * sin(j1) * sin(j2_phys)
q_fk[1] = radians(j2_corr)

IMU 稳定性判断：

• settle 1.5s；
• 连续 3 个 0.5s 窗口 std < 0.05°；
• 稳定后才记录；
• 每条记录保存：j1~j5 编码器角度、j2_enc、j2_phys、j2_corr、link6 R/p、棋盘格角点像素。

2.3 求解与验证

• 对每帧 solvePnP + solvePnPRefineLM；
• 用 calibrateHandEye 得到 T_base_cam；
• 验证 T_base_board 在多帧之间的一致性；
• 输出 T_BASE_CAM 给 pick 程序硬编码或参数文件。

---

3. IMU 零位与 J2 形变修正链

IMU 零位标定流程：

1. 把 J2 摆到 0° 附近（大臂竖直）；
2. 采样 IMU 平均值 imu_zero；
3. 输入物理 J2 角度 zero_phys；
4. 保存 imu_zero、zero_phys、IMU_SIGN。

实时修正：

j2_phys = zero_phys + IMU_SIGN * (imu_avg - imu_zero)

手眼标定 FK 中再加 J1-J2 耦合修正：

j2_corr = j2_phys + J2_OFF - J1J2_SIN * sin(j1) * sin(j2_phys)

注意：后来离线表拟合的是 Δj2 = j2_phys - j2_enc，不再依赖这条固定公式，但 IMU 零位标定仍是记录 j2_phys 的基础。

---

4. 离线表采集：Phase A~E

采集程序是 PyQt5 + ROS2 + MoveIt 的自动采集工具，支持急停、IMU 稳定 20s 超时停机、按 label 去重。

4.1 采集网格

Phase	目的	网格
A	J2×J3	j1=0，j2∈[10,50]，j3∈[0,-40]
B	J1×J2	j3=0，j1∈[-40,40]，j2∈[10,50]
C	带载	j1=0，j2∈[10,50]，load=1.5
D	局部加密	j1=±15,±25,±35，j2=30,37,45，j3=0,-20
E	交叉补采	j1=-30,-25,-20,20,25,30，j2=0,5,10，j3=20,30,40

Phase D/E 是后来发现 j1=25, j2=37, j3 交叉区域偏差大后补的。
E 阶段最初想用 j3 负值，后来改为正值，并用 label 去重跳过已采集点。

4.2 记录字段

每条记录包含：

{
  "phase": "E",
  "label": "E_j1=+25_j2=+5_j3=+20",
  "target": [25, 5, 20, 0, 0],
  "j1_enc": 25.01,
  "j2_enc": 5.00,
  "j3_enc": 20.02,
  "j4_enc": 0,
  "j5_enc": 0,
  "imu_avg": -3.2,
  "imu_std": 0.01,
  "j2_phys": 8.1,
  "delta_j2": 3.1,
  "load": 1.0,
  "pos_warn": false
}

---

5. 离线表拟合：从 11 参数三角函数到 RBF 残差

5.1 v2：11 参数三角函数

拟合目标：

Δj2(j1,j2,j3) = c0
 + c1 sin(j1) + c2 cos(j1)
 + c3 sin(j2) + c4 cos(j2)
 + c5 sin(j3) + c6 cos(j3)
 + c7 sin(j1)sin(j2) + c8 sin(j1)cos(j2)
 + c9 cos(j1)sin(j2) + c10 cos(j1)cos(j2)

带载时再加 c_load * sin(j2_enc)。

优点：全局光滑、外推安全；缺点：交叉区域拟合不够细。

5.2 v3：三角函数 + RBF 残差

在 v2 基础上，用 RBF 拟合残差：

K(p, c_i) = exp(-||p - c_i||² / (2 ε²))
Δj2 = 三角函数 + Σ β_i K(p, c_i)

求解：

K = exp(-||centers - p||² / (2 * eps²))
beta = solve(K + ridge * I, residuals)

关键参数：

• eps = 12.0°；
• ridge = 0.05；
• 中心数 = 样本数；
• 5 折交叉验证对比纯三角与混合模型。

5.3 拟合结果

168 条数据时：

模型	CV mean	CV p95	CV max
纯三角 11 参数	0.1722°	0.3562°	0.6886°
三角 + RBF	0.1000°	0.3029°	0.8414°

RBF 将平均误差降低约 42%，但 max 略增，说明局部跳跃更明显。
分阶段训练残差：Phase A 0.0494°，B 0.0608°，D 0.0318°，E 0.0407°。

5.4 加载 v3 模型

pick 程序中 offline_j2_phys 需支持 RBF：

def offline_j2_phys(j1_deg, j2_enc_deg, j3_deg, load=1.0):
    # 11 参数三角函数
    delta = np.dot(feat, coef_main)

    # 负载项
    if use_load and load != 1.0:
        delta += sin(j2r) * coef_load[0]

    # RBF 残差
    if use_rbf:
        p = np.array([j1_deg, j2_enc_deg, j3_deg])
        d2 = np.sum((rbf_centers - p)**2, axis=1)
        kern = np.exp(-d2 / (2 * rbf_eps**2))
        delta += np.dot(kern, rbf_beta)

    return j2_enc_deg + delta

---

6. 运动控制方案演变：融合 → 纯离线表一步到位

6.1 最初融合方案

思路：

大范围：离线表粗定位（缩进 8mm）
接近目标：IMU 实测 j2_phys，精定位
残差 > 2mm：小步微调
2cm 内：跳过粗定位，直接 IMU 闭环

核心原则：

IMU 只修正 J2 角度形变，不用 IMU 积分位移；位置基准仍是编码器 FK。

6.2 实测问题

融合模式微调震荡：

[粗定位后] 残差 8.4mm
[精定位] IMU j2_phys=+6.050°
[微调 1/3] |e|=4.6mm
[微调 2/3] |e|=8.3mm
[微调 3/3] |e|=4.1mm
最终 |e|=5.6mm

纯离线表一步到位：

最终 |e|=15.5mm
e=(+9.4, +12.0, -2.7)
[IMU记录] diff=+0.146°

结论：

• z 偏差 2.7mm，与 j2 角度差 0.146° 对应，离线表 j2 预测基本准；
• xy 偏差 15mm，与 j2 无关，来自横向形变、手眼标定残差、IK 构型跳变；
• 融合微调震荡是因为用“当前 j2”预测形变，走到目标后 j2 变了，预测失效。

6.3 最终选择：纯离线表一步到位

增加顶层开关：

USE_IMU_FEEDBACK = False

纯离线表模式下：

• 一次规划 + 一次执行；
• 不微调；
• 不区分 2cm 小范围；
• IMU 只在到位后记录一次 j2_phys 用于诊断。

---

7. 纯离线表一步到位代码要点

7.1 模式入口

def _test_one_point(self, idx):
    ...
    q_seed = self.bridge.get_q_enc_5()
    if q_seed is None:
        result['error'] = 'no joint_states'
        return result

    if not USE_IMU_FEEDBACK:
        return self._test_offline_direct(idx, p_target, q_seed, result)

7.2 离线表迭代预测 IK 目标

def _predict_ik_offline(self, p_target_m, q_seed_5):
    q_predict = q_seed_5.copy()
    p_ik_target = None
    delta_bend_at_target = None

    for _ in range(OFFLINE_PREDICT_ITERS):
        p_enc = self.bridge.tip_urdf(q_predict)
        p_off = self.bridge.tip_offline(q_predict)
        delta_bend = p_off - p_enc
        p_ik_target = p_target_m - delta_bend

        R6, _ = self.bridge.fk_link6(q_predict)
        pose = PoseStamped()
        pose.header.frame_id = BASE_FRAME
        pose.pose.position.x = float(p_ik_target[0])
        pose.pose.position.y = float(p_ik_target[1])
        pose.pose.position.z = float(p_ik_target[2])
        pose.pose.orientation = R_to_quat(R6)

        q_ik = self.bridge.solve_ik(pose, q_predict)
        if q_ik is None:
            break
        q_predict = q_ik

    if p_ik_target is None:
        return None
    return p_ik_target, delta_bend_at_target, q_predict

7.3 一步到位执行

关键改动：直接用 _predict_ik_offline 返回的 q_predict 规划，避免二次 IK 构型跳变。

traj = self.bridge.plan_traj(
    q_ik_final, vel=OFFLINE_STEP_VEL, acc=0.10)
if traj is None:
    result['error'] = 'step plan failed'
    return result

if not self.bridge.execute_traj_blocking(traj, timeout=90.0):
    result['error'] = 'step exec failed'
    return result

self.bridge.settle(FUSION_IMU_SETTLE_S)
q_after = self.bridge.get_q_enc_5()

p_final = self.bridge.tip_offline(q_after)
e_final = p_final * 1000.0 - p_target
result['err_after_offline'] = float(np.linalg.norm(e_final))
self._record_imu_snapshot(q_after, result)

7.4 IMU 仅记录

def _record_imu_snapshot(self, q_5, result):
    j2_imu = self.bridge.get_j2_phys_imu(q_5, imu_dur=1.0)
    j2_off = offline_j2_phys(j1, j2_enc, j3)
    result['j2_diff_record'] = j2_imu - j2_off

不参与任何控制决策，只在结果表显示 Δj2(IMU-离线)。

---

8. 调试中遇到的问题与解决

8.1 Phase E 初始化顺序错误

_rebuild_points() 在 _enable_phase_e 定义之前调用，报：

AttributeError: 'MainWindow' object has no attribute '_enable_phase_e'

解决：把所有 _phase_* 和 _enable_phase_* 提前到 _rebuild_points() 之前。

8.2 IndentationError

_on_auto_run_all 里 try: 下的 while 多缩进 1 个空格：

try:
     while ...

改为 12 空格对齐。

8.3 拟合模型未生效

pick 程序仍加载 j2_phys_model_v2.json，且 offline_j2_phys 没读 RBF。
解决：改 OFFLINE_MODEL_FILE 为 v3，load_offline_model 预转换 rbf_centers/beta/eps，offline_j2_phys 叠加 RBF。

8.4 纯离线表一步到位仍偏 15mm

拆解：

• z -2.7mm，对应 j2 差 0.146°；
• xy +15mm，与 j2 无关。

判断：离线表 j2 修正有效，xy 是横向形变 / 手眼标定 / IK 构型跳变。
解决方向：

1. 用 q_ik_final 直接规划，避免二次 IK 跳变；
2. 若 xy 仍大，加 XY_FEEDFORWARD_MM 常数前馈；
3. 长期补横向形变模型或重做手眼标定。

---

9. 准确度评估：离线表到底有多准

9.1 j2 角度预测

指标	数值
CV mean	0.1000°
CV p95	0.3029°
CV max	0.8414°
分阶段训练残差	0.03~0.06°
单次实测 diff	+0.146°（改前）

结论：离线表 j2 预测典型误差 0.10°，worst case 约 0.3~0.8°。

9.2 末端位置

j2 每差 0.1°，末端 z 约偏 0.8mm（杆长 0.95m）。
所以：

• j2 误差 0.10° → z 约 0.8mm；
• worst case 0.30° → z 约 2.4mm；
• 实测 z -2.7mm 基本吻合。

xy 偏差 10~15mm 不是离线表 j2 能修的，来源：

• 工具杆横向柔性；
• 手眼标定残差；
• IK 构型跳变；
• MoveIt 轨迹跟踪误差。

9.3 改 q_ik_final 后的预期

指标	改前	改后预期
Δj2(IMU-离线)	0.146°	< 0.10°
z 残差	-2.7mm	±1.5mm
xy 残差	+9.4/+12.0mm	基本不变

若 xy 仍大，用 XY 前馈可降到 3~5mm 量级。

---

10. 经验总结

1. 手眼标定必须用 IMU 修正后的 J2 做 FK，否则标定板位姿链会带形变误差。
2. IMU 稳定性判断不能省：settle + 多窗口 std 阈值，否则记录数据抖动大。
3. 离线表先三角函数，再 RBF 残差：全局光滑 + 局部精确，比单纯多套三角函数稳。
4. 采样要覆盖交叉区：只沿轴采不够，j1×j2×j3 同时非零的区域必须补 Phase D/E。
5. 纯离线表一步到位时，IK 解要复用：_predict_ik_offline 返回的 q_predict 直接给 plan_traj，避免二次 IK 构型跳变。
6. IMU 不积分位移：2cm 内也不靠 IMU 算位置，位置基准始终是编码器 FK，IMU 只修 J2 角度。
7. xy 偏差要单独治：离线表修的是 z/j2；xy 要靠手眼标定、横向形变模型或常数前馈。
8. 保留融合模式开关：USE_IMU_FEEDBACK 方便对比纯离线表与融合模式，不建议彻底删死代码。

---

附：关键文件清单

文件	作用
collect_handeye_imu.py	手眼标定采集：棋盘格 + IMU 稳定 + J2 修正
solve_handeye_imu.py	手眼标定求解：PnP + calibrateHandEye + 一致性验证
diag_P.py	离线表采集 v3/v4：Phase A~E + 急停 + 去重
fit_offline_v3.py	离线表拟合：11 参数三角函数 + RBF 残差 + 5 折 CV
pick 程序	融合/纯离线表运动控制，USE_IMU_FEEDBACK 切换
j2_phys_model_v3.json	离线表模型：coef_main、rbf、coverage
imu_cal_rt.json	IMU 零位：imu_zero、zero_phys、IMU_SIGN

---

本文记录的是实际调试过程，参数和结论基于当前机械臂与工具杆。不同硬件需重新标定与采集，不建议直接套用数值。