33、机器人系统的任务空间控制与视觉控制

机器人系统的任务空间控制与视觉控制

一、任务空间控制基础

1.1 奇异值与雅可比矩阵

在机器人控制中，最小奇异值可衡量矩阵接近奇异的程度。例如，图像雅可比矩阵在与旋转角 𝜋 相关的构型邻域内可能出现秩亏或奇异的情况，这种困难在许多机器人控制问题中都存在。

1.2 关节空间控制与任务空间控制的区别

以往的控制策略多是基于关节空间控制，目标是使广义坐标 q 及其导数 ̇q 在 t 趋于无穷时趋近于期望轨迹，即 q(t) → qd(t) 和 ̇q(t) → ̇qd(t)。对于设定点控制器，期望轨迹是常数轨迹（̇qd(t) ≡ 0）；对于轨迹跟踪控制器，则考虑时变的期望轨迹。这些控制器的性能标准是最小化以关节自由度明确表示的误差。

然而，在实际应用中，通过反馈控制要实现的目标通常自然地用与当前问题相关的变量来表达，但这些变量不易用关节变量表示。例如，设计一个控制器使工具或末端执行器位于工作空间的某个位置。假设工具尖端的位置 p 由向量 r0,p(t) = x1(t)x0 + x2(t)y0 + x3(t)z0 给出，控制策略应保证当 t 趋于无穷时，x1(t) → x1,d，x2(t) → x2,d，x3(t) → x3,d。但 (x1, x2, x3) 不是机器人操作器的关节变量，之前讨论的关节空间控制策略不能直接应用。虽然原则上可以用任务空间变量重新表述运动方程，但对于实际的机器人系统，这可能是一个繁琐的问题。(x1, x2, x3) 就是当前问题的一组任务空间变量的例子。

1.3 基于任务空间雅可比矩阵的控制定理

存在多种推导任务空间控制器的技术，其中一种方法采用任务空间雅可比矩阵，相关定理如下：
假设 {q1 … qN} 是机器人系统的 N 个广义坐标，其控制方程具有特定形式。再假设给定 N 个任务空间坐标 {x1 … xN}，任务空间雅可比矩阵 𝕁 = 𝜕x/𝜕q 的 (i, j) 项定义为 𝕁ij ∶= 𝜕xi/𝜕qj（i, j = 1, … , N），任务空间跟踪误差定义为 ex(t) = x(t) - xd(t)，其中 xd(t) 是期望的任务空间轨迹。假设满足以下条件：
1. 广义质量矩阵 M(q) 和非线性右侧 n(q, ̇q) 满足特定假设。
2. 任务空间雅可比 𝕁 = 𝕁(q) 及其导数 ̇𝕁 = ̇𝕁(t, ℝN) 分别在 ℝN 和 ℝ+ × ℝN 上是连续函数。
3. 任务空间雅可比在均匀可逆的意义上，存在常数 c1 和 c2，使得对于所有 w ∈ ℝN 和 q ∈ ℝN，有 c1‖w‖2 ≤ wT𝕁(q)w ≤ c2‖w‖2。

则计算扭矩控制律 𝝉 = Mv - n（其中 v = 𝕁−1(̈xd - ̇𝕁̇q - G1(J ̇q - ̇xd) - G0(x - xd))）会产生一个闭环系统，使任务空间跟踪误差及其导数的原点渐近稳定。

证明过程如下：
将计算扭矩控制律代入运动控制方程，得到 ̈q = v = 𝕁−1(̈xd - ̇𝕁̇q - G1(𝕁̇q - ̇xd) - G0(x - xd))。
重新排列此方程可得 ((𝕁̈q + ̇𝕁̇q) - ̈xd) + G1(𝕁̇q - ̇xd) + G0(x - xd) = 0。
根据链式法则，̇x = d/dt (x(q)) = 𝜕x/𝜕q ̇q = 𝕁̇q，̈x = 𝕁̈q + ̇𝕁̇q。
因此，闭环控制方程可表示为 (̈x - ̈xd) + G1( ̇x - ̇xd) + G0(x - xd) = 0，即 ̈ex + G1 ̇ex + G0ex = 0，其中 ex(t) ∶= x(t) - xd(t)。所以，原点 {00} = {eTx ̇eTx} 渐近稳定。

