第六章：微分运动学 (Differential Kinematics)

约 7476 个字 4 张图片预计阅读时间 25 分钟

Part 1: 微分运动学基础

什么是微分运动学？

微分运动学（Differential Kinematics）研究的是关节速度与机器人末端执行器（end-effector）的线速度和角速度之间的映射关系。

简单来说，正向运动学告诉我们给了关节角度能算出末端在哪（位姿），而微分运动学则告诉我们给了关节转速能算出末端运动得多快（速度）。

这部分内容包括：
- 速度的表示方法
- 正向速度运动学
- 逆向速度运动学
- 正向加速度运动学与静力学
- 逆向加速度运动学

6.1 速度的表示方法

角速度 (Angular Velocity) 的表示

想象一个点 $p$ 绕着一个轴进行旋转，它的线速度 v 是什么呢？

|218

从位移到速度：
- 点 $p$ 绕轴旋转一个微小的角度 Δθ，会产生一个微小的位移 Δp。
- 从图中几何关系可知，位移 Δp 的大小为： $|\Delta \mathbf{p}| = |\Delta \theta| \cdot |\mathbf{p}| \cdot \sin \phi$。
- 这个关系可以用叉乘（cross product）来表示： $\Delta \mathbf{p} = \Delta \theta \times \mathbf{p}$。
- 速度是位移对时间的导数。当时间趋近于零，我们得到瞬时速度 v 和瞬时角速度 ω：
  $v = \dot{p} = \lim_{\Delta t \to 0} \frac{\Delta p}{\Delta t} = \lim_{\Delta t \to 0} \frac{\Delta \theta \times p}{\Delta t} = \omega \times p$
叉乘到矩阵乘法：
叉乘运算可以等效地转换成一个反对称矩阵 (skew-symmetric matrix) 的乘法。
$v = \dot{p} = \omega \times p = S(\omega)p$

其中，向量 $\omega = [\omega_x, \omega_y, \omega_z]^T$，对应的反对称矩阵 $S(\omega)$ 为：
$\omega \times \mathbf{p} = \begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix} \times \begin{bmatrix} p_x \\ p_y \\ p_z \end{bmatrix} = \begin{bmatrix} \omega_y p_z - \omega_z p_y \\ \omega_z p_x - \omega_x p_z \\ \omega_x p_y - \omega_y p_x \end{bmatrix} = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix} \begin{bmatrix} p_x \\ p_y \\ p_z \end{bmatrix}$

核心公式：角速度与线速度

$v = \dot{p} = \omega \times p = S(\omega)p$
这个公式将叉乘运算转换为了矩阵乘法，是机器人运动学中的一个基本且重要的工具。

线速度 (Linear Velocity) 的表示

假设一个点 $p$ 的坐标是在局部坐标系 1 中表示为 $^{1}\mathbf{p}$，通过旋转矩阵 $\mathbf{R}$ 转换到全局坐标系 0 中：
$^0\mathbf{p} = R \cdot ^1\mathbf{p}$

为了书写简洁，我们省略上标，记作 $\mathbf{p} = R\mathbf{p}_{local}$。我们想求点 $p$ 在全局坐标系下的线速度 $\mathbf{v}$。

$\mathbf{v} = \dot{\mathbf{p}} = \frac{d}{dt}(R\mathbf{p}_{local})$
因为 $\mathbf{p}_{local}$ 在局部坐标系中是固定的，所以它是一个常数向量。根据求导法则，我们得到：
$\mathbf{v} = \dot{R}\mathbf{p}_{local}$

将 $\mathbf{p}_{local} = R^{-1}\mathbf{p} = R^T\mathbf{p}$ (因为旋转矩阵是正交阵, $R^{-1} = R^T$) 代入上式：
$\mathbf{v} = \dot{R}R^T\mathbf{p}$

这里我们引入一个重要的性质：
因为 $RR^T = I$ (单位矩阵)，对其两边求导：
$\frac{d}{dt}(RR^T) = \dot{R}R^T + R\dot{R}^T = 0$
所以，$\dot{R}R^T = -(R\dot{R}^T)^T = -(\dot{R}R^T)^T$
这证明了 $\dot{R}R^T$ 是一个反对称矩阵。我们可以将其记为 $S(\omega)$。

核心公式：旋转与速度

令 $S(\omega) = \dot{R}R^T$，我们得到：
$\mathbf{v} = S(\omega)\mathbf{p}$
这与我们之前通过叉乘得到的结论完全一致！同时，我们还得到了旋转矩阵导数的重要关系：
$\dot{R} = S(\omega)R$

反对称矩阵 $S(\omega)$ 的性质

定义：一个 $3 \times 1$ 的向量 $\omega$ 可以通过 ~ 运算符转换为一个 $3 \times 3$ 的反对称矩阵 $S(\omega)$。
$\tilde{\omega} = S(\omega) = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix}$
逆运算：一个反对称矩阵可以通过 V 运算符恢复成原来的向量。
$\omega = S^\vee(\omega)$
常用性质：
1. $S(\omega) = -S^T(\omega)$ (反对称)
2. $\dot{R} = S(\omega)R$ (旋转矩阵的导数)
3. $RS(\omega)R^T = S(R\omega)$ (坐标系变换性质)

