速度与雅可比¶

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

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

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}\) 是零空间投影——利用多余自由度实现次要目标（如避障、远离关节限位）。

4.2 工程实操：7 自由度机械臂的速度 IK¶

7 自由度仿人机械臂的雅可比矩阵是 \(J \in \mathbb{R}^{6 \times 7}\)。它把 7 个关节速度映射到 6 维末端速度，因此反求关节速度时天然有 1 维冗余：

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

这里第一项负责完成末端速度任务，第二项在理论上不会改变末端速度，因为：

\[ J(I - J^\dagger J)z \approx 0 \]

这就是“手掌不动，肘部还能绕一绕”的数学来源。

1）任务速度怎么来¶

如果规划器给的是目标位姿 \(T_d\)，控制周期内通常先把位姿误差变成期望末端速度：

\[ v_d = \begin{bmatrix} K_p(p_d - p) \\ K_R \text{Log}(R^T R_d) \end{bmatrix} \]

然后用速度 IK 得到 \(\dot{q}\)，积分得到下一步关节角：

\[ q_{k+1} = q_k + \dot{q}\Delta t \]

控制周期

上位机规划常见周期是 50~200 Hz，底层电机电流/速度环更快。速度 IK 的输出通常不是直接给电机力矩，而是给关节位置/速度控制器作为参考。

2）阻尼伪逆更适合实际机器人¶

靠近奇异点时，普通伪逆会让关节速度突然变大。实际项目中更常用阻尼伪逆：

\[ J^\dagger_\lambda = J^T(JJ^T + \lambda^2 I)^{-1} \]

可以根据最小奇异值 \(\sigma_{\min}\) 自动调阻尼：

情况	策略
\(\sigma_{\min}\) 较大	小阻尼，保证跟踪精度
\(\sigma_{\min}\) 接近 0	大阻尼，牺牲一点精度换稳定性

3）零空间目标的常见写法¶

远离关节限位：

\[ z = k(q_{\text{mid}} - q) \]

其中 \(q_{\text{mid}} = (q_{\min}+q_{\max})/2\)。

保持自然肘姿：

\[ z = k(q_{\text{home}} - q) \]

例如让肘部保持轻微弯曲，避免机械臂长时间处在完全伸直的边界奇异附近。

提高可操作性：

\[ z = k \nabla w(q) \]

其中 \(w(q)=\sqrt{\det(JJ^T)}\)。这类方法更强，但需要计算梯度，通常在仿真和调试稳定后再加入。

4）速度 IK 下发前的检查¶

每个控制周期都建议做这些检查：

\(\dot{q}\) 是否超过关节速度限制
预测的 \(q_{k+1}\) 是否超过关节位置限制
末端误差是否在变小，否则降低步长或增大阻尼
是否接近奇异点，必要时降低末端速度
是否触发碰撞距离阈值，触发后优先执行避障零空间项

五、奇异性与可操作性¶

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} \]

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