1.4 示例：球形机器人的任务空间控制

考虑一个球形机器人操作器，任务空间坐标 (x1, x2, x3) 定义为框架 3 的原点 r 在框架 0 中的坐标，即 r0,r(t) ∶= x1(t)x0 + x2(t)y0 + x3(t)z0。控制器要使任务空间坐标跟踪期望轨迹：
x1,d(t) = 1 + A1 sin 𝛺1t
x2,d(t) = 1 + A2 sin 𝛺2t
x3,d(t) = 1 + A3 sin 𝛺3t
其中 Ai = 1/10，𝛺i = 2 rad s−1。选择增益矩阵 G0 = g0𝕀 和 G1 = g1𝕀，(g0, g1) 取值为 (1, 1)、(5, 5)、(10, 10) 或 (100, 100)。假设机器人的初始构型为 𝜃1 = 𝜋/2 rad，𝜃2 = 𝜋/2 rad，dp,q = 1 m。

通过运动学分析，可得任务空间坐标与广义坐标 (q1, q2, q3) ∶= (𝜃1, 𝜃2, dq,r) 的关系：
[
\begin{cases}
x_1 = d_{p,q} \sin \theta_1 + d_{q,r} \cos \theta_1 \sin \theta_2 \
x_2 = -d_{p,q} \cos \theta_1 + d_{q,r} \sin \theta_1 \sin \theta_2 \
x_3 = d_{o,p} - d_{q,r} \cos \theta_2
\end{cases}
]

任务空间雅可比矩阵可以通过对上述表达式求偏导数或使用之前开发的工具来计算，得到：
[
\begin{bmatrix}
\dot{x} 1 \
\dot{x}_2 \
\dot{x}_3
\end{bmatrix}
=
\begin{bmatrix}
(d {p,q} \cos \theta_1 - d_{q,r} \sin \theta_1 \sin \theta_2) & d_{q,r} \cos \theta_1 \cos \theta_2 & \cos \theta_1 \sin \theta_2 \
(d_{p,q} \sin \theta_1 + d_{q,r} \cos \theta_1 \sin \theta_2) & d_{q,r} \sin \theta_1 \cos \theta_2 & \sin \theta_1 \sin \theta_2 \
0 & d_{q,r} \sin \theta_2 & -\cos \theta_2
\end{bmatrix}
\begin{bmatrix}
\dot{\theta} 1 \
\dot{\theta}_2 \
\dot{d} {q,r}
\end{bmatrix}
]

对于这种特定的任务空间坐标选择，雅可比矩阵恰好等于之前引入的速度雅可比矩阵 Jv。任务空间雅可比矩阵的时间导数虽然计算冗长，但可以直接计算。

通过绘制状态轨迹、跟踪误差和控制输入随时间的变化来评估控制器的性能。结果表明，跟踪误差以指数速率收敛到零，点 r 在惯性坐标中的轨迹收敛到由期望轨迹方程参数描述的直线。同时，随着增益的增加，启动时需要较大的瞬态扭矩。

二、任务空间与视觉控制

2.1 传统视觉伺服控制的局限性

之前讨论的计算扭矩控制策略的内外环架构可用于推导任务空间变量的跟踪或设定点控制器。然而，在视觉伺服控制问题中，假设控制器的输入是相机框架的速度和角速度向量 u(t) ∶= {vℂ0,c 𝝎ℂ0,ℂ} = -𝜆L+syse(t)，其中 e 是特征点在图像平面的跟踪误差。但相机框架的速度和角速度不是执行器的输出，而是响应变量，其值取决于执行器提供的力和力矩。在实际中，很难精确实现这些速度和角速度的指定。

2.2 示例：配备相机和激光测距系统的球形机器人

假设球形机器人操作器配备了相机和激光测距系统，相机安装在机器人上，使相机框架与框架 3 重合，相机焦距 f = 1。一个反射目标 t 位于特定位置，其在框架 0 中的坐标为 r00,t = [dp,q dq,r + f + Dp do,p]T。

