视觉伺服控制完整解析

视觉伺服控制简介

视觉伺服控制（Visual servo control）简单来说，就是利用计算机视觉得到的数据来控制机器人的移动。一般来说，根据相机的位置不同又分为两种，一种是直接将相机放置在机器人或者机械臂上，另一种是将相机固定在工作空间的某个位置，前者也被称作eye-in-hand。

相关符号及概念的说明

坐标变换

我们都知道三维空间下刚体的运动可以分解为两个部分，平移和旋转。平移没什么好说的，直接用一个向量$\mathbf{t}$就可以表示。对于旋转，一般情况下，我们都采用一个$3\times 3$的旋转矩阵来表示。在数学中，旋转矩阵的集合有一个单独的定义，称为特殊正交群，记为
S O ( 3 ) = { R ∈ ℜ 3 × 3 ∣ R R T = I , d e t ( R ) = 1 } SO(3) = \{ \mathbf{R} \in \Re^{3\times 3}\mid \mathbf{R} \mathbf{R}^T = \mathbf{I}, det( \mathbf{R}) = 1\} SO(3)={ R∈ℜ3×3∣RRT=I,det(R)=1}这里的$R$表示旋转矩阵。
当然了，我们都知道，一次旋转只有三个自由度，而旋转矩阵有9个变量，那么很明显存在很多的冗余项。理想情况下只需要三个量就能表示一次旋转，比如欧拉角，但是欧拉角表示法存在万向锁问题，为了解决这个问题，人们又发明了一种新的表示方法，四元数。四元数简单的理解就是用一些规则记录旋转轴和旋转角，这并不是这篇博客的重点，大家感兴趣可以自行搜索。
既然知道了平移和旋转，那么坐标变换就可以写为
$${\mathbf{a}}^{\prime }=\mathbf{R}\mathbf{a}+\mathbf{t}$$为了写成齐次坐标的形式，引入齐次坐标系，将上式写成如下形式
$$\left[\begin{array}{c}{\mathbf{a}}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}\mathbf{a}\\ 1\end{array}\right]=\mathbf{T}\left[\begin{array}{c}\mathbf{a}\\ 1\end{array}\right]$$类似于旋转矩阵 $\mathbf{R}$，变换矩阵$\mathbf{T}$也可以组成一个集合，称做特殊欧式群，记为
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ ℜ 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ ℜ 3 } SE(3)=\{\mathbf{T}=\begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} \in \Re^{4\times 4}\mid \mathbf{R} \in \mathbf{SO}(3), \mathbf{t}\in \Re^3\} SE(3)={ T=[R0T​t1​]∈ℜ4×4∣R∈SO(3),t∈ℜ3}机器人的工作空间记为$\mathcal{T}=SE(3)$
记点$P$相对于参考系$x$的坐标为${}^x\mathbf{P}$，参考系$y$相对于$x$的旋转矩阵记为${}^x{\mathbf{R}}_y$，同理，坐标原点之间的平移变换记为${}^x{\mathbf{t}}_y$，同时包含平移和旋转的变换记为${}^x{\mathbf{x}}_y$。即
$${}^x\mathbf{P}={}^x{\mathbf{x}}_y{(}^y\mathbf{P})={{}^x{\mathbf{R}}_y}^y\mathbf{P}+{}^x{\mathbf{t}}_y$$坐标变换之间满足结合律，即
$${}^x\mathbf{P}={{}^x\mathbf{x}}_y{(}^y\mathbf{P})={}^x{\mathbf{x}}_y{(}^y{\mathbf{x}}_z{(}^z\mathbf{P}))=({}^x{\mathbf{x}}_y{\circ }^y{\mathbf{x}}_z){(}^z\mathbf{P})={}^x{\mathbf{x}}_z{(}^z\mathbf{P})$$由简单的刚体运动可知，坐标系变换存在如下关系
$${}^x{\mathbf{R}}_z={{}^x{\mathbf{R}}_y}^y{\mathbf{R}}_z$$$${}^x{\mathbf{t}}_z={{}^x{\mathbf{R}}_y}^y{\mathbf{t}}_z+{}^x{\mathbf{t}}_y$$对不同参考系，如无特殊说明，均默认为世界参考系，常用参考系角标如下

记号	对应参考系
$e$	机器人末端执行机构（end-effector）
$t$	目标物体（target）
$c_i$	第$i$个相机（camera）

刚体运动

假设考虑末端执行器的工作空间$\mathcal{T}\in SE(3)$，$\mathbf{P}$的坐标为$(x,y,z)$。在世界参考系下，对应的角速度和速度为
$$\mathbf{\Omega }(t)=[{\omega }_x(t),{\omega }_y(t),{\omega }_z(t)]$$$$\mathbf{T}(t)=[T_x(t),T_y(t),T_z(t)]$$根据刚体运动学关系，有
x ˙ = z ω y − y ω z + T x \dot{x}=z\omega_y -y\omega_z +T_x