手眼标定实战(一)-手眼系统与坐标系

相机+机械臂组成的手眼系统，如同人的眼睛和手，眼睛看到东西后，让手能够准确的移动到物体的位置并抓取。相机所能看到的图像是2D屏幕，而手的移动是在3D空间中，因此手眼系统的功能就是能够将相机所拍摄到的2D画面，转换为手所在的3D空间的坐标。

常见的手眼系统有两种安装形式：

图a)的安装方式，相机安装在机械臂上，称之为眼在手上(eye in hand)；图b)的安装方式，相机固定在机械臂之外的安装架上，称之为眼在手外(eye to hand)。

无论是眼在手上还是眼在手外，本质上都是将相机图像中的像素坐标系，转换到机械臂所在的空间坐标系中，手眼系统标定的意义就是找到从相机画面的像素坐标系转换到机械臂所在的三维空间的机械臂坐标系的变换过程，从而实现通过像素坐标以及到目标的距离可以准确的求解目标在机械臂坐标空间的三维坐标。

其中转换过程可以分为相机部分和机械臂部分，相机部分包含

• 像素坐标系
• 图像坐标系
• 相机坐标系

机械臂部分包含

• 基座坐标系
• 末端坐标系
• 夹具/工具坐标系

由于相机部分的坐标转换具备相同的规律和方法，而机械臂坐标根据使用夹具/工具的不同可能会有不同的结果，因此，我们在做手眼标定时，往往标定的是相机到机械臂末端(法兰盘)的变换关系，机械臂末端的坐标是可以直接读取并且保证精度。我们接下来描述的机器人坐标都认为是机械臂末端法兰盘的坐标，机械臂基座坐标系、工具坐标系转换的这一部分通常由机器人集成商来完成，对于视觉应用我们先不讨论机器人坐标部分，我们先只关注相机部分。

所以，我们先来分析手眼系统中相机相关的坐标系概念与转换关系。

相机模型

在讨论坐标系之前，我们先来了解一下相机模型。手眼系统的眼完成将二维平面(单位像素)映射到三维世界中的坐标点(单位为米)，这个过程可以用一个相机模型来描述：

相机模型通常分为两类，一类是针孔模型，一类是鱼眼模型。

针孔模型：也叫小孔成像模型，是基于相机透镜畸变较小的情况下，物体的反射光经过透镜折射到图像传感器上，物和像之间的位置关系满足相似三角形的关系，即物体表面的坐标点和图像坐标点经过透镜后坐标之间为线性关系。

鱼眼模型：鱼眼镜头通常由多个不同的透镜组成，入射光经过多次折射投影到图像传感器上，鱼眼镜头因此可以获得更大范围的视角。因为多透镜的组合，鱼眼成像模型较为复杂，但是可以近似为单位球面的投影模型。

相机相关的坐标系

工业相机通常畸变程度较小，我们可以用针孔模型来计算，因此，按照针孔模型的小孔成像原理，手眼系统所涉及的相机各个坐标系表示如下图：

图中涉及到以下四个坐标系

uv:像素坐标系，原点为图像左上角，单位pixel

o-xy:图像坐标系，基于相机中感光元器件的现实平面空间，原点为成像平面中点，单位mm

Oc-XcYcZc:相机坐标系，光心为原点，单位m

Or-XrYrZr: 机器人坐标系，描述机械臂移动的位置，单位m

P:机器人坐标系中的一点，表示识别目标在机械臂三维空间中的位置

p:点p在图像中的成像点，在图像坐标系中的坐标为(x,y),在像素坐标系中的坐标为(u,v)

注：以下讨论的所有坐标系满足笛卡尔坐标系右手法则。

像素坐标系转换为图像坐标系

其中，O-uv是像素坐标系，O-xy是图像坐标系，要完成从pixel量纲到mm量纲的转换，就需要知道xy方向的像素尺寸dx和dy，单位mm，表示一个像素点的在传感器上的实际尺寸大小。设图像分辨率为1280x960,传感器尺寸为32mmx24mm，转换关系：
$$d_x=32mm/1280$$
$$d_y=24mm/960$$
$$x=(u-640)d_x=u{d}_x-640d_x=udx-u_0{d}_x$$
$$y=(v-480)d_y=v{d}_y-480d_y=v{d}_y-v_0{d}_y$$
其中u_0和v_0为像素坐标系到图像坐标系的平移分量。

矩阵运算形式：
$$\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{ccccc}{d}_x& & 0& & -u_0{d}_x\\ 0& & d_y& & -v_0{d}_y\\ 0& & 0& & 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$$