要设计一个控制器，使点 t 的图像接近图像平面的原点，并且距离 Z 接近某个期望的常数值 Zd。控制器采用计算扭矩 𝝉 = Mv - n，其中 v = 𝕁−1(̈xd - ̇𝕁̇q - G1(𝕁̇q - ̇xd) - G0(x - xd))。任务空间坐标选择为 x = {u, v, Z}T，其中图像平面坐标 (u, v) 为 u = fX/Z 和 v = fY/Z。

任务空间雅可比矩阵 𝕁 可以写成两个矩阵的乘积 𝕁 = LJ，其中：
- J 是关节空间雅可比矩阵，可通过直接计算速度和角速度或使用之前的专用程序推导得到：
[
J =
\begin{bmatrix}
d_{q,r} \cos \theta_2 & d_{q,r} & 0 \
-d_{q,r} \sin \theta_2 & 0 & 0 \
d_{p,q} \sin \theta_2 & 0 & 1 \
\sin \theta_2 & 0 & 0 \
0 & 1 & 0 \
-\cos \theta_2 & 0 & 0
\end{bmatrix}
]
- L 可以通过交互矩阵和之前引入的矩阵 D 确定：
[
L =
\begin{bmatrix}
-\frac{1}{Z} & 0 & \frac{u}{Z} & \frac{uv}{f} & -\frac{f^2 + u^2}{f} & \frac{v}{f} \
0 & -\frac{1}{Z} & \frac{v}{Z} & \frac{f^2 + v^2}{f} & -\frac{uv}{f} & -\frac{u}{f} \
0 & 0 & -1 & -\frac{vZ}{f} & \frac{uZ}{f} & 0
\end{bmatrix}
]

任务空间雅可比矩阵的时间导数通过 ̇𝕁 = d/dt (L ⋅ J) = ̇LJ + L̇J 计算，其中 ̇L 和 ̇J 的表达式如下：
[
\dot{L} =
\begin{bmatrix}
\frac{1}{z^2} \dot{z} & 0 & (\frac{1}{z} \dot{u} - \frac{u}{z^2} \dot{z}) & (\dot{u}v + u \dot{v}) & -2u \dot{u} & \dot{v} \
0 & \frac{1}{z^2} \dot{z} & (\frac{1}{z} \dot{v} - \frac{v}{z^2} \dot{z}) & 2v \dot{v} & -(\dot{u}v + u \dot{v}) & -\dot{u} \
0 & 0 & 0 & -(\dot{v}z + v \dot{z}) & (\dot{u}z + u \dot{z}) & 0
\end{bmatrix}
]
[
\dot{J} =
\begin{bmatrix}
(\dot{d} {q,r} \cos \theta_2 - d {q,r} \dot{\theta} 2 \sin \theta_2) & \dot{d} {q,r} & 0 \
(-\dot{d} {q,r} \sin \theta_2 - d {q,r} \dot{\theta} 2 \cos \theta_2) & 0 & 0 \
d {p,q} \dot{\theta}_1 \cos \theta_2 & 0 & 0 \
\dot{\theta}_2 \cos \theta_2 & 0 & 0 \
0 & 0 & 0 \
\dot{\theta}_2 \sin \theta_2 & 0 & 0
\end{bmatrix}
]

使用两组初始条件评估控制器的性能：
- 初始条件 {qT(0) ̇qT(0)} = {[𝜋/4 𝜋/2 1] [0 0 0]}：结果显示，当 t 趋于无穷时，𝜃1(t) → 𝜋/2，𝜃2(t) ≡ 𝜋/2，dq,r(t) → 1，这意味着任务坐标满足 u(t) → 0，v(t) → 0，z(t) → 2。跟踪误差 ev 和 eZ 渐近收敛到零。
- 初始条件 {qT(0) ̇qT(0)} = {[𝜋/2 𝜋/4 1] [0 0 0]}：角度 𝜃1 保持在 𝜋/2，而 𝜃2(t) 和 dq,r(t) 在达到稳态之前有很大变化。跟踪误差 𝜃u 和 𝜃z 渐近趋近于零。

2.3 总结