角速度的其他表示方法

角轴表示 (Angle-Axis)
角速度可以分解为一个标量角速率 $\omega$ (magnitude) 和一个单位向量旋转轴 $\mathbf{u}$ (direction)。
$\omega = \omega \mathbf{u}$
其中，$\omega = \sqrt{\omega_x^2 + \omega_y^2 + \omega_z^2}$，$\mathbf{u} = \frac{\omega}{\omega}$。
欧拉角表示 (Euler Angles)
如果一个姿态是由XYZ定轴欧拉角 $(\alpha, \beta, \gamma)$ 描述的，即 $R = R_z(\gamma)R_y(\beta)R_x(\alpha)$，那么全局角速度 $\omega$ 可以通过对旋转矩阵求导得到 $S(\omega) = \dot{R}R^T$。
最终可以得到 $\omega$ 和欧拉角速率 $(\dot{\alpha}, \dot{\beta}, \dot{\gamma})$ 的映射关系：
【请在此处插入第 8 页的图片】
$\omega = \begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix} = \begin{bmatrix} \dot{\alpha}c\beta c\gamma - \dot{\beta}s\gamma \\ \dot{\beta}c\gamma + \dot{\alpha}c\beta s\gamma \\ \dot{\gamma} - \dot{\alpha}s\beta \end{bmatrix} = \begin{bmatrix} c\beta c\gamma & -s\gamma & 0 \\ c\beta s\gamma & c\gamma & 0 \\ -s\beta & 0 & 1 \end{bmatrix} \begin{bmatrix} \dot{\alpha} \\ \dot{\beta} \\ \dot{\gamma} \end{bmatrix}$
四元数表示 (Quaternion)
使用单位四元数 $e = e_0 + \mathbf{e}$ 表示旋转，点 $p$ 的旋转可以表示为：
$^0p = e(t) \cdot {^1p} \cdot e^*(t)$
其中 $e^*$ 是共轭四元数。
对时间求导，可以推导出线速度 v 与四元数导数 $\dot{e}$ 的关系：
$\mathbf{v} = \dot{\mathbf{p}} = 2\dot{e}e^*\mathbf{p}$
对比 $\mathbf{v} = S(\omega)\mathbf{p}$，我们可以得到角速度的四元数表示：
$\tilde{\omega}^0 = \omega \triangleq 2\dot{e}e^*$
$\tilde{\omega}^1 = \omega \triangleq 2e^*\dot{e}$
经过一系列推导（如幻灯片10-11页所示），可以得到角速度 $\omega$ 和四元数导数 $\dot{e}$ 之间的线性关系：
$\omega = 2Q(e)\dot{e} \quad \implies \quad \dot{e} = \frac{1}{2}Q^{-1}(e)\omega = \frac{1}{2}Q^T(e)\omega$

