视觉伺服 二、IBVS原理

前言

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

图像雅可比

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

首先通过相机内参把像素坐标逆投影到归一化相机坐标平面下： x = X Z = u − c u f y = Y Z = v − c v f x = \frac{X}{Z} = \frac {u - c_u}{f} \\ y = \frac{Y}{Z} = \frac {v - c_v}{f} \\ x=ZX​=fu−cu​​y=ZY​=fv−cv​​ 归一化相机坐标速度可表示为： x ˙ = X ˙ Z − X Z ˙ Z 2 = ( X ˙ Z − x Z ˙ Z ) y ˙ = Y ˙ Z − Y Z ˙ Z 2 = ( Y ˙ Z − y Z ˙ Z ) \dot x = \frac {\dot XZ - X\dot Z}{Z^2} = (\frac {\dot X}{Z} - \frac {x\dot Z}{Z} ) \\ \dot y = \frac {\dot YZ - Y\dot Z}{Z^2} = (\frac {\dot Y}{Z} - \frac {y\dot Z}{Z} ) \\ x˙=Z2X˙Z−XZ˙​=(ZX˙​−ZxZ˙​)y˙​=Z2Y˙Z−YZ˙​=(ZY˙​−ZyZ˙​) 相机末端各轴的移动速度 x ˙ , y ˙ , z ˙ \dot x,\dot y, \dot z x˙,y˙​,z˙与旋转速度 w w w、平移速度 t t t的关系可表示为： X ˙ = Z w y − Y w z + t x Y ˙ = X w z − Z w x + t y Z ˙ = Y w x − X w y + t z \dot X = Zw_y - Yw_z + t_x \\ \dot Y = Xw_z - Zw_x + t_y \\ \dot Z = Yw_x - Xw_y + t_z \\ X˙=Zwy​−Ywz​+tx​Y˙=Xwz​−Zwx​+ty​Z˙=Ywx​−Xwy​+tz​ 代入归一化坐标速度： x ˙ = w y − Y Z w z + 1 Z t x − Y Z x w x + X Z x w y − 1 Z x t z = 1 Z t x + 0 t y − x Z t z − x y w x + ( 1 + x 2 ) w y − y w z y ˙ = x w z − w x + 1 Z t y − y 2 w x + x y w y − y Z t z = 0 t x + 1 Z t y − y Z t z − ( 1 + y 2 ) w x + x y w y + x w z \dot x = w_y- \frac {Y}{Z}w_z + \frac{1}{Z}t_x - \frac{Y}{Z}xw_x + \frac{X}{Z}xw_y - \frac{1}{Z}xt_z \\ \quad \\ = \frac{1}{Z} t_x + 0t_y - \frac{x}{Z}t_z -xyw_x + (1 + x^2) w_y - yw_z \\ \quad \\ \dot y = xw_z - w_x + \frac{1}{Z}t_y - y^2w_x + xyw_y - \frac{y}{Z}t_z \\ \quad \\ = 0t_x + \frac{1}{Z}t_y -\frac{y}{Z}t_z - (1+y^2)w_x +xyw_y +xw_z x˙=wy​−ZY​wz​+Z1​tx​−ZY​xwx​+ZX​xwy​−Z1​xtz​=Z1​tx​+0ty​−Zx​tz​−xywx​+(1+x2)wy​−ywz​y˙​=xwz​−wx​+Z1​ty​−y2wx​+xywy​−Zy​tz​=0tx​+Z1​ty​−Zy​tz​−(1+y2)wx​+xywy​+xwz​ 写成矩阵形式，就得到了归一化平面点速度与相机移动速度的关系： [ x ˙ y ˙ ] = [ 1 Z 0 − x Z − x y ( 1 + x 2 ) − y 0 1 Z − y Z − ( 1 + y 2 ) x y x ] [ t x t y t z w x w y w z ] i . e . e ˙ = L e v c \begin{bmatrix} \dot x \\ \dot y \\ \end{bmatrix} = \begin{bmatrix} \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{bmatrix} \begin{bmatrix} t_x \\ t_y \\ t_z \\ w_x \\ w_y \\ w_z \\ \end{bmatrix} \\ i.e. \quad \dot{\bf e}={\bf L_ev_c} [x˙y˙​​]=[Z1​0​0Z1​​−Zx​−Zy​​−xy−(1+y2)​(1+x2)xy​−yx​]⎣⎢⎢⎢⎢⎢⎢⎡​tx​ty​tz​wx​wy​wz​​⎦⎥⎥⎥⎥⎥⎥⎤​i.e.e˙=Le​vc​

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

图像雅可比矩阵： [ 1 Z 0 − x Z − x y ( 1 + x 2 ) − y 0 1 Z − y Z − ( 1 + y 2 ) x y x ] \begin{bmatrix} \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{bmatrix} [Z1​0​0Z1​​−Zx​−Zy​​−xy−(1+y2)​(1+x2)xy​−yx​] 可以看到这个矩阵包含未知深度 Z Z Z，因此在使用RGB相机做IBVS时，通常需要估计使用的图像特征点的深度，这就给系统带来了不确定性。

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

后记

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