通过上述示例可以看出，基于任务空间和视觉控制的方法可以有效地解决机器人在实际应用中的控制问题。在设计控制器时，需要考虑任务空间坐标的选择、雅可比矩阵的计算以及初始条件的影响。同时，要注意实际中难以精确控制相机框架的速度和角速度等问题。

三、问题求解

3.1 问题列表

以下是一些相关的问题及求解思路：
1. 相机校准问题 ：给定相机和校准图案的配置，已知焦距、图像分辨率和成像传感器尺寸，确定将像素坐标 (𝜂, 𝜈) 与视网膜坐标相关联的校准矩阵。
2. 坐标映射问题 ：对于不同类型的机器人（如 PUMA 机器人、球形手腕、SCARA 机器人、圆柱形机器人等），已知相机的安装位置和方向，以及机器人的特定构型，求解世界中的点与规范焦平面坐标或像素坐标之间的映射关系。
3. 轨迹跟踪控制器设计问题 ：对于圆柱形机器人和 SCARA 机器人，要求设计控制器使特定点跟踪给定的轨迹，基于任务空间计算扭矩控制定律进行设计。
4. 相机和激光测距系统的控制器设计问题 ：对于配备相机和激光测距系统的机器人，设计控制器使点的规范焦平面坐标接近焦平面的原点，并且深度接近某个固定值。
5. 交互矩阵推导问题 ：证明焦距为 f 的交互矩阵为 L = [具体矩阵表达式]。

3.2 问题求解步骤

以相机校准问题为例，求解步骤如下：
1. 明确已知条件，包括焦距、图像分辨率和成像传感器尺寸。
2. 根据相机的几何关系和成像原理，建立像素坐标与视网膜坐标之间的数学模型。
3. 通过已知的配置信息（如相机轴与校准图案的关系），确定模型中的参数。
4. 计算得到校准矩阵。

对于其他问题，也可以根据具体的条件和要求，按照相应的理论和方法进行求解。

四、总结与展望

4.1 总结

本文介绍了机器人系统的任务空间控制和视觉控制相关内容。任务空间控制通过引入任务空间雅可比矩阵，提供了一种解决传统关节空间控制难以处理的问题的方法。视觉控制结合了相机和激光测距系统等传感器，使机器人能够更好地适应实际环境。通过具体的示例和问题求解，展示了这些方法的应用和有效性。

4.2 展望

未来，随着机器人技术的不断发展，任务空间控制和视觉控制将在更多领域得到应用。例如，在工业自动化中，提高机器人的操作精度和灵活性；在服务机器人领域，使机器人能够更好地与人类和环境交互。同时，还需要进一步研究如何提高控制器的鲁棒性和适应性，以应对复杂多变的实际情况。

以下是一个简单的 mermaid 流程图，展示任务空间控制器设计的主要步骤：

graph LR
    A[定义任务空间坐标] --> B[计算任务空间雅可比矩阵]
    B --> C[检查雅可比矩阵条件]
    C -->|满足条件| D[设计计算扭矩控制律]
    C -->|不满足条件| E[调整或重新设计]
    D --> F[进行仿真验证]
    F --> G[评估性能]
    G -->|性能良好| H[应用于实际系统]
    G -->|性能不佳| E

通过这个流程图，可以清晰地看到任务空间控制器设计的主要流程，从任务空间坐标的定义到最终应用于实际系统，每个步骤都相互关联，需要不断地进行调整和优化。

五、技术细节深入分析

5.1 任务空间雅可比矩阵的重要性

任务空间雅可比矩阵在任务空间控制中起着核心作用。它建立了广义坐标变化与任务空间坐标变化之间的联系。从数学角度看，其元素 𝕁ij ∶= 𝜕xi/𝜕qj 描述了第 j 个广义坐标的微小变化对第 i 个任务空间坐标的影响。

在实际应用中，雅可比矩阵的性质直接影响控制器的性能。例如，当雅可比矩阵接近奇异时，系统的控制变得不稳定，就像在与旋转角 𝜋 相关的构型邻域内图像雅可比矩阵可能出现的情况。而当雅可比矩阵满足均匀可逆条件时，即存在常数 c1 和 c2 使得 c1‖w‖2 ≤ wT𝕁(q)w ≤ c2‖w‖2 ，可以保证控制器的稳定性。

