运动学¶

运动学研究不考虑力和力矩的情况下，关节变量与末端位姿之间的关系。正运动学是给定关节角求末端位姿，逆运动学则反过来。

一、DH 参数与连杆坐标系¶

1.1 为什么需要 DH 参数¶

机器人由多个连杆和关节组成，要建立从基座到末端的位姿关系，需要在每个关节处建立坐标系。Denavit-Hartenberg（DH）参数提供了一套标准化的方法来描述相邻连杆之间的几何关系。

1.2 DH 参数定义¶

在标准 DH 约定下，描述连杆 \(i-1\) 到连杆 \(i\) 之间的关系只需要 4 个参数：

参数	符号	含义
连杆长度	\(a_{i-1}\)	沿 \(x_{i-1}\) 轴，从 \(z_{i-1}\) 到 \(z_i\) 的距离
连杆扭角	\(\alpha_{i-1}\)	绕 \(x_{i-1}\) 轴，从 \(z_{i-1}\) 到 \(z_i\) 的旋转角度
连杆偏距	\(d_i\)	沿 \(z_i\) 轴，从 \(x_{i-1}\) 到 \(x_i\) 的距离
关节角	\(\theta_i\)	绕 \(z_i\) 轴，从 \(x_{i-1}\) 到 \(x_i\) 的旋转角度

哪个参数是变量？

对于转动关节（R）：\(\theta_i\) 是关节变量，其余三个是常数
对于移动关节（P）：\(d_i\) 是关节变量，其余三个是常数

1.3 建立坐标系的步骤¶

确定 \(z\) 轴：\(z_i\) 沿着关节 \(i+1\) 的运动轴
确定原点：\(z_{i-1}\) 和 \(z_i\) 的公垂线与 \(z_i\) 的交点
确定 \(x\) 轴：\(x_i\) 沿公垂线方向（从 \(z_{i-1}\) 指向 \(z_i\)）
确定 \(y\) 轴：由右手定则确定

特殊情况

当两个相邻的 \(z\) 轴平行或重合时，公垂线不唯一，\(x\) 轴的选取有一定自由度。通常选择使 \(d_i = 0\) 的方式来简化参数。

1.4 连杆变换矩阵¶

由 4 个 DH 参数可以写出从坐标系 \(\{i-1\}\) 到 \(\{i\}\) 的齐次变换矩阵：

\[ {}^{i-1}_i T = R_z(\theta_i) \cdot T_z(d_i) \cdot T_x(a_{i-1}) \cdot R_x(\alpha_{i-1}) \]

展开为：

\[ {}^{i-1}_i T = \begin{bmatrix} \cos\theta_i & -\sin\theta_i \cos\alpha_{i-1} & \sin\theta_i \sin\alpha_{i-1} & a_{i-1}\cos\theta_i \\ \sin\theta_i & \cos\theta_i \cos\alpha_{i-1} & -\cos\theta_i \sin\alpha_{i-1} & a_{i-1}\sin\theta_i \\ 0 & \sin\alpha_{i-1} & \cos\alpha_{i-1} & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix} \]

记忆技巧

这个矩阵看似复杂，但本质上就是四步变换的连乘：绕 \(z\) 转 → 沿 \(z\) 移 → 沿 \(x\) 移 → 绕 \(x\) 转。

二、正运动学¶

2.1 基本思路¶

正运动学（Forward Kinematics）：给定所有关节变量 \(q = [\theta_1, \theta_2, \ldots, \theta_n]^T\)，求末端执行器相对于基座的位姿。

\[ {}^0_n T = {}^0_1 T(\theta_1) \cdot {}^1_2 T(\theta_2) \cdot \; \cdots \; \cdot {}^{n-1}_n T(\theta_n) \]

步骤：

为每个关节建立 DH 坐标系
列出 DH 参数表
写出每个 \({}^{i-1}_i T\)
连乘得到 \({}^0_n T\)

2.2 实例：平面 2R 机械臂¶

考虑一个最简单的平面两关节机械臂：

     θ₁        θ₂
 ◎────L₁────◎────L₂────✕ (末端)
基座

第一步：建立 DH 参数表

关节 \(i\)	\(a_{i-1}\)	\(\alpha_{i-1}\)	\(d_i\)	\(\theta_i\)
1	0	0	0	\(\theta_1\)
2	\(L_1\)	0	0	\(\theta_2\)

第二步：写出各连杆变换矩阵

\[ {}^0_1 T = \begin{bmatrix} c_1 & -s_1 & 0 & 0 \\ s_1 & c_1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}, \quad {}^1_2 T = \begin{bmatrix} c_2 & -s_2 & 0 & L_1 \\ s_2 & c_2 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \]

其中 \(c_i = \cos\theta_i\)，\(s_i = \sin\theta_i\)。

第三步：连乘得到末端位姿

\[ {}^0_2 T = {}^0_1 T \cdot {}^1_2 T = \begin{bmatrix} c_{12} & -s_{12} & 0 & L_1 c_1 + L_2 c_{12} \\ s_{12} & c_{12} & 0 & L_1 s_1 + L_2 s_{12} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \]

从中提取末端位置：

\[ x = L_1 \cos\theta_1 + L_2 \cos(\theta_1 + \theta_2) \]

\[ y = L_1 \sin\theta_1 + L_2 \sin(\theta_1 + \theta_2) \]

直觉理解

第一个关节决定第一根杆的方向，第二个关节在此基础上再叠加一个转角。末端位置就是两根杆的端点叠加。

