手眼标定EyeToHand原理与Piper机械臂实战推导-平芜编程栈

手眼标定（EyeToHand）不是调个参数、点几下软件就能搞定的“配置项”，而是一套必须亲手推导、亲手验证、亲手调试的几何建模过程。我带过三届机器人方向的本科生课程设计，也给五家工业集成商做过视觉引导产线落地——几乎所有第一次做EyeToHand标定的人，都会在坐标系理解上卡住超过48小时；70%以上的现场失败案例，根源不在相机或机械臂精度，而在标定前没画清楚那张最基础的坐标系关系图。今天这篇不讲OpenCV函数怎么调，也不堆砌公式截图，而是从一张白纸开始，带你把“手眼标定（EyeToHand）推导”这件事，真正拆解成可写、可算、可验、可复现的完整链条。核心关键词就三个：手眼标定、EyeToHand、piper机械臂手眼标定——如果你正用Piper这类轻量级六轴臂做视觉分拣、螺丝抓取或PCB定位，或者刚拿到一个带外参初值的标定板数据但不知道下一步该解什么方程，那你接下来读的每一行，都是我踩过坑后重新捋顺的逻辑线。它不面向纯理论研究者，也不服务一键式封装工具用户，只给那些愿意打开笔记本、手写矩阵、逐帧核对齐次变换的人——因为真正的标定能力，永远长在你推导完最后一行公式、跑通第一组实测数据的那一刻。

1. EyeToHand标定的本质与设计逻辑

1.1 它到底在解什么？——不是“配对”，而是“坐标系嫁接”

很多人误以为手眼标定就是让机械臂末端和相机看到的同一个点“对上”，于是拿棋盘格拍一堆图、记录对应位姿、扔进OpenCV的calibrateHandEye函数就完事。这种做法能跑通，但一旦换镜头、换安装方式、换标定板材质，结果就飘。根本原因在于：EyeToHand标定不是拟合点对点映射，而是在构建一个稳定的坐标系转换链路。

我们先明确场景：EyeToHand，即“眼在手上”的反义——相机固定不动（比如装在产线上方支架），机械臂运动，目标物体静止或缓慢移动。典型应用如：传送带上的零件识别+抓取、固定工位的螺丝孔定位、装配台上的PCB元件拾取。此时，整个系统存在四个关键坐标系：

{C}：相机坐标系（Camera frame），原点在光心，Z轴沿光轴正向；
{W}：世界坐标系（World frame），通常以标定板左上角为原点，Z轴垂直板面朝外；
{E}：机械臂末端坐标系（End-effector frame），原点在法兰盘中心，姿态随关节变化；
{B}：机械臂基座坐标系（Base frame），固定于地面，是所有关节运动的参考基准。

而EyeToHand要解的核心变量，只有一个：从相机坐标系{C}到机械臂基座坐标系{B}的齐次变换矩阵 T_{C}^{B}。注意，不是T_{B}^{C}，也不是T_{E}^{C}——这个方向决定了后续所有视觉引导动作的计算路径。为什么必须是T_{C}^{B}？因为实际运行时，相机拍到目标在{C}中的坐标P_C，我们要把它转到{B}中，再结合当前末端位姿T_{B}^{E}，才能算出末端应移动到哪去抓取。整条链路是：
P_C → (× T_{C}^{B}) → P_B → (× (T_{B}^{E})^{-1}) → P_E

所以T_{C}^{B}是承上启下的枢纽。它不随机械臂运动而变（相机固定），也不随标定板摆放而变（只要相机没动），是一个需要一次性高精度求解的“静态外参”。

提示：很多初学者把T_{C}^{B}和T_{E}^{C}搞混，结果把视觉坐标直接当末端坐标用，导致抓取偏移20cm以上。记住口诀：“EyeToHand，眼不动，手乱动，外参连眼和基座”。

1.2 为什么不能直接测？——物理不可达性与误差放大效应

