本文最后更新于：2026年8月5日 下午

本文梳理激光雷达(纯点云)与机械臂的手眼标定理论,覆盖**眼在手上(eye-in-hand,雷达随臂动)与眼在手外(eye-to-hand,雷达固定)**两种配置:从 $AX=XB$ 方程的来源、经典求解器,到纯点云场景下用自然环境替代标定物的可行性、可观测性条件与采集运动设计。一个贯穿全文的判断:外参是雷达与机械臂的固定物理关系,与环境无关;环境只是测量工具——理解它,一类常见疑虑(如"换个房间会不会得到不同外参")就有了答案。

本文属 Robotics 笔记系列;齐次变换与机械臂基础另见《齐次变换矩阵与 SE(3)》《机械臂技术基础》,本文涉及处不再展开。结论来自文末公开文献与几何推导。

本文脉络:§1 立问题、符号与两类配置 → §2 推出方程(eye-in-hand 的 $AX=XB$;eye-to-hand 消元后归为同形)→ §3 经典闭式求解器;§4 指出纯点云场景下 $B$ 只能靠点云配准(eye-in-hand)或逐帧标定物拟合(eye-to-hand)间接获得 → §5 回答核心问题:eye-in-hand 能否弃用标定物、仅靠环境(严格等价于 $AX=XB$)→ §6 哪些运动能解出 $X$、精度从哪来 → §7 单面墙平面标定(完整案例)→ §8 配准的精度边界 → §9 工程实操;§10 总结。

1. 问题设定与符号约定

1.1 两类配置与坐标系

激光雷达与机械臂的手眼标定有两类配置,外参 $X$ 的定义随之不同:

  • 眼在手上(eye-in-hand):雷达刚性固连在机械臂末端、随臂运动,外参 $X={}^{E}T_L$ 是雷达系在末端系下的位姿,固定不变、待标定;
  • 眼在手外(eye-to-hand):雷达固定在外部(三脚架、天花板),不随臂动,待标定的是雷达在基座系下的位姿 $X={}^{W}T_L$。

两种配置下机械臂底座都固定安装,世界系 $W$ 与基座系重合(见《机械臂技术基础》第 2 节)。eye-in-hand 涉及三个坐标系:世界系 $W$(固定)、末端系 $E_i$(随位姿变化,可取法兰 tool0 或已标定的 TCP,见《机械臂技术基础》第 11、12 节)、雷达系 $L_i$(随末端一起运动);eye-to-hand 中雷达系 $L$ 固定不动,随时间变化的是被观测物(末端上的标定物)在雷达系里的位姿。

eye-in-hand 配置:世界系 W、末端系 E、雷达系 L,雷达刚性固连末端,外参 X 固定不变

eye-to-hand 配置:雷达固定在外部(三脚架),观测末端上的标定物,外参 X 为雷达在基座系的位姿

本文以 eye-in-hand 为主线展开(其点云配准与"用环境替代标定物"的故事更丰富,§4–§8),eye-to-hand 的方程与差异在 §2.3 给出,并在相关处标注两种配置的不同。

1.2 记号约定

全文用 4×4 齐次变换描述刚体位姿,记 ${}^{A}T_B$ 为 $B$ 系在 $A$ 系下的位姿。齐次变换同时承载坐标变换、位姿描述、相对运动三层含义,以及复合、求逆的链式规则——完整论述见《齐次变换矩阵与 SE(3)》,这里只列后文反复用到的两条:

  • 逆:$({}^{A}T_B)^{-1}={}^{B}T_A$(对调观察者);
  • 复合(右乘链):${}^{A}T_C={}^{A}T_B\cdot{}^{B}T_C$,要求中间系匹配(前项尾 = 后项头),故 ${}^{A}T_B$ 只能右乘到"以 $A$ 结尾"的位姿链上、把它延续到 $B$。

一条定义性质贯穿推导(尤其 §5.1):${}^{A}T_B$ 既描述 $B$ 在 $A$ 下的位姿,也实现把点从 $B$ 系坐标换算到 $A$ 系——两者由同一组 $(\mathbf{R},\mathbf{t})$ 决定。记号上,本文把变换的平移列记作 $\mathbf{t}$、把点的坐标记作 $\mathbf{p}$ 以示区分(姊妹文统一记作 $\mathbf{p}$,同一量、记法偏好不同)。任意变换 $T$ 由旋转 $R$ 与平移 $\mathbf{t}$ 组成,记 $T=(R,\mathbf{t})$;后文 $R_X,\mathbf{t}_X$、$R_A,\mathbf{t}_A$ 等带下标者,指对应变换的旋转、平移部分;也写作 $R(T),\mathbf{t}(T)$。

1.3 符号表

为避免符号随用随定义造成的混乱,全文符号集中如下:

符号 含义 性质
$W$ 世界系(= 基座系) 固定
$E_i$ 第 $i$ 时刻末端系 法兰或 TCP
$L_i$ 第 $i$ 时刻雷达系 随末端运动
$G_i={}^{W}T_{E_i}$ 末端在世界的位姿 正运动学给出,已知
$X={}^{E}T_L$ 外参:雷达系在末端系的位姿($={}^{E_i}T_{L_i}$,对所有 $i$ 相同) 固定,待求
$P_i$ 第 $i$ 帧点云(各点坐标在 $L_i$ 系下) 雷达测量
$A_i={}^{E_{i+1}}T_{E_i}=G_{i+1}^{-1}G_i$ 末端相对运动 由 $G_i$ 算出,已知
$B_i={}^{L_{i+1}}T_{L_i}$ 雷达相对运动 由点云配准测出,观测
${}^{W}T_{L_i}=G_iX$ 雷达在世界的位姿 复合中间量
$C_i={}^{L_i}T_{\text{obj}}$ 标定物在传感器系下的位姿 由观测得到(§2.1)

方向约定(重要):$A_i$ 与 $B_i$ 都取新看旧方向(下标大的系看下标小的系),二者同向配对。这一约定是第 5 节 $AX=XB$ 对称性的来源——若把其中一个取逆,方程形式会随之改变。

上表为 eye-in-hand 的符号;eye-to-hand 下外参改为 $X={}^{W}T_L$(雷达固定),并引入标定物位姿 $Z={}^{E}T_{\text{obj}}$,推导见 §2.3。

手眼标定的目标:从若干组 $(G_i,P_i)$ 估计外参 $X$。

2. 经典手眼标定:AX=XB 的来源

2.1 标定物场景的推导

下面是 eye-in-hand 场景的经典推导:传感器随末端运动、标定物固定在世界系中(传统相机标定用棋盘格,雷达可用三维标定靶)。设第 $i$ 个时刻:

  • 末端位姿 $G_i = {}^{W}T_{E_i}$(由正运动学已知);
  • 相机/雷达看到标定物,得到标定物在传感器系下的位姿 $C_i = {}^{L_i}T_{\text{obj}}$;
  • 外参 $X = {}^{E}T_L$ 固定。

标定物在世界系中固定,故其世界位姿 ${}^{W}T_{\text{obj}}$ 不随时间变。从两个方向都能表达它:

$${}^{W}T_{\text{obj}} = {}^{W}T_{E_i}\,{}^{E_i}T_{L_i}\,{}^{L_i}T_{\text{obj}} = G_i\, X\, C_i$$

由于 ${}^{W}T_{\text{obj}}$ 对所有 $i$ 相同,相邻两帧有:

$$G_i\, X\, C_i = G_{i+1}\, X\, C_{i+1}$$

两边左乘 $G_{i+1}^{-1}$、右乘 $C_i^{-1}$,等号两端的 $G_{i+1}$、$C_i$ 消去:

$$\underbrace{G_{i+1}^{-1}G_i}_{A_i}\, X = X\, \underbrace{C_{i+1}C_i^{-1}}_{B_i}$$

把这两项相对运动显式展开(套用 §1.2 的逆规则 $({}^{A}T_B)^{-1}={}^{B}T_A$ 与右乘链,中间系需匹配):

$$A_i = G_{i+1}^{-1}G_i = ({}^{W}T_{E_{i+1}})^{-1}\,{}^{W}T_{E_i} = {}^{E_{i+1}}T_W\,{}^{W}T_{E_i} = {}^{E_{i+1}}T_{E_i}$$

即末端从 $E_i$ 到 $E_{i+1}$ 的相对运动(新系 $E_{i+1}$ 回看旧系 $E_i$,由正运动学已知)。同理:

$$B_i = C_{i+1}C_i^{-1} = {}^{L_{i+1}}T_{\text{obj}}\,({}^{L_i}T_{\text{obj}})^{-1} = {}^{L_{i+1}}T_{\text{obj}}\,{}^{\text{obj}}T_{L_i} = {}^{L_{i+1}}T_{L_i}$$

即传感器从 $L_i$ 到 $L_{i+1}$ 的相对运动(新系 $L_{i+1}$ 回看旧系 $L_i$,由观测标定物得到)。两者同取“新看旧”方向、同向配对——这正是 $AX=XB$ 形式对称的来源。记作:

$$\boxed{A_i\, X = X\, B_i}$$