图像坐标系转换为相机坐标系

点p是图像坐标系上的一点，P是相机坐标系上的一点，f是相机焦距，O到Oc也就是传感器到光心的距离。由相似三角形可以计算：
$$\frac{Xc}{x}=\frac{Zc}{f}=\frac{Yc}{y}$$

转换关系：

$$Xc=\frac{Zc}{f}\times x$$
$$Yc=\frac{Zc}{f}\times y$$

矩阵运算形式：
$$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=z_c\left[\begin{array}{ccccc}\frac{1}{f}& & 0& & 0\\ 0& & \frac{1}{f}& & 0\\ 0& & 0& & 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=z_c\left[\begin{array}{ccccc}\frac{1}{f}& & 0& & 0\\ 0& & \frac{1}{f}& & 0\\ 0& & 0& & 1\end{array}\right]\left[\begin{array}{ccccc}{d}_x& & 0& & -u_0{d}_x\\ 0& & d_y& & -v_0{d}_y\\ 0& & 0& & 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$$

Zc表示相机距离目标的距离，可以通过3D相机直接获取，因此可以转换为二维平面上的坐标计算。

相机坐标系转换为机器人坐标系

O_r-X_rY_rZ_r是机器人坐标系

O_c-X_cY_cZ_c是相机坐标系

P点在相机坐标系下的坐标为：(x_c,y_c,z_c)，和x轴夹角为a，到原点的距离为R

假定机器人坐标系和相机坐标系Z轴重合，机器人坐标系绕着Z轴旋转一定的角度yaw

P点在机器人坐标系下(x_r,y_r,z_r)的坐标计算：

$$x_r=R\times \cos (a+yaw)=R\times cosa\times \cos (yaw)-R\times sina\times sin(yaw)$$

$$y_r=R\times \sin (a+yaw)=R\times sina\times \cos (yaw)+R\times cosa\times sin(yaw)$$

$$x_c=R\times cosa$$

$$y_c=R\times sina$$
于是有：

$$x_r=x_c\times \cos (yaw)-y_c\times \sin (yaw)$$

$$y_r=x_c\times \sin (yaw)+y_c\times \cos (yaw)$$

$$z_r=z_c$$

转换为矩阵形式：
$$\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\end{array}\right]=\left[\begin{array}{ccccc}\cos (yaw)& & -\sin (yaw)& & 0\\ \sin (yaw)& & \cos (yaw)& & 0\\ 0& & 0& & 1\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ zc\end{array}\right]$$
其中
$$\left[\begin{array}{ccccc}\cos (yaw)& & -\sin (yaw)& & 0\\ \sin (yaw)& & \cos (yaw)& & 0\\ 0& & 0& & 1\end{array}\right]就是所谓的绕Z轴旋转a角度的旋转矩阵R_z^{-1}$$

$$注：之所以是R_z^{-1}表示，是因为诸如opencv的函数以及学术论文讨论,都是以机器人坐标系到像素坐标系的旋转矩阵作为R，这里我们是以像素坐标系到机器人坐标系变换来讨论，因此用逆矩阵R_z^{-1}来表示$$

同理可得，绕着X轴旋转roll的旋转矩阵为：
$$R_x^{-1}=\left[\begin{array}{ccccc}1& & 0& & 0\\ 0& & \cos (roll)& & -\sin (roll)\\ 0& & \sin (roll)& & \cos (roll)\end{array}\right]$$

绕着Y轴旋转pitch的旋转矩阵为：
$$R_y^{-1}=\left[\begin{array}{ccccc}\cos (pitch)& & 0& & \sin (pitch)\\ 0& & 1& & 0\\ -\sin (pitch)& & 0& & \cos (pitch)\end{array}\right]$$
当三个坐标轴都有旋转时，旋转矩阵:
$$R^{-1}=R_z^{-1}{R}_y^{-1}{R}_x^{-1}=\left[\begin{array}{ccccc}\cos (yaw)\cos (pitch)& & \cos (yaw)\sin (pitch)\sin (roll)-\sin (yaw)\cos (roll)& & \cos (yaw)\sin (pitch)cos(roll)+\sin (yaw)\sin (roll)\\ \sin (yaw)\cos (pitch)& & \sin (yaw)\sin (pitch)\sin (roll)+\cos (yaw)\cos (roll)& & \sin (yaw)\sin (pitch)\cos (roll)-\cos (yaw)\sin (roll)\\ -\sin (pitch)& & \cos (pitch)\sin (roll)& & \cos (pitch)\cos (roll)\end{array}\right]$$
如果考虑平移，在X，Y，Z分量上加上一个固定的平移数值
$$\left[\begin{array}{c}\Delta x\\ \Delta y\\ \Delta z\end{array}\right]，构成3\times 1$$
的平移矩阵-T

注：这里用-T表示是为了和Opencv的输出的tvecs区分，opencv输出平移矩阵是从机器人坐标系到像素坐标系，记为T。我们计算从像素坐标系到机器人坐标系时取反运算即可，记为-T

即：
$$\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\end{array}\right]=R^{-1}(\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]-T)$$