5.2 计算扭矩控制律的原理

计算扭矩控制律 𝝉 = Mv - n 是实现任务空间控制的关键。其中，v = 𝕁−1(̈xd - ̇𝕁̇q - G1(J ̇q - ̇xd) - G0(x - xd)) 。这个控制律的设计基于对机器人动力学模型的深入理解。

M 是广义质量矩阵，它描述了机器人各部分的惯性特性；n 是非线性右侧项，包含了机器人运动过程中的各种非线性因素，如重力、摩擦力等。通过计算 v 并代入控制律，可以使机器人的运动跟踪期望的任务空间轨迹。

具体来说，̈xd 是期望的任务空间加速度，̇𝕁̇q 考虑了雅可比矩阵随时间的变化对系统的影响，G1 和 G0 是增益矩阵，用于调整系统的响应速度和稳定性。通过合理选择增益矩阵，可以使系统的跟踪误差以指数速率收敛到零。

5.3 视觉控制中的坐标变换

在视觉控制中，涉及到多个坐标系之间的变换，如世界坐标系、相机坐标系和图像坐标系。以配备相机和激光测距系统的球形机器人为例，需要将世界中的点的坐标转换到相机坐标系，再进一步转换到图像坐标系。

设点 t 在世界坐标系中的坐标为 r00,t ，在相机坐标系中的坐标为 r33,t = Xx3 + Yy3 + Zz3 ，在图像坐标系中的坐标为 (u, v) 。通过一系列的坐标变换公式，如 u = fX/Z 和 v = fY/Z ，可以建立起这些坐标系之间的联系。

同时，在计算任务空间雅可比矩阵时，也需要考虑这些坐标变换的影响。例如，任务空间雅可比矩阵 𝕁 可以写成 𝕁 = LJ 的形式，其中 L 和 J 分别与不同的坐标变换相关。

六、实际应用案例分析

6.1 工业自动化中的应用

在工业自动化领域，任务空间控制和视觉控制可以大大提高机器人的操作精度和效率。例如，在装配线上，机器人需要将零件准确地安装到指定位置。通过任务空间控制，可以直接以零件的目标位置作为任务空间坐标，设计控制器使机器人的末端执行器精确地到达该位置。

结合视觉控制，机器人可以利用相机和激光测距系统实时获取零件的位置和姿态信息，及时调整运动轨迹。例如，当零件的位置出现微小偏差时，视觉系统可以检测到并将误差信息反馈给控制器，控制器根据误差调整机器人的运动，保证装配的准确性。

6.2 服务机器人领域的应用

在服务机器人领域，如餐厅服务机器人、家庭清洁机器人等，任务空间控制和视觉控制可以使机器人更好地与人类和环境交互。以餐厅服务机器人为例，它需要将菜品准确地送到顾客的餐桌上。

通过任务空间控制，机器人可以规划出从厨房到餐桌的最优路径，并控制自身的运动沿着该路径行驶。视觉控制则可以帮助机器人识别餐桌的位置、顾客的位置以及周围的障碍物。当遇到障碍物时，机器人可以根据视觉信息及时调整路径，避免碰撞。

6.3 医疗机器人领域的应用

在医疗机器人领域，如手术机器人，任务空间控制和视觉控制的精度和可靠性至关重要。手术机器人需要在微小的空间内进行精确的操作，如切除肿瘤、缝合伤口等。

任务空间控制可以使机器人的手术工具准确地到达目标位置，视觉控制可以提供高分辨率的图像，帮助医生实时观察手术部位的情况。同时，通过激光测距系统可以精确测量手术工具与目标组织的距离，保证手术的安全性和准确性。

七、技术挑战与解决方案

7.1 雅可比矩阵奇异问题

雅可比矩阵奇异是任务空间控制中常见的问题。当雅可比矩阵接近奇异时，其逆矩阵可能不存在或变得不稳定，导致控制器无法正常工作。

