速度与雅可比¶

正运动学解决的是位置关系，而很多场景还需要知道速度关系——关节速度如何映射为末端速度？这就是雅可比矩阵的核心作用。

一、末端速度与关节速度¶

1.1 问题引入¶

机器人在运动过程中，我们关心：

给定各关节角速度 \(\dot{\theta}_1, \dot{\theta}_2, \ldots, \dot{\theta}_n\)，末端执行器的线速度和角速度是多少？
反过来，要让末端以特定速度运动，各关节需要多快？

末端速度可以分解为两部分：

\[ \mathbf{v} = \begin{bmatrix} \mathbf{v}_{\text{linear}} \\ \boldsymbol{\omega} \end{bmatrix} \in \mathbb{R}^6 \]

其中 \(\mathbf{v}_{\text{linear}} \in \mathbb{R}^3\) 是末端原点的线速度，\(\boldsymbol{\omega} \in \mathbb{R}^3\) 是末端坐标系的角速度。

1.2 雅可比矩阵的核心关系¶

\[ \mathbf{v} = J(q) \; \dot{q} \]

\[ \begin{bmatrix} \mathbf{v}_{\text{linear}} \\ \boldsymbol{\omega} \end{bmatrix} = J(q) \begin{bmatrix} \dot{\theta}_1 \\ \dot{\theta}_2 \\ \vdots \\ \dot{\theta}_n \end{bmatrix} \]

其中 \(J(q) \in \mathbb{R}^{6 \times n}\) 就是雅可比矩阵（Jacobian Matrix）。

直觉理解

雅可比矩阵是正运动学的"微分版本"。正运动学把关节位置映射到末端位姿，雅可比矩阵把关节速度映射到末端速度。它是正运动学关系在 \(q\) 处的一阶线性近似。

二、雅可比矩阵的构建——矢量积法¶

2.1 基本原理¶

雅可比矩阵 \(J\) 的第 \(i\) 列描述了第 \(i\) 个关节单独运动时对末端速度的贡献。

对于 \(n\) 自由度机器人：

\[ J = \begin{bmatrix} J_1 & J_2 & \cdots & J_n \end{bmatrix} \]

每一列 \(J_i \in \mathbb{R}^{6}\)，分为上下两部分：

\[ J_i = \begin{bmatrix} J_{v_i} \\ J_{\omega_i} \end{bmatrix} \]

2.2 转动关节（R）的贡献¶

\[ J_i = \begin{bmatrix} z_{i-1} \times (p_n - p_{i-1}) \\ z_{i-1} \end{bmatrix} \]

线速度部分 \(J_{v_i} = z_{i-1} \times (p_n - p_{i-1})\)：关节 \(i\) 旋转产生的末端线速度（叉积）
角速度部分 \(J_{\omega_i} = z_{i-1}\)：关节 \(i\) 的旋转轴方向

其中：

\(z_{i-1}\)：关节 \(i\) 的旋转轴方向向量（\({}^0_{i-1}T\) 旋转矩阵的第三列）
\(p_{i-1}\)：关节 \(i\) 坐标系原点在基座坐标系中的位置（\({}^0_{i-1}T\) 的平移列）
\(p_n\)：末端坐标系原点在基座坐标系中的位置

2.3 移动关节（P）的贡献¶

\[ J_i = \begin{bmatrix} z_{i-1} \\ \mathbf{0} \end{bmatrix} \]

线速度部分：\(z_{i-1}\)（沿关节轴方向的平移速度）
角速度部分：\(\mathbf{0}\)（移动关节不会产生旋转）

汇总

关节类型	\(J_{v_i}\)（线速度）	\(J_{\omega_i}\)（角速度）
转动 R	\(z_{i-1} \times (p_n - p_{i-1})\)	\(z_{i-1}\)
移动 P	\(z_{i-1}\)	\(\mathbf{0}\)

2.4 实例：平面 2R¶

对于平面 2R 机械臂（两个转动关节），雅可比矩阵简化为 \(2 \times 2\)（只看平面内的线速度）：

\[ J = \begin{bmatrix} -L_1 s_1 - L_2 s_{12} & -L_2 s_{12} \\ L_1 c_1 + L_2 c_{12} & L_2 c_{12} \end{bmatrix} \]