旋转矩阵的时间导数 (Rodrigues' Equation)

我们有一个非常重要的微分方程：$\dot{R} = S(\omega)R$。
给定初始条件 $R(0)=I$，这个方程的解是：
$R(t) = e^{S(\omega)t}$
这个矩阵指数函数可以展开成泰勒级数。如果角速度 $\omega = \omega \mathbf{u}$ 是恒定的，旋转角度为 $\phi = \omega t$，那么这个解可以简化为著名的罗德里格斯公式 (Rodrigues' Equation)：

$R(\phi) = e^{\tilde{\mathbf{u}}\phi} = I + \tilde{\mathbf{u}}\sin\phi + \tilde{\mathbf{u}}^2(1-\cos\phi)$
或者写成更常见的形式：
$^0_1R = I\cos\phi + \tilde{\mathbf{u}}\sin\phi + \mathbf{u}\mathbf{u}^T(1-\cos\phi)$

6.2 正向速度运动学 (Forward Velocity Kinematics)

什么是正向速度运动学？

它是指在已知机械臂各个关节变量的变化率（即关节速度 $\dot{q}_1, ..., \dot{q}_n$）的情况下，求解末端执行器在笛卡尔空间中的速度（线速度 $\mathbf{v}_n$ 和角速度 $\omega_n$）。

输入：关节速度 $\dot{q}$
输出：末端速度 $\mathbf{v}_n, \omega_n$

【请在此处插入第 14 页的图片】

刚体连杆的速度分析

1. 角速度的传递

【请在此处插入第 15 页的图片】

一个基本物理结论：连杆B的绝对角速度，等于其父连杆A的绝对角速度，加上B相对于A的相对角速度。
$\omega_B^G = \omega_A^G + \omega_B^A$
($G$ 代表全局坐标系 Global Frame)

对于一个由多个连杆组成的机械臂，这个规律可以迭代使用。连杆 i 的绝对角速度 $\omega_i$ 等于前一个连杆 i-1 的绝对角速度 $\omega_{i-1}$ 加上连杆 i 相对于 i-1 的相对角速度。
$\omega_i = \omega_{i-1} + \text{relative angular velocity}$

根据D-H约定，连杆 i 绕着 $z_{i-1}$ 轴旋转，所以其相对角速度为 $\dot{\theta}_i \mathbf{z}_{i-1}$。
$\omega_i = \omega_{i-1} + \dot{\theta}_i \mathbf{z}_{i-1}$

角速度递推公式

$\omega_i = \omega_{i-1} + \omega_i^{i-1}$

$\omega_i$ : 连杆 i 在全局坐标系下的角速度
$\omega_{i-1}$ : 连杆 i-1 在全局坐标系下的角速度
$\omega_i^{i-1}$ : 连杆 i 相对于连杆 i-1 的角速度

2. 线速度的传递

【请在此处插入第 17 页的图片】

连杆B上某一点的绝对线速度，等于父连杆A上对应点的绝对线速度，加上B相对于A的相对线速度，再加上由B自身旋转引起的线速度。

对于机械臂连杆 i 的原点 $o_i$，其全局线速度 $\mathbf{p}_i$ 为：
$\dot{\mathbf{p}}_i = \dot{\mathbf{p}}_{i-1} + \mathbf{v}_i^{i-1} + \omega_i \times \mathbf{r}_i^{i-1}$

$\dot{\mathbf{p}}_{i-1}$: 父连杆原点 $o_{i-1}$ 的线速度。
$\mathbf{v}_i^{i-1}$: $o_i$ 相对于 $o_{i-1}$ 的相对平移速度。
$\omega_i \times \mathbf{r}_i^{i-1}$: 由连杆 i 的绝对旋转引起的 $o_i$ 的线速度。

区分转动关节与移动关节

根据D-H约定，我们可以将上述通用公式具体化：
* 转动关节 (Revolute Joint): 只有转动 $\dot{\theta}_i$，没有相对平移。
* 相对角速度: $\omega_i^{i-1} = \dot{\theta}_i \mathbf{z}_{i-1}$
* 相对线速度: $\mathbf{v}_i^{i-1} = 0$
* 移动关节 (Prismatic Joint): 只有沿 $z_{i-1}$ 轴的移动 $\dot{d}_i$，没有相对转动。
* 相对角速度: $\omega_i^{i-1} = 0$
* 相对线速度: $\mathbf{v}_i^{i-1} = \dot{d}_i \mathbf{z}_{i-1}$

为了统一公式，我们引入指示变量 $\sigma_i$ 和 $\bar{\sigma}_i$:
* 转动关节: $\sigma_i = 0, \bar{\sigma}_i = 1$
* 移动关节: $\sigma_i = 1, \bar{\sigma}_i = 0$
* 关节变量变化率: $\dot{q}_i = \bar{\sigma}_i \dot{\theta}_i + \sigma_i \dot{d}_i$

最终递推公式：

速度递推公式

角速度:
$\omega_i = \omega_{i-1} + \bar{\sigma}_i \dot{q}_i \mathbf{z}_{i-1}$
线速度:
$\mathbf{v}_i = \mathbf{v}_{i-1} + \sigma_i \dot{q}_i \mathbf{z}_{i-1} + \omega_i \times \mathbf{r}_i^{i-1}$

其中 $\mathbf{v}_i = \dot{\mathbf{p}}_i$。基座的速度为零：$\omega_0 = 0, \mathbf{v}_0 = 0$。通过这个公式，我们可以从基座开始，一步步计算出末端执行器的速度。

计算优化

上述公式中的向量 $\mathbf{z}_{i-1}$ 和 $\mathbf{r}_i^{i-1}$ 都是在全局坐标系下表示的，每次计算都需要进行坐标变换，效率不高。
一个更高效的方法是在局部坐标系中进行计算。根据D-H约定，在连杆 i-1 坐标系中，$z$ 轴的向量总是 $^T$。
我们可以将递推公式变换到连杆 i 的坐标系中进行计算（如幻灯片20页推导），得到：
$\hat{\omega}_i = {^i}R_{i-1}(\hat{\omega}_{i-1} + \bar{\sigma}_i \dot{q}_i \mathbf{z})$
$\hat{\mathbf{v}}_i = {^i}R_{i-1}(\hat{\mathbf{v}}_{i-1} + \sigma_i \dot{q}_i \mathbf{z}) + \hat{\omega}_i \times {^i}\mathbf{r}_i^{i-1}$
（注：这里的 $\hat{\omega}_i$ 表示在连杆i坐标系下的角速度，$\mathbf{z} =^T$）

例题 6-2-1: SCARA 机器人正向速度运动学

【请在此处插入第 21 页的图片】

问题: 已知 SCARA 机器人的关节速度和当前关节角度，求末端执行器的速度。
参数:

连杆长度: $L_1=L_2=d=1$
关节速度: $[\dot{\theta}_1, \dot{\theta}_2, \dot{d}_3, \dot{\theta}_4] = [1, 1, -0.5, 0.1]$
关节位置: $[\theta_1, \theta_2, d_3, \theta_4] = [\pi/4, \pi/2, 0, 0.2]$ (注：PPT图示为$\theta_3, d_4$，但根据SCARA结构和变量名应为$d_3, \theta_4$)

解法: 使用在局部坐标系中递推的公式 $\hat{\omega}_i = {^i}R_{i-1}(\hat{\omega}_{i-1} + \bar{\sigma}_i \dot{q}_i \mathbf{z})$ 和 $\hat{\mathbf{v}}_i = {^i}R_{i-1}(\hat{\mathbf{v}}_{i-1} + \sigma_i \dot{q}_i \mathbf{z}) + \hat{\omega}_i \times {^i}\mathbf{r}_i^{i-1}$ 进行迭代计算。

初始化: $\hat{\omega}_0=0, \hat{\mathbf{v}}_0=0$
计算关节1: $\hat{\omega}_1, \hat{\mathbf{v}}_1$
计算关节2: $\hat{\omega}_2, \hat{\mathbf{v}}_2$
计算关节3 (移动): $\hat{\omega}_3, \hat{\mathbf{v}}_3$
计算关节4: $\hat{\omega}_4, \hat{\mathbf{v}}_4$

最终得到在连杆4坐标系下的速度，再通过旋转矩阵 $^0R_4$ 转换回全局坐标系，即可得到末端速度。PPT上直接给出了在全局坐标系下的递推结果，这需要预先计算出每个$z_{i-1}$在全局的表示。

通过齐次变换矩阵的导数求解

另一种方法是直接对整个机器人的齐次变换矩阵 $^0_n T$ 求导。
$^0_n T = {^0_1 T}(q_1) \cdot {^1_2 T}(q_2) \cdots {^{n-1}_n T}(q_n)$

对时间求导，根据链式法则，结果比较复杂。一个更实用的方法是使用迭代公式。
令 $^0_i \dot{T}$ 为从基座到连杆 i 的齐次变换矩阵的导数。
$^0_i \dot{T} = \frac{d}{dt}(^0_{i-1}T \cdot {^{i-1}_i T}) = {^0_{i-1}\dot{T}} \cdot {^{i-1}_i T} + {^0_{i-1}T} \cdot {^{i-1}_i \dot{T}}$

其中 ${^{i-1}_i \dot{T}} = \frac{d}{dt}{^{i-1}_i T}(q_i) = \frac{\partial {^{i-1}_i T}}{\partial q_i} \dot{q}_i = Q_i {^{i-1}_i T} \dot{q}_i$。
矩阵 $Q_i$ 对于转动关节和移动关节有固定的形式。
* 转动关节: $Q_i = \begin{bmatrix} 0 & -1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix}$
* 移动关节: $Q_i = \begin{bmatrix} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \end{bmatrix}$

齐次矩阵导数递推公式

$^0_i \dot{T} = {^0_{i-1}\dot{T}} \cdot {^{i-1}_i T} + {^0_{i-1}T} Q_i {^{i-1}_i T} \dot{q}_i$

简化后得到一个更高效的迭代形式：
$^0_i \dot{T} = (^0_{i-1}\dot{T} + \dot{q}_i {^0_{i-1}T} Q_i) {^{i-1}_i T}$

初始条件: $^0_0 \dot{T} = 0, ^0_0 T = I$

当计算出最终的 $^0_n \dot{T}$ 后，末端的线速度和角速度可以从中提取出来：
$^0_n \dot{T} = \begin{bmatrix} \dot{R} & \dot{p} \\ 0 & 0 \end{bmatrix} = \begin{bmatrix} S(\omega)R & \mathbf{v} \\ 0 & 0 \end{bmatrix}$
* 线速度: $\mathbf{v} = \dot{p}$ (第四列的前三个元素)
* 角速度: 从 $\dot{R}R^T = S(\omega)$ 中求解 $\omega$。

例题 6-2-2: SCARA 机器人 (方法二)

【请在此处插入第 25-26 页的图片】

问题: 与例 6-2-1 相同，但使用齐次矩阵求导的方法。
解法:

建立D-H表并写出 $Q_i$ 矩阵: 根据关节类型确定每个 $Q_i$。
写出 $^0_i T$: 计算出每个关节位置下的齐次变换矩阵。
迭代计算 $^0_i \dot{T}$:
- $i=1: ^0_1 \dot{T} = (\dot{q}_1 {^0_0 T} Q_1) {^0_1 T}$
- $i=2: ^0_2 \dot{T} = (^0_1 \dot{T} + \dot{q}_2 {^0_1 T} Q_2) {^1_2 T}$
- ... 直到 $i=4$
从 $^0_4 \dot{T}$ 中提取速度:
- $\dot{p}_4$ 就是 $^0_4 \dot{T}$ 的第四列，即线速度 $\mathbf{v}$。
- $\dot{R}_4$ 是 $^0_4 \dot{T}$ 左上角的 $3 \times 3$ 部分。
- 计算 $S(\omega) = \dot{R}_4 R_4^T$，然后从 $S(\omega)$ 中提取角速度 $\omega$。

Part 2: 雅可比矩阵 (Jacobian Matrix)

6.3 雅可比矩阵

什么是雅可比矩阵？

雅可比矩阵 J 是一个描述输入变化率和输出变化率之间线性关系的矩阵。在机器人学中，它直接建立了关节速度 $\dot{q}$ 和末端执行器笛卡尔速度 $\dot{X}$ 之间的映射。
$\dot{X} = J(q)\dot{q}$

其中 $\dot{X} = [\mathbf{v}^T, \omega^T]^T$ 是一个 $6 \times 1$ 的向量，包含了线速度和角速度。

雅可比矩阵的重要性

雅可比矩阵是机器人学中最重要的工具之一，因为它可用于：
* 奇异点分析 (Singularities): 判断机器人何时会失去某个方向的运动能力。
* 冗余度分析 (Redundancy): 分析具有多余自由度的机器人。
* 逆运动学算法 (Inverse kinematics): 求解逆向速度问题。
* 静力学分析 (Statics): 建立末端受力与关节力矩之间的关系。

雅可比矩阵的定义

如果末端位姿 $X = T(q)$ 是关节变量 $q$ 的函数，那么末端速度 $\dot{X}$ 就是：
$\dot{X} = \frac{d}{dt}T(q) = \frac{\partial T(q)}{\partial q} \frac{dq}{dt} = \frac{\partial T(q)}{\partial q} \dot{q}$

这里的偏导数矩阵就是雅可比矩阵：
$\(J(q) = \frac{\partial T(q)}{\partial q} = \begin{bmatrix} \frac{\partial T_1}{\partial q_1} & \cdots & \frac{\partial T_1}{\partial q_n} \\ \vdots & \ddots & \vdots \\ \frac{\partial T_6}{\partial q_1} & \cdots & \frac{\partial T_6}{\partial q_n} \end{bmatrix} = \begin{bmatrix}J_{11} & J_{12} & \cdots & J_{1n} \\J_{21} & J_{22} & \cdots & J_{2n} \\ \vdots & \vdots & \ddots & \vdots \\J_{61} & J_{62} & \cdots & J_{6n}\end{bmatrix} = [J_1, J_2, \cdots, J_n]$\)
矩阵的第 i 列 $J_i$ 代表了只有第 i 个关节以单位速度运动时，对末端速度的贡献。

|450

末端执行器的速度，和每个关节的速度线性相关

几何雅可比 vs 解析雅可比

几何雅可比 (Geometrical Jacobian): 直接从几何关系推导，将关节运动分解为对末端线速度和角速度的贡献。这是我们下面主要讨论的。
解析雅可比 (Analytical Jacobian): （基于欧拉角的）通过对正运动学方程直接求偏导数得到。

雅可比矩阵与坐标系

核心要点

雅可比矩阵的值不是唯一的，它取决于我们选择在哪个坐标系下表示末端执行器的速度。

如果我们想把在全局坐标系（默认）下表示的雅可比矩阵 J，转换到另一个坐标系 u 下表示，我们需要知道从全局坐标系到坐标系 u 的旋转矩阵 R。

假设：
* 全局坐标系下的线速度和角速度为 $\mathbf{v}$ 和 $\omega$。
* 坐标系 u 下的线速度和角速度为 $^u\mathbf{v}$ 和 $^u\omega$。

向量的坐标变换法则是：
* $^u\mathbf{v} = R\mathbf{v}$
* $^u\omega = R\omega$

我们可以将完整的末端速度向量 $\dot{X} = [\mathbf{v}^T, \omega^T]^T$ 进行变换：
$\begin{bmatrix} ^u\mathbf{v} \\ ^u\omega \end{bmatrix} = \begin{bmatrix} R & 0 \\ 0 & R \end{bmatrix} \begin{bmatrix} \mathbf{v} \\ \omega \end{bmatrix}$

因为 $\dot{X} = J\dot{q}$，那么在 u 坐标系下就是 $^u\dot{X} = {^uJ}\dot{q}$。将关系代入上式：
${^uJ}\dot{q} = \begin{bmatrix} R & 0 \\ 0 & R \end{bmatrix} (J\dot{q})$

由于这个等式对任意关节速度 $\dot{q}$ 都成立，我们可以得到雅可比矩阵的变换公式：

雅可比矩阵的坐标变换公式

$^uJ = \begin{bmatrix} R & 0 \\ 0 & R \end{bmatrix} J$

这个公式告诉我们，如何将一个在基座坐标系下计算出的雅可比矩阵，转换到另一个坐标系（例如，手部坐标系）下。

例题 6-3-1: 2R 平面臂的线速度

|184

问题: 求解一个 2R 平面机械臂末端点的线速度。
这是一个典型的通过解析法（直接对正运动学方程求导）求解雅可比矩阵的例子。

步骤 1: 写出正运动学方程
首先，我们写出末端点 (x, y) 的坐标与关节角 θ₁ 和 θ₂ 之间的关系：
$x = l_1 \cos(\theta_1) + l_2 \cos(\theta_1 + \theta_2) = T_1(\theta_1, \theta_2)$
$y = l_1 \sin(\theta_1) + l_2 \sin(\theta_1 + \theta_2) = T_2(\theta_1, \theta_2)$

步骤 2: 对时间求导，得到速度关系
末端的速度 $(\dot{x}, \dot{y})$ 和关节角速度 $(\dot{\theta}_1, \dot{\theta}_2)$ 的关系可以通过链式法则得到：
$\begin{bmatrix} \dot{x} \\ \dot{y} \end{bmatrix} = J \begin{bmatrix} \dot{\theta}_1 \\ \dot{\theta}_2 \end{bmatrix}$

这里的 J 就是我们要求的雅可比矩阵。根据定义，$J$ 是由位置对角度的偏导数组成的：
$J = \begin{bmatrix} \frac{\partial x}{\partial \theta_1} & \frac{\partial x}{\partial \theta_2} \\ \frac{\partial y}{\partial \theta_1} & \frac{\partial y}{\partial \theta_2} \end{bmatrix}$

步骤 3: 计算偏导数

$\frac{\partial x}{\partial \theta_1} = -l_1 \sin(\theta_1) - l_2 \sin(\theta_1 + \theta_2)$
$\frac{\partial x}{\partial \theta_2} = -l_2 \sin(\theta_1 + \theta_2)$
$\frac{\partial y}{\partial \theta_1} = l_1 \cos(\theta_1) + l_2 \cos(\theta_1 + \theta_2)$
$\frac{\partial y}{\partial \theta_2} = l_2 \cos(\theta_1 + \theta_2)$

步骤 4: 组合得到雅可比矩阵
将计算出的偏导数填入矩阵，我们就得到了这个 2R 臂的雅可比矩阵：
$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_{12} = \cos(\theta_1 + \theta_2)$ 是简写)

