【机器人手眼标定算法简介】

1. 引言

在工业自动化和机器人领域中，机械臂的精度和灵活性是其核心性能之一。为了实现高精度的操作，机械臂的"手眼系统"（即机械臂末端执行器与视觉传感器之间的几何关系）必须进行精确的标定。手眼标定是确定视觉传感器坐标系与机械臂末端执行器坐标系之间的转换关系的过程。本文将详细介绍手眼标定的基本概念、常用的标定算法以及实现步骤。

2. 手眼标定的基本概念

手眼标定通常分为两类：Eye-to-Hand和Eye-in-Hand。

Eye-in-Hand：视觉传感器安装在机械臂的末端执行器上，随机械臂一起运动，如图(a)所示。
Eye-to-Hand：视觉传感器固定在环境中，观察机械臂末端执行器的运动，如图(b)所示。

无论哪种配置，手眼标定的核心问题都是求解视觉传感器坐标系与机械臂末端执行器坐标系之间的转换矩阵。

3. 坐标系定义

{ W } \{W\} {W}: 世界坐标系 (World frame)
{ B } \{B\} {B}: 机械臂基座坐标系 (Base frame)
{ F } \{F\} {F}: 机械臂法兰坐标系 (Flange frame)
{ C } \{C\} {C}: 相机坐标系 (Cam frame)
{ P } \{P\} {P}: 棋盘格标定板或April Tag坐标系 (Chess frame)
T w b ∈ S E ( 3 ) T_{wb} \in SE(3) Twb∈SE(3): 基座坐标系 { B } \{B\} {B}在世界坐标系 { W } \{W\} {W}下的4x4空间变换矩阵
θ i ∈ R n \theta_i \in R^n θi∈Rn: i i i时刻的机器人关节角， θ i = [ θ 1 i , θ 2 i , . . . , θ n i ] \theta_i = [\theta_{1i}, \theta_{2i}, ..., \theta_{ni} ] θi=[θ1i,θ2i,...,θni], n n n为关节数量
T b f ( θ i ) ∈ S E ( 3 ) T_{bf}(\theta_i) \in SE(3) Tbf(θi)∈SE(3): 关节角为 θ i \theta_i θi时，法兰坐标系 { F } \{F\} {F}在 { B } \{B\} {B}系下的空间变换矩阵，可简写成 T b f i T_{bf}^i Tbfi
T c p i ∈ S E ( 3 ) T_{cp}^i \in SE(3) Tcpi∈SE(3): 关节角为 θ i \theta_i θi时，棋盘格坐标系 { P } \{P\} {P}在相机坐标系 { C } \{C\} {C}的空间变换矩阵，由相机测量得到
T f c ∈ S E ( 3 ) T_{fc} \in SE(3) Tfc∈SE(3): 相机坐标系 { C } \{C\} {C}在法兰坐标系 { F } \{F\} {F}的空间变换矩阵，对Eye-in-Hand类型来说，是需要标定的参数
T f p ∈ S E ( 3 ) T_{fp} \in SE(3) Tfp∈SE(3): 棋盘格坐标系 { P } \{P\} {P}在法兰坐标系 { F } \{F\} {F}的空间变换矩阵
T b c ∈ S E ( 3 ) T_{bc} \in SE(3) Tbc∈SE(3): 相机坐标系 { C } \{C\} {C}在基座坐标系 { B } \{B\} {B}的空间变换矩阵，对Eye-to-Hand类型来说，是需要标定的参数

4. 系统建模

4.1 Eye-in-Hand建模

如图(a)所示，Eye-in-Hand模式下，棋盘格或April Tag标签的位置固定（即 T w p T_{wp} Twp是定值），机械臂带着相机在不同姿态下进行拍照，需要标定的参数是相机相对法兰的固定坐标系 T f c T_{fc} Tfc。考虑 i i i和 j j j两个时刻，可得:
T w p = T w b ⋅ T b f ( θ i ) ⋅ T f c ⋅ T c p i , T w p = T w b ⋅ T b f ( θ j ) ⋅ T f c ⋅ T c p j , \begin{align} T_{wp} &= T_{wb} \cdot T_{bf}(\theta_i) \cdot T_{fc} \cdot T_{cp}^i, \tag{1} \\ T_{wp} &= T_{wb} \cdot T_{bf}(\theta_j) \cdot T_{fc} \cdot T_{cp}^j, \tag{2} \end{align} TwpTwp=Twb⋅Tbf(θi)⋅Tfc⋅Tcpi,=Twb⋅Tbf(θj)⋅Tfc⋅Tcpj,(1)(2)

