核心思想
该文提出一种无需标定的基于图像的视觉伺服方法,相对于其他的视觉伺服方法其主要的优势在于无需提前对相机的内参进行标定,而且在视觉伺服过程中能够对相机内参进行准确的估计。该文将移动机器人向目标位置移动的过程分为三步:第一步在原地旋转,使机器人指向目标方向;第二步,沿直线行驶到目标位置;第三步,在目标位置处旋转调整机器人的朝向。整个过程如下图所示
其中,在第一阶段还会完成对于相机内参的估计过程。首先,我们建立相机当前位置的坐标系
F
c
F^c
Fc和期望位置的坐标系
F
∗
F^*
F∗,然后就可以得到在
F
∗
F^*
F∗坐标系下,当前位置坐标系
F
c
F^c
Fc的平移矩阵
∗
T
c
^*T_c
∗Tc和旋转矩阵
c
∗
R
^*_cR
c∗R,如下式
同理可得在当前位置坐标系下,期望位置坐标系
F
∗
F^*
F∗的平移向量
c
T
∗
^cT_*
cT∗和旋转矩阵
∗
c
R
^c_*R
∗cR,r如下式
他们之间的关系如下图所示
由简单的运动学关系可得
第一步旋转机器人指向目标方向。当前位置坐标系
F
c
F^c
Fc和期望位置坐标系
F
∗
F^*
F∗之间的本征矩阵(essential matrix relating,描述两个相机坐标系之间的关系)
E
∗
c
E^{*c}
E∗c定义如下
而两个坐标系之间的基本矩阵(fundamental matrix,描述两个图像坐标系之间的关系)
F
∗
c
F^{*c}
F∗c如下
其中
K
K
K表示相机的内部参数矩阵,定义如下
整理上述公式可得
假设第
i
i
i个目标特征点在两个坐标系中的坐标分别为
p
i
∗
p^*_i
pi∗和
p
i
c
p^c_i
pic
则二者之间存在以下关系
因为矩阵
F
∗
c
F^{*c}
F∗c中包含六个未知的参数,因此需要6组对应的特征点位置坐标,就可以通过最小二乘法对其求解。将
F
∗
c
F^{*c}
F∗c除以
F
12
∗
c
F^{*c}_{12}
F12∗c(第一行第二列的元素),可得
可以发现相机内参矩阵中的参数
v
0
v_0
v0可得
而且在机器人原地旋转的过程中
∗
T
c
z
^*T_{cz}
∗Tcz为负值且保持不变,而
c
T
∗
z
^cT_{*z}
cT∗z当机器人指向目标方向时取得最大值(可以参考上面的坐标系示意图),也就是说当
F
S
21
∗
c
=
c
T
∗
z
∗
T
c
z
F^{*c}_{S21}=\frac{^cT_{*z}}{^*T_{cz}}
FS21∗c=∗TczcT∗z取最小值时,机器人指向目标方向且
c
T
∗
x
=
0
^cT_{*x}=0
cT∗x=0,令此时
F
S
21
∗
c
=
F
S
21
s
p
e
∗
c
F^{*c}_{S21}=F^{*c}_{S21spe}
FS21∗c=FS21spe∗c,
F
S
23
∗
c
=
F
S
23
s
p
e
∗
c
F^{*c}_{S23}=F^{*c}_{S23spe}
FS23∗c=FS23spe∗c,则内参
u
0
u_0
u0可得
在这一步的控制过程中,我们令机器人按照一个固定的角速度原地旋转,并且观察
F
S
21
∗
c
F^{*c}_{S21}
FS21∗c的数值,当他达到一个最小值,然后又开始逐渐增加时,就领机器人反向旋转,最终使其收敛到
F
S
21
∗
c
F^{*c}_{S21}
FS21∗c取最小值的方向。这一控制过程即采用视觉伺服的过程,偏差
e
1
e_1
e1定义为
其导数为
带入前面的公式可得
对应的控制律为
然后再对内参矩阵中的另一个参数
f
u
f_u
fu进行估计,通过将两个图像坐标系中的坐标系原点平移到
(
0
,
0
)
(0,0)
(0,0)处可得目标特征点的新坐标
则此时两个图像坐标系之间的单应性矩阵(homography matrix)
G
G
G定义为
其中
n
∗
n^*
n∗表示垂直于特征点所在平面的单位法向量,
d
∗
d^*
d∗沿
n
∗
n^*
n∗方向特征点与目标位置坐标系
F
∗
F^*
F∗之间的距离,该距离是一个未知数。把
G
G
G展开可得
且
G
G
G满足以下关系
λ
i
\lambda_i
λi是一个系数,利用三组特征点对应的坐标就可以对
G
G
G进行估计。且当机器人指向目标方向时,即
c
T
∗
x
=
0
^cT_{*x}=0
cT∗x=0时,可得
此时可得内参
f
u
f_u
fu为
而对于内参
f
v
f_v
fv其数值通常与
f
u
f_u
fu非常接近,且在后续的视觉伺服控制过程中并未用到,因此该文没有对其进行计算。
第二阶段控制机器人沿直线运动到目标位置。由于在运动过程中受到外部的干扰,机器人可能不能一直保持一个固定的方向沿直线运动,因此必须利用相应的控制律,使其保持直线运动。本文设计的偏差
e
2
v
e_{2v}
e2v和
e
2
w
e_{2w}
e2w分别为
其导数为
整理后得
则对应得控制律为
第三阶段控制机器人在目标位置处旋调整其方向与目标方向保持一致。当机器人运行到目标位置处时,此时得单应性矩阵
G
G
G为
该文设计的偏差
e
3
e_3
e3为
其导数为
则对应的控制律为
算法评价
该文和之前解读的一篇论文《Adaptive Active Visual Servoing of Nonholonomic Mobile Robots》(同一个组的成果)一样都是采用了三步法来实现视觉伺服的过程,本文的最大特点就是充分利用了在当前位置坐标系和目标位置坐标系下特征点的坐标之间的对应关系,结合本征矩阵和单应性矩阵实现了对于无标定相机的内参估计,并设计了相应的控制律来保证机器人按照期望的状态运行。作者文中对于各个步骤都给出了详细的数学推导和证明,值得学习。
如果大家对于深度学习与计算机视觉领域感兴趣,希望获得更多的知识分享与最新的论文解读,欢迎关注我的个人公众号“深视”。