如何建立机械手末端位姿与图像雅克比矩阵之间的关系呢？这句话大家一听可能会有点懵逼，这是什么，为什么要研究？

首先，但凡想入门机械手控制这个坑的人应该都看过下面的这几句话和公式。

1. 基础知识—视觉伺服的基本思想

———基于视觉的伺服控制方法的目的是最小化一个图像误差，该误差可以定义为：$e(t)=s(m(t),a)-s^{\ast }$ —————————————- (1)
———式(1)为视觉伺服控制方法的通用形式。其中$m(t)$是一系列的图像特征，如图像点特征、形状特征、位姿特征或频域特征等。 $s(m(t),a)$表示通过特征$m(t)$与一些额外的系统参数构成的映射函数。将提取的特征$m(t)$映射至目标特征$s^{\ast }$的空间中，并与$s^{\ast }$进行比较，当两者差为零时，即系统误差$e(t)$为零，系统迗到稳定的收敛位置。
———视觉伺服方法的不同主要体现在设计不同的目标图像特征$s$。其中最经典的方法包括基于图像的视觉伺服以及基于位置的视觉伺服。基于图像的视觉伺服方法通过从图像中获取的特征点直接控制机械臂的运动，而基于位置的伺服方法首先通过图像特征点计算目标的位姿，再根据机械臂与目标物体的相对位姿规划运动方案。一旦确定了图像特征$s$的选择，即可确定控制器的设计形式。最筒单的控制方法是设计一个速度控制器来控制机械臂的移动。为了得到图像特征$s$随时间的变化率与摄像机运动速度的关系，设摄像机在摄像机坐标系下的瞬时速度为${\mathbf{v}}_c=(v_c,{\omega }_c)$，其中$v_c$为在摄像机坐标系下的瞬时线速度，${\omega }_c$为摄像机坐标系下的瞬时角速度。则图像特征随时间的变化率$\dot{s}$与摄像机速度${\mathbf{v}}_c$的关系可以表示为：
——————————————————$\dot{s}={\mathbf{L}}_s{\mathbf{v}}_{{}_c}$ —————————————— (2)

———其中${\mathbf{L}}_s\in {\mathbb{R}}^{k\times 6}$为图像雅克比矩阵。将式(2)代入(1)可得：
—————————————————-$\dot{e}={\mathbf{L}}_e{\mathbf{v}}_c$ —————————————— (3)

———其中${\mathbf{L}}_e={\mathbf{L}}_s$,考虑将${\mathbf{v}}_c$作为机械臂速度控制的输入，并保证图像误差的指数型下降，则可以通过对公式(3)中的雅克比矩阵求逆得到：
————————————————-${\mathbf{v}}_c=-\lambda {\mathbf{L}}_e^{+}\mathbf{e}$——————————(4)
———其中${\mathbf{L}}_e^{+}\in {\mathbb{R}}^{6\times k}$是${\mathbf{L}}_e$的伪逆，即当矩阵${\mathbf{L}}_e$为非方阵或奇异时，${\mathbf{L}}_e^{+}=({\mathbf{L}}_e^T{\mathbf{L}}_e{)}^{-1}{\mathbf{L}}_e^T$。在实际的视觉伺服系统中，在控制阶段的每一时刻获得精确的${\mathbf{L}}_e$或者${\mathbf{L}}_e^{+}$是十分困难的。因此需要对图像雅克比矩阵进行估计。因此实际的视觉伺服控制方程应改写为：
————————————————-${\mathbf{v}}_c=-\lambda {\hat{\mathbf{L}}}_e^{+}\mathbf{e}$——————————(5)
式(5)为视觉伺服控制的基本公式，在此公式的基础上，仅需要考虑三个问题来设计视觉伺服控制的方程：(1)如何选择图像特征$s$。(1)基于图像特征$s$的雅克比矩阵${\mathbf{L}}_s$如何推导；(3)${\hat{\mathbf{L}}}_e^{+}\mathbf{e}$如何估计

———上面这个段，我在关于机械手的第一篇文章里也讲过了。

