AprilTag 视觉精细对齐系统
核心项目独立开发7829 行基于 numpy SE(3) + opencv PnP + pupil_apriltags 开发机器人视觉精细对齐系统，毫米级停靠（位置 5mm/朝向 0.5deg），PnP 旋转 + 深度图平移融合，50Hz 闭环 P 控制。
背景
机器人末端停靠精度不够（轮式底盘定位误差 cm 级），需视觉二次精对齐。
技术方案
PnP 位姿估计 + 深度图 z 反投影替代平移（PnP 平移有噪声）；SE(3) 闭环：T_target=T_start@T_correction，50Hz 残差 P 控制；标定质量门控（每标签 ≥15 样本/50% 成功率/1mm 位置离散/0.2deg 朝向标准差）；四步精细对齐（朝向→XY→配对重试×3）。
算法流程
关键难点
- 平面标签 PnP 平移噪声 → 深度图融合：PnP 旋转稳定用于朝向，平移用深度图 z 反投影替代，T_C_T=[R_pnp | t_depth]
- SE(3) 闭环不重新解释误差：控制层只做变换复合，T_target=T_start@T_correction，残差=inv(T_current)@T_target
- gdk_twist_velocity 180° 取反：机器人指令系相对 base_link 旋转 180°，三分量都取反
- 死区分层：视觉死区（5mm/0.5deg）决定是否 do_move；odom 死区（1mm/0.057deg）决定底盘何时停
成果
7829
行代码
5mm
位置精度
0.5°
朝向精度
50Hz
闭环频率