视觉伺服 二、IBVS原理

前言

本篇记录IBVS基于图像的视觉伺服原理，后续可能会添加实现代码。

图像雅可比

视觉伺服的目标误差变化速度与末端相机移动速度的关系为：
$$\dot{\mathbf{e}}={\mathbf{L}}_{\mathbf{e}}{\mathbf{v}}_{\mathbf{c}}$$
因此，计算${\mathbf{L}}_{\mathbf{e}}$是视觉伺服中的一个核心问题。IBVS中的${\mathbf{L}}_{\mathbf{e}}$被称为图像雅可比，表示了图像中的2D点坐标与相机速度的关系。下面来推导图像雅可比形式。

首先通过相机内参把像素坐标逆投影到归一化相机坐标平面下：
$$\begin{gathered} x=\frac{X}{Z}=\frac{u-c_u}{f} \\ y=\frac{Y}{Z}=\frac{v-c_v}{f} \end{gathered}$$
归一化相机坐标速度可表示为：
$$\begin{gathered} \dot{x}=\frac{\dot{X}Z-X\dot{Z}}{Z^2}=(\frac{\dot{X}}{Z}-\frac{x\dot{Z}}{Z}) \\ \dot{y}=\frac{\dot{Y}Z-Y\dot{Z}}{Z^2}=(\frac{\dot{Y}}{Z}-\frac{y\dot{Z}}{Z}) \end{gathered}$$
相机末端各轴的移动速度$\dot{x},\dot{y},\dot{z}$与旋转速度$w$、平移速度$t$的关系可表示为：
$$\begin{gathered} \dot{X}=Z{w}_y-Y{w}_z+t_x \\ \dot{Y}=X{w}_z-Z{w}_x+t_y \\ \dot{Z}=Y{w}_x-X{w}_y+t_z \end{gathered}$$
代入归一化坐标速度：
$$\begin{gathered} \dot{x}=w_y-\frac{Y}{Z}{w}_z+\frac{1}{Z}{t}_x-\frac{Y}{Z}x{w}_x+\frac{X}{Z}x{w}_y-\frac{1}{Z}x{t}_z \\ =\frac{1}{Z}{t}_x+0t_y-\frac{x}{Z}{t}_z-xy{w}_x+(1+x^2)w_y-y{w}_z \\ \dot{y}=x{w}_z-w_x+\frac{1}{Z}{t}_y-y^2{w}_x+xy{w}_y-\frac{y}{Z}{t}_z \\ =0t_x+\frac{1}{Z}{t}_y-\frac{y}{Z}{t}_z-(1+y^2)w_x+xy{w}_y+x{w}_z \end{gathered}$$
写成矩阵形式，就得到了归一化平面点速度与相机移动速度的关系：
$$\begin{gathered} \left[\begin{array}{c}\dot{x}\\ \dot{y}\end{array}\right]=\left[\begin{array}{cccccc}\frac{1}{Z}& 0& -\frac{x}{Z}& -xy& (1+x^2)& -y\\ 0& \frac{1}{Z}& -\frac{y}{Z}& -(1+y^2)& xy& x\end{array}\right]\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\\ w_x\\ w_y\\ w_z\end{array}\right] \\ i.e.\dot{\mathbf{e}}={\mathbf{L}}_{\mathbf{e}}{\mathbf{v}}_{\mathbf{c}} \end{gathered}$$

进一步代入像素投影方程：
$$\begin{gathered} \dot{x}=\frac{\dot{u}}{f} \\ \dot{y}=\frac{\dot{v}}{f} \end{gathered}$$
就可以得到像素速度与相机移动速度的关系，这里就不多写了。

图像雅可比矩阵：
$$\left[\begin{array}{cccccc}\frac{1}{Z}& 0& -\frac{x}{Z}& -xy& (1+x^2)& -y\\ 0& \frac{1}{Z}& -\frac{y}{Z}& -(1+y^2)& xy& x\end{array}\right]$$
可以看到这个矩阵包含未知深度$Z$，因此在使用RGB相机做IBVS时，通常需要估计使用的图像特征点的深度，这就给系统带来了不确定性。

并且每个特征点为图像雅可比提供两个方程，而相机速度${\mathbf{v}}_{\mathbf{c}}$有六个未知量，因此至少要三个点才能获得方程的解，并可能出现奇异矩阵的情况。因此实际需要多于三个特征点。

后记

本篇主要就是IBVS中的图像雅可比矩阵推导。下篇会从图像雅可比的估计，以及IBVS的稳定性估计入手。