2.3 实例：6 自由度机械臂（概念）¶

对于工业 6R 机械臂，正运动学的流程完全相同，只是矩阵更大：

\[ {}^0_6 T = {}^0_1 T \cdot {}^1_2 T \cdot {}^2_3 T \cdot {}^3_4 T \cdot {}^4_5 T \cdot {}^5_6 T \]

结果矩阵中包含：

左上 3×3：末端姿态（旋转矩阵）
右上 3×1：末端位置

三、逆运动学¶

3.1 基本问题¶

逆运动学（Inverse Kinematics）：已知末端期望位姿 \({}^0_n T_{\text{desired}}\)，求关节变量 \(q = [\theta_1, \ldots, \theta_n]^T\)。

逆运动学的难点

与正运动学相比，逆运动学面临三个挑战：

非线性：方程组是超越方程，包含 sin/cos
多解：同一个末端位姿可能对应多组关节角（如人手肘可以朝上或朝下）
可能无解：目标位姿在工作空间之外

3.2 求解方法¶

解析法（Closed-form Solution）¶

通过代数推导直接得到关节变量的表达式。只适用于特定构型的机器人（如满足 Pieper 准则的 6R 机器人）。

反正切函数的使用（推荐使用 atan2）：

\[ \theta = \text{atan2}(y, x) \]

为什么用 atan2 而不是 atan？

atan2(y, x) 能区分四个象限，返回 \((-\pi, \pi]\) 范围内的角度；普通的 atan(y/x) 只能返回 \((-\pi/2, \pi/2)\)，还有除零风险。

几何法¶

利用三角形的几何关系（余弦定理等）直接求解。适合简单构型。

例：平面 2R 的逆运动学

已知末端位置 \((x, y)\)，求 \((\theta_1, \theta_2)\)。

              P(x, y)
             ╱
        L₂ ╱  
          ╱ θ₂
    ◎───╱──────
    │  L₁    
    │ θ₁     
    ◎ 基座    
        r = √(x² + y²)

求 \(\theta_2\)：三角形三边为 \(L_1, L_2, r = \sqrt{x^2 + y^2}\)，由余弦定理：

\[ r^2 = L_1^2 + L_2^2 + 2 L_1 L_2 \cos\theta_2 \]

\[ \cos\theta_2 = \frac{x^2 + y^2 - L_1^2 - L_2^2}{2 L_1 L_2} \]

\[ \theta_2 = \text{atan2}(\pm\sqrt{1 - \cos^2\theta_2}, \; \cos\theta_2) \]

\(\pm\) 对应两组解——肘部朝上或肘部朝下两种构型。

求 \(\theta_1\)：把 \(\theta_1\) 分解为 "基座到末端的方位角" 减去 "第二连杆的偏移角"：

\[ \theta_1 = \text{atan2}(y, x) - \text{atan2}(L_2 \sin\theta_2, \; L_1 + L_2 \cos\theta_2) \]

解的存在性

当 \(r > L_1 + L_2\)（伸不到）或 \(r < |L_1 - L_2|\)（够不着）时，无解
当 \(r = L_1 + L_2\) 或 \(r = |L_1 - L_2|\) 时，唯一解（奇异位形）
其他情况下，两组解

数值法¶

当解析解不存在时，可以使用迭代数值方法：

Newton-Raphson 法：利用雅可比矩阵迭代求解
梯度下降法：最小化位姿误差

\[ q_{k+1} = q_k + J^{-1}(q_k)(\mathbf{x}_{\text{desired}} - \mathbf{x}(q_k)) \]

数值法的优缺点

✅ 通用性强，适用于任意构型
❌ 需要初始值，可能收敛到局部解
❌ 在奇异点附近可能不收敛

3.3 多解的处理¶

6R 机械臂的逆运动学最多可以有 16 组解。工程上需要选择最优解，常用准则：

最近解：选择与当前关节角最接近的解（最小关节位移）
避碰：排除会导致碰撞的解
关节限位：排除超出关节可动范围的解
可操作性：选择远离奇异位形的解

四、工作空间¶

4.1 定义¶

工作空间（Workspace） 是末端执行器能够到达的空间区域。

可达工作空间：末端至少以一种姿态可以到达的位置集合
灵活工作空间：末端能以任意姿态到达的位置集合（通常远小于可达工作空间）

4.2 影响因素¶

连杆长度
关节可动范围（关节限位）
关节类型（R 或 P）
各关节轴的相对位置关系

平面 2R 的工作空间

可达工作空间是一个环形区域：

外径 = \(L_1 + L_2\)（两杆完全伸直）
内径 = \(|L_1 - L_2|\)（两杆完全折叠）

如果 \(L_1 = L_2\)，内径为 0，可达工作空间是一个完整的圆盘。

五、总结¶

概念	输入	输出	难度
正运动学	关节变量 \(q\)	末端位姿 \(T\)	简单（矩阵连乘）
逆运动学	末端位姿 \(T\)	关节变量 \(q\)	困难（非线性、多解）

graph LR
    A[关节空间<br>θ₁, θ₂, ..., θₙ] -->|正运动学<br>矩阵连乘| B[笛卡尔空间<br>x, y, z, α, β, γ]
    B -->|逆运动学<br>解析/数值| A

正运动学的核心公式：

\[ {}^0_n T = \prod_{i=1}^{n} {}^{i-1}_i T(\theta_i) \]

每个 \({}^{i-1}_i T\) 由 DH 参数唯一确定。