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

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 未完待续...

相关推荐
karmueo461 小时前
视频序列和射频信号多模态融合算法Fusion-Vital解读
算法·音视频·多模态
写代码的小球4 小时前
求模运算符c
算法
大千AI助手7 小时前
DTW模版匹配:弹性对齐的时间序列相似度度量算法
人工智能·算法·机器学习·数据挖掘·模版匹配·dtw模版匹配
YuTaoShao9 小时前
【LeetCode 热题 100】48. 旋转图像——转置+水平翻转
java·算法·leetcode·职场和发展
生态遥感监测笔记9 小时前
GEE利用已有土地利用数据选取样本点并进行分类
人工智能·算法·机器学习·分类·数据挖掘
Tony沈哲10 小时前
macOS 上为 Compose Desktop 构建跨架构图像处理 dylib:OpenCV + libraw + libheif 实践指南
opencv·算法
刘海东刘海东10 小时前
结构型智能科技的关键可行性——信息型智能向结构型智能的转变(修改提纲)
人工智能·算法·机器学习
lingling00910 小时前
扫描仪:工业自动化的智能之眼——迁移科技赋能智能制造新纪元
数码相机
pumpkin8451411 小时前
Rust 调用 C 函数的 FFI
c语言·算法·rust