有人会问：既然相机和机械臂基座都固定在机架上，用激光跟踪仪或全站仪直接测它们之间的位姿不行吗？理论上可以，但实践中几乎不可行，原因有三：

第一，安装公差累积。相机支架、机械臂底座、标定板夹具，三者各自安装平面的平行度误差常达0.1°~0.3°，对应1m距离上的Z向偏差可达1.7~5.2mm。而视觉引导对Z向误差极其敏感——Piper机械臂重复定位精度约±0.05mm，但若外参Z轴偏差1mm，末端在工作空间远端的定位误差可能放大至3~5mm。

第二，基准不统一。激光跟踪仪测的是支架金属面，而相机成像模型依赖光心位置，二者物理原点不重合；机械臂厂商提供的基座坐标系原点，往往定义在减速机壳体某螺纹孔，而非实际安装面。这些“名义原点”和“真实光学/运动原点”之间存在亚毫米级偏移，无法通过机械测量消除。

第三，动态耦合干扰。即使初始标定完成，产线振动、温漂、紧固件微松动，都会让相机支架发生微米级位移。而直接测量法无法在线补偿——它是一锤子买卖。相比之下，基于图像观测的标定方法，天然具备“用观测反推真实位姿”的能力，且可通过多组数据平均抑制随机噪声。

因此，EyeToHand标定本质上是一种基于运动学约束的逆向求解问题：我们控制机械臂末端（{E}）按已知轨迹运动，同时用相机（{C}）观测固定标定板（{W}），记录每组位姿下的图像特征，再利用{E}→{B}、{W}→{C}之间的几何约束，反推出未知的T_{C}^{B}。这个过程不依赖任何外部测量设备，仅靠系统自身运动与观测，正是其工程价值所在。

1.3 Piper机械臂的特殊性：轻量级带来的标定挑战与简化机会

Piper系列机械臂（如Piper 5、Piper 7）作为国产轻量级六轴协作臂，在高校实验室和中小产线中普及率很高。它的结构特点直接影响标定策略：

关节编码器分辨率有限：多数型号采用14bit绝对值编码器，单圈分辨率为16384，对应旋转最小步进约0.022°。这意味着末端位姿T_{B}^{E}的理论精度受制于关节反馈噪声，实测重复性约±0.08mm（XYZ）/±0.05°（RPY）。因此，标定时必须采集足够多组数据（建议≥15组），用SVD或非线性优化平滑掉关节抖动影响。
无内置力觉/视觉接口：Piper不提供原生ROS驱动或深度相机直连协议，需通过URDF建模+MoveIt!或自研运动控制器获取T_{B}^{E}。这就要求标定前必须完成精确的DH参数标定——如果DH模型本身有偏差，T_{B}^{E}输入就是错的，再好的手眼算法也救不回来。我见过太多案例：标定结果看起来残差很小，但实际抓取总偏左2cm，最后发现是第3关节的连杆偏距d3被设成了125mm，实际应为124.3mm。
法兰盘接口标准化：Piper采用ISO 9409-1-50-4-M6标准法兰，中心孔距50mm，允许安装200g以内相机。这个确定性带来一个巨大优势：T_{E}^{C}可预先标定并固化。只要相机安装方式不变（比如用同一块L型支架+M3螺丝），T_{E}^{C}就是常量。而EyeToHand中，我们真正要求解的是T_{C}^{B}，它与T_{E}^{C}无关——这点常被忽略。很多人误以为要先标T_{E}^{C}，其实完全不必。只要保证相机相对于基座不动，T_{C}^{B}就是唯一未知量。

所以针对Piper的标定流程，我们应做减法：放弃复杂的T_{E}^{C}标定环节，聚焦于T_{C}^{B}的鲁棒求解；用多组低速匀速运动代替高动态轨迹，规避关节瞬态响应误差；在标定板选择上，优先使用亚克力背光棋盘格（而非纸质），避免因光照不均导致角点检测偏移。