这就是手眼标定方程 $AX = XB$:$A$ 已知、$B$ 由观测得到、$X$ 待求。

AX=XB 的闭环推导链:标定物世界位姿经 W→E→L→obj 两条路径表达,消去后得 A·X = X·B

2.2 数据要求

把 $A=(R_A,\mathbf{t}_A)$、$X=(R_X,\mathbf{t}_X)$、$B=(R_B,\mathbf{t}_B)$ 代入 $AX=XB$:两端都是齐次变换,左端 $AX=(R_A R_X,\ R_A\mathbf{t}_X+\mathbf{t}_A)$、右端 $XB=(R_X R_B,\ R_X\mathbf{t}_B+\mathbf{t}_X)$。两个齐次变换相等,要求旋转块、平移块分别相等:

  • 旋转:$R_A R_X = R_X R_B$(只含 $R$,可先解 $R_X$);
  • 平移:$R_A \mathbf{t}_X + \mathbf{t}_A = R_X \mathbf{t}_B + \mathbf{t}_X$($R_X$ 解出后代入,线性求 $\mathbf{t}_X$)。

"先 $R$ 后 $\mathbf{t}$"正是分离法(§3)的依据。理论下界是至少 2 组相对运动(即 3 个站位),且两组运动的旋转轴不平行。实际工程中建议 10–20 组以上,约 50 组后精度增益趋缓。

2.3 eye-to-hand 配置

§2.1 是 eye-in-hand(传感器随末端动)的推导。eye-to-hand 正相反:雷达固定在外部不动,被观测的是固连在末端的标定物。设末端位姿 $G_i={}^{W}T_{E_i}$,雷达观测到标定物在雷达系下的位姿 $C_i={}^{L}T_{\text{obj}}$(雷达不动,故 $L$ 无下标)。两个固定未知量:外参 $X={}^{W}T_L$(雷达在基座系下的位姿)、标定物在末端系下的位姿 $Z={}^{E}T_{\text{obj}}$。标定物的世界位姿既可经末端链、也可经雷达链表达,二者相等:

$$G_i\, Z = X\, C_i$$

用相邻两帧 $i, i+1$ 消去 $Z$:由 $G_iZ=XC_i$ 得 $Z=G_i^{-1}XC_i$,代入 $G_{i+1}Z=XC_{i+1}$ 并右乘 $C_i^{-1}$,得与 eye-in-hand 同形的方程:

$$\underbrace{G_{i+1}G_i^{-1}}_{A}\, X = X\, \underbrace{C_{i+1}C_i^{-1}}_{B}$$

仍是 $AX=XB$。展开看物理含义。代入 $G_i={}^{W}T_{E_i}$:

$$A = G_{i+1}G_i^{-1} = {}^{W}T_{E_{i+1}}\,({}^{W}T_{E_i})^{-1} = {}^{W}T_{E_{i+1}}\,{}^{E_i}T_W$$

即末端从 $E_i$ 到 $E_{i+1}$ 的相对运动、以世界系为参考(空间描述)。它不再约成单个 ${}^{A}T_B$:链式复合 ${}^{A}T_B\cdot{}^{B}T_C={}^{A}T_C$ 要求中间系前后是同一个系,而这里中间是末端系的两个不同时刻 $E_{i+1}\neq E_i$。对比 eye-in-hand 的 $A_i={}^{E_{i+1}}T_W\cdot{}^{W}T_{E_i}$:中间系是固定的世界 $W$(同一个系),故约成 ${}^{E_{i+1}}T_{E_i}$。同理 $B$($C_i={}^{L}T_{\text{obj}_i}$,$\text{obj}_i$ 为标定物在第 $i$ 帧的位姿):

$$B = C_{i+1}C_i^{-1} = {}^{L}T_{\text{obj}_{i+1}}\,({}^{L}T_{\text{obj}_i})^{-1} = {}^{L}T_{\text{obj}_{i+1}}\,{}^{\text{obj}_i}T_L$$

是标定物相对运动、以雷达系为参考;中间系 $\text{obj}_{i+1}\neq\text{obj}_i$ 同样不是同一个系,故也不约(eye-in-hand 的 $B_i={}^{L_{i+1}}T_{\text{obj}}\cdot{}^{\text{obj}}T_{L_i}$ 因标定物固定而约成 ${}^{L_{i+1}}T_{L_i}$)。根因是 eye-to-hand 里运动的是末端与标定物、雷达固定,与 eye-in-hand(传感器动、标定物固定)对偶,故 $A$、$B$ 都呈 $XY^{-1}$ 的空间形式。它们虽不约简,却都是确定的相对运动;而 $AX=XB \Leftrightarrow A=XBX^{-1}$ 表明两者是同一刚体运动经外参 $X={}^{W}T_L$ 的共轭——这正是 eye-to-hand 与 eye-in-hand 共享 $AX=XB$ 的几何根源。

因此 eye-to-hand 与 eye-in-hand 共享同一方程与 §3 的全套求解器,差别只有两点:其一,$X$ 的物理含义不同(基座-雷达 vs 末端-雷达);其二,$B$ 的来源不同——eye-in-hand 的 $B$ 是雷达自身运动(靠相邻帧点云配准,§4),eye-to-hand 的 $B$ 是标定物的运动(靠逐帧拟合标定物:球心、平面法向等)。

若不消去 $Z$ 而是同时求 $X$、$Z$,即 robot-world-hand-eye 问题 $AX=ZB$(传感器随末端动、且机器人基座到世界的关系也未知,同时求手眼 $X$ 与基座-世界 $Z$)。Dornaika-Horaud 给出四元数线性闭式解(先解旋转 $a\otimes q=q\otimes b$,$a,q,b$ 为 $A,X,B$ 的旋转四元数,再回代平移),Li 等用对偶四元数把 $R,\mathbf{t}$ 同时线性化;它与 eye-to-hand 同属“两个未知刚体”骨架,数据要求相同(≥2 组运动、旋转轴不平行)。

3. 经典闭式求解器

$AX=XB$ 的求解有成熟的闭式方法谱系。按"是否把旋转和平移解耦"可分为两类(eye-in-hand 与 eye-to-hand 同形,以下求解器两者通用)。

记号:下文 $[\mathbf v]_\times$ 为向量 $\mathbf v$ 的反对称矩阵;$\mathbf n_A$ 等为 $A$ 等的旋转轴单位向量、$\theta_X$ 为 $X$ 的旋转角,Tsai 法的 $\mathbf n'_X$ 为其旋转轴的修正参数;Park 法的 $\boldsymbol\alpha_i,\boldsymbol\beta_i$ 是 $A_i,B_i$ 的旋转向量(即李代数对数 $\log A_i,\log B_i$)。

闭式求解器谱系:分离法(先 R 后 t:Tsai-Lenz / Park / Shah)vs 同时法(R、t 联立:Andreff / Daniilidis / Horaud)

分离法(先解旋转 $R_X$,再代入解平移 $\mathbf{t}_X$):

  • Tsai-Lenz (1989):用旋转轴的对偶性 $[\mathbf{n}_A+\mathbf{n}_B]_\times \mathbf{n}'_X = \mathbf{n}_A-\mathbf{n}_B$ 解旋转,旋转角 $\theta_X=2\tan^{-1}\|\mathbf{n}'_X\|$;平移由 $(R_A-I)\mathbf{t}_X = R_X\mathbf{t}_B-\mathbf{t}_A$ 最小二乘求解。简单、极快,但对旋转噪声敏感。
  • Park-Martin (1994):基于李群对数映射,把相对旋转映射为旋转向量后线性化,$R_X=(M^\top M)^{-1/2}M^\top$($M=\sum\boldsymbol{\beta}_i\boldsymbol{\alpha}_i^\top$,SVD 取逆矩阵平方根)。在多法中计算最快。
  • Shah (2013):用 Kronecker 积显式线性化,解出旋转后用 SVD 投影到最近正交矩阵,并用正交化后的旋转重新计算平移(区别于下面的 Andreff/Horaud)。在经典法中相对旋转误差最低。

同时法(旋转和平移一起求解):

  • Andreff et al. (1999/2001):放弃 SO(3) 正交性约束,把旋转矩阵 9 个元素都作未知数,用 Kronecker 积将整个问题化为单一齐次线性系统,SVD 解零空间。缺点是噪声下解出的 $R_X$ 可能偏离 SO(3),正交化后又未同步更新平移,引入平移误差。
  • Daniilidis (1999):用单位对偶四元数 $\hat{x}=q_r+\varepsilon q_d$(约束 $\|q_r\|=1,\ q_r^\top q_d=0$)统一编码旋转($q_r$)与平移($q_d$),$AX=XB$ 化为对偶四元数的 Sandhu 方程 $\hat{a}\hat{x}=\hat{x}\hat{b}$。关键定理:把 $A,B$ 视作螺旋运动,其轴线为 Plücker 直线 $\mathbf{l}+\varepsilon\mathbf{m}$,手眼变换只把"手"的螺旋轴集合刚体映到"眼"的螺旋轴集合;每组运动给出关于该直线系数的 2 个标量线性约束,栈叠后一次 SVD 同时解出 $R$ 和 $\mathbf{t}$(而 Tsai-Lenz 等需先 $R$ 后 $\mathbf{t}$),且只需"线系数"而非完整位姿,几何意义优雅、对旋转噪声最鲁棒。Dekel 等(2020)进一步给出对偶四元数下的精确最小二乘最优解并支持外参先验项,适合有 CAD 初值或需正则化退化方向的场景。
  • Horaud-Dornaika (1995):同时提供四元数闭式解与 Levenberg-Marquardt 非线性联合优化,后者对噪声最鲁棒但需初值。