联立方程后可得：
T b f ( θ i ) ⋅ T f c ⋅ T c p i = T b f ( θ j ) ⋅ T f c ⋅ T c p j , (3) T_{bf}(\theta_i) \cdot T_{fc} \cdot T_{cp}^i = T_{bf}(\theta_j) \cdot T_{fc} \cdot T_{cp}^j, \tag{3} Tbf(θi)⋅Tfc⋅Tcpi=Tbf(θj)⋅Tfc⋅Tcpj,(3)

即：
T b f − 1 ( θ i ) ⋅ T b f ( θ j ) ⋅ T f c = T f c ⋅ T c p i ⋅ T c p j − 1 . (4) T_{bf}^{-1}(\theta_i) \cdot T_{bf}(\theta_j) \cdot T_{fc} = T_{fc} \cdot T_{cp}^i \cdot {T_{cp}^j}^{-1}. \tag{4} Tbf−1(θi)⋅Tbf(θj)⋅Tfc=Tfc⋅Tcpi⋅Tcpj−1.(4)

令 T a i j = T b f − 1 ( θ i ) ⋅ T b f ( θ j ) T_{a_{ij}}=T_{bf}^{-1}(\theta_i) \cdot T_{bf}(\theta_j) Taij=Tbf−1(θi)⋅Tbf(θj)， T c i j = T c p i ⋅ T c p j − 1 T_{c_{ij}}=T_{cp}^i \cdot {T_{cp}^j}^{-1} Tcij=Tcpi⋅Tcpj−1， T b = T f c T_b = T_{fc} Tb=Tfc，则Eq(4)可简化为：
T a i j ⋅ T b = T b ⋅ T c i j . (5) T_{a_{ij}} \cdot T_b = T_b \cdot T_{c_{ij}}. \tag{5} Taij⋅Tb=Tb⋅Tcij.(5)

4.2 Eye-to-Hand建模

如图(b)所示，Eye-to-Hand模式下，标定板固定在机械臂法兰（即 T f p T_{fp} Tfp是定值），相机在固定位置上对不同位置的标定板进行拍照，需要标定的参数是相机相对基座的固定坐标系 T b c T_{bc} Tbc。同样地，考虑 i i i和 j j j两个时刻，可得:
T b f ( θ i ) ⋅ T f p = T b c ⋅ T c p i , T b f ( θ j ) ⋅ T f p = T b c ⋅ T c p j , \begin{align} T_{bf}(\theta_i) \cdot T_{fp} &= T_{bc} \cdot T_{cp}^i, \tag{6} \\ T_{bf}(\theta_j) \cdot T_{fp} &= T_{bc} \cdot T_{cp}^j, \tag{7} \end{align} Tbf(θi)⋅TfpTbf(θj)⋅Tfp=Tbc⋅Tcpi,=Tbc⋅Tcpj,(6)(7)

由Eq(7)求逆再右乘到Eq(6)，可得：
T b f ( θ i ) ⋅ T b f − 1 ( θ j ) ⋅ T b c = T b c ⋅ T c p i ⋅ T c p j − 1 . (8) T_{bf}(\theta_i) \cdot T_{bf}^{-1}(\theta_j) \cdot T_{bc} = T_{bc} \cdot T_{cp}^i \cdot {T_{cp}^j}^{-1}. \tag{8} Tbf(θi)⋅Tbf−1(θj)⋅Tbc=Tbc⋅Tcpi⋅Tcpj−1.(8)