2. 核心数学推导：从坐标系链路到AX=XB方程

2.1 坐标系链路建模：写出每一环的齐次变换

标定推导的第一步，永远是画图+写变换。拿出一张草稿纸，按顺序列出所有已知和未知变换，并标注方向（上标为参考系，下标为目标系）：

T_{B}^{E}(i)：第i次运动后，机械臂末端{E}相对于基座{B}的位姿。这是由Piper控制器实时输出的，假设我们已通过ROS topic/piper/pose或Modbus寄存器读取到6D位姿（x,y,z,rx,ry,rz），并转换为4×4齐次矩阵。注意：Piper默认输出的是ZYX欧拉角，需用旋转矩阵公式R_Z(ψ)R_Y(θ)R_X(φ)转换，不能直接用cv2.Rodrigues()——后者对应旋转向量，格式不同。
T_{W}^{C}(i)：第i次拍摄时，标定板{W}相对于相机{C}的位姿。这是通过OpenCV的solvePnP求得的。关键细节：solvePnP输入的是标定板上角点在{W}中的理论坐标（如棋盘格每个格子15mm，则第0个角点为(0,0,0)，第1个为(15,0,0)…），输出的是Rvec/Tvec，需用cv2.Rodrigues(Rvec)转为3×3旋转矩阵，再拼成4×4齐次矩阵T_{W}^{C}。注意：OpenCV的solvePnP默认使用UPNP算法，对初始值敏感，建议强制指定cv2.SOLVEPNP_ITERATIVE，并传入相机内参和畸变系数。
T_{C}^{B}：待求量，记为X。这是我们最终要解的矩阵。
T_{W}^{B}：标定板相对于基座的位姿。由于标定板固定在工作台上，且工作台与机械臂基座刚性连接，T_{W}^{B}是常量。但它未知，也不能直接测——这正是EyeToHand的精妙之处：我们不需要知道T_{W}^{B}，它会在方程中被消去。

现在，从两个视角描述同一点P（比如标定板原点）的位置：

从机械臂视角：P在{W}中坐标为P_W = [0,0,0,1]^T（取原点），经T_{W}^{B}到{B}，再经T_{B}^{E}到{E}，即：
P_E = T_{B}^{E} × T_{W}^{B} × P_W
从相机视角：P在{W}中同样是P_W，经T_{W}^{C}到{C}，再经T_{C}^{B}到{B}，即：
P_B = T_{C}^{B} × T_{W}^{C} × P_W

但P_B和P_E是同一物理点在不同坐标系下的表示，它们通过T_{B}^{E}关联：P_B = T_{B}^{E} × P_E。将上式代入：
T_{C}^{B} × T_{W}^{C} × P_W = T_{B}^{E} × (T_{W}^{B} × P_W)

由于P_W是任意向量（我们取原点，但推导适用于任意点），两边消去P_W，得到：
T_{C}^{B} × T_{W}^{C} = T_{B}^{E} × T_{W}^{B}

整理为：
T_{C}^{B} × T_{W}^{C} × (T_{W}^{B})^{-1} = T_{B}^{E}

令X = T_{C}^{B}，Y = T_{W}^{B}，则：
X × T_{W}^{C} × Y^{-1} = T_{B}^{E}

即：
X × T_{W}^{C} = T_{B}^{E} × Y

这个Y仍然未知。但我们有i=1,2,…,n组数据，对每组i，都有：
X × T_{W}^{C}(i) = T_{B}^{E}(i) × Y

将i=1的式子右乘Y^{-1}，得：
X × T_{W}^{C}(1) × Y^{-1} = T_{B}^{E}(1)

再将i=2的式子代入：
X × T_{W}^{C}(2) × Y^{-1} = T_{B}^{E}(2)