OpenCV 的 cv2.calibrateHandEye 提供五种方法的统一接口,与上述论文一一对应(CALIB_HAND_EYE_TSAI / PARK / HORAUD / ANDREFF / DANIILIDIS)。ROS 生态的 easy_handeye 默认封装 Tsai-Lenz 法。

两条工程规律

Enebuse et al. (2022) 的系统比较给出两条重要规律:

  1. 同时法对旋转噪声更鲁棒,分离法对平移噪声更鲁棒。同时法把噪声在 $R$、$\mathbf{t}$ 之间分摊;分离法一旦 $R$ 错,$\mathbf{t}$ 直接受污染。
  2. 运动范围的影响相反:增大旋转幅度有利于分离法、不利于同时法;增大平移幅度则反之。

由此:算法选择应与运动特征匹配,不存在普适最优——这与"用 Daniilidis 一劳永逸"的直觉不同。在联合噪声下,综合最可靠的是 Daniilidis、Park、Chou。一个实践注意:旋转角接近 0° 或 180° 时,多种方法可能失稳,Park 相对最稳。

补充两点。其一,近年仍有新的闭式方法提出,例如 Sarabandi 等(IRI/UPC)的两阶段闭式法,求解思路与 Tsai-Lenz 同类(先旋转后平移)但数值上更稳健。其二,一个被反复报告的工程现象:在某些数据集上 Tsai、Andreff、Horaud、Daniilidis 多种方法会给出明显错误乃至发散的结果,只有 Park 法保持稳定,OpenCV 官方仓库的 issue #24871 记录了这一现象。这并非否定其它方法的价值,而是再次印证——多种方法交叉验证、而非依赖单一方法,才是稳妥的做法。

4. 纯点云场景的根本差异

本节针对 eye-in-hand:雷达随臂动,$B$ 来自相邻帧点云配准。eye-to-hand 的 $B$ 来自逐帧拟合末端上的标定物(见 §2.3),不涉及配准难题。

相机手眼标定的 $B_i$ 来自检测棋盘格角点经 PnP 求解,精确且确定性强。纯点云激光雷达没有 2D 角点、没有像素灰度,不存在天然的同名点对应。要得到 $B_i$,必须先解决一个前置问题——两帧点云之间的刚体变换估计,即点云配准。

这意味着:

  1. $B_i$ 的精度完全取决于点云配准的精度;
  2. 配准本身是一个非凸优化问题(ICP 或其变体),有局部最优、初值敏感等固有困难;
  3. 若不使用标定物,$B_i$ 只能从自然环境结构中提取,约束更弱。

纯点云手眼标定的精度上限,往往受制于配准质量,而非机器人运动学精度。 这是它与图像手眼标定的根本差异,也是第 8 节要专门讨论配准局限的原因。

ICP 的角色

ICP(Iterative Closest Point)通过两步迭代——“在当前变换下为源点云每个点找目标云中最近对应点"与"最小化对应点对距离更新变换”——估计两帧点云间的刚体变换,即雷达自身的相对运动 $B_k$。两种主流变体:point-to-point(通用但收敛慢)与 point-to-plane(利用法向量约束,Open3D 实测 30 次迭代即可达到 point-to-point 需 2000 次的效果)。

关键定位:ICP 是局部精配准,需要粗略初始对齐。标准流程是先用全局配准(RANSAC + FPFH 特征)给初值,再用 ICP 精化。这个初值依赖在第 8 节会再回到。