这样，只要我们知道了任意时刻的关节角度 $(\theta_1, \theta_2)$ 和关节速度 $(\dot{\theta}_1, \dot{\theta}_2)$，就可以通过 $\dot{X}=J\dot{q}$ 计算出末端的笛卡尔速度。

几何雅可比的推导

我们可以将雅可比矩阵分解为两部分：
$J = \begin{bmatrix} J_D \\ J_R \end{bmatrix}$
* $J_D=[J_{D1} \dots J_{Dn}]^T$: 位移雅可比 (Displacement Jacobian)，关联线性速度 $\mathbf{v} = \begin{bmatrix} \dot{X} \\ \dot{Y} \\ \dot{Z} \end{bmatrix} = J_D \dot{q}$
* $J_R = [J_{R1} \cdots J_{Rn}]^T$: 旋转雅可比 (Rotation Jacobian)，关联角速度 $\omega = \begin{bmatrix} \omega_X \\ \omega_Y \\ \omega_Z \end{bmatrix} = J_R \dot{q}$

Info

$\bar{\sigma}_i$ 和 $\sigma_i$ 是一对指示变量 (indicator variables) 或者叫 开关变量 (switch variables):

如果关节 i 是 转动关节 (Revolute)：
- $\bar{\sigma}_i = 1$
- $\sigma_i = 0$
如果关节 i 是 移动关节 (Prismatic)：
- $\bar{\sigma}_i = 0$
- $\sigma_i = 1$