解决方案之一是采用奇异值分解（SVD）技术。通过对雅可比矩阵进行 SVD 分解，可以得到其奇异值和奇异向量。当奇异值接近零时，可以对其进行适当的处理，如设置一个阈值，当奇异值小于阈值时，采用伪逆矩阵代替逆矩阵，保证控制器的稳定性。

另一种解决方案是调整机器人的构型，避免进入雅可比矩阵奇异的区域。可以通过实时监测雅可比矩阵的奇异值，当发现奇异值接近临界值时，控制器自动调整机器人的关节角度，使雅可比矩阵远离奇异状态。

7.2 视觉传感器误差问题

视觉传感器在实际应用中可能存在误差，如相机的畸变、激光测距系统的测量误差等。这些误差会影响机器人对环境的感知和控制的准确性。

对于相机畸变问题，可以采用相机校准技术进行校正。通过拍摄已知的校准图案，建立相机的畸变模型，然后对拍摄的图像进行畸变校正，提高图像的准确性。

对于激光测距系统的测量误差，可以采用多传感器融合的方法。结合其他传感器，如超声波传感器、红外传感器等，对激光测距系统的测量结果进行验证和修正，提高测量的准确性。

7.3 实时性问题

在实际应用中，机器人需要实时地处理视觉信息并做出控制决策。然而，视觉处理和控制计算可能会消耗大量的时间，导致系统的实时性受到影响。

解决方案之一是采用高性能的硬件平台，如 GPU 加速计算。GPU 具有强大的并行计算能力，可以大大提高视觉处理和控制计算的速度。

另一种解决方案是优化算法，减少计算量。例如，采用简化的模型和算法，在保证一定精度的前提下，提高计算效率。同时，可以采用预测控制的方法，提前预测机器人的运动状态，减少实时计算的负担。

八、未来发展趋势

8.1 多传感器融合技术

未来，多传感器融合技术将成为任务空间控制和视觉控制的重要发展趋势。除了相机和激光测距系统，还可以融合其他传感器，如触觉传感器、听觉传感器等，使机器人能够更全面地感知环境。

例如，在机器人抓取物体时，触觉传感器可以提供物体的硬度、形状等信息，结合视觉传感器的信息，机器人可以更准确地调整抓取的力度和姿态，提高抓取的成功率。

8.2 深度学习的应用

深度学习在计算机视觉领域已经取得了巨大的成功，未来也将在机器人的任务空间控制和视觉控制中得到广泛应用。通过深度学习算法，可以训练机器人自动识别和理解复杂的环境信息，如物体的类别、位置、姿态等。

例如，利用卷积神经网络（CNN）可以对相机拍摄的图像进行实时分析，识别出目标物体的特征和位置。然后，将这些信息反馈给控制器，实现更智能的控制。

8.3 人机协作

随着机器人技术的发展，人机协作将成为一个重要的发展方向。在任务空间控制和视觉控制中，机器人需要与人类进行更紧密的协作。

例如，在工业生产中，机器人可以与工人共同完成一项任务。机器人可以根据工人的动作和意图，实时调整自己的运动轨迹，避免与工人发生碰撞。同时，机器人可以通过视觉系统监测工人的状态，提供必要的帮助和支持。

以下是一个 mermaid 流程图，展示多传感器融合在机器人控制中的应用流程：

graph LR
    A[相机传感器] --> B[数据采集]
    C[激光测距传感器] --> B
    D[触觉传感器] --> B
    B --> E[数据预处理]
    E --> F[多传感器融合算法]
    F --> G[控制器]
    G --> H[机器人运动控制]
    H --> I[环境反馈]
    I --> A
    I --> C
    I --> D

通过这个流程图，可以清晰地看到多传感器融合在机器人控制中的应用过程。从不同传感器的数据采集到数据预处理，再到多传感器融合算法的处理，最后将融合后的信息输入到控制器中，实现机器人的运动控制。同时，环境反馈信息又可以不断地更新传感器的数据，形成一个闭环控制系统。

综上所述，任务空间控制和视觉控制在机器人领域具有重要的应用价值和发展前景。通过不断地研究和创新，解决实际应用中遇到的问题，这些技术将在更多领域得到广泛应用，推动机器人技术的不断发展。