一些标定入门方法杂谈（二）：手眼标定及tsai法

qq_776882262

于 2025-05-06 16:08:10 发布

阅读量834

点赞数 12

CC 4.0 BY-SA版权

文章标签：图像处理计算机视觉

本文链接：https://blog.csdn.net/qq_41155229/article/details/147739374

引言

手眼标定主要用于确定摄像头与机器人末端执行器（通常是机械臂末端）之间的空间变换关系。标定可以分为眼在手上和眼在手外两种。

眼在手上：

详细推导和代码实现可以参考：3D视觉工坊-手眼标定（附opencv实现代码）-CSDN博客

这里对一些关键部分解释:

眼在手上主要为了标定机械臂末端（end）坐标系到相机坐标系（cam）之间的变换关系: ${ }_{\text {end }}^{\text {cam}} M$ 。

首先假设坐标系定义如下：

base：机械臂基座坐标系
cam：相机坐标系
panel：棋盘格坐标系
end：机械臂末端执行器坐标系

对于每一张图像和机械臂姿态我们可以得出以下等式：
${ }_{\text {end}}^{{\text {cam}}} M={ }_{\text {panel}}^{\mathrm{cam}} M \cdot{ }_{\text {base}}^{\text {panel}} M \cdot{ }_{\text {end}}^{\text {base}} M$

由式可知 ${ }_{\text {base}}^{\text {panel}} M$ 未知，但可知每组图像中标定板相对于基座的姿态没有变换，因此可得变换关系：

${ }_{\text {base}}^{{\text {panel}}} M={ }_{\text {panel}}^{\mathrm{cam}} M^{-1} \cdot{ }_{\text {end}}^{\text {cam}} M \cdot{ }_{\text {end}}^{\text {base}} M^{-1}$

进一步，对于多幅图像可得等式：

${ }_{\text {panel}}^{\mathrm{cam}} M_{1}^{-1} \cdot{ }_{\text {end}}^{\text {cam}} M \cdot{ }_{\text {end}}^{\text {base}} M_{1}^{-1}={ }_{\text {panel}}^{\mathrm{cam}} M_{2}^{-1} \cdot{ }_{\text {end}}^{\text {cam}} M \cdot{ }_{\text {end}}^{\text {base}} M_{2}^{-1}$

${ }_{\text {panel}}^{\mathrm{cam}} M_{2} \cdot{ }_{\text {panel}}^{\mathrm{cam}} M_{1}^{-1} \cdot{ }_{\text {end}}^{\text {cam}} M ={ }_{\text {end}}^{\text {cam}} M \cdot{ }_{\text {end}}^{\text {base}} M_{2}^{-1}\cdot{ }_{\text {end}}^{\text {base}} M_{1}$

${ }_{\text {panel}}^{\mathrm{cam}} M_{n} \cdot{ }_{\text {panel}}^{\mathrm{cam}} M_{n-1}^{-1} \cdot{ }_{\text {end}}^{\text {cam}} M ={ }_{\text {end}}^{\text {cam}} M \cdot{ }_{\text {end}}^{\text {base}} M_{n}^{-1}\cdot{ }_{\text {end}}^{\text {base}} M_{n-1}$

令 $A={ }_{\text {panel}}^{\mathrm{cam}} M_{n} \cdot{ }_{\text {panel}}^{\mathrm{cam}} M_{n-1}^{-1}$ ， $X={ }_{\text {end}}^{\mathrm{cam}} M$ ， $B={ }_{\text {end}}^{\text {base}} M_{n}^{-1}\cdot{ }_{\text {end}}^{\text {base}} M_{n-1}$

则上式变为 $AX=XB$

眼在手外：

同样地可以建立等式：
${ }_{\text {base}}^{{\text {cam}}} M={ }_{\text {panel}}^{\mathrm{cam}} M \cdot{ }_{\text {end}}^{\text {panel}} M \cdot{ }_{\text {base}}^{\text {end}} M$

这里 ${ }_{\text {end}}^{\text {panel}} M$ 未知，同样的此处棋盘格与末端位姿保持不变，因此可得：
${ }_{\text {end}}^{{\text {panel}}} M={ }_{\text {panel}}^{\mathrm{cam}} M^{-1} \cdot{ }_{\text {base}}^{\text {cam}} M \cdot{ }_{\text {base}}^{\text {end}} M^{-1}$

因此进一步可知：

${ }_{\text {panel}}^{\mathrm{cam}} M_{1}^{-1} \cdot{ }_{\text {base}}^{\text {cam}} M \cdot{ }_{\text {base}}^{\text {end}} M_{1}^{-1}={ }_{\text {panel}}^{\mathrm{cam}} M_{2}^{-1} \cdot{ }_{\text {base}}^{\text {cam}} M \cdot{ }_{\text {base}}^{\text {end}} M_{2}^{-1}$