———但是读到这边，大家会不会有一点疑问，我们讲了机械手的控制模式，机械手的求解原理。我们现在会利用机械手的正运动学分析求机械手的末端执行器的位姿，也会用机械手的逆运动学分析求机械手的各关节的旋转变量。
———此外，我们还知道了如何使用图像雅克比矩阵建立特征误差与摄像机的空间速度${\mathbf{v}}_c$之间的关系。但是，图像雅克比机矩阵是如何与机械手末端位姿之间的关系时什么呢？这个关系隐藏在哪里呢？
———我为什么要提出这个问题？这是因为我想到，我们控制机械手的运动，是需要知道期望点的期望位姿，需要知道机械手的当前位置，那位姿又是什么呢？位姿是机械后末端到世界坐标系的转换T。
———然而我们给出的基于视觉的机械手控制公式$e(t)=s(m(t),a)-s^{\ast }$和${\mathbf{v}}_c=-\lambda {\hat{\mathbf{L}}}_e^{+}\mathbf{e}$貌似与末端位姿无关呀？该怎么办呢？

———我们手机先要从机械手的相机模型开始。

2. 摄像机成像模型

———很久以前人们就知道，一个简单的小孔能够在黑暗的房间墙上显现出完好的倒立图像。一些海洋软体动物有着类似针孔相机的眼睛，例如鹦鹉螺。所有脊椎动物都是通过一个透镜在视网膜上形成一个倒置的图像，数码相机在原理上与其相似：通过一组透镜在半导体芯片表面上形成图像，芯片上的光感器件则把光转换成数字图像。眼睛或相机的成像过程都包含着把一个三维世界物体投影到二维平面上，从而失去了深度信息，以至我们无法从图像中分辨出它是远处的一个大物体，还是近处的一个更小的物体。这种从三维到二维的转换被称为透视投影。

——————————————————————————————图1。 中央透视模型

———摄像机模型是一个复杂的光学成像关系的简化模型，最简单的模型是线性模型，也就是针孔透视投影模型。针孔透视模型的核心思想是：物点通过透镜成像后，在像平面上可以找到与之对应的理想像点。
在计算机视觉中，通常应用上图1 所示中央透射模型，图像平面是在距离相机原点前 f 的位置，在该平面上形成的一个正像。相机的坐标系采用右手系，其z 轴定义了视场中心，光线在相机坐标系{C}
原点聚合，并在 $z=f$处的图像平面上投射出一个正像。世界坐标系{O}中的一点 $P=(X,Y,Z)$根据下式被投射到图像平面上 $p=(x,y)$

—————————————————————————————————————————-(5)
———这是一个从空间到图像平面的投影变换，更确切地说是一个透视投影，它具有以下特征：

1. 执行了从三维空间到二维图像平面的映射
2. 空间中的直线被投影到图像平面上仍为直线。
3. 空间中的平行线投影为如图2（a）所示的一个相交于远处消失点的直线，但平行于像平面的平面中的直线例外，这些直线不会聚合。
4. 空间中的圆锥截面投影到像平面仍为圆锥截面，或者是如图 2（b）所示的椭圆。
5. 这种映射不是一对一的，其逆映射并不唯一。也就是说，给定像平面的点$(x,y)$，我们不能唯一确定在其空间中的点$(X,Y,Z)$。只能说空间中的点一定在如图 1 所示的射线 CP 上。
6. 这种变换不一定是保角的，因为内部的角度是不能维持原来的大小。平移、旋转和比例缩放都是保角变换的例子。

—————————————————————————————————————图2

由上可以写出图像平面上一个点的齐次形式 $\tilde{p}=(x^{\prime },y^{\prime },z^{\prime })$，其中；

——————————————————————————————-(6)
采用矩阵形式表示；

———————————————————————————————（7）

其中，非齐次的图像坐标是

————————————————————————————————————-（8）
如果把空间点的坐标也写成齐次形式 $\widetilde{{}_{}^C\text{P}}=(x^{\prime },y^{\prime },z^{\prime })$，则透视投影可以表示为如下线性形式；

——————————————————————————————————-（9）
或者

————————————————————————————————————（10）

———其中，C 是一个$3\times 4$矩阵，也称为相机矩阵。$\widetilde{{}_{}^C\text{P}}$表示空间点的坐标，该点坐标是关于相机坐标系{C} 的。波浪号表示它是齐次量。C 的第三列是在世界坐标系中平行于光轴的一个向量。相机矩阵可以因式分解为