两式左端都含X和Y^{-1}，相除（矩阵意义下为左乘逆）：
[T_{B}^{E}(1)]^{-1} × X × T_{W}^{C}(1) × Y^{-1} = I
[T_{B}^{E}(2)]^{-1} × X × T_{W}^{C}(2) × Y^{-1} = I

更简洁的做法是：对任意两组i,j，有
T_{B}^{E}(i) × Y = X × T_{W}^{C}(i)
T_{B}^{E}(j) × Y = X × T_{W}^{C}(j)

将第一式右乘[T_{W}^{C}(j)]^{-1}，第二式右乘[T_{W}^{C}(i)]^{-1}，再联立消Y，最终导出经典形式：
T_{B}^{E}(i) × [T_{B}^{E}(j)]^{-1} = X × T_{W}^{C}(i) × [T_{W}^{C}(j)]^{-1} × X^{-1}

令A_{ij} = T_{B}^{E}(i) × [T_{B}^{E}(j)]^{-1}，B_{ij} = T_{W}^{C}(i) × [T_{W}^{C}(j)]^{-1}，则：
A_{ij} = X × B_{ij} × X^{-1}

这就是著名的AX = XB问题。它表明：机械臂末端两次位姿的相对变换A_{ij}，必须与相机观测到的标定板两次位姿的相对变换B_{ij}，通过同一个相似变换X（即T_{C}^{B}）关联。X是连接运动空间与视觉空间的“桥梁矩阵”。

注意：AX=XB不是线性方程，不能直接用最小二乘求解。它包含旋转和平移两部分，且旋转部分满足R_A = R_X R_B R_X^T，平移部分满足t_A = R_X t_B + (I - R_X) t_X。必须分离处理，否则会得到病态解。

2.2 旋转部分求解：从SO(3)群性质到四元数线性化

AX=XB的旋转子问题为：R_A = R_X R_B R_X^T。这是一个正交矩阵相似变换，解空间是SO(3)群上的共轭类。直接在旋转矩阵空间求解极易陷入局部极小，且难以保证正交性。工程上通用做法是转到四元数表示。

设R_X对应的单位四元数为q_X = [w,x,y,z]^T，满足w²+x²+y²+z²=1。R_A和R_B对应的四元数分别为q_A、q_B。利用四元数旋转公式：R_X R_B R_X^T 对应的四元数为 q_X ⊗ q_B ⊗ q_X^*，其中⊗为四元数乘，*为共轭（w,-x,-y,-z）。

因此方程变为：q_A = q_X ⊗ q_B ⊗ q_X^*

展开四元数乘法，可整理为一个关于q_X的齐次线性方程组：K q_X = 0，其中K是12×4矩阵（每组i,j提供3个独立方程，因旋转矩阵只有3自由度）。对n组数据，可构造超定方程组，用SVD求最小二乘解：取K^T K最小特征值对应的特征向量，即为q_X。

实操中，OpenCV的calibrateHandEye函数内部就采用此法。但如果你手写，要注意三点：

四元数符号歧义：q和-q表示同一旋转。SVD解出的q_X需检查w分量，若w<0，则整体取负，确保主分支连续。
数据配对策略：不是任意两两组合都有效。应选择机械臂运动幅度大、旋转轴变化丰富的配对（如i=1在水平面，j=5在竖直面），避免A_{ij}接近单位阵——此时K矩阵秩亏，解不稳定。
Piper关节运动限制：Piper的肩部（J1）和肘部（J3）运动范围大，但腕部（J4-J6）易出现奇异位形。标定时应避开J5=0°或J6=±90°的构型，否则T_{B}^{E}(i)的雅可比矩阵条件数恶化，R_A计算误差放大。

我实测过：用Piper采集15组位姿，随机选105组(i,j)对，其中仅62组A_{ij}的旋转角>5°，其余因机械臂微调导致R_A≈I，参与求解反而引入噪声。因此，预筛选是必要的——计算每组R_A的迹tr(R_A)，若|tr(R_A)-3| < 0.01，直接剔除。

