运动学¶

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

一、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 工程实操：7 自由度仿人机械臂求 IK¶

以仿人右臂为例，常见关节可以抽象为：

关节	典型含义	作用
\(q_1,q_2,q_3\)	肩部 yaw / pitch / roll	决定上臂朝向
\(q_4\)	肘部 pitch	决定伸屈
\(q_5,q_6,q_7\)	腕部 yaw / pitch / roll	调整末端姿态

末端位姿任务是 6 维，而关节数是 7，因此它是冗余机械臂：同一个末端位姿通常有无穷多组 \(q\)。工程上一般不追求一个“唯一解析解”，而是用数值 IK，在完成末端任务的同时，用多出的 1 个自由度去远离关节限位、避开奇异位形或保持自然肘姿。

1）项目输入要准备什么¶

做 IK 前先整理这些数据：

输入	说明
机器人模型	URDF / DH 参数 / CAD 导出的连杆长度与坐标系
关节限位	每个关节的 \(q_{\min}, q_{\max}\)，最好还有速度、加速度限幅
当前关节角	\(q_{\text{cur}}\)，作为迭代初值，能保证解尽量连续
目标位姿	\({}^0T_d = (R_d, p_d)\)，来自规划器、视觉或遥操作
任务权重	位置和姿态的权重，例如抓取时姿态很重要，指向任务中姿态可放松

2）定义位姿误差¶

正运动学得到当前末端位姿 \({}^0T(q) = (R(q), p(q))\) 后，位置误差直接写成：

\[ e_p = p_d - p(q) \]

姿态误差常用旋转矩阵的对数映射：

\[ e_R = \text{Log}(R(q)^T R_d) \]

于是 6 维任务误差为：

\[ e = \begin{bmatrix} w_p e_p \\ w_R e_R \end{bmatrix} \]

其中 \(w_p,w_R\) 用来调节“位置优先”还是“姿态优先”。

姿态误差不要直接减欧拉角

欧拉角有万向节死锁和角度跳变问题。实际代码中优先用旋转矩阵对数映射或四元数误差，避免 \(179^\circ \to -179^\circ\) 这种跳变。

3）用阻尼最小二乘迭代¶

冗余臂的常用更新式是阻尼最小二乘法（Damped Least Squares, DLS）：

\[ \Delta q_{\text{task}} = J^T(JJ^T + \lambda^2 I)^{-1} e \]

其中 \(\lambda\) 是阻尼系数。靠近奇异点时增大 \(\lambda\)，可以避免伪逆爆炸；远离奇异点时减小 \(\lambda\)，收敛更快。

7 自由度的优势在于可以加入零空间项：

\[ \Delta q = \Delta q_{\text{task}} + (I - J^\dagger J)\Delta q_{\text{null}} \]

常用零空间目标：

目标	做法
远离关节限位	让 \(q\) 向关节范围中点 \(q_{\text{mid}}\) 靠近
保持自然肘姿	给肘部或肩部设置偏好姿态 \(q_{\text{ref}}\)
提高可操作性	沿 \(\nabla w(q)\) 方向运动，远离奇异位形

一个足够实用的零空间项是：

\[ \Delta q_{\text{null}} = k_{\text{null}}(q_{\text{ref}} - q) \]

4）IK 伪代码¶

q = q_current.copy()

for k in range(max_iter):
    T = forward_kinematics(q)
    e = pose_error(T, T_desired)       # [position_error, orientation_error]

    if norm(e) < tolerance:
        break

    J = geometric_jacobian(q)
    lam = choose_damping(J)            # singularity-aware damping
    J_dls = J.T @ inv(J @ J.T + lam**2 * eye(6))

    dq_task = J_dls @ e
    dq_null = k_null * (q_ref - q)
    dq = dq_task + (eye(7) - J_dls @ J) @ dq_null

    dq = clip(dq, -dq_max, dq_max)
    q = clip(q + dq, q_min, q_max)

实际项目中的停止条件

不要只看位置误差。建议同时检查：位置误差、姿态误差、迭代次数、关节限位、最小奇异值、碰撞检测结果。只要其中一项不满足，就不要把 IK 解直接下发给电机。

5）调试顺序¶

先只做正运动学，确认模型坐标系和末端方向正确
再只求位置 IK，关闭姿态权重 \(w_R\)
位置收敛后加入姿态误差
最后加入关节限位、零空间目标和碰撞检测

如果 IK 抖动，优先检查：目标位姿是否连续、姿态误差是否跳变、雅可比坐标系是否一致、阻尼是否太小、关节步长是否过大。

3.4 多解的处理¶

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 参数唯一确定。