比 ICP 更进一步的配准方法。 配准质量直接决定 $B_k$ 精度,因此现代工具箱远不止 ICP:(1)概率配准——GICP 把源/目标局部邻域各建模为高斯,最小化马氏距离 $d^2=(p_i-p_j')^\top(\Sigma_i+R\Sigma_j'R^\top)^{-1}(p_i-p_j')$,统一并推广了 point-to-point 与 point-to-plane;NDT 把参考云体素化为高斯分布、对面到分布优化,无需显式对应——结构化墙面场景下二者比纯 ICP 更稳。(2)抗外点的全局配准——对应含大量误匹配时 ICP 会陷局部极小,TEASER++ 用截断最小二乘(TLS)把 SE(3) 配准解耦为 scale/rotation/translation 三步、给出可证明全局最优证书,可承受 >90% 外点率;FGR 用 FPFH 描述子 + $L^1$ 鲁棒核全局优化、免粗对齐,可作为两步法第一步粗配准或退化场景下 $B_k$ 精化的现代替换。这些方法降低了 §8 将讨论的初值敏感与外点脆弱性。

值得指出,即便使用点云传感器,只要借助已知标定物(如三维标定靶),仍可获得比纯环境配准干净得多的 $B_i$——这正是 Wan 等《基于法兰的 3D 相机手眼标定》一类工作的思路。换言之,点云介质本身不必然带来困难,真正的困难在于"无标定物、纯靠环境配准"这一情形。

5. 用环境替代标定物:严格等价于 AX=XB

这一节回答核心问题:能否不用标定物,仅靠自然环境(如普通办公室)的点云,标定出外参? 答案是可以,且它严格等价于 $AX=XB$。

本节针对 eye-in-hand:靠相邻帧点云配准测雷达自身运动 $B$。eye-to-hand 若也要免标定物,需在环境点云中分割/跟踪末端,难度高得多,实践中仍多用标定物(见 §2.3)。

5.1 推导:环境标定就是 AX=XB

记末端位姿序列 $G_i$、点云 $P_i$、外参 $X$(符号定义见 §1.3)。相邻两帧间,雷达自身的相对运动记为 $B_i={}^{L_{i+1}}T_{L_i}$(取"新看旧"方向,与 $A_i$ 同向)。下面先厘清 $B_i$ 的方向直觉,再用四步推出它与 $A_i$、$X$ 的关系。

$B_i$ 的方向直觉:为什么是"新看旧"。 ${}^{L_{i+1}}T_{L_i}$ 读作"新位置 $L_{i+1}$ 回看旧位置 $L_i$"。举例:雷达沿自身 $x$ 轴(朝前)平移 1 米到达下一帧,则"旧看新"的 ${}^{L_i}T_{L_{i+1}}$ 平移为 $(1,0,0)$(新位置在前方 1 米),而 $B_i={}^{L_{i+1}}T_{L_i}$ 是它的逆,平移为 $(-1,0,0)$(回看:旧位置在后方 1 米)。"回看"方向乍看反直觉,但选它有切实理由:把第 $i$ 帧点云 $P_i$ 配准到第 $i+1$ 帧 $P_{i+1}$ 时,配准器直接吐出的正是"把 $P_i$ 搬到 $P_{i+1}$ 所在系"的变换,即 $B_i={}^{L_{i+1}}T_{L_i}$,无需再取逆。对应地,末端相对运动也取同向 $A_i={}^{E_{i+1}}T_{E_i}=G_{i+1}^{-1}G_i$——$A_i$、$B_i$ 同向配对,正是下面 $AX=XB$ 形式对称的根源。

四步推导。

① 雷达在世界的位姿。 雷达固连末端,外参 $X={}^{E_i}T_{L_i}$ 对所有 $i$ 相同;末端在世界的位姿为 $G_i={}^{W}T_{E_i}$。按右乘链复合(中间系 $E_i$ 匹配):

$${}^{W}T_{L_i} = {}^{W}T_{E_i}\cdot{}^{E_i}T_{L_i} = G_i\, X, \qquad {}^{W}T_{L_{i+1}} = G_{i+1}X$$

② 经世界系中转求相对运动。 $L_i$ 与 $L_{i+1}$ 之间没有共同中间系、无法直连,需经固定的世界系 $W$ 中转:$L_i\to W\to L_{i+1}$。用逆规则 $({}^{A}T_B)^{-1}={}^{B}T_A$ 把 ${}^{L_{i+1}}T_W$ 换成 $({}^{W}T_{L_{i+1}})^{-1}$:

$${}^{L_{i+1}}T_{L_i} = \underbrace{{}^{L_{i+1}}T_W}_{=({}^{W}T_{L_{i+1}})^{-1}}\cdot\underbrace{{}^{W}T_{L_i}}_{=G_iX} = (G_{i+1}X)^{-1}(G_iX)$$

③ 展开逆,归并中间项。 用 $(AB)^{-1}=B^{-1}A^{-1}$ 拆开 $(G_{i+1}X)^{-1}$,再用结合律:

$$(G_{i+1}X)^{-1}(G_iX) = \big(X^{-1}G_{i+1}^{-1}\big)(G_iX) = X^{-1}\,\underbrace{G_{i+1}^{-1}G_i}_{A_i}\, X$$

关键是 $X$ 与 $X^{-1}$ 被挤到等号两端,中间的 $G_{i+1}^{-1}G_i$ 自成一项,正是 §1.3 定义的 $A_i$。

④ 整理成 $AX=XB$。 由 ②③,$B_i={}^{L_{i+1}}T_{L_i}=X^{-1}A_iX$。两边左乘 $X$ 消去左端 $X^{-1}$:

$$XB_i = X(X^{-1}A_iX) = A_iX \;\;\Longrightarrow\;\; \boxed{\,A_iX = XB_i\,}$$

这就是 $AX=XB$:$A_i$ 由机器人位姿算出(已知),$B_i$ 由点云配准测出(观测),$X$ 待求。

$B_i$ 如何测出。 把第 $i$ 帧点云 $P_i$ 配准到第 $i+1$ 帧 $P_{i+1}$,所得相对变换即为 $B_i={}^{L_{i+1}}T_{L_i}$(同一片环境、两个雷达位姿之间的变换 = 雷达自身的相对运动)。注意这与第 2.1 节用标定物得到的 $B_i=C_{i+1}C_i^{-1}$ 是同一个物理量,只是测量途径不同:标定物靠已知角点对应,环境靠点云配准。

位姿递推:为什么是 ${}^{W}T_{L_i}\cdot B_i^{-1}$ 而非乘 $B_i$。 把雷达在世界的位姿 ${}^{W}T_{L_i}=G_iX$ 往下一帧递推,位姿复合要求复合进去的是"旧看新"的 ${}^{L_i}T_{L_{i+1}}=B_i^{-1}$(右乘链:前项尾 $L_i$ = 后项头 $L_i$):

$${}^{W}T_{L_{i+1}} = {}^{W}T_{L_i}\cdot{}^{L_i}T_{L_{i+1}} = {}^{W}T_{L_i}\cdot B_i^{-1}$$

而不是乘 $B_i$:$B_i={}^{L_{i+1}}T_{L_i}$ 是"新看旧",其头部是 $L_{i+1}$,与链尾的 $L_i$ 对不上,复合非法。验证:${}^{W}T_{L_i}\cdot B_i^{-1}=G_iX\cdot X^{-1}G_i^{-1}G_{i+1}X=G_{i+1}X={}^{W}T_{L_{i+1}}$ ✓。反向亦成立:${}^{W}T_{L_i}={}^{W}T_{L_{i+1}}\cdot B_i$(从 $L_{i+1}$ 回退,右乘链合法)。一句话规则(§1.2 已述):${}^{A}T_B$ 永远右乘到"以 $A$ 结尾"的位姿上——$B_i={}^{L_{i+1}}T_{L_i}$ 右乘到 ${}^{W}T_{L_{i+1}}$ 上得到 ${}^{W}T_{L_i}$,左乘则无意义。

别和点坐标变换搞混(方向相反,但不矛盾)。 位姿递推用的是 $B_i^{-1}$(右乘),而点坐标变换 $\mathbf{p}^{(L_{i+1})}=B_i\,\mathbf{p}^{(L_i)}$ 用的是 $B_i$(左乘)。一个右乘其逆、一个左乘本身,只因对象不同:前者复合的是坐标系位姿,后者搬动的是点坐标。这正是 SE(3) 的对偶——坐标系(雷达)前进 1 米,固定的点在它眼里就后退 1 米。从位姿复合式取逆即得点变换式,两者完全自洽,根源都在 §1.2 那条"$B\to A$ 变换 = $B$ 在 $A$ 的位姿"的定义性质。

整个推导里,环境只在"如何测量 $B_i$"这一步登场。 $B_i$ 的物理真值由 $A_i$ 和 $X$ 决定(刚体运动学),与环境无关。环境是一把测量尺:观察环境特征在两帧间的变化,反推雷达怎么动了。一旦 $B_i$ 测出,后面就是纯几何的 $AX=XB$,环境长什么样不再参与。

5.2 正确性检验:旋转环境,结果不变

一个检验方法正确性的思想实验:把整个办公室物理旋转 90°,机械臂保持不动、重复完全相同的位姿序列 $\{G_i\}$,采到新点云 $\{P'_i\}$。逐项看:

量 旋转后 原因
$A_i = G_{i+1}^{-1}G_i$ 不变 机械臂动作相同
$X$(外参) 不变 雷达-末端物理连接没动
$B_i = X^{-1}A_iX$(物理真值) 不变 $A_i,X$ 都没变
配准测出的 $B_i$ 不变 配准测的是"雷达怎么动",不是"环境怎么长"

虽然雷达看到的点云内容变了($P'_i \neq P_i$),但雷达自己从 $L_i$ 到 $L_{i+1}$ 的运动没变。点云配准测量的是雷达的相对运动,不是环境的内容;办公室旋转对第 $i$ 帧和第 $i+1$ 帧是同一个刚体变换,在"两帧之间求相对运动"时被消掉了。

所以 $A_i$ 不变、$B_i$ 不变 → $AX=XB$ 是完全相同的方程 → 解出完全相同的 $X$。旋转环境,标定结果不变,正好说明环境标定是正确的——它把"外参"(雷达-末端关系,与环境无关)和"环境"(测量工具)正确分离了。类比:用尺子量一根棍子的长度,把尺子转 90°,量出的长度不变——长度是棍子的属性,不是尺子的属性。

5.3 两步法 vs 自洽融合法

得到 $B$ 之后有两种用法,二者理论等价(用同样的数据),区别在工程组织:

  • 两步法(解耦):步骤 1,先用点云配准独立估计每个 $B_k$;步骤 2,把 $B_k$ 和已知的 $A_k$ 代入 $AX=XB$ 闭式求解。$X$ 与地图解耦。
  • 自洽融合法(耦合):不单独求 $B$,让 $X$ 直接优化"所有帧点云在世界系融合后的全局一致性"(残差最小)。

理论等价性:$B_k$ 本身就是由"两帧点云融合"定义的,自洽融合法的全局优化在相邻帧层面隐含了同样的 $B_k$ 约束,用的数据完全一样。区别在于:

维度 两步法 自洽融合法
$X$ 与 $B$ 的关系 解耦,$B$ 先独立测出 耦合,$B$ 与 $X$ 一起优化
优化性质 步骤 2 是闭式线性,稳定 全局非线性,易陷局部最优
可诊断性 $B_k$ 可逐个检查、剔除坏帧 误差纠缠,难定位
误差结构 配准误差与求解误差分离 两类误差耦合,可能有偏

所以推荐两步法:它把"配准"和"求解"分离,每一步都可独立检查,误差结构清晰。这不是说自洽融合法"理论上行不通",而是说它把本可分离的误差耦合在一起,在工程上更难诊断和控制。

两步法(配准求 B 与求解 X 解耦)与自洽融合法(B 与 X 耦合优化)的流程对比

5.4 环境对称性是另一个独立问题

需要明确区分:5.2 讨论的是"物理旋转一个不对称环境",结果是 $X$ 不变。但如果环境本身具有对称性(房间精确 90° 旋转对称、长走廊的平移对称、重复柱阵),那确实会出问题——但那是另一个机制:

  • 点云配准可能把雷达的真实运动 $\theta$ 误配成对称等价的 $\theta+90°$,估出错误的 $B_i$,$X$ 跟着错。
  • 这是信息缺失(环境给不出足够信息区分等价位姿),根源在数据(配准退化),与"物理旋转不对称环境"无关。

这个风险是真的,但它要求环境精确对称,普通结构不对称的办公室不会触发;在真实场景里它降级为"在大片平整墙面、地面这类局部对称区域,沿对称方向约束弱、$B_i$ 估得松、该方向精度打折"——是连续打折而非崩溃,且能用第 6 节的可观测性判据量化。详见第 8 节的走廊退化分析。

6. 可观测性:什么运动能解出 X

本节回答"哪些采集运动能解出 X、解出的 X 有多准":先给可解性条件(§6.1),建立可观测性雅可比(§6.2)与指标(§6.3),枚举退化运动(§6.4)与一个反直觉结论(§6.5),最后是运动设计原则(§6.6)与误差来源(§6.7)。这些判据的完整案例——只用一面墙的平面标定——另立 §7。可观测性只依赖运动的 $AX=XB$ 结构,对两种配置同样适用——eye-to-hand 把标定物运动代入同一个雅可比即可。(本节用 $k$ 编号第 $k$ 组运动对,共 $L$ 组,与 §1–§2 的帧下标 $i$ 同义;雅可比奇异值降序记 $\sigma_1\geq\dots\geq\sigma_L$,$\sigma_1$ 最大、$\sigma_L$ 最小即 $\sigma_{\min}$。)

6.1 可解性的基本条件

$AX=XB$ 并非对任意运动都有唯一解。Shiu-Ahmad (1989)、Tsai-Lenz (1989) 最早指出:解的唯一性的充分条件是存在两个旋转轴不平行(非共线)的标定运动(Chen 1991 给出几何证明)。若所有运动旋转轴相互平行(如 SCARA,所有关节轴同向),旋转部分构成的矩阵秩不超过 2,系统不可解。

6.2 可观测性雅可比

可观测性分析的标准做法是把约束方程在解附近线性化,考察"识别雅可比"的秩。先约定记号(李群工具,详见《机械臂技术基础》§3.7):$\boldsymbol\xi=[\boldsymbol\omega;\mathbf v]\in\mathbb R^6$ 是位姿的微小扰动(前三维角速度 $\boldsymbol\omega$、后三维线速度 $\mathbf v$);$\hat{\boldsymbol\xi}\in\mathfrak{se}(3)$ 是它的 4×4 矩阵形式("hat"映射);$\exp$、$\log$ 为矩阵指数/对数,在 $\mathfrak{se}(3)$ 与 $SE(3)$ 间互逆;$\mathrm{Ad}_T$ 是 $T$ 的 6×6 伴随,满足共轭关系 $T\hat{\boldsymbol\eta}T^{-1}=\widehat{\mathrm{Ad}_T\boldsymbol\eta}$;$I_6$ 为 6 阶单位阵。

衡量 $X$ 偏离真解的残差定义为

$$\mathbf{r}_k(X) = \log(A_k\, X\, B_k^{-1}\, X^{-1}) \;\in\; \mathbb{R}^6$$

当 $A_kX=XB_k$ 时括号内为 $I$、残差为零。对 $X$ 施加右扰动 $X\to X\exp(\hat{\boldsymbol\xi})$,把残差对 $\boldsymbol\xi$ 一阶展开。依次用共轭关系(取 $T=B_k^{-1}$ 得 $\exp(\hat{\boldsymbol\xi})\,B_k^{-1}=B_k^{-1}\exp\!\big(\widehat{\mathrm{Ad}_{B_k}\boldsymbol\xi}\big)$)与 $\exp(\pm\hat{\boldsymbol\xi})\approx I\pm\hat{\boldsymbol\xi}$:

$$\begin{aligned} A_kX\exp(\hat{\boldsymbol\xi})B_k^{-1}\exp(-\hat{\boldsymbol\xi})X^{-1} &\approx A_kX B_k^{-1}\big(I+\widehat{(\mathrm{Ad}_{B_k}-I_6)\boldsymbol\xi}\big)X^{-1}\\ &\xrightarrow{\,A_kX=XB_k\,\Rightarrow\, A_kXB_k^{-1}=X\,} X\big(I+\widehat{(\mathrm{Ad}_{B_k}-I_6)\boldsymbol\xi}\big)X^{-1}\\ &= I+\widehat{\mathrm{Ad}_X(\mathrm{Ad}_{B_k}-I_6)\boldsymbol\xi} \end{aligned}$$

再取 $\log$(一阶下 $\log(I+\hat{\boldsymbol\eta})=\boldsymbol\eta$):

$$\mathbf{r}_k \;\approx\; \mathrm{Ad}_X\,(\mathrm{Ad}_{B_k}-I_6)\,\boldsymbol\xi$$

把所有 $\mathbf{r}_k$ 堆叠,得到关于扰动 $\boldsymbol\xi$ 的线性系统 $\mathbf{r}=J\boldsymbol\xi$($\mathrm{Ad}_X$ 作为块对角左乘因子出现)。
对 $J$ 做 SVD 得奇异值 $\sigma_1\geq\dots\geq\sigma_L$。

关键结论:雅可比块里的 $\mathrm{Ad}_X$ 恒可逆(任何 $X\in SE(3)$ 的伴随都非奇异),作为可逆左乘因子不改变 $J$ 的秩——可观测性/零空间判定不受影响;它一般非正交,条件数会偏移 $\kappa(\mathrm{Ad}_X)$ 量级,但不影响秩判定。因此真正决定可观测性的是 $\{B_k\}$ 这组运动本身:

$$\text{可观测性} \;\longleftarrow\; \text{堆叠的 } (\mathrm{Ad}_{B_k}-I_6) \;\longleftarrow\; \text{运动 } \{B_k\}$$

这与待估的 $X$ 无关。实务上直接取 $J_k=\mathrm{Ad}_{B_k}-I_6$ 堆叠做 SVD——它等价于 $I_6-\mathrm{Ad}_{B_k^{-1}}=\mathrm{Ad}_{B_k}^{-1}(\mathrm{Ad}_{B_k}-I_6)$(仅差一个可逆左乘因子,秩与条件数判定完全一致),后文 §9.1 步骤 4 即用此式。工程含义是:采集传感器数据之前,即可预判可观测性——先用机器人位姿算出 $A_k$,再用先验或粗估的 $X$ 由 $B_k=X^{-1}A_kX$ 估出 $B_k$,就能算条件数、判断这组运动够不够好,而不必等采完点云才发现不可解。这也是 Nahvi-Hollerbach、Daney 把可观测性用于"位姿试验设计"的理论基础。把约束写成伴随/旋量(screw)形式的思想,见 Pachtrachai 等的伴随变换算法 ATA,其 6×6 伴随矩阵正是上式中的 $\mathrm{Ad}_X$(与《机械臂技术基础》§3.7 形式一致)。

6.3 可观测性指标

基于奇异值,文献定义了多种指标:

指标 公式 含义
$O_1$ $(\sigma_1\cdots\sigma_L)^{1/m}/\sqrt{m}$ 超椭球体积(几何均值)
$O_2$ $\sigma_L/\sigma_1$ 条件数倒数 $\kappa^{-1}$,越接近 1 越好
$O_3$ $\sigma_L$ 最小奇异值,最差方向灵敏度
$O_4$ $\sigma_L^2/\sigma_1$ 噪声放大指数($=O_2\times O_3$)

($m$ 为雅可比的行数,见各指标定义。)

Nahvi & Hollerbach (1996) 证明 $O_4$ 对标定误差最敏感;Schroer 等建议条件数应低于 100 才能获得可靠结果。Sun & Hollerbach (2008) 的比较表明 $O_1$ 表现差于 $O_2$–$O_4$。

可观测性:雅可比 J=Ad_B−I 的奇异值椭球,σ_min 越小该方向越不可观;纯平移/共轴旋转使椭球塌缩

6.4 退化运动

退化运动的本质是雅可比降秩(某些奇异值趋于零),对应方向参数不可辨识:

  1. 纯平移($R=I$):经典两阶段法的旋转方程退化为恒等式,旋转不可观测(见 §6.5 的反直觉结论);
  2. 绕同一轴旋转(共轴):旋转轴全平行,无法定完整三自由度旋转(SCARA);
  3. 共面运动:运动限制在单一平面内,垂直该平面的自由度不可观测;
  4. 旋转角过小😒\theta\to 0$ 时最小参数化奇异,Daniilidis 的 screw 参数化用 $\sin(\theta/2)$ 做分母,数值不稳。

6.5 反直觉结论:纯平移不能定平移,却能约束旋转

Andreff et al. (2001) 给出不同运动组合的可观测性表($\lambda$ 为结构恢复带来的未知尺度因子,用标定板时已知):

运动 1 运动 2 可求解
纯平移 纯平移 $R_X$, $\lambda$(无 $\mathbf{t}_X$)
纯平移 纯旋转 $R_X$, $\lambda$, $\mathbf{t}_X$(部分)
纯旋转 纯旋转 $R_X$, $\mathbf{t}_X$(带尺度 $\lambda$)
一般运动 一般运动 $R_X$, $\mathbf{t}_X$, $\lambda$(完全解)

关键且反直觉的一点:三个线性无关的纯平移运动,可以线性估计出手眼旋转 $R_X$,但平移 $\mathbf{t}_X$ 不可观测。 物理解释:纯平移时方程退化为 $\mathbf{t}_A = R_X\,\mathbf{t}_B$,相机/雷达平移方向与末端平移方向之间的"旋转映射"恰好暴露了 $R_X$;但纯平移不含尺度信息,无法定位 $\mathbf{t}_X$。

但需注意:同一退化运动在不同算法框架下可观测性不同。Daniilidis 的对偶四元数解耦解法无法利用纯平移的旋转信息——纯平移对应 screw 角 $\theta=0$,旋转方程退化为恒等 $q_r=q_r$,对旋转无约束。也就是说,同样的数据,用 Andreff 的线性框架能提取旋转,用 Daniilidis 的对偶四元数框架却不能——这是选择算法时必须留意的。

7. 单面墙平面标定

前几节的环境标定用到了两帧点云的完整配准($B_k$ 是 6 自由度刚体变换)。一个常见的简化想法是:办公室里有平整墙面,能否只利用"墙是一个固定平面"这个约束?这避开了完整 ICP 的初值与退化难题(平面拟合是凸问题,RANSAC + 主成分稳定),代价是只用到了环境的低维信息。这一节分析它到底能解出多少、精度受什么制约。一个关键结论先行:单面墙缺的不是一个固定的旋转自由度,而是要求运动里有"横向旋转";运动选对了,一面墙就够。

单面墙平面约束:法向(只含旋转)定 R_X,距离(含平移)定 t_X;退化运动是纯绕墙法向自转

(1) 平面的变换。 设墙在世界系 $W$ 中是固定平面,方程 ${}^{W}\mathbf{n}^\top\mathbf{x}={}^{W}d$(${}^{W}\mathbf{n}$ 单位法向,${}^{W}d$ 有向距离,$\mathbf{x}$ 为平面上任意一点)。雷达在位姿 ${}^{W}T_{L_i}=G_iX$ 下观测墙面,拟合出雷达系内的平面 ${}^{L_i}\mathbf{n}_i^\top\mathbf{x}={}^{L_i}d_i$。这里 ${}^{W}T_{L_i}=G_iX$ 是雷达的世界位姿(eye-in-hand 链式合成:$G_i={}^{W}T_{E_i}$ 为末端世界位姿、$X={}^{E}T_L$ 为待标手眼)。要用这帧雷达系观测去约束世界系里的墙,得先把点坐标在两系之间搬运——把位姿 ${}^{W}T_{L_i}$ 作用在齐次点上:

$$\begin{pmatrix}{}^{W}\mathbf{x}\\1\end{pmatrix}=\begin{pmatrix}R(G_iX)&\mathbf{t}(G_iX)\\0&1\end{pmatrix}\begin{pmatrix}{}^{L}\mathbf{x}\\1\end{pmatrix}=\begin{pmatrix}R(G_iX)\,{}^{L}\mathbf{x}+\mathbf{t}(G_iX)\\1\end{pmatrix}$$

旋转块 $R(G_iX)$ 的三列正是雷达系三轴在世界系里的指向,负责把雷达系下测得的分量 ${}^{L}\mathbf{x}$ 重排成世界朝向;平移块 $\mathbf{t}(G_iX)$ 是雷达原点的世界位置,补上原点偏移。丢掉齐次占位行即点变换 ${}^{W}\mathbf{x}=R(G_iX)\,{}^{L}\mathbf{x}+\mathbf{t}(G_iX)$——齐次位姿左乘点,就是"先按 $R$ 旋回世界朝向、再按 $\mathbf{t}$ 平移到世界原点"。代入墙方程 ${}^{W}\mathbf{n}^\top{}^{W}\mathbf{x}={}^{W}d$ 整理得

$$\bigl[R(G_iX)^\top\,{}^{W}\mathbf{n}\bigr]^\top{}^{L}\mathbf{x} \;=\; {}^{W}d - {}^{W}\mathbf{n}^\top\mathbf{t}(G_iX)$$

与 ${}^{L_i}\mathbf{n}_i^\top\mathbf{x}={}^{L_i}d_i$ 逐项比对系数,并展开 $G_iX$ 的旋转块 $R(G_iX)=R_{G_i}R_X$、平移块 $\mathbf{t}(G_iX)=R_{G_i}\mathbf{t}_X+\mathbf{t}_{G_i}$,得平面参数的变换关系:

  • 法向(只含旋转):${}^{L_i}\mathbf{n}_i = R_X^\top\, R_{G_i}^\top\,{}^{W}\mathbf{n}$
  • 距离(含平移):${}^{L_i}d_i = {}^{W}d - {}^{W}\mathbf{n}^\top\bigl(R_{G_i}\mathbf{t}_X+\mathbf{t}_{G_i}\bigr)$

法向与距离的分离是关键:法向只通过旋转 $R_X$ 进入,距离才含平移 $\mathbf{t}_X$。这把标定在物理上拆成两段——先靠法向定旋转、再靠距离定平移,与第 3 节分离求解器"先 $R$ 后 $\mathbf{t}$"的思路同构,只是这里是几何上的天然解耦。

(2) 法向约束 → 旋转 $R_X$,可观测条件是"运动有横向旋转"。 法向方程消去未知的世界法向 ${}^{W}\mathbf{n}$,等价于对所有 $i$:

$$R_{G_i}\,R_X\,{}^{L_i}\mathbf{n}_i \;=\; {}^{W}\mathbf{n} \qquad(\text{同一个世界法向})$$

即各姿态下"末端系看到的墙法向 $R_X{}^{L_i}\mathbf{n}_i$,经机器人姿态 $R_{G_i}$ 转回世界系后处处一致"。这是关于 $R_X$ 与 ${}^{W}\mathbf{n}$ 的约束,可用正交 Procrustes 或线性化求解。

可观测性取决于一个量:末端系内的墙法向 $\mathbf{a}_i=R_{G_i}^{\top}{}^{W}\mathbf{n}$ 是否随姿态变化。

  • 若运动包含垂直于墙法向的旋转分量,$\mathbf{a}_i$ 随姿态指向不同方向,$R_X$ 受到约束——理论上约 3 个不同朝向的姿态即可定出 $R_X$ 与 ${}^{W}\mathbf{n}$。
  • 退化运动是纯绕墙法向 ${}^{W}\mathbf{n}$ 的旋转:此时 $R_{G_i}^{\top}{}^{W}\mathbf{n}\equiv{}^{W}\mathbf{n}$ 不变,各姿态的雷达系法向观测 ${}^{L_i}\mathbf{n}_i=R_X^{-1}\mathbf{a}_i$ 完全相同,$R_X$ 无从约束。直观理解:雷达绕墙法向自转时,墙面在雷达系内的法向不动、距离不动,这一自转不可观测。

需要澄清一个常见误解:这里缺的不是"绕墙法向那个固定的旋转自由度"。单帧时,绕该帧雷达系法向的自转固然不可观测;但多姿态聚合后,只要运动让 $\mathbf{a}_i$ 有变化,旋转 $R_X$ 就能完整定出。换言之,单面墙的退化是运动设计问题(有无横向旋转),而非自由度缺失;选对运动,一面墙即可解旋转。

(3) 距离约束 → 平移 $\mathbf{t}_X$,退化条件与旋转相同。 把距离方程按 $\mathbf{t}_X$ 整理成线性形式:

$$\bigl(R_{G_i}^\top\,{}^{W}\mathbf{n}\bigr)^\top\,\mathbf{t}_X \;=\; {}^{W}d - {}^{L_i}d_i - {}^{W}\mathbf{n}^\top\mathbf{t}_{G_i}$$

左端系数 $\mathbf{a}_i=R_{G_i}^\top{}^{W}\mathbf{n}$ 即第(2)步定义的末端系法向。每帧给一个关于 $\mathbf{t}_X$ 的标量方程,堆叠成线性最小二乘。能解出 3 个平移分量的条件是 $\{\mathbf{a}_i\}$ 张满三维——这与法向可观测条件完全是同一个:都要 $R_{G_i}^\top{}^{W}\mathbf{n}$ 随姿态变化。纯平移($R_{G_i}=I$)时所有 $\mathbf{a}_i={}^{W}\mathbf{n}$ 相同,秩为 1,只能定 $\mathbf{t}_X$ 沿墙法向的一个分量;墙内两个切向平移不可观测。

(4) 单面墙到底解了多少。 汇总平面约束的可观测性:

运动类型 旋转 $R_X$ 平移 $\mathbf{t}_X$
一般旋转(含横向旋转) 可定($\geq3$ 不同朝向姿态) 可定(需旋转覆盖三维)
纯平移 法向观测不变,无信息 仅沿墙法向 1 个分量
纯绕墙法向旋转 无信息 仅沿墙法向 1 个分量

所以"一面墙够不够"的答案是:只要机械臂做包含横向旋转(垂直于墙法向的轴)的运动,一面墙就能解出完整的 $R_X$ 与 $\mathbf{t}_X$(连同墙参数 ${}^{W}\mathbf{n},{}^{W}d$ 一起)。办公室机械臂做几个不同朝向的姿态即可满足。多面不平行墙(如地面 + 两面相邻墙)的作用主要是改善条件数、增加冗余与鲁棒性,降低对运动设计的敏感,而非数学上不可或缺。PlaneHEC 等工作利用机械臂多视角下的平面/任意点云约束做手眼标定,可作为这类方法的实现参考。

(5) 精度为何受限:条件数的几何。 多面墙“主要改善条件数”并非经验之谈——条件数由约束方向的散布决定,单面墙把所有约束方向绑在一根法向上,点数再多也不改变方向,故救不了条件数。这可从平面约束的雅可比推出来:

(a) 单帧平面只给 3 个有效约束,且都拴在法向上。 在解附近对 $X$ 线性化。法向是单位向量、只依赖旋转,其扰动为

$$\delta\mathbf{n}_i \;=\; \mathbf{n}_i\times\delta\boldsymbol\omega \;=\; [\mathbf{n}_i]_\times\,\delta\boldsymbol\omega$$

其中 $[\mathbf{n}_i]_\times$ 的核为 $\mathrm{span}(\mathbf{n}_i)$——绕法向自身那一转使 $\mathbf{n}_i$ 不动,法向对它零敏感;法向的全部旋转信息只落在 $\perp\mathbf{n}_i$ 的二维切面里(秩 2),这正是“法向信息集中在一个二维子空间”的精确含义。距离只依赖平移,扰动 $\delta d_i=-\mathbf{a}_i^\top\delta\mathbf{t}_X$ 只感知沿 $\mathbf{a}_i$ 的平移(秩 1)。故每帧有效约束是 2(旋转)+1(平移)=3 个,远少于完整配准每帧的 6 个自由度,且这 3 个的方向全由同一个 $\mathbf{a}_i$ 决定。

(b) 条件数 = 约束方向散布的各向异性(以平移块看得最清楚)。 把各帧平移约束 $\mathbf{a}_i^\top\mathbf{t}_X=\cdots$ 堆叠,信息矩阵为墙法向的二阶矩

$$S \;=\; \sum_{i=1}^{N}\mathbf{a}_i\mathbf{a}_i^\top,\qquad \mathrm{eig}(S)=(\lambda_1\geq\lambda_2\geq\lambda_3)$$

平移块的奇异值即 $\sqrt{\lambda_1},\sqrt{\lambda_2},\sqrt{\lambda_3}$,最差方向 $\sigma_{\min}\propto\sqrt{\lambda_3}$,条件数 $\kappa\propto\sqrt{\lambda_1/\lambda_3}$。$S$ 的特征值刻画“墙法向在各末端姿态下看出去有多散”。旋转块同理——其可观测充要条件正是 $\{\mathbf{a}_i\}$ 张满三维(本节 (2)),条件数受同一组散布制约,绕法向的“自转”始终是结构上的弱方向。它与可观测性表逐行吻合:纯平移时 $\mathbf{a}_i$ 不变,$S$ 秩为 1($\lambda_2=\lambda_3=0$),只剩沿法向 1 个平移分量;纯绕墙法向转时 $\mathbf{a}_i\equiv{}^{W}\mathbf{n}$ 同样不变,信息崩溃。

© 单面墙让 $S$ 各向异性,多面墙让它各向同性。 单面墙的所有 $\mathbf{a}_i=R_{G_i}^\top{}^{W}\mathbf{n}$ 都是同一个世界法向 ${}^{W}\mathbf{n}$ 在各姿态下的像,方向高度相关、挤在球面一个带状区域 → $S$ 各向异性,$\lambda_3\ll\lambda_1$ → $\sigma_{\min}$ 小、$\kappa$ 大。多面不平行墙提供相互独立的法向族:墙 B 的法向照亮墙 A 留下的暗角,$\{\mathbf{a}_i\}$ 在球面铺开 → $S$ 各向同性 → $\kappa$ 小。完整配准每帧给满 6 自由度的 $B_k$,约束方向天然铺满,$\kappa$ 更好。这与 GPS 的精度衰减因子 DOP 同构——DOP 由卫星方向二阶矩的各向异性决定,纯几何、与测距精度无关。

(d) 点数为什么救不了。 进入 $S$ 的只是各帧法向的方向 $\mathbf{a}_i$,而 $\mathbf{a}_i$ 由墙面朝向与机械臂姿态决定。同一面墙上再多的点,只是把同一个 $(\mathbf{n}_i,d_i)$ 拟合得更精(缩小测量噪声 $\sigma_n$),完全不改变 $\mathbf{a}_i$ 的方向、因而完全不进 $S$ 的特征值。最终外参误差 $\sim\sigma_n\times\kappa$:点数只缩小分子 $\sigma_n$,分母 $\kappa$ 由几何注定、纹丝不动。一把再准的尺,量一个几乎与尺向垂直的方向,精度也被几何吃掉——这正是 §6.3 的 $O_2=\sigma_L/\sigma_1=\kappa^{-1}$、$O_4=\sigma_L^2/\sigma_1$(Nahvi-Hollerbach 证 $O_4$ 对标定误差最敏感)所量的几何衰减。

(e) 但运动充分时,单面墙并不注定差——差的不是下限,是门槛与稳健性。 上面“单面墙 $S$ 各向异性”的前提,是法向方向 $\mathbf{a}_i$ 没铺开。一旦机械臂运动把 $\mathbf{a}_i=R_{G_i}^\top{}^{W}\mathbf{n}$ 在球面上铺到各向同性,$S\to(N/3)I$、$\lambda_1/\lambda_3\to1$,$\kappa$ 逼近最小值,与多面墙理想情形同档——单面墙没有数学上的 $\kappa$ 下限,运动充分时精度可与多面墙、完整配准同量级。代价在三处:(i)门槛高——世界系里固定的墙法向要扫遍雷达系各方向,得大角度旋转,其中不少姿态让墙面处于掠射(点稀、拟合差)或出视野;多面墙靠几何自带独立法向族,温和运动就各向同性。(ii)每帧信息减半(平面 3 标量 vs 完整配准 6),同样 $\kappa$ 需约 2 倍姿态数。(iii)$\kappa$ 脆——暗方向(绕法向自转、墙内切向平移)在任一单帧都不可见,只靠多帧 $\mathbf{a}_i$ 散布间接三角化,覆盖稍有空缺 $\kappa$ 即飙升;多面墙每帧直接约束多方向,运动不完美 $\kappa$ 也稳。一句话:多面墙把好条件数白送,单面墙要靠精心设计、充分铺开的运动去挣——挣到了一样好,挣不到就明显差、且更敏感。

(6) 精度:受什么制约,大概到什么量级。 平面法的精度上限由三层叠加决定:

  1. 平面拟合精度:墙面越平、点云越密、距离越近,拟合的法向 ${}^{L_i}\mathbf{n}_i$ 与距离 ${}^{L_i}d_i$ 越准。平整墙面、近距(1–5 m)、点数充足时,法向角误差通常在 $0.05°$–$0.5°$ 量级(取决于雷达测距噪声)。
  2. 可观测性放大:求解的条件数 $\kappa$ 把拟合误差放大后落地(单面墙 $\kappa$ 天然偏大的几何原因见 (5)),与第 6.3 节的 $O_2$/$O_4$ 指标是同一回事,是平面法精度的主要短板。
  3. 退化方向的残留:若运动里横向旋转不足,$\kappa$ 飙升,对应方向误差被剧烈放大。

据此给出量级估计(须强调这是基于误差传播的估计、非实测;实际精度取决于雷达型号、墙面质量与运动设计,建议标定后用留出残差验证):

  • 旋转:横向旋转充分、姿态数足够时,$R_X$ 角误差有望压到 $0.1°$–$1°$ 量级;横向旋转不足则该方向系统偏差大。
  • 平移:距离观测受雷达测距噪声限制(近距毫米级、远距厘米级),经运动几何(条件数)放大后,$\mathbf{t}_X$ 典型 $1$–$10\text{ mm}$;条件数差时可劣化到厘米级。

(7) 平面法 vs 完整配准:信息更少,但更稳。 把平面法放回第 5 节框架:完整的环境配准(两帧 6 自由度 $B_k$)用到环境全部三维结构,信息充分;平面法只用每个平面 3 个参数,是环境信息的低维投影,因此条件数更差、对运动更敏感。但平面法有完整配准不具备的鲁棒性:平面拟合是凸优化(RANSAC + 主成分),没有 ICP 的初值敏感与局部最优,也不受墙上非平面杂物干扰(可 RANSAC 剔除)。所以在结构化室内(墙面清晰、平整),平面法往往比直接做全局 ICP 更可靠;在几何杂乱、缺乏大平面的场景,完整配准或标定物更合适。两者可以混用——平面给稳健初值,再按需精化。

6.6 运动设计原则

综合各文献,设计采集动作的原则:

  1. 旋转轴多样化:至少两组非平行轴,理想覆盖三维不同方向;
  2. 最小化条件数:选使 $\kappa=\sigma_1/\sigma_L$ 尽量小的运动组;
  3. 足够旋转幅度:几十度量级,避免过小(轴估计不准)和接近 180°(参数奇异);
  4. 旋转与平移搭配:分离法(先 $R$ 后 $\mathbf{t}$)适合大旋转 + 小平移;同时法适合小旋转 + 大平移;
  5. 数据剪枝:用向量量化等方法使旋转轴在旋转空间均匀分布,剔除冗余和近退化运动。

6.7 误差来源与传播

可观测性回答"运动能否解出 $X$“,误差传播回答"解出的 $X$ 有多准”。两者经同一个雅可比 $J$ 相连:残差 $\mathbf{r}\approx J\boldsymbol\xi$ 反演给出外参扰动 $\Delta\boldsymbol\xi\approx J^+\mathbf{r}$($J^+$ 为 $J$ 的伪逆),被条件数 $\kappa=\sigma_1/\sigma_L$ 放大(§6.3)。残差 $\mathbf{r}$ 有两个来源:

  • $A_i$ 的误差(机器人侧):$A_i=G_{i+1}^{-1}G_i$ 由正运动学给出,误差来自关节编码器噪声与连杆几何参数(即绝对精度)。工业机器人重复精度高(典型 $\pm0.02$–$0.1\text{ mm}$)、绝对精度低($\pm0.1$–$2\text{ mm}$,且与构型相关),故 $A_i$ 常含系统性、构型相关的偏差,可能给 $X$ 带来偏置。
  • $B_i$ 的误差(配准侧):来自点云配准残差,含测距噪声、退化方向的模糊、ICP 收敛误差(详见 §8)。

主导关系:纯点云手眼标定中,$B_i$ 的配准误差通常是主导项——机器人重复性远高于单次点云配准精度(§4)。因此提升精度的首要杠杆在配准侧:更优的配准初值、几何更丰富的场景、更密的点云、退化检测与剔除,而不是一味追求机器人绝对精度。一个容易被忽略的误差源是时间同步:雷达帧与机器人位姿若未对齐,运动中的错位会等效成位姿误差,需用硬件触发或在时间戳间插值对齐。

8. 点云配准的局限与精度边界

本节针对 eye-in-hand 的点云配准精度;eye-to-hand 用标定物、不走配准,可略过。

第 4 节指出纯点云标定的精度受制于配准。本节展开配准的固有局限——这是理解"用环境标定"可行边界的最后一块拼图。

8.1 局部最优与初值敏感

ICP 总是单调收敛到均方误差距离指标的最近局部最小值,初值不够好则收敛到错误结果。Li et al. (2023) 的实验定量验证:纯 ICP 在"部分点云 + 大位姿差异"场景下成功率仅 5.4%;FGR-FPFH 5.2%,TEASER++ 仅 1.2%——需要深度学习网络提供优质初值后 ICP 才生效。这意味着:相邻帧的位姿差异不宜过大,否则配准失败,$B_k$ 无效。Yan 等针对基于 ICP 的手眼标定精度做了专门的改进研究,其结论同样指向初值质量与场景几何丰富度的关键性。

8.2 对称/重复结构导致的退化

走廊/隧道效应:走廊中法向量主要分布在两个轴上(来自地面、天花板、墙壁),“normal vectors are predominantly distributed along only two axes, leading to pose ambiguity along the direction of the corridor”——沿走廊方向产生位姿模糊,这是单向退化。退化恶化的机制:与走廊方向正交的平面因远、稀疏而在迭代中被拒绝,法向量分布失衡导致位姿漂移,条件数急增。

这正是 §5.4 提到的"局部对称区域精度打折"的机理:不是标定方法的问题,而是配准在这些方向给不出约束。

8.3 应对:退化检测与自适应

GenZ-ICP 按平面度(planarity)分类对应点,自适应混合 point-to-plane(结构化环境)与 point-to-point(非结构化)。关键数学:point-to-point 的 Hessian 平移部分为单位矩阵,条件数恒为 1,加入 point-to-point 分量能稳定数值条件,防止退化场景下的位姿发散。在 SubT-MRS Long_Corridor 数据集上,GenZ-ICP 的 APE 均值为 1.69 m,远优于 CT-ICP(44.18 m)和 point-to-plane ICP(32.84 m)。

工程启示:采集标定数据时,应选择几何丰富、非退化的场景(多方向平面、家具、建筑结构),避开空旷走廊、隧道、大块平整墙面朝向的运动方向。

9. 工程实践

综合以上各节,给出环境两步法的实操要点(以下针对 eye-in-hand;eye-to-hand 用标定物,见 §2.3)。

9.1 两步法流程

  1. 采数据:控制机械臂走一组位姿序列 $\{G_i\}$,每个位姿同步采一帧点云 $P_i$。
  2. 求 $B_k$:对相邻帧 $(P_i, P_{i+1})$ 做点云配准,得到 $B_k = {}^{L_{i+1}}T_{L_i}$。配准建议:全局配准(FPFH + RANSAC)给初值,point-to-plane ICP 精化。
  3. 算 $A_k$:$A_k = G_{i+1}^{-1}G_i$(由控制器位姿直接算)。
  4. 可观测性预检:用 $J_k = \mathrm{Ad}_{B_k}-I_6$ 堆叠求条件数(§6.2),剔除使 $\kappa$ 飙升的帧,目标 $\kappa < 100$。
  5. 解 $AX=XB$:用 OpenCV calibrateHandEye 或自实现求解器;建议同时跑几种方法交叉验证。
  6. 验证:用留出的位姿对检查重投影/配准残差,或用 §5.2 的思想实验做正确性抽检。

9.2 采集运动清单

项目 建议 依据
运动组数 ≥10–20 组(理论下界 2 组相对运动)
收益拐点 ~50 组后趋平
旋转轴 至少 2 个非平行,尽量 3 正交方向
单次旋转幅度 几十度量级,避免 <5° 和接近 180°
平移幅度 与算法匹配:分离法小平移,同时法大平移
相邻帧位姿差 不宜过大(否则 ICP 配准失败)
避开位姿 纯平移、共轴旋转、共面运动、机构奇异 第 6.4 节、《机械臂技术基础》第 7 节
场景 几何丰富、非退化;避开走廊/隧道/大块平整墙面

9.3 何时该用标定物

标定物不是数学上的必需品(第 5 节已证明环境标定等价于 $AX=XB$),而是工程捷径:它提供精确、无歧义的 $B_k$(角点 + PnP),绕开了点云配准的所有困难(局部最优、初值敏感、退化)。当对精度要求高、或环境几何贫乏、或需要可重复的标定基准时,标定物仍是首选。eye-to-hand 尤其依赖标定物:固定雷达免标定物需分割末端、工程上不现实,常用球靶(球心位姿稳健、易从点云提取)逐帧给出 $C_i$。

9.4 连续时间与可观测性感知标定(现代方法)

上述两步法把位姿当作离散时刻的快照。高频率、异步的 LiDAR/IMU 用离散时间会引入运动畸变与时刻错配,现代方法用连续时间批优化克服:用累积三次 B 样条参数化 6-DoF 轨迹,其解析导数直接给出任意时刻的加速度/角速度,使雷达点时刻可在 IMU 时间轴上精确插值(含时间偏移),把外参与轨迹统一进单一非线性最小二乘。代表工作 OA-LICalib 在此框架下用 Fisher 信息矩阵的截断 SVD(TSVD)自动识别并锁定不可观方向——只更新可观方向、把不可观方向冻结在先验上(与 §6 的条件数判据同源,但落地为数值更新规则);其结构化环境(墙、地面 surfel)point-to-plane 约束联合标定 LiDAR/IMU 内外参、免任何标定物,精度基准(仿真、全激励)约平移 0.40 cm、旋转 0.18°,真实多段联合标定重复性约平移 ±2 cm、旋转 ±0.2°,时间偏移可达毫秒级。该方法显式印证 §6 的退化结论:单轴旋转使沿该轴的外参平移不可观(被自动锁定)。同类近期工作还有 GRIL-Calib(地面机器人平面运动免标定物)、iKalibr(统一多传感器免标定物时空标定);可观测性感知的思路也常以信息度量(trace/det/最大特征值)驱动段选择。

10. 总结

  1. 手眼标定求解 $AX=XB$,eye-in-hand 与 eye-to-hand 同形:$A$ 是末端相对运动(已知),$B$ 是观测得到的相对运动(eye-in-hand 为传感器自身运动,eye-to-hand 为末端上标定物的运动),$X$ 是外参(eye-in-hand 为末端-雷达,eye-to-hand 为基座-雷达)。
  2. 经典求解器分分离法(Tsai、Park、Shah)与同时法(Andreff、Daniilidis、Horaud);同时法对旋转噪声鲁棒,分离法对平移噪声鲁棒,且运动范围影响相反——算法应与运动匹配,无普适最优。
  3. 纯点云场景的根本差异:$B$ 必须经点云配准间接获得,多了一层非凸优化,引入局部最优、初值敏感、退化等误差源;精度上限受制于配准而非运动学。
  4. 用环境替代标定物严格等价于 $AX=XB$:环境只是测量 $B$ 的工具,$B$ 的物理真值由 $A$ 和 $X$ 决定,与环境无关。旋转环境而机械臂不动,$X$ 不变——这正是正确性的证明。
  5. 两步法(先配准求 $B$,再闭式解 $X$)与自洽融合法理论等价,但两步法把配准与求解解耦,可诊断、误差结构清晰,工程上更优。
  6. 可观测性可用雅可比 $J_k = \mathrm{Ad}_{B_k}-I_6$ 的条件数量化,且只依赖 $B_k$,可在采数据前预判。纯平移不能定平移却能约束旋转(反直觉),且不同算法框架下同一退化运动的可观测性不同。
  7. 标定物是工程捷径(提供干净 $B$),不是数学门槛。
  8. 退化情形——只用一面墙的平面约束:法向定旋转、距离定平移,二者可观测的充要条件都是运动含横向旋转(垂直墙法向的轴)。运动选对,一面墙即可解出完整外参;纯平移或纯绕墙法向转则信息崩溃。平面法信息少于完整配准、条件数更差,但拟合凸、抗干扰、无需初值,结构化室内常更稳健(详见 §7)。

参考资料



文章链接：
https://www.zywvvd.com/notes/3d/handeye-calibration/handeye-calibration/

“觉得不错的话，给点打赏吧 ୧(๑•̀⌄•́๑)૭”

微信二维码

微信支付

支付宝二维码

支付宝支付

Robotics
#Robotics #HandEye #Calibration #LiDAR
激光雷达与机械臂手眼标定理论
https://www.zywvvd.com/notes/3d/handeye-calibration/handeye-calibration/
作者
Yiwei Zhang
发布于
2026年7月31日
许可协议