其中 \(s_1 = \sin\theta_1\)，\(c_1 = \cos\theta_1\)，\(s_{12} = \sin(\theta_1+\theta_2)\)，\(c_{12} = \cos(\theta_1+\theta_2)\)。

三、微分运动与雅可比（微分变换法）¶

3.1 微分运动¶

坐标系的微小运动可以用微分平移和微分旋转描述：

\[ \delta \mathbf{x} = \begin{bmatrix} \delta x \\ \delta y \\ \delta z \\ \delta_x \\ \delta_y \\ \delta_z \end{bmatrix} \]

前三个分量是微分平移，后三个分量是绕三个轴的微分旋转角度。

3.2 微分变换法构建雅可比¶

从正运动学出发，对 \({}^0_n T\) 中每个元素分别对各关节变量求偏导：

\[ J_{ij} = \frac{\partial x_i}{\partial q_j} \]

其中 \(\mathbf{x} = [x, y, z, \alpha, \beta, \gamma]^T\) 是末端位姿的参数化表达。

两种方法对比

方法	优点	缺点
矢量积法	物理意义清晰，直接利用 DH 参数	需要计算叉积
微分变换法	系统性强，适合符号推导	涉及偏导计算较多

两种方法得到的结果相同，只是推导路径不同。

四、逆速度与雅可比逆¶

当需要从期望的末端速度反求关节速度时：

\[ \dot{q} = J^{-1}(q) \; \mathbf{v} \]

但 \(J^{-1}\) 只在 \(J\) 为方阵且非奇异时存在。

4.1 冗余情况（\(n > 6\)）¶

\[ \dot{q} = J^{\dagger} \mathbf{v} + (I - J^{\dagger} J) \mathbf{z} \]

其中 \(J^{\dagger} = J^T(JJ^T)^{-1}\) 是伪逆，\((I - J^{\dagger}J)\mathbf{z}\) 是零空间投影——利用多余自由度实现次要目标（如避障、远离关节限位）。

五、奇异性与可操作性¶

5.1 奇异性（Singularity）¶

当雅可比矩阵降秩（行列式为零）时，机器人处于奇异位形：

\[ \det(J(q)) = 0 \]

奇异位形的后果

在某些方向上无法移动（丧失自由度）
逆雅可比趋向无穷大 → 需要无限大的关节速度才能产生微小末端速度
控制不稳定，末端抖动

5.2 常见的奇异类型¶

类型	描述	例子
边界奇异	末端接近工作空间边界	手臂完全伸直
内部奇异	工作空间内部的奇异位形	两个关节轴重合（如腕部奇异）

平面 2R 的奇异分析：

\[ \det(J) = L_1 L_2 \sin\theta_2 \]

当 \(\theta_2 = 0\) 或 \(\theta_2 = \pi\) 时为奇异——即手臂完全伸直或完全折叠时。

5.3 可操作性（Manipulability）¶

Yoshikawa 可操作性度量：

\[ w(q) = \sqrt{\det(J(q) J^T(q))} \]

\(w = 0\)：处于奇异位形
\(w\) 越大：末端在各方向的运动能力越均衡

可操作性椭球

将 \(\|{\dot{q}}\| \leq 1\) 约束下，末端速度 \(\mathbf{v} = J\dot{q}\) 所能达到的范围画出来，就是一个椭球。椭球越接近圆球，各方向运动能力越均匀；椭球越扁（趋向退化），越接近奇异。

六、总结¶

graph TD
    A[关节速度 q̇] -->|J| B[末端速度 v]
    B -->|J⁻¹| A
    C[雅可比 J] --> D{det J = 0?}
    D -->|是| E[奇异位形<br>丧失运动能力]
    D -->|否| F[正常工作<br>可操作性 w > 0]

核心公式：

\[ \mathbf{v} = J(q) \dot{q}, \quad J = \begin{bmatrix} J_1 & J_2 & \cdots & J_n \end{bmatrix} \]

雅可比矩阵是连接运动学与动力学的桥梁，也是速度控制和力控制的基础工具。