第二章 运动控制基础理论：坐标系与变换、刚体运动学与正逆运动学

各位同学，欢迎来到运动控制的核心理论课。说实话，很多做运动控制的工程师，写代码一把好手，但一碰到坐标系变换就头疼。我当年刚入行时也踩过这个坑——有一次调试六轴机械臂，死活对不准目标点，折腾了两天，最后发现是坐标系搞反了。嗯，从那以后，我再也不敢轻视这些基础理论了。

2.1 坐标系与齐次坐标变换

先聊聊坐标系。你想想看，机械臂要抓一个杯子，它得知道杯子在哪儿，自己的手在哪儿。这些位置信息，都得靠坐标系来描述。

常用的坐标系有三种：

• 世界坐标系：固定在空间中的参考系，所有物体都相对于它定义
• 工具坐标系：固定在机械臂末端执行器上，随它一起运动
• 基坐标系：固定在机械臂底座上，是机械臂自身的参考系

那怎么描述一个坐标系相对于另一个坐标系的位置和姿态呢？这就引出了齐次坐标变换矩阵。

T = [ R   p ]
    [ 0   1 ]

我个人习惯用齐次坐标，因为它能把复杂的变换写成矩阵乘法。比如，从基坐标系到工具坐标系的变换，可以写成：

T_tool_base = T_1 * T_2 * T_3 * ...

每个 T 代表一个关节的变换。这样写出来，代码结构特别清晰。

2.2 刚体运动学基础

刚体运动学，说白了就是研究物体怎么动，但不考虑让它动的力。我们关心的是位置、速度、加速度之间的关系。

一个刚体在空间中有6个自由度：3个平移自由度（x, y, z）和3个旋转自由度（roll, pitch, yaw）。描述刚体运动，我们需要：

• 位置：用三维向量表示
• 姿态：用旋转矩阵、欧拉角或四元数表示
• 速度：线速度和角速度

这里有个关键概念——雅可比矩阵。它把关节空间的速度映射到末端执行器的速度：

v_end = J(q) * q_dot

其中 v_end 是末端速度，q_dot 是关节速度，J(q) 就是雅可比矩阵。

2.3 正运动学：以2R机械臂为例

正运动学，就是已知关节角度，求末端位置。说白了，你告诉电机转多少度，我告诉你手在哪儿。

拿最简单的2R机械臂来说——它有两个旋转关节，像人的上臂和前臂。

设两个连杆长度分别为 L1 和 L2，关节角度为 θ1 和 θ2。末端位置 (x, y) 可以写成：

x = L1 * cos(θ1) + L2 * cos(θ1 + θ2)
y = L1 * sin(θ1) + L2 * sin(θ1 + θ2)

这个公式看着简单，但背后是齐次变换矩阵的连乘结果。我建议你亲手推导一遍，对理解多轴机器人很有帮助。

def forward_kinematics(theta1, theta2, L1, L2):
    x = L1 * cos(theta1) + L2 * cos(theta1 + theta2)
    y = L1 * sin(theta1) + L2 * sin(theta1 + theta2)
    return x, y

2.4 逆运动学：从位置反推关节角

逆运动学就反过来了——已知末端位置，求关节角度。这在实际应用中更常见，因为我们要告诉机器人「去抓那个杯子」，而不是「转多少度」。

对于2R机械臂，逆运动学有解析解。给定目标位置 (x, y)，我们可以求出：

cos(θ2) = (x² + y² - L1² - L2²) / (2 * L1 * L2)
θ2 = ± arccos(cos(θ2))  // 注意有两个解

θ1 = atan2(y, x) - atan2(L2 * sin(θ2), L1 + L2 * cos(θ2))

这里有个有意思的地方——θ2 有正负两个解，对应「肘部向上」和「肘部向下」两种姿态。这就是所谓的多解问题。

2.5 知识体系总览

为了让你更直观地理解本章的知识结构，我画了一张图：

2.6 本章小结

这一章我们聊了三个核心内容：

1. 坐标系与齐次变换——用矩阵统一描述位置和姿态
2. 刚体运动学——理解物体怎么动，雅可比矩阵是关键
3. 正逆运动学——以2R机械臂为例，掌握从关节到末端、从末端到关节的映射

这些理论看着抽象，但实际调试时处处用得到。我建议你找个2R机械臂的仿真环境，亲手写一遍正逆运动学的代码，把公式变成可运行的函数。只有亲手写过，才能真正理解。