${ }_{\text {panel}}^{\mathrm{cam}} M_{2} \cdot{ }_{\text {panel}}^{\mathrm{cam}} M_{1}^{-1} \cdot{ }_{\text {base}}^{\text {cam}} M ={ }_{\text {base}}^{\text {cam}} M \cdot{ }_{\text {base}}^{\text {end}} M_{2}^{-1}\cdot{ }_{\text {base}}^{\text {end}} M_{1}$

${ }_{\text {panel}}^{\mathrm{cam}} M_{n} \cdot{ }_{\text {panel}}^{\mathrm{cam}} M_{n-1}^{-1} \cdot{ }_{\text {base}}^{\text {cam}} M ={ }_{\text {base}}^{\text {cam}} M \cdot{ }_{\text {base}}^{\text {end}} M_{n}^{-1}\cdot{ }_{\text {base}}^{\text {end}} M_{n-1}$

令 $A={ }_{\text {panel}}^{\mathrm{cam}} M_{n} \cdot{ }_{\text {panel}}^{\mathrm{cam}} M_{n-1}^{-1}$ ， $X={ }_{\text {base}}^{\text {cam}} M$ ， $B={ }_{\text {base}}^{\text {end}} M_{n}^{-1}\cdot{ }_{\text {base}}^{\text {end}} M_{n-1}$

则上式变为 $AX=XB$

Tsai求解：

由上可知求解手眼标定问题就是求解 $AX=XB$ 问题。

这里介绍Tsai法求解眼在手上的 $AX=XB$ 问题:

令 $H_{gij}$ 为机械臂末端执行器坐标系之间的变换关系（即从第i组末端执行器到第j组末端执行器的变换关系），同理令 $H_{cij}$ 为相机坐标系之间的变换关系。令 $H_{cg}$ 为相机坐标系到末端执行器坐标系之间的关系。则 $AX=XB$ 可变为 $H_{gij}H_{cg}=H_{cg}H_{cij}$ ，展开可得：

$\begin{pmatrix} R_{gij} & T_{gij} \\ 0 & 1 \end{pmatrix} \begin{pmatrix} R_{cg} & T_{cg} \\ 0 & 1 \end{pmatrix} =\begin{pmatrix} R_{cg} & T_{cg} \\ 0 & 1 \end{pmatrix} \begin{pmatrix} R_{cij} & T_{cij} \\ 0 & 1 \end{pmatrix}$

写成方程式为：

$\begin{cases} R_{gij} R_{cg} = R_{cg} R_{cij} \\ (R_{gij} - I) T_{cg} = R_{cg} T_{cij} - T_{gij} \end{cases}$

求解过程：

对旋转矩阵进行罗德里格斯变换：
$r_{gij}=rod(R_{gij})$

$r_{cij}=rod(R_{cij})$

向量归一化：

$\begin{cases} \theta _{gij}=\left \| r_{gij} \right \|_{2} \ ,N_{rgij}=\frac{r_{gij}}{\theta _{gij}}\\ \theta _{cij}=\left \| r_{cij} \right \|_{2} \, \ ,N_{rgij}=\frac{r_{cij}}{\theta _{cij}}\end{cases}$

修正的罗德里格斯可以表示为：
$P_{g i j}=2 \sin \left(\frac{\theta_{g i j}}{2}\right) N_{r_{g i j}}, \quad P_{c i j}=2 \sin \left(\frac{\theta_{c i j}}{2}\right) N_{r_{c i j}}$

计算初始旋转变量 $P_{c g}^{\prime}$ ：

$\operatorname{skew}\left(P_{g i j}+P_{c i j}\right) \cdot P_{c g}^{\prime}=P_{c i j}-P_{g i j}$

其中 $P_{g i j}$ ， $P_{c i j}$ ：是从多个变换对中提取出的旋转向量（Rodrigues向量或等效轴角）。
skew(⋅)：反对称矩阵（用于叉乘的矩阵形式）

skew反对称矩阵表达形式如下：
$\operatorname{skew}(V)=\left[\begin{array}{ccc}0 & -v_{z} & v_{y} \\ v_{z} & 0 & -v_{x} \\ -v_{y} & v_{x} & 0\end{array}\right]$

建立叉乘和内积关系如下所示： $\operatorname{skew}(a) \cdot b=a \times b$

计算旋转变量 $P_{c g}$ ：

$P_{c g}=\frac{2 P_{c g}^{\prime}}{\sqrt{1+\left|P_{c g}^{\prime}\right|^{2}}}$

计算平移向量 $T_{c g}$ ：

$\left(R_{g i j}-I\right) \cdot T_{c g}=R_{c g} \cdot T_{c i j}-T_{g i j}$

由上可得X的所有成员变量。同样的可以推导眼在手外的求解过程。