2.3 平移部分求解：从旋转解出发的线性回归

一旦R_X确定，平移向量t_X就变成线性问题。原AX=XB的平移部分为：
t_A = R_X t_B + t_X - R_X t_X

整理得：
t_A - R_X t_B = (I - R_X) t_X

记左侧为d_{ij}，右侧系数矩阵为M_{ij} = I - R_X，则：
d_{ij} = M_{ij} t_X

对所有有效(i,j)对，堆叠成超定线性系统 D = M t_X，用最小二乘求解：t_X = (M^T M)^{-1} M^T D

这里的关键陷阱是：当R_X接近单位阵时，M = I - R_X接近零矩阵，方程病态。这正是为什么旋转求解必须严格筛选大角度运动——否则平移解会发散。例如，若R_X的旋转角仅0.5°，则M的特征值最小约为1e-4，D中微米级观测噪声会被放大万倍。

解决方案有两个：

增加运动幅度：让Piper末端在空间中划一个边长≥200mm的立方体轨迹，确保每组(i,j)的相对旋转角>10°，平移距离>50mm。
引入伪逆稳健求解：不用(M^T M)^{-1}，而用Moore-Penrose伪逆M⁺ = V Σ⁺ U^T（SVD分解后，对Σ中非零奇异值取倒数，零值保持0）。OpenCV底层正是如此实现。

对于Piper用户，还有一个实操技巧：固定标定板Z向，只做XY平面运动。这样t_B的Z分量恒为0，d_{ij}的Z分量只含R_X的Z行与t_X的乘积，可单独解出t_Xz，再解XY。我用此法在无Z向位移的简易工装上，将t_X解算误差从±1.2mm压到±0.15mm。

3. 实操全流程：从数据采集到结果验证

3.1 Piper机械臂数据采集：运动规划与位姿同步

标定效果70%取决于数据质量，而非算法。Piper虽轻量，但运动控制精度足够支撑高标定质量，前提是采集规范。

硬件准备：

相机：推荐Basler acA1920-40gm（全局快门，1920×1200），搭配Computar 12mm定焦镜头（F1.4，畸变<0.05%）。避免使用USB免驱相机，其曝光同步性差。
标定板：6×9棋盘格，格子尺寸25mm，亚克力基底+LED背光（亮度均匀性>95%）。纸质板在产线光照下易反光，角点检测失败率超30%。
环境：照度>500lux，避免直射阳光；背景用哑光深灰布，消除干扰边缘。

运动规划要点：

不用手动示教！Piper支持Python SDK（piper_sdk）或ROS MoveIt!。编写脚本生成15组位姿，每组包含：
- 末端位置：在以标定板中心为原点的立方体内均匀采样（X: ±150mm, Y: ±100mm, Z: +50~+200mm）
- 末端姿态：保持Z轴始终指向标定板中心（即“眼盯”模式），用axis-angle生成随机小扰动（绕X/Y轴±3°，绕Z轴±5°），避免奇异位形。
运动速度：≤15%最大速，加速度≤0.2g。Piper在高速启停时有0.5~1mm振荡，会污染T_{B}^{E}。

位姿同步机制：这是最容易被忽视的致命环节。相机曝光和机械臂位姿读取必须时间对齐，否则T_{B}^{E}(i)和T_{W}^{C}(i)不对应同一时刻。

理想方案：用Piper的GPIO触发相机快门。Piper SDK提供digital_out_set()函数，可在move_to()完成后立即拉高电平，延时10ms后触发Basler相机（需配置相机为硬件触发模式）。
退而求其次：软件同步。在move_to()后插入time.sleep(0.5)，等机械臂完全静止（观察末端抖动消失），再调用cv2.VideoCapture.read()。实测延迟<50ms，对Piper的0.05mm重复精度影响可忽略。
绝对禁止：边运动边拍照！我见过某客户用手机拍Piper运动视频再逐帧提取，结果标定残差达8mm——因为手机帧率仅30Hz，而Piper运动周期<200ms。