最终变换

把以上所有的公式合并起来，得到像素坐标到机器人坐标的变换公式：
$$\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\end{array}\right]=R^{-1}(z_c\times \left[\begin{array}{ccccc}\frac{1}{f}& & 0& & 0\\ 0& & \frac{1}{f}& & 0\\ 0& & 0& & 1\end{array}\right]\left[\begin{array}{ccccc}{d}_x& & 0& & -u_0{d}_x\\ 0& & d_y& & -v_0{d}_y\\ 0& & 0& & 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]-T)$$
根据矩阵结合律运算结合中间两个矩阵：
$$\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\end{array}\right]=R^{-1}(z_c\times \left[\begin{array}{ccccc}\frac{d_x}{f}& & 0& & -\frac{u_0{d}_x}{f}\\ 0& & \frac{d_y}{f}& & -\frac{v_0{d}_y}{f}\\ 0& & 0& & 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]-T)$$
$$通常我们用f_x=\frac{d_x}{f}表示$$
$$\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\end{array}\right]=R^{-1}(z_c\times \left[\begin{array}{ccccc}\frac{1}{f_x}& & 0& & -\frac{u_0}{f_x}\\ 0& & \frac{1}{f_y}& & -\frac{v_0}{f_y}\\ 0& & 0& & 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]-T)$$
右边从左往右第一个是相机的外参矩阵的逆矩阵，第二个是相机的内参矩阵的逆矩阵（为什么是逆矩阵，因为opencv的函数计算的关系，下面会讲）

如果对矩阵不好理解，我们展开为线性方程：
$$x_r=\cos (yaw)\cos (pitch)\times (z_c\times \frac{u-u_0}{f_x}-\Delta x)+(\cos (yaw)\sin (pitch)\sin (roll)-\sin (yaw)\cos (roll))\times (z_c\times \frac{v-v_0}{f_y}-\Delta y)+\cos (yaw)\sin (pitch)cos(roll)+\sin (yaw)\sin (roll)\times (z_c-\Delta z)$$
$$y_r=\sin (yaw)\cos (pitch)\times (z_c\times \frac{u-u_0}{f_x}-\Delta x)+(\sin (yaw)\sin (pitch)\sin (roll)+\cos (yaw)\cos (roll))\times (z_c\times \frac{v-v_0}{f_y}-\Delta y)+\sin (yaw)\sin (pitch)\cos (roll)-\cos (yaw)\sin (roll)\times (z_c-\Delta z)$$
$$z_r=-\sin (pitch)\times (z_c\times \frac{u-u_0}{f_x}-\Delta x)+\cos (pitch)\sin (roll)\times (z_c\times \frac{v-v_0}{f_y}-\Delta y)+\cos (pitch)\cos (roll)\times (z_c-\Delta z)$$
$$从展开式中可以看出，对于某一个式子，z_c深度值，f相机焦距，d_x、d_y像素尺寸，u_0、v_0像素坐标系偏移量都是已知的$$

一共有$$pitch、roll、yaw、\Delta x、\Delta y、\Delta z$$六个未知量

根据n元一次方程的求解要求，必须提供6组以上的非线性解才能确定方程的唯一系数，因此，在求解过程中，至少需要机械臂移动6个非线性点，才能完全确定3D空间内像素坐标系到机器人坐标系的变换关系。如果是2D平面，相当于去掉yaw和z的偏移，有4个未知量的线性方程，则需要至少4个点。

其实到这里，根据以上理论编码求解方程就能够实现求解。由于上述方程比较复杂，求解不是简单的事情，对于工程师来说没有必要把理论实践一遍，opencv已经给我们提供了封装好的接口，我们可以基于封装好的轮子快速得到我们需要的结果。

上面我们分析的是从像素坐标到机器人坐标的转换，但是在opencv提供的方法往往是按照机器人坐标(世界坐标系)到像素坐标转换的公式计算，得到的矩阵如下，

