正向运动学

0. 前言

正向运动学 是机器人运动学中的第一类问题，也是最基础的部分。
简单来说，它的核心任务是：已知机械臂各个关节的角度，计算末端执行器（手爪）在空间中的位置和姿态。
这就像是你知道自己肩膀、肘部和手腕各自弯曲了多少度，然后通过计算得出你的手指尖具体在身体的哪个方位。
以下从定义、数学原理、通用解法（D-H参数法）和具体案例四个方面详细展开：

1. 定义与输入输出

输入：关节变量向量 $\boldsymbol{q} = [q_1, q_2, \dots, q_n]^T$。对于旋转关节，这是角度 ($\theta$)；对于移动关节，这是位移 ($d$)。
输出：末端执行器相对于基坐标系的位姿。
位置：$(x, y, z)$ 坐标。
姿态：末端执行器的方向，通常用旋转矩阵 $R$ 或欧拉角表示。
数学映射：

$$
\boldsymbol{x} = f(\boldsymbol{q})
$$

其中 $\boldsymbol{x}$ 代表末端位姿。

2. 数学核心：齐次变换矩阵

在正向运动学中，我们不直接计算坐标，而是通过坐标系变换来推导。
假设机械臂有 $n$ 个连杆，我们在每个关节处建立一个坐标系。正向运动学的过程就是将所有相邻坐标系的变换矩阵以此相乘：

$$
T_{n}^{0} = T_{1}^{0} \cdot T_{2}^{1} \cdot T_{3}^{2} \cdots T_{n}^{n-1}
$$

$T_{n}^{0}$：表示末端坐标系相对于基坐标系的位姿（即我们想求的结果）。
$T_{i}^{i-1}$：表示第 $i$ 个坐标系相对于第 $i-1$ 个坐标系的变换。
每个变换矩阵 $T$ 通常是一个 $4 \times 4$ 的矩阵：

$$
T = \begin{bmatrix} R_{3\times3} & P_{3\times1} \ 0_{1\times3} & 1 \end{bmatrix}
$$

$R$：旋转矩阵，描述姿态。
$P$：位置向量，描述位置。

3. 通用解法：D-H 参数法

3.1 概括

为了系统性地建立上述变换矩阵，最常用的方法是 Denavit-Hartenberg (D-H) 参数法。它为每个连杆定义了4个参数，从而唯一确定相邻坐标系的关系。
四个关键参数：
连杆长度：沿 $x_i$ 轴测量的 $z_{i-1}$ 和 $z_{i+1}$ 轴之间的距离。当前轴到下一个轴的距离（当前—下一个，之间的关系）
连杆扭转角：$z_{i}$ 轴绕 $x_i$ 轴旋转到 $z_{i+1}$ 轴的角度。当前轴旋转到下一个轴的角度（当前—下一个，之间的关系）
连杆偏距：沿 $z_{i-1}$ 轴测量的 $x_{i-1}$ 和 $x_i$ 轴之间的距离。上一个x轴距离当前x轴的距离 （上一个—当前，之间的关系）
关节角：$x_{i-1}$ 轴绕 $z_{i-1}$ 轴旋转到 $x_i$ 轴的角度（对于旋转关节，这是变量）。上一个x轴和当前x轴之间的角度 ,（上一个—当前，之间的关系）

注意：

1. x轴为沿着两个轴之间的垂线方向（当两个轴垂直时，x轴垂直于这两个轴组成的平面，有两种情况，向外向内）

2. 连杆扭转角有正负区分，具体为，当前轴转到下个轴的角度，沿着x轴反方向看过去，逆正顺负。

求解步骤：
建立坐标系：为每个关节指定 $z$ 轴（沿旋转轴方向）和 $x$ 轴。
列表：列出所有关节的 D-H 参数表。
推导矩阵：根据参数表写出每个关节的通用变换矩阵 $T_i^{i-1}$。
相乘求解：将所有矩阵相乘得到最终的正运动学方程。

4. 案例与推到

4.1 案例

案例1：

案例2：

4.2 推导

5. 正向运动学的特点与应用

特点：
解的唯一性：给定一组关节角，末端位姿是唯一确定的（不像逆向运动学可能有多个解或无解）。
计算简单：主要是矩阵乘法和三角函数计算，现代计算机可以实时完成。
主要应用场景：
机器人仿真：在电脑屏幕上显示机器人的姿态，必须先算出各连杆在空间的位置。
运动控制验证：在发送指令给电机前，验证当前目标角度是否会导致碰撞。
状态监测：通过电机编码器读取角度，实时计算出末端执行器当前在哪里。
总结
正向运动学是机器人控制系统的“计算器”。它将机器人的内部状态（关节角）翻译成外部状态（空间坐标），是后续进行轨迹规划、逆向运动学求解以及视觉伺服控制的基础。

6. 标准坐标系