第4章主动视觉标定

4、主动视觉标定：基于电机编码器的位姿估计、手眼标定（Eye-in-Hand vs Eye-to-Hand）、AX=XB求解

各位同学，大家好。今天我们聊一个在仿生眼系统里非常核心的话题——主动视觉标定。

什么叫主动视觉？说白了，就是让眼睛动起来。我们人类看东西，眼球会不停地转动、扫视，这叫主动感知。仿生眼也一样，它得靠电机驱动，在运动中获取信息。那问题来了：电机转了多少角度？相机跟着动了多少？这两个量之间怎么对齐？

嗯，这就是我们今天要啃的硬骨头。

4.1 基于电机编码器的位姿估计

先说说电机编码器。我早期做仿生眼项目时，踩过一个坑：编码器读数明明很准，但相机位姿算出来就是不对。后来发现，问题出在运动学模型上。

电机编码器能告诉我们关节转了多少度。但相机装在关节末端，它的位姿变化，得靠正运动学来算。这里有个关键点：编码器测量的是关节空间，我们需要的是任务空间（相机位姿）。

核心公式：

对于串联结构的仿生眼，假设有n个关节，每个关节的编码器读数为θ₁, θ₂, ..., θₙ，则相机坐标系相对于基座坐标系的变换矩阵为：

T_cam_base = T_1(θ₁) · T_2(θ₂) · ... · T_n(θₙ) · T_cam_end

其中T_i(θᵢ)是第i个关节的齐次变换矩阵，T_cam_end是相机在末端关节上的安装矩阵。

我个人习惯，在搭建仿生眼系统时，会先用激光跟踪仪验证一下运动学模型。你想想看，如果模型本身有偏差，后面所有标定都是白搭。

避坑指南：

我曾经遇到过编码器回零误差的问题。电机每次上电，编码器位置可能都不一样。解决办法是：在系统启动时，让电机回到机械限位，以此作为绝对零点。别问我怎么知道的，问就是吃过亏。

4.2 手眼标定：Eye-in-Hand vs Eye-to-Hand

手眼标定，这个名字很形象。在机器人领域，「手」是机械臂末端，「眼」是相机。仿生眼系统里，这个「手」其实就是电机驱动的关节。

两种经典配置：

Eye-in-Hand（眼在手上）：相机固定在电机末端，跟着电机一起动。仿生眼基本都是这种结构。
Eye-to-Hand（眼在手外）：相机固定在外界，看着电机运动。这种在工业机器人视觉引导里更常见。

为什么要把它们分开讲？因为标定方程不一样。

配置类型	标定方程	典型应用
Eye-in-Hand	AX = XB	仿生眼、移动机械臂
Eye-to-Hand	AX = ZB	固定相机视觉引导

我重点讲Eye-in-Hand，因为这是仿生眼的主流方案。你想想看，仿生眼要模仿人眼，眼球转动时相机跟着转，这不就是典型的眼在手上吗？

4.3 AX=XB 求解

好，重头戏来了。AX=XB，这个方程看起来简单，但解起来门道很多。

先解释一下每个符号的含义：

A：电机运动前后，编码器测得的末端位姿变化（齐次变换矩阵）
B：相机运动前后，通过视觉方法（比如PnP）测得的相机位姿变化
X：我们要找的——相机坐标系到电机末端坐标系的变换矩阵

方程AX=XB，说白了就是：电机动了多少，相机就应该跟着动多少，但中间差了一个固定的旋转平移关系X。

求解方法：

经典的解法分两步：

先解旋转部分：R_A · R_X = R_X · R_B，用四元数或罗德里格斯公式求解
再解平移部分：(R_A - I) · t_X = R_X · t_B - t_A

我记得最早做这个的时候，用的是Tsai-Lenz方法。这个方法稳定，但需要至少两组非平行的运动。为什么？因为如果电机只做纯平移，旋转部分就解不出来。

重要提醒：

采集数据时，一定要让电机做至少两次旋转轴不同的运动。我曾经偷懒，只做了同一轴上的两次旋转，结果解出来的X完全不对。嗯，这个坑我替你们踩过了。

代码实现上，我习惯用OpenCV的calibrateHandEye函数。但要注意，这个函数默认输入格式是旋转向量+平移向量，不是矩阵。我贴一段我常用的代码片段：

# Python + OpenCV 示例
import cv2
import numpy as np

# 假设我们有N组数据
# R_gripper2base: 电机末端相对于基座的旋转矩阵列表
# t_gripper2base: 电机末端相对于基座的平移向量列表
# R_target2cam: 标定板相对于相机的旋转矩阵列表
# t_target2cam: 标定板相对于相机的平移向量列表

R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye(
    R_gripper2base, t_gripper2base,
    R_target2cam, t_target2cam,
    method=cv2.CALIB_HAND_EYE_TSAI
)

# 结果：X = [R_cam2gripper | t_cam2gripper]
# 这就是相机到电机末端的变换矩阵

这里有个细节：OpenCV的calibrateHandEye输入的是电机末端到基座的变换，以及标定板到相机的变换。它内部会帮你构造A和B矩阵。我个人觉得这个接口设计得挺巧妙的，省去了手动构造的麻烦。

4.4 本章小结

主动视觉标定，核心就三件事：

用编码器算电机位姿（正运动学）
理解Eye-in-Hand和Eye-to-Hand的区别
解AX=XB方程，找到相机和电机之间的固定关系

我做了这么多年标定，最大的体会是：数据质量比算法重要。再牛的算法，喂进去垃圾数据，出来的也是垃圾。所以，花时间设计好的数据采集流程，比纠结用哪种求解方法更值得。