想要更好的食用，建议先品尝如下两篇论文

1. 视觉伺服入门第一步：带你从经典论文阅读Visual servo control. I. Basic approach开启视觉伺服之旅
2. 视觉伺服入门第二步：带你从经典论文阅读Visual Servo Control Part II: Advanced Approaches进阶版

论文摘要

在本文中提出了一个多传感器机器人在多种约束条件下的控制框架。我们将来自多个传感器的特征处理为单个特征向量。我们方法的核心思想是考虑所有的约束因素，利用加权矩阵平衡每个特性的贡献。约束的添加使得我们的控制环节更加平滑。在控制设计中引入了多传感器建模的这一方法与线性二次控制相似。本篇论文主要讲述 的是三维定位6自由度机械臂的视觉伺服。任务要确保多个约束组合的同时执行。
我们所考虑的约束条件有可视性以及关节限制规避等。

A framework is presented in this paper for the control of a multi-sensor robot under several constraints. In this approach, the features coming from several sensors are treated as a single feature vector. The core of our approach is a weighting matrix that balances the contribution of each feature, allowing to take constraints into account.

多传感器建模

This section presents the general modeling of a multi-sensor robot. First, we define the global kinematic model, then we introduce the weighted signal error that will be used in the control law. We propose a generic weighting function that allows both balancing the sensor features and taking into account unilateral constraints.

我们之前已经推导过了特征雅克比的公式。
$$\dot{\mathbf{s}}={\mathbf{L}}_{\mathbf{s}}{}^c{\mathbf{W}}_e\mathbf{J}(q)\dot{q}$$

多传感器的测量向量为$\mathbf{s}=({\mathbf{s}}_1,{\mathbf{s}}_2,{\mathbf{s}}_3,{\mathbf{s}}_4...,{\mathbf{s}}_k)$,每一个测量的维度为m。我们可以得到如下运动学模型。

$${\mathbf{J}}_s={\mathbf{L}\mathbf{W}}_e{\mathbf{J}}_q=\left[\begin{array}{ccc}{\mathbf{L}}_1& \dots & 0\\ ⋮& \ddots & ⋮\\ 0& \dots & {\mathbf{L}}_k\end{array}\right]\left[\begin{array}{c}{}^1{\mathbf{W}}_e\\ ⋮\\ {}^k{\mathbf{W}}_e\end{array}\right]{}^e{\mathbf{J}}_q$$

此外，我们需要设置每一个任务的加权误差矩阵$e=\mathbf{H}{e}^{\ast }$:
$$\mathbf{H}=\left[\begin{array}{ccc}{\mathbf{H}}_1& \dots & 0\\ ⋮& \ddots & ⋮\\ 0& \dots & {\mathbf{H}}_k\end{array}\right]$$
原始的control law为$e=s-s^{\ast }$,换成关节空间
$$\mathbf{H}\mathbf{J}\dot{q}=\mathbf{H}(s-s^{\ast })s.t.\dot{q}=(\mathbf{H}\mathbf{J}{)}^{+}\mathbf{H}(s-s^{\ast })$$
我们的优化函数也在变$$min||\mathbf{J}\dot{q}-{\dot{e}}^{\ast }|{|}^2——>min||\mathbf{H}\mathbf{J}\dot{q}-\mathbf{H}{\dot{e}}^{\ast }|{|}^2$$

$$\forall i\in [1,m]:h_i=h_i^t+h_i^c$$
我们一般考虑设置任务一定会用的特征，其加权$h_i^t=1$；如果单纯考虑约束，我们把特征设置在$h_i^t=0$；现在我们集中讨论的是$h_i^c$对于约束的影响。

例如下图，我们考虑的是图像坐标的约束，$h_i^c$如何表达？
我们有安全区的存在。速写板

$$\{\begin{array}{rl}{s}_i^{\mathrm{s}-}& =s_i^{-}+{\rho }_i\left(s_i^{+}-s_i^{-}\right)\\ s_i^{\mathrm{s}+}& =s_i^{+}-{\rho }_i\left(s_i^{+}-s_i^{-}\right)\end{array}$$

$$h_i^c=\{\begin{array}{cc}\frac{s_i-s_i^{8+}}{s_i^{+}-s_i}& \text{ if }{s}_i>s_i^{\mathrm{s}+}\\ \frac{s_i^{i-}-s_i}{s_i-s_i^{-}}& \text{if }{s}_i<s_i^{\mathrm{s}-}\\ 0& \text{ otherwise }\end{array}$$

参考文献

[1] Olivier Kermorgant, François Chaumette. Dealing with constraints in sensor-based robot control.IEEE Transactions on Robotics, IEEE, 2014, 30 (1), pp.244-257. ff10.1109/TRO.2013.2281560ff. ffhal00855724f