$$Z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccccc}\frac{1}{d_x}& & 0& & u_0\\ 0& & \frac{1}{d_y}& & v_0\\ 0& & 0& & 1\end{array}\right]\left[\begin{array}{ccccccc}f& & 0& & 0& & 0\\ 0& & f& & 0& & 0\\ 0& & 0& & 1& & 0\end{array}\right]\left[\begin{array}{ccc}R& & T\\ 0& & 1\end{array}\right]\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\\ 1\end{array}\right]=\left[\begin{array}{ccccccc}{f}_x& & 0& & u_0& & 0\\ 0& & f_y& & v_0& & 0\\ 0& & 0& & 1& & 0\end{array}\right]\left[\begin{array}{ccc}R& & T\\ 0& & 1\end{array}\right]\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\\ 1\end{array}\right]$$
$$其中\left[\begin{array}{ccccccc}{f}_x& & 0& & u_0& & 0\\ 0& & f_y& & v_0& & 0\\ 0& & 0& & 1& & 0\end{array}\right]为相机内参$$

$$\left[\begin{array}{ccc}R& & T\\ 0& & 1\end{array}\right]为相机外参$$

注:opencv外参求解cvFindExtrinsicCameraParams2源码：

返回的旋转矩阵的结果是按照从世界坐标到像素坐标的映射关系计算的。

因此当我们使用opencv得到内参和外参矩阵时，还需要求解它的逆矩阵，才能通过像素坐标转换为机器人坐标。从上述分析已经知道了内参矩阵的逆矩阵，而至于外参矩阵，由于其本身是一个正交矩阵，所以，转置矩阵也是他的逆矩阵，我们只需要把opencv计算得到的外参矩阵转置即可，感兴趣的小伙伴可以自己去验证一下。
$$R={R^{-1}}^T=\left[\begin{array}{ccccc}\cos (yaw)\cos (pitch)& & \sin (yaw)\cos (pitch)& & -\sin (pitch)\\ \cos (yaw)\sin (pitch)\sin (roll)-\sin (yaw)\cos (roll)& & \sin (yaw)\sin (pitch)\sin (roll)+\cos (yaw)\cos (roll)& & \cos (pitch)\sin (roll)\\ \cos (yaw)\sin (pitch)cos(roll)+\sin (yaw)\sin (roll)& & \sin (yaw)\sin (pitch)\cos (roll)-\cos (yaw)\sin (roll)& & \cos (pitch)\cos (roll)\end{array}\right]$$

注：以上得到的结果是按照x-roll,y-pitch,z-yaw来计算的，如果坐标系位置和定义关系不同，只需要把角度替换即可。如，上述我们定义绕y轴转是pitch，绕z转是yaw，如果你自己的坐标系定义是绕y转是yaw，绕x轴转是pitch，绕z轴旋转是roll，那么矩阵的[0][0]元素相应的变为cos(yaw)cos(roll)，也就是矩阵的第一个元素是绕y轴旋转和绕z轴旋转角度的三角函数乘积，这一点是不变的。

相机畸变

前面分析的坐标变换都是没有考虑畸变的，各个坐标系之间满足线性变换，但实际上，相机镜头由于镜面曲率的关系导致放大率并一定是均匀的，所以相机在成像过程中会存在一定的畸变，相机的畸变通常分为两类：

• 径向畸变，通常因为镜头表面曲率不同产生的畸变，这种畸变是对称的。径向畸变根据放大率的不同又分为两类：
  • 枕形畸变，视野中边缘区域的放大率远大于中心区域的放大率，常见于远摄摄像头(下图左)
  • 桶形畸变，视野中中心区域的放大率远大于边缘区域的放大率，常见于广角摄像头和鱼眼摄像头(下图右)
• 切向畸变，通常是因为相机装配过程，透镜平面和CCD/CMOS传感器平面没有平行，这种畸变不对称，在图像区域表现为单侧畸变，类似透视畸变。

在工业相机中，比较常见的畸变类型为桶形畸变和切向畸变。

桶形畸变矫正

根据前面讨论，我们把手眼系统的相机坐标系看做小孔成像模型，认为他们坐标系之间满足线性变换的关系。但是由于畸变的存在，从相机坐标系到图像坐标系之间不满足线性关系，因此为了能够应用小孔成像模型对坐标系之间进行线性变换的计算，我们需要对相机成像后的图像坐标进行畸变矫正。

矫正的计算公式可以简化为(推导过程不赘述，感兴趣的可以自行查阅资料)：

$$x_{distort}=x_{correct}\frac{1+k_1{r}^2+k_2{r}^4+k_3{r}^6}{1+k_4{r}^2+k_5{r}^4+k_6{r}^6}$$