每组采集后，保存：

T_{B}^{E}(i)：4×4矩阵，文本格式
图像：PNG无损压缩，文件名含时间戳
角点坐标：np.array([x,y])列表，用于后续solvePnP验证

3.2 OpenCV标定代码实现：从solvePnP到calibrateHandEye

以下为可直接运行的Python核心代码（适配Piper+OpenCV 4.8+）：

import cv2 import numpy as np import json # 1. 相机内参加载（需提前用OpenCV标定获得） with open('camera_intrinsics.json') as f: intrinsics = json.load(f) mtx = np.array(intrinsics['mtx']) dist = np.array(intrinsics['dist']) # 2. 标定板参数 board_size = (6, 9) # 内角点数 square_size = 0.025 # 米 objp = np.zeros((board_size[0]*board_size[1], 3), np.float32) objp[:, :2] = np.mgrid[0:board_size[0], 0:board_size[1]].T.reshape(-1, 2) * square_size # 3. 数据加载 poses_B_E = [] # 存储T_B^E列表 poses_W_C = [] # 存储T_W^C列表 for i in range(15): # 加载第i组位姿 with open(f'pose_{i:02d}.json') as f: pose_data = json.load(f) # 转换为4x4齐次矩阵：[R|t]拼接 R = cv2.Rodrigues(np.array(pose_data['rvec']))[0] t = np.array(pose_data['tvec']).reshape(3,1) T_B_E = np.hstack((R, t)) T_B_E = np.vstack((T_B_E, [0,0,0,1])) poses_B_E.append(T_B_E) # 加载第i组图像，求解T_W^C img = cv2.imread(f'img_{i:02d}.png') gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) ret, corners = cv2.findChessboardCorners(gray, board_size, None, flags=cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_FAST_CHECK + cv2.CALIB_CB_NORMALIZE_IMAGE) if not ret: print(f"Image {i} failed to find corners") continue # 亚像素优化 criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) corners_sub = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria) # solvePnP，强制使用ITERATIVE _, rvec, tvec = cv2.solvePnP(objp, corners_sub, mtx, dist, flags=cv2.SOLVEPNP_ITERATIVE) R_W_C = cv2.Rodrigues(rvec)[0] T_W_C = np.hstack((R_W_C, tvec.reshape(3,1))) T_W_C = np.vstack((T_W_C, [0,0,0,1])) poses_W_C.append(T_W_C) # 4. 手眼标定 # OpenCV提供5种方法，Piper推荐TSAI（精度高）或HORAUD（鲁棒性强） R_C_B, t_C_B = cv2.calibrateHandEye( [np.linalg.inv(T) for T in poses_B_E], # A: T_E^B = (T_B^E)^-1 poses_W_C, # B: T_W^C method=cv2.CALIB_HAND_EYE_TSAI ) # 构造T_C^B T_C_B = np.hstack((R_C_B, t_C_B.reshape(3,1))) T_C_B = np.vstack((T_C_B, [0,0,0,1])) print("T_C^B =", T_C_B)

关键参数说明：

cv2.calibrateHandEye的第一个参数是A矩阵，即T_E^B = (T_B^E)^{-1}，不是T_B^E本身。这是OpenCV约定，务必取逆。
method选择：TSAI法假设相机和机械臂运动均为刚体，适合Piper这种高刚性臂；HORAUD法对噪声更鲁棒，但需要更多数据（≥20组）。
poses_W_C必须是T_W^C，即标定板到相机，不能传T_C^W——方向错误会导致结果完全颠倒。

3.3 结果验证三重校验法：不止看残差

OpenCV返回的reprojectionError（重投影误差）只是中间指标，不能代表实际引导精度。必须做三层验证：

第一层：重投影验证

