开发环境：Unbuntu 18.04 LTS + ROS Melodic + ViSP 3.3.1
在上一篇博客中介绍了使用Pionner3dx移动机器人完成视觉伺服任务的例程，收到篇幅限制，对于视觉伺服过程并没有具体介绍，本文针对过程中使用的几个关键函数做一下介绍。
task.setServo(vpServo::EYEINHAND_L_cVe_eJe)
设定视觉伺服控制律的计算方法，例程中选择的是EYEINHAND_L_cVe_eJe方法，EYEINHAND表示相机固定在机器人上，L表示交互矩阵（图像雅可比矩阵），cVe表示相机和机器人坐标系之间的速度扭曲变换矩阵（velocity twist transformation matrix），eJe表示机器人的雅可比矩阵，该控制律计算方法如下式
q
˙
=
−
λ
(
L
e
^
c
V
e
e
J
e
)
+
e
\dot{q}=-\lambda(\hat{L_e}{^cV_e} {^eJ_e})^+\mathbf{e}
q˙=−λ(Le^cVeeJe)+e
式中，
L
e
^
\hat{L_e}
Le^描述了图像中坐标的移动和相机运动速度之间的关系，
c
V
e
{^cV_e}
cVe描述了相机的运动速度和机器人运动速度之间的关系，
e
J
e
{^eJ_e}
eJe描述了机器人运动速度与机器人速度控制律之间的关系。因此根据图像中的特征误差
e
\mathbf{e}
e就可以计算得到机器人的运动控制律
q
˙
\dot{q}
q˙。除了EYEINHAND_L_cVe_eJe方法外，visp_ros中还提供了以下几种计算方法
task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE)
设定交互矩阵的类型，DESIRED表示使用期望位置的特征信息来计算交互矩阵，此外visp_ros还提供了以下集中计算方式
PSEUDO_INVERSE表示使用广义逆矩阵的方法来处理交互矩阵，此外还可以使用直接计算转置的方法来处理交互矩阵
task.setLambda
设定增益参数 λ \lambda λ，取值范围为0.1-1，增益参数越大，则计算出来的速度越大。
task.set_cVe(cVe)
设定速度扭曲变换矩阵
c
V
e
{^cV_e}
cVe，利用该矩阵我们可以在已知一个点在相机坐标系下的速度求出该点在机器人坐标系下的速度，该矩阵的计算方法如下
c
V
e
=
[
c
R
e
[
c
t
e
]
×
c
R
e
0
3
×
3
c
R
e
]
^{c}V_{e}=\begin{bmatrix} ^{c}R_{e}& \left [ ^{c}t_{e}\right ]_{\times} {^{c}R_{e}}\\ 0_{3\times3} & ^{c}R_{e} \end{bmatrix}
cVe=[cRe03×3[cte]×cRecRe]
式中，
c
R
e
^{c}R_{e}
cRe表示相机坐标系和机器人坐标系之间的旋转矩阵，
c
t
e
^{c}t_{e}
cte表示相机坐标系和机器人坐标系之间的平移矩阵，
[
c
t
e
]
×
[^{c}t_{e}]_{\times}
[cte]×表示将平移矩阵转化为斜对称矩阵，转换方法如下
a
=
[
a
1
a
2
a
3
]
T
a=\begin{bmatrix} a_1 & a_2 &a_3 \end{bmatrix}^T
a=[a1a2a3]T
[
a
]
×
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
\left [ a\right ]_{\times}=\begin{bmatrix} 0 & -a_3 & a_2\\ a_3& 0 & -a_1\\ -a_2 & a_1 & 0 \end{bmatrix}
[a]×=⎣⎡0a3−a2−a30a1a2−a10⎦⎤
task.set_eJe(eJe)
设定机器人雅可比矩阵
e
J
e
{^eJ_e}
eJe，关于机器人雅可比矩阵本身是比较复杂的，这里可以简单的理解为我们施加在机器人各个自由度上的运动指令和机器人真实的运动速度之间的关系，即
v
=
e
J
e
q
˙
v={^eJ_e}\dot{q}
v=eJeq˙。因为对于差速移动机器人来说只有两个方向的自由度，x方向上的移动
v
x
v_x
vx和z方向上的旋转
w
z
w_z
wz，且我们计算出速度控制律
q
˙
=
[
v
x
,
w
z
]
T
\dot{q}=[v_x,w_z]^T
q˙=[vx,wz]T，所以该机器人雅可比矩阵为
e
J
e
=
[
1
0
0
0
0
0
0
0
0
0
0
1
]
{^eJ_e}=\begin{bmatrix} 1 &0 \\ 0 &0\\ 0 &0 \\ 0 &0 \\ 0 &0 \\ 0 &1 \end{bmatrix}
eJe=⎣⎢⎢⎢⎢⎢⎢⎡100000000001⎦⎥⎥⎥⎥⎥⎥⎤
s_xd.buildFrom(x, y, Z);
构建二维视觉特征，x，y分别对应经过归一化的图像坐标（比如图像正中心应该是x=0.5，y=0.5），而Z表示此时在空间坐标系下的三维深度信息，单位是m。例如s_xd.buildFrom(0.5, 0.5, 1);这个描述视觉特征就是在相机与目标特征点相距1m的位置，目标特征点处于图像正中心（个人理解）。
s_Z.buildFrom(x，y, Z, log(Z / Zd));
构建三维视觉特征，x，y分别对应经过归一化的图像坐标，而Z表示此时在空间坐标系下的三维深度信息，单位是m，log(Z/Zd)表示当前位置的深度Z和期望位置的深度Zd比值的对数。
task.addFeature(s_x, s_xd);
将当前位置的视觉特征s_x和期望位置的视觉特征s_xd添加到视觉伺服任务中。默认是将s_x和s_xd包含的所有视觉特征都添加到任务中，但通过增加特征选取参数，可以选择特定维度的特征，例如task.addFeature(s_x, s_xd, vpFeaturePoint::selectX());就表示只选择x维度上的特征添加到任务中。
task.computeControlLaw() ;
根据视觉伺服算法计算控制律，默认的计算方法如下
q
˙
=
±
λ
J
^
e
+
e
\dot{q}=\pm\lambda\hat{J}_e^+\mathbf{e}
q˙=±λJ^e+e
式中，
q
˙
\dot{q}
q˙表示作用在机器人上的速度控制律；
±
\pm
±符号取决于视觉伺服的形式是eyeinhand 还是 eyetohand；
λ
\lambda
λ是增益参数；
J
^
e
+
\hat{J}_e^+
J^e+是视觉伺服任务的雅可比矩阵，是关于图像雅可比矩阵和机器人雅可比矩阵的函数；
e
=
(
s
−
s
∗
)
\mathbf{e}=(s-s^*)
e=(s−s∗)表示视觉特征误差。
task.getError()
返回视觉特征误差 e = ( s − s ∗ ) \mathbf{e}=(s-s^*) e=(s−s∗)，返回的是一个向量形式的结果，依次表示x，y，z三个维度上的视觉特征误差，可以使用索引值选择需要的维度。
task.print();
打印输出视觉伺服任务的相关信息，默认打印所有信息，可以通过添加参数选择需要打印的信息
task.kill();
结束视觉伺服任务，删除所有的视觉特征。
如果大家对于深度学习与计算机视觉领域感兴趣，希望获得更多的知识分享与最新的论文解读，欢迎关注我的个人公众号“深视”。
扫一扫
838
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