$$y_{distort}=y_{correct}\frac{1+k_1{r}^2+k_2{r}^4+k_3{r}^6}{1+k_4{r}^2+k_5{r}^4+k_6{r}^6}$$

$$其中(x_{distort},y_{distort})表示在原图像上的图像坐标，不是像素坐标，是带畸变的$$。

$$(x_{correct},y_{correct})表示畸变矫正后的图像坐标。r表示该点坐标到原点的距离$$

$$k_1,k_2,k_3,k_4,k_5,k_6为径向畸变系数$$

切向畸变矫正

切向畸变计算公式

$$x_{distort}=x_{correct}+[2p_1{x}_{correct}{y}_{correct}+p_2(r^2+2x_{correct}^2)]$$

$$y_{distort}=y_{correct}+[2p_2{x}_{correct}{y}_{correct}+p_1(r^2+2y_{correct}^2)]$$

总的畸变矫正公式为：
$$x_{distort}=x_{correct}\frac{1+k_1{r}^2+k_2{r}^4+k_3{r}^6}{1+k_4{r}^2+k_5{r}^4+k_6{r}^6}+[2p_1{x}_{correct}{y}_{correct}+p_2(r^2+2x_{correct}^2)]$$

$$y_{distort}=y_{correct}\frac{1+k_1{r}^2+k_2{r}^4+k_3{r}^6}{1+k_4{r}^2+k_5{r}^4+k_6{r}^6}+[2p_2{x}_{correct}{y}_{correct}+p_1(r^2+2y_{correct}^2)]$$

把畸变考虑进来，和前面线性变换的公式组合起来，一个相机系统从像素坐标到机器人的坐标变换过程

• 畸变像素坐标->畸变图像坐标

  (u,v)畸变像素坐标，内参矩阵

• 畸变图像坐标->无畸变图像坐标

  畸变系数

• 无畸变图像坐标->相机坐标

  Zc相机到目标的高度

• 相机坐标->机器人坐标

  旋转平移矩阵RT

把畸变计算公式和坐标系变换公式合并：
$$x_r=\cos (yaw)\cos (pitch)\times (z_c\times x_{correct}-\Delta x)+(\cos (yaw)\sin (pitch)\sin (roll)-\sin (yaw)\cos (roll))\times (z_c\times y_{correct}-\Delta y)+\cos (yaw)\sin (pitch)cos(roll)+\sin (yaw)\sin (roll)\times (z_c-\Delta z)$$
$$y_r=\sin (yaw)\cos (pitch)\times (z_c\times x_{correct}-\Delta x)+(\sin (yaw)\sin (pitch)\sin (roll)+\cos (yaw)\cos (roll))\times (z_c\times y_{correct}-\Delta y)+\sin (yaw)\sin (pitch)\cos (roll)-\cos (yaw)\sin (roll)\times (z_c-\Delta z)$$

$$z_r=-\sin (pitch)\times (z_c\times x_{correct}-\Delta x)+\cos (pitch)\sin (roll)\times (z_c\times y_{correct}-\Delta y)+\cos (pitch)\cos (roll)\times (z_c-\Delta z)$$

$$\left[\begin{array}{c}{x}_{distort}\\ y_{distort}\\ 1\end{array}\right]=\left[\begin{array}{ccccc}\frac{1}{f_x}& & 0& & -\frac{u_0}{f_x}\\ 0& & \frac{1}{f_y}& & -\frac{v_0}{f_y}\\ 0& & 0& & 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$$

---

Final

到这里我们详细讲解了手眼系统的相机部分坐标变换过程，各个坐标系的理论计算，以及畸变矫正的理论计算，并且结合opencv初步展示了理论计算与实际工程如何去结合使用，通过计算我们了解到，在坐标系变换过程中产生了两个矩阵参数，分别是内参矩阵和外参矩阵，内参矩阵只与相机的内部构造和参数有关，外参矩阵只与相机、机械臂的安装位姿有关。

在手眼系统中计算求解相机内参、外参矩阵的过程我们称之为相机标定,当我们得到这两个矩阵参数时，就可以按照线性方程的格式通过像素坐标求解机械臂坐标。虽然说网上存在很多的资料推导讲解，当然，这都是理论部分，准备哪些已知量，如何准备，得到之后如何按照理论公式计算，这是工程应用中需要真正解决的问题。很多资料并没有系统的讲解这些问题，这让很多人在明白理论后仍然无法下手去实践应用手眼标定。所以后续会根据眼在手上、眼在手外这两种安装方式，讨论手眼标定的实践方式以及工程实现。