对末端执行器线速度的贡献（使用D-H）：

|169
$$
\mathbf{v}n = J_D \dot{\mathbf{q}} = J}\dot{q1 + \dots + J}\dot{qn = \sum_i)}^n J_{Di}\dot{q}_i \quad (\text{注意: } \dot{q}_i = \bar{\sigma}_i \dot{\theta}_i + \sigma_i \dot{d
$$
- 求和中的每一项代表了当其他关节都静止时，单个关节 i 的速度对末端执行器线速度的贡献。

如果关节 i 是移动关节 (prismatic joint)，即 $\dot{q}_i = \sigma_i \dot{d}_i$，关节 i 的贡献是：
$$
J_{Di}\dot{q}i = \mathbf{z}}\dot{di = \sigma_i \mathbf{z}}\dot{qi \quad \implies \quad J} = \sigma_i \mathbf{z}_{i-1
$$
如果关节 i 是转动关节 (revolute joint)，即 $\dot{q}_i = \bar{\sigma}_i \dot{\theta}_i$，对线速度的贡献是：
$$
J_{Di}\dot{q}i = \omega_i^{i-1} \times \mathbf{r}_i = \bar{\sigma}_i \mathbf{z}}\dot{\thetai \times (\mathbf{p}_n - \mathbf{p}}) \quad \implies \quad J_{Di} = \bar{\sigmai \mathbf{z}} \times (\mathbf{pn - \mathbf{p})
$$

因此，
$$
J_{Di} = \bar{\sigma}i \mathbf{z}} \times (\mathbf{pn - \mathbf{p}}) + \sigma_i \mathbf{z}_{i-1
$$

对末端执行器n的角速度的贡献（使用D-H约定）：

\[ \omega_n = \sum_{i=1}^n \omega_i^{i-1} = \sum_{i=1}^n J_{Ri}\dot{q}_i \]

如果关节 i 是移动关节 (prismatic)，关节 i 的贡献是：
$$
J_{Ri}\dot{q}i = 0 \quad \implies \quad J = 0
$$
如果关节 i 是转动关节 (revolute)，对角速度的贡献是：
$$
J_{Ri}\dot{q}i = \omega_i^{i-1} = \mathbf{z}}\dot{\thetai = \bar{\sigma}_i \mathbf{z}}\dot{qi \quad \implies \quad J} = \bar{\sigmai \mathbf{z}
$$

因此，
$$
J_{Ri} = \bar{\sigma}i \mathbf{z}
$$

总结
$$
J_i = \begin{bmatrix} J_{Di} \ J_{Ri} \end{bmatrix} = \bar{\sigma}i \begin{bmatrix} \mathbf{z}} \times (\mathbf{pn - \mathbf{p}}) \ \mathbf{z{i-1} \end{bmatrix} + \sigma_i \begin{bmatrix} \mathbf{z}} \ \mathbf{0} \end{bmatrix
$$

几何雅可比的列向量公式

将以上两种情况用指示变量统一起来，得到雅可比矩阵第 i 列的计算公式：
$J_i = \begin{bmatrix} J_{Di} \\ J_{Ri} \end{bmatrix} = \bar{\sigma}_i \begin{bmatrix} z_{i-1} \times (p_n - p_{i-1}) \\ z_{i-1} \end{bmatrix} + \sigma_i \begin{bmatrix} z_{i-1} \\ 0 \end{bmatrix}$

转动关节 ($\bar{\sigma}_i=1, \sigma_i=0$): $J_i = \begin{bmatrix} z_{i-1} \times (p_n - p_{i-1}) \\ z_{i-1} \end{bmatrix}$
移动关节 ($\bar{\sigma}_i=0, \sigma_i=1$): $J_i = \begin{bmatrix} z_{i-1} \\ 0 \end{bmatrix}$

如何计算 $z_{i-1}$ 和 $p_{i-1}$ ?
这些向量都是在全局坐标系下表示的。它们可以从齐次变换矩阵 $^0_{i-1}T$ 中轻松获得：
* $z_{i-1}$ 是 $^0_{i-1}T$ 中旋转部分 $^0_{i-1}R$ 的第三列。
* $p_{i-1}$ 是 $^0_{i-1}T$ 中平移部分的第四列。
* $p_n$ 是末端执行器的位置向量，可从 $^0_n T$ 获得。

例题 6-3-2: 3R 平面臂的雅可比矩阵

【请在此处插入第 14-15 页的图片】

问题: 求 3R 平面臂的雅可比矩阵。
方法1: 解析法 (直接求导)

写出正运动学方程: $x = l_1c_1 + l_2c_{12} + l_3c_{123}$，$y = l_1s_1 + l_2s_{12} + l_3s_{123}$。
对 $x, y$ 分别求关于 $\theta_1, \theta_2, \theta_3$ 的偏导数，填入 $J_D$ 矩阵。
角速度 $\omega_z = \dot{\theta}_1 + \dot{\theta}_2 + \dot{\theta}_3$。所以 $J_R =$。

方法2: 几何法

所有关节都是转动关节。公式为 $J_i = [z_{i-1} \times (p_n - p_{i-1}); z_{i-1}]$。
对于平面臂，$z_0 = z_1 = z_2 =^T$。
计算出每个连杆末端的位置向量 $p_0, p_1, p_2$ 以及末端点位置 $p_n=p_3$。
依次计算 $J_1, J_2, J_3$ 并组合成最终的雅可比矩阵。
两种方法得到的结果应该是一致的。

6.4 运动学奇异点 (Kinematic Singularities)

什么是奇异点？

奇异点是指机器人在某些特定的关节构型下，其雅可比矩阵 J 会降秩 (rank-deficient)。
在奇异点处，机器人会失去一个或多个方向的运动能力。

奇异点的后果

运动能力降低: 无法实现末端在任意方向的运动。
逆运动学无解: 在奇异点附近，$\dot{q} = J^{-1}\dot{X}$ 会出现问题，因为 $J$ 的逆不存在或接近不存在。
关节速度无限大: 为了在奇异方向上产生一个很小的末端速度，可能需要无限大的关节速度，这在物理上是不可能的，并且可能损坏机器人。

如何找到奇异点？

数学上，当雅可比矩阵的行列式为零时，该构型就是奇异的。
$det(J) = 0$
对于非方阵的雅可比矩阵，可以通过 $det(JJ^T) = 0$ 来判断。

例题 6-4-1: 2R 平面臂的奇异点

【请在此处插入第 22 页的图片】

写出 2R 臂的雅可比矩阵 $J$。
计算其行列式: $det(J) = l_1l_2s_2$。
令 $det(J) = 0$，得到 $l_1l_2\sin(\theta_2) = 0$。
解得 $\theta_2 = 0$ 或 $\theta_2 = k\pi$。

物理意义: 当 $\theta_2=0$ 时，两个连杆完全伸直；当 $\theta_2=\pi$ 时，两个连杆完全折叠。在这两种情况下，机器人都无法在径向方向上移动，失去了该方向的自由度。

奇异点的类型

【请在此处插入第 23 页的图片】

边界奇异点 (Boundary Singularities): 发生在机器人工作空间的边界处，通常是机械臂完全伸展或完全收缩时。
内部奇异点 (Internal Singularities): 发生在工作空间内部。通常是由于两个或多个关节轴线重合造成的。

奇异点解耦

对于带有球腕 (spherical wrist) 的常见机器人（即末端三个旋转关节的轴线交于一点），可以把奇异点问题分解为两个独立的问题：
* 臂部奇异点 (Arm Singularities)
* 腕部奇异点 (Wrist Singularities)

此时雅可比矩阵可以分块，且 $det(J) = det(J_{11})det(J_{22})$。我们只需分别让 $det(J_{11})=0$ 和 $det(J_{22})=0$ 即可找到所有奇异点。

腕部奇异点: 通常发生在 $\theta_5 = 0$ 或 $\pi$ 时，此时关节4和关节6的轴线重合，失去了一个旋转自由度（万向节锁 gimbal lock）。
臂部奇异点:
- 肘部奇异 (Elbow singularity): 类似于2R臂的奇异，发生在 $\theta_3 = 0$ 或 $\pi$。
- 肩部奇异 (Shoulder singularity): 腕部中心点落在了基座的 $z_0$ 轴上。

6.5 逆向速度运动学 (Inverse Velocity Kinematics)

什么是逆向速度运动学？

它是指在已知机器人末端执行器期望的笛卡尔速度（$\mathbf{v}_n, \omega_n$）的情况下，反求出为实现该速度所需要的各个关节速度（$\dot{q}_1, ..., \dot{q}_n$）。

输入: 末端速度 $\dot{X}$
输出: 关节速度 $\dot{q}$

求解方法

从 $\dot{X} = J\dot{q}$ 出发，我们可以得到：
$\dot{q} = J^{-1}(q)\dot{X}$

求解步骤:
1. 计算雅可比矩阵 J: 根据机器人当前的关节构型 q 计算出雅可比矩阵。
2. 求逆: 计算 $J^{-1}$。
3. 相乘: 将期望的末端速度 $\dot{X}$ 与 $J^{-1}$ 相乘，得到所需的关节速度 $\dot{q}$。

轨迹跟踪

如果我们知道一个期望的末端运动轨迹 $X(t)$，我们可以通过逆向速度运动学来规划关节空间的轨迹 $q(t)$。
$\dot{q}(t) = J^{-1}(q(t))\dot{X}(t)$
然后通过数值积分来更新关节位置：
$q(t_{k+1}) \approx q(t_k) + \dot{q}(t_k)\Delta t$

不同情况的解

根据雅可比矩阵的维度 m x n（通常m=6，n是关节数），解的情况不同：

情况1: $m=n$ (非冗余机器人)
如果机器人是非奇异的（$J$ 可逆），则有唯一的解：$\dot{q} = J^{-1}\dot{X}$。
情况2: $m > n$ (欠驱动机器人)
关节数少于任务空间的自由度。通常无解。只能求一个最小二乘解来最小化末端速度误差。
情况3: $m < n$ (冗余机器人)
关节数多于任务空间的自由度。有无穷多组解。可以利用冗余的自由度来优化某些性能，例如避开障碍物、避免奇异点或最小化关节运动能量。
此时需要使用伪逆 (pseudo-inverse) 来求解：
$\dot{q} = J^\dagger \dot{X} = J^T(JJ^T)^{-1}\dot{X}$

例题 6-5-1: 关节型机械臂轨迹规划

【请在此处插入第 33-36 页的图片】

问题: 规划一个关节型机械臂的末端从 P1 移动到 P2，并确定关节空间的速度。
解法:

笛卡尔空间轨迹规划: 首先规划末端点在直线上的速度 $V(t)$ 和位置 $P(t)$。
求解关节空间轨迹:
- 方法1 (逆运动学): 对轨迹上的每个点 $P(t)$，使用逆运动学公式直接求解出关节角度 $\theta(t)$。然后对 $\theta(t)$ 求导得到关节速度 $\dot{\theta}(t)$。这种方法计算量大且可能不连续。
- 方法2 (雅可比逆): 这是更常用的方法。
  a. 从 $P(t)$ 求导得到末端速度 $V(t)$。
  b. 使用迭代方法：在每个时间步 $t_k$:
  i. 获取当前关节角 $\theta(t_k)$。
  ii. 计算当前构型下的雅可比矩阵 $J(\theta(t_k))$。
  iii. 计算关节速度 $\dot{\theta}(t_k) = J^{-1}(\theta(t_k))V(t_k)$。
  iv. 更新关节角度 $\theta(t_{k+1}) = \theta(t_k) + \dot{\theta}(t_k)\Delta t$。

其中，计算雅可比矩阵 $J$ 的步骤，就是应用我们之前推导的几何雅可比公式。 (如幻灯片第36页所示)。