令 T ~ a i j = T b f ( θ i ) ⋅ T b f − 1 ( θ j ) \tilde{T}{a{ij}}=T_{bf}(\theta_i) \cdot T_{bf}^{-1}(\theta_j) T~aij=Tbf(θi)⋅Tbf−1(θj)， T ~ c i j = T c p i ⋅ T c p j − 1 \tilde{T}{c{ij}}=T_{cp}^i \cdot {T_{cp}^j}^{-1} T~cij=Tcpi⋅Tcpj−1， T ~ b = T b c \tilde{T}b = T{bc} T~b=Tbc, 则Eq(8)可简化为：
T ~ a i j ⋅ T ~ b = T ~ b ⋅ T ~ c i j . (9) \tilde{T}{a{ij}} \cdot \tilde{T}b = \tilde{T}b \cdot \tilde{T}{c{ij}}. \tag{9} T~aij⋅T~b=T~b⋅T~cij.(9)

4.3 模型统一

可以看出，Eq(5)和Eq(9)具有相同的形式。因此，Eye-in-Hand模式和Eye-to-Hand模式的手眼标定具有相同形式的求解方程，即机械臂移动 N N N个关节角，构造 N − 1 N-1 N−1组方程，求解最优化问题：
arg min ⁡ T b Σ i = 1 , j = i + 1 N − 1 ∥ L o g ( T a i j ⋅ T b ⋅ ( T b ⋅ T c i j ) − 1 ) ∥ , (10) \argmin_{T_b} \Sigma_{i=1,j=i+1}^{N-1} {\| Log(T_{a_{ij}} \cdot T_b \cdot (T_b \cdot T_{c_{ij}})^{-1}) \|}, \tag{10} TbargminΣi=1,j=i+1N−1∥Log(Taij⋅Tb⋅(Tb⋅Tcij)−1)∥,(10)

其中， L o g ( ⋅ ) Log(\cdot) Log(⋅) 表示李代数。

5. 两步法求解 T a ⋅ T b = T b ⋅ T c T_a \cdot T_b = T_b \cdot T_c Ta⋅Tb=Tb⋅Tc

由于 T a T_a Ta、 T b T_b Tb和 T c T_c Tc都是4x4的空间变换矩阵，可展开为旋转与平移形式，即 T a ⋅ T b = T b ⋅ T c T_a \cdot T_b = T_b \cdot T_c Ta⋅Tb=Tb⋅Tc可展开为:

R a t a 0 1 \] ⋅ \[ R b t b 0 1 \] = \[ R b t b 0 1 \] ⋅ \[ R c t c 0 1 \] , \\left\[ \\begin{array}{cc} R_a \& t_a \\\\ \\bold{0} \& 1 \\end{array} \\right\] \\cdot \\left\[ \\begin{array}{cc} R_b \& t_b \\\\ \\bold{0} \& 1 \\end{array} \\right\] = \\left\[ \\begin{array}{cc} R_b \& t_b \\\\ \\bold{0} \& 1 \\end{array} \\right\] \\cdot \\left\[ \\begin{array}{cc} R_c \& t_c \\\\ \\bold{0} \& 1 \\end{array} \\right\], \[Ra0ta1\]⋅\[Rb0tb1\]=\[Rb0tb1\]⋅\[Rc0tc1\], 即： R a ⋅ R b = R b ⋅ R c , R a ⋅ t b + t a = R b ⋅ t c + t b . \\begin{align} R_a \\cdot R_b \&= R_b \\cdot R_c, \\tag{11} \\\\ R_a \\cdot t_b + t_a \&= R_b \\cdot t_c + t_b. \\tag{12} \\\\ \\end{align} Ra⋅RbRa⋅tb+ta=Rb⋅Rc,=Rb⋅tc+tb.(11)(12) 因此，可先求解 T b T_b Tb的旋转分量 R b R_b Rb，再求解 T b T_b Tb的平移分量 t b t_b tb。 ### 5.1 求解 T b T_b Tb的旋转分量 #### 5.1.1 先验知识: 反对称矩阵 向量 a = \[ a x , a y , a z \] T ∈ R 3 \\bold{a} = \[a_x, a_y, a_z\]\^T \\in \\R\^3 a=\[ax,ay,az\]T∈R3的反对称矩阵(skew-symmetric matrix)定义为 a ∧ \\bold{a}\^\\wedge a∧： a ∧ = \[ 0 − a z a y a z 0 − a x − a y a x 0 \] . \\bold{a}\^\\wedge = \\left\[ \\begin{array}{ccc} 0 \& -a_z \& a_y \\\\ a_z \& 0 \& -a_x \\\\ -a_y \& a_x \& 0 \\end{array} \\right\]. a∧= 0az−ay−az0axay−ax0 . #### 5.1.2 四元数乘法算子 设有四元数 q \\bold{q} q： q = q w + q x i + q y j + q z k = \[ q w q v \] , \\bold{q} = q_w + q_x \\bold{i} + q_y \\bold{j} + q_z \\bold{k} = \\left\[ \\begin{array}{c} q_w \\\\ \\bold{q}_v \\end{array} \\right\], q=qw+qxi+qyj+qzk=\[qwqv\], 其中， q w , q x , q y , q z ∈ R q_w, q_x, q_y, q_z \\in \\R qw,qx,qy,qz∈R, q v = \[ q x , q y , q z \] T ∈ R 3 x 1 \\bold{q}_v = \[q_x, q_y, q_z\]\^T \\in \\R\^{3\\mathbf{x}1} qv=\[qx,qy,qz\]T∈R3x1。旋转矩阵乘法可等效于四元数乘法，即 R a c = R a b ⋅ R b c    ⟺    q a c = q a b ⨂ q b c , = \[ q a b \] L ⋅ q b c = \[ q b c \] R ⋅ q a b , (13) R_{ac} = R_{ab} \\cdot R_{bc} \\iff \\begin{align\*} \\bold{q}_{ac} \&= \\bold{q}_{ab} \\bigotimes \\bold{q}_{bc}, \\\\ \&= \[\\bold{q}_{ab}\]_L \\cdot \\bold{q}_{bc} = \[\\bold{q}_{bc}\]_R \\cdot \\bold{q}_{ab}, \\\\ \\end{align\*} \\tag{13} Rac=Rab⋅Rbc⟺qac=qab⨂qbc,=\[qab\]L⋅qbc=\[qbc\]R⋅qab,(13) 其中， ⨂ \\bigotimes ⨂是四元数乘法运算符， \[ q \] L \[\\bold{q}\]_L \[q\]L和 \[ q \] R \[\\bold{q}\]_R \[q\]R分别是四元数左乘算子和右乘算子： \[ q \] L = \[ q w − q v T q v q w I 3 × 3 + q v ∧ \] \[ q \] R = \[ q w − q v T q v q w I 3 × 3 − q v ∧ \] , (14) \\begin{array}{cc} \[\\bold{q}\]_L \&= \\left\[ \\begin{array}{cc} q_w \& -\\bold{q}_v\^T \\\\ \\bold{q}_v \& q_w \\bold{I}_{3\\times3} + {\\bold{q}_v}\^\\wedge \\end{array} \\right\] \\\\ \[\\bold{q}\]_R \&= \\left\[ \\begin{array}{cc} q_w \& -\\bold{q}_v\^T \\\\ \\bold{q}_v \& q_w \\bold{I}_{3\\times3} - {\\bold{q}_v}\^\\wedge \\end{array} \\right\] \\end{array}, \\tag{14} \[q\]L\[q\]R=\[qwqv−qvTqwI3×3+qv∧\]=\[qwqv−qvTqwI3×3−qv∧\],(14) 其中, I 3 × 3 \\bold{I}_{3\\times3} I3×3是3x3的单位矩阵， q v ∧ \\bold{q}_v\^\\wedge qv∧是 q v \\bold{q}_v qv的反对称矩阵。 #### 5.1.3 求解 R b R_b Rb 利用公式(13)和(14)，可将公式(11)重写为： q a ⨂ q b = q b ⨂ q c ,    ⟺    ( \[ q a \] L − \[ q c \] R ) ⋅ q b = 0 4 × 1 \\begin{align} \& \\ \\ \\ \\ \\ \\ \\ \\ \\ \\ \\ \\bold{q}_a \\bigotimes \\bold{q}_b = \\bold{q}_b \\bigotimes \\bold{q}_c, \\\\ \&\\iff \\left( \[\\bold{q}_a\]_L - \[\\bold{q}_c\]_R \\right) \\cdot \\bold{q}_b = \\bold{0}_{4\\times 1} \\end{align} qa⨂qb=qb⨂qc,⟺(\[qa\]L−\[qc\]R)⋅qb=04×1 未完待续...