相机内参标定
将世界坐标系固定在棋盘上,则棋盘格上的任一点的物理坐标W=0,标定板上的每一个格子大小固定,可以得到每个角点在世界坐标系下的物理坐标 (U, V, W = 0) 。通过这些坐标利用最小二乘法计算出内参矩阵和外参矩阵的积,再依次计算出内参矩阵和外参矩阵。
已知矩阵
H
=
A
(
R
1
R
2
T
)
H=A\left(\begin{array}{lll}R 1 & R 2 & T\end{array}\right)
H=A(R1R2T), 利用
R
1
,
R
2
R 1, R 2
R1,R2 的单位正交关系, 可得
R
1
T
R
2
=
0
R
1
T
R
1
=
R
2
T
R
2
=
1
\begin{gathered} R_{1}^{T} R_{2}=0 \\ R_{1}^{T} R_{1}=R_{2}^{T} R_{2}=1 \end{gathered}
R1TR2=0R1TR1=R2TR2=1
可得
H
1
T
A
−
T
A
−
1
H
2
=
0
H
1
T
A
−
T
A
−
1
H
1
=
H
2
T
A
−
T
A
−
1
H
2
=
1
\begin{gathered} H 1^{T} A^{-T} A^{-1} H 2=0 \\ H 1^{T} A^{-T} A^{-1} H 1=H 2^{T} A^{-T} A^{-1} H 2=1 \end{gathered}
H1TA−TA−1H2=0H1TA−TA−1H1=H2TA−TA−1H2=1
用矩阵
A
A
A 表示矩阵
B
B
B, 可得
只要求解出向量 b ,即可得到矩阵 B 。每张标定板图片可以提供一个vb=0 的约束关系,该约束关系含有两个约束方程。但是,向量 b 有6个未知元素。因此,单张图片提供的两个约束方程是不足以解出来向量 b。因此,我们只要取3张标定板照片,得到3个b的约束关系,即6个方程,即可求解向量 b。当标定板图片的个数大于3时(事实上一般需要15到20张标定板图片),可采用最小二乘拟合最佳的向量 b ,并得到矩阵B。
手眼标定
机器人基座与目标之间位置相对固定的关系建立以下等式:
e
n
d
r
1
T
⋅
r
1
c
a
m
1
T
⋅
c
a
m
1
o
b
j
T
=
e
n
d
r
2
T
⋅
r
2
c
a
m
2
T
⋅
c
a
m
2
o
b
j
T
{ }_{e n d}^{r 1} T \cdot{ }_{r 1}^{c a m 1} T \cdot{ }_{c a m 1}^{o b j} T={ }_{e n d}^{r 2} T \cdot{r 2}^{c a m 2} T \cdot{c a m 2}^{o b j} T
endr1T⋅r1cam1T⋅cam1objT=endr2T⋅r2cam2T⋅cam2objT
对上式进行变换:
e
n
d
r
2
T
−
1
⋅
e
n
d
r
1
T
⋅
r
1
c
a
m
1
T
=
r
2
c
a
m
2
T
⋅
c
a
m
2
o
b
j
T
⋅
⋅
c
a
m
1
o
b
j
T
{ }_{e n d}^{r 2} T^{-1} \cdot{ }_{e n d}^{r 1} T \cdot{ }_{r 1}^{c a m 1} T={ }_{r 2}^{c a m 2} T \cdot{ }_{c a m 2}^{o b j} T \cdot{\cdot c a m 1}^{o b j} T
endr2T−1⋅endr1T⋅r1cam1T=r2cam2T⋅cam2objT⋅⋅cam1objT
由于相机与末端之间的坐标关系不变, 可令
r
1
c
a
m
1
T
=
r
2
c
a
m
2
T
=
X
{ }_{r 1}^{c a m 1} T={ }_{r 2}^{c a m 2} T=X
r1cam1T=r2cam2T=X
上述式子可写成:
(
r
2
e
n
d
T
−
1
⋅
e
n
d
r
1
T
)
⋅
X
=
X
⋅
(
o
b
j
c
a
m
2
T
⋅
c
a
m
1
o
b
j
T
)
2
\left(\begin{array}{l} r 2 \\ e n d \end{array} T^{-1} \cdot_{e n d}^{r 1} T\right) \cdot X=X \cdot\left(\underset{c a m 2}{o b j} T \cdot{c a m 1}^{o b j} T\right)_{2}^{}
(r2endT−1⋅endr1T)⋅X=X⋅(cam2objT⋅cam1objT)2
使用OpenCV中的calibrateHandEye函数来实现手眼标定过程,只需要用机械臂获取不同关节角度下识别物体的位姿,得到六组工具坐标系相对于固定坐标系的笛卡尔坐标和对应的六组物体相对于相机的笛卡尔坐标作为输入参数文件,编写程序读取坐标组进行处理转化成旋转矩阵和平移向量,传入calibrateHandEye函数中得到手眼标定矩阵。
视觉引导
由于方块具有较为鲜明的颜色特征,所以基于颜色空间进行分割最容易提取出其特征并进行识别提取。但是颜色特征在不同光照强度下,像素值会发生一定波动,所以如下图所示,使用Matlab分析对物块在不同光照强大下进行颜色空间特征分析。
使用Matlab的Color Thresholder模块可以很方便的对图像进行不同颜色空间上的特征分析,如下图,可以看到图像的像素值在不同色彩空间下空间特征分布。观察空间特征分布,选取易于进行空间平面分割的色彩空间,这里选择RGB色彩空间进行分割,如下图。
通过调整滑块进行色彩空间分割,直到将感兴趣色彩完全从图像中提取出来后,创建掩膜并导出函数,即可得到色彩空间上分割范围。对不同光照强度下相机捕获到的物块图像进行上述分析,取色彩空间范围的并集,即可满足在不同环境下对物块进行识别。
对提取出的掩膜进行图像二值化处理后,提取轮廓并对轮廓进行特征筛选即可获得物块的最小旋转外接矩形,实现对物块的识别。
基于上述结果, 当物块进入到机械臂上相机的视场时, 我们可以计算得到物体相 对于相机的位姿
c
a
m
o
b
j
T
{ }_{c a m}^{o b j} T
camobjT 和相机坐标系相对于工具坐标系的变换矩阵
gripper
T
{ }_{\text {gripper }} T
gripper T, 已知机械 臂上各关节的角度, 可以通过正运动学得到工具坐标系相对于固定坐标系的变换矩阵 gripper
T
T
T, 则有 base
ba
o
b
j
P
base
=
gripper
base
T
⋅
gripper
cam
T
⋅
cam
o
b
j
P
\underset{\text { base }}{\stackrel{o b j}{\text { ba }} P}=\underset{\text { base }}{\operatorname{gripper}} T \cdot{ }_{\text {gripper }}^{\text {cam }} T \cdot{ }_{\text {cam }}^{o b j} P
base ba objP= base gripperT⋅gripper cam T⋅cam objP
根据机械臂的结构和尺寸, 选择适当的夹取位姿, 得到夹取时末端的 RPY 角, 结 合
base
o
b
j
P
{ }_{\text {base }}^{o b j} \mathrm{P}
base objP, 将
base
o
b
j
P
_{\text {base }}^{o b j} \mathrm{P}
base objP 的
z
\mathrm{z}
z 进行一定的调整, 设置为常数
15
(
c
m
)
15(\mathrm{~cm})
15( cm), 可以得到夹取时机械 臂末端的相对于基座的目标笛卡尔坐标, 作为逆运动学的输入, 即可计算出夹爪运动 到夹取期望位姿时对应的各关节角度。