————————————————————————————————（11）
其中第二个矩阵是投影矩阵。
利用 MATLAB 中的 rvctools 工具箱创建一个中央投影相机模型，例如

>> cam = Central Camera(‘focal’, 0.015);

• 1

它会返回一个对象的实例，该对象有一个 15mm 的透镜。默认情况下，相机在世界坐标系的原点，其光轴指向世界坐标系的 z 方向，例如图 1 中央投射模型所示。定义一个空间点 P = [0.3, 0.4, 3.0]；单位是米，它对应的像平面坐标是

———这个像平面上的点相对于主点的位置是[1.5,2.0]，这是一个很小的位移，但是它与典型的图像传感器大小相匹配。
———通常，在世界坐标系中相机具有一个任意位姿 ${\xi }_C$ ，如图 3 所示，空间点相对于照相机坐标系齐次坐标形式为

———————————————————————————(12)

———————————————————————图3. 相机的坐标系

可以通过将照相机向左移动 0.5m 证明上述关系；

———整个实验室在仿真环境下进行的，所以我们执行该代码的目的是让相机在仿真环境下移动
———其中，第三个参数作transl(0.5, 0, 0)为齐次变换的相机位姿${\xi }_C$ 。可以看出 x 坐标轴从1.5m增加到 4.0mm，这就说图像点向右移动了.

图4 展示了相机平面与离散像素的关系
———在数码相机中，图像平面是一个$W\times H$的光敏单元网格，光敏单元也称为图素，它直接与图像的像素相对应，如图 4 所示。像素坐标是一个非负整数的二维向量$（u,v）$，按惯例，坐标系原点在图像平面的左上角。在 MATLAB 中，左上角的坐标是(1,1) 。每个像素的大小一致，并位于各自网格的中心，所以像素坐标与图像平面坐标直接相关，即

————————————————————————————-(12)
其中，${\rho }_w$, ${\rho }_h$是每个像素点各自的宽和高，（$u_0$ ，$v_0$）是主点坐标（主点是图像平面与光轴的交点）。因此可以得出空间点P = [0.3, 0.4, 3.0]的像素坐标为（$u$ ，$v$）=(622,170)，即

通过预设相机的参数矩阵为 K，可以写出像素坐标的公式（11）

————————————————————————————————————————————-(13)
———其中， $\tilde{p}=(u^{\prime },v^{\prime },w^{\prime })$是空间点 P在像素坐标系中的齐次坐标。结合公式（12）和（14）可以写出一般形式的相机投影为

——————————————————————————————————————————(14)
———其中，所有项都被包进相机矩阵C 中。这是一个的齐次变换，执行了缩放、平移、透视投影。
———位姿估计问题就是要确定目标物坐标系{T}相对于相机坐标系{C}的位姿${}_{}^C{\xi }_T$ ，目标物的几何形体已知，就是说我们知道目标物的某些点在{T}中的位置$(X_i,Y_i,Z_i)$已知，$i\in [1,N]$。相机的内在参数也是已知的。因此使用像平面坐标$(u_i,v_i)$、特征点$(X_i,Y_i,Z_i)$和相机内在参数来估计目标物位姿，被称为 n 点透视问题，或简称 PnP 问题。
———举例说明位姿估计问题，首先创建一个已标定的相机，且其参数已知

另外目标物设为一个简单的几何立方体模型，边长 0.2m，它相对于的标记点坐标是

假设该目标物处于相机坐标系{C}中某个未知的位姿${}_{}^C{\xi }_T$ ：

在这个未知的位姿${}_{}^C{\xi }_T$时，目标物的特征点在像平面的坐标是

现在只利用目标物模型、观测到的图像特征$p$和已标定的相机 cam 来估计目标物的相对位姿${}_{}^C{\xi }_T$

———它与目标物的未知位姿 T_unknown 相同（精确到小数点后四位）。
———实际上，图像特征坐标将不能被视觉系统准确地估计。为此，可以通过在图像特征坐标上加入零均值高斯噪声来进行模拟。