用T_C^B将标定板原点P_W = [0,0,0,1]^T转到{C}：P_C = T_C^B × T_W^B × P_W。但T_W^B未知？没关系，我们有T_W^C，而T_W^B = T_W^C × T_C^B。所以P_C = T_W^C × T_C^B × P_W？不对！正确链路是：P_W → T_W^C → P_C，无需T_C^B。重投影验证应这样做：取一组未参与标定的数据，用T_C^B和当前T_B^E，预测标定板在图像中的位置，与实际检测对比。误差应<1.5像素（Piper工作距离0.8m时，1像素≈0.1mm）。

第二层：物理抓取验证

在标定板上贴一个直径5mm的红色圆点（非角点），用Piper移动末端到该点正上方10mm处。执行抓取指令，测量实际抓取点与圆点中心的XY距离。15次测试，平均误差应≤0.3mm。我实测Piper 5在此条件下达到0.22±0.07mm。

第三层：跨工况泛化验证

更换标定板位置（平移±50mm，旋转±5°），不重新标定，仅用原T_C^B，测试新位置下的抓取精度。若误差突增>1mm，说明标定过程受局部形变影响（如支架热胀），需检查相机安装刚性。

注意：验证时务必关闭Piper的“力控模式”。某些版本默认开启，末端会微调位置补偿接触力，导致位姿读取与实际不符。

4. Piper专属避坑指南与实操心得

4.1 数据采集阶段的5个致命错误

标定板晃动：Piper末端触碰标定板支架导致微振动，角点检测偏移。对策：用磁吸底座固定标定板，或用真空吸附台；采集前轻敲支架，确认无余振。
光照突变：产线LED灯频闪（尤其50Hz工频），导致图像明暗交替。对策：相机设置固定曝光时间（如10ms），关闭自动增益；或改用红外光源+IR滤镜，彻底规避可见光干扰。
位姿插值错误：Piper SDK返回的位姿是离散点，但运动是连续的。若在运动中途读取，T_B^E对应的是中间构型，而非稳态。对策：只在move_to()回调函数中读取，或监听/piper/statustopic的motion_state == 3（到位状态）。
坐标系混淆：OpenCV的solvePnP输出T_W^C，Z轴指向相机，但Piper的T_B^E中Z轴指向末端工具。二者Z轴定义一致，但X/Y可能镜像。对策：用标定板上已知特征（如左上角标记点）手动验证方向，必要时对R_W^C做X轴翻转。
时间戳漂移：树莓派等嵌入式主机NTP同步误差达200ms，导致图像与位姿时间错位。对策：禁用NTP，用Piper控制器内部时钟作为主时钟，所有设备通过GPIO同步。

4.2 Piper SDK常见陷阱与修复

rvec/tvec单位错误：Piper Python SDK返回的rvec是旋转向量（单位：弧度），但OpenCV的solvePnP输入要求rvec为旋转向量，输出也是旋转向量——看似一致，实则SDK的rvec是Rodrigues形式，而solvePnP输出需再次Rodrigues转换。正确流程：SDK rvec → cv2.Rodrigues() → R矩阵 → 传入solvePnP的rvec参数？不！solvePnP的rvec参数是输出，输入是None。此处易混淆，应直接用SDK的rvec/tvec初始化solvePnP的guess参数，提高收敛性。
TCP偏移未补偿：Piper默认TCP（Tool Center Point）在法兰盘中心，但实际夹爪有偏移。若未在SDK中设置tcp_offset，T_B^E的tvec会包含该偏移，导致标定结果系统性偏移。对策：用Piper示教器测出夹爪中心相对于法兰的偏移（如[0,0,85]mm），在SDK中调用set_tcp([0,0,0.085])。
关节限位误触发：Piper在标定运动中可能因J2/J3角度超限报错。对策：提前用get_joint_limits()查询各关节软限位，规划轨迹时预留5°余量；或临时放宽限位（set_joint_limits([...])），标定完成后再恢复。