3、基础数学知识回顾：坐标系与变换（齐次坐标）、刚体运动学、动力学基础（牛顿-欧拉方程）、PID控制原理
各位同学，欢迎来到运动控制算法仿真环境搭建的第一章。说实话，很多初学者一上来就急着写代码、调参数，结果连坐标系都没搞明白，仿真出来的东西自然是一团糟。我当年也吃过这个亏——花了两周搭的仿真模型，跑起来机器人直接飞到了天上去，后来才发现是坐标变换矩阵写反了。所以，咱们先把地基打牢。
这一章，我会带你回顾四个核心数学基础：坐标系与齐次变换、刚体运动学、动力学基础（牛顿-欧拉方程）以及PID控制原理。别怕，咱们不搞纯理论推导，我会结合我实际项目中的经验来讲。
核心观点：运动控制算法的本质，就是「在正确的坐标系下，用正确的动力学模型，算出正确的控制量」。缺一个，仿真就是空中楼阁。
3.1 坐标系与齐次变换
先问大家一个问题：你在仿真里看到的机器人位置，和它实际在「世界坐标系」中的位置，是一回事吗？
答案显然是否定的。每个刚体都有自己的局部坐标系，而我们最终要把它统一到世界坐标系下。这就是齐次变换要做的事。
我个人习惯用 4×4 齐次变换矩阵 来表示两个坐标系之间的关系。它长这样：
| R3×3 t3×1 |
| 01×3 1 |其中 R 是旋转矩阵，t 是平移向量。为什么用 4×4 而不是 3×4？说白了，就是为了把旋转和平移统一成一个矩阵乘法，方便连续变换。
我的小技巧：在写仿真代码时，我习惯把齐次变换矩阵封装成一个类。每次做坐标变换，直接调用 transform(point) 方法，而不是手动算矩阵乘法。这样既不容易出错，代码也干净。
举个例子，假设我有一个机械臂的末端执行器，它在基座坐标系下的位姿是 T_base_to_end。如果我想知道末端执行器上的一个点 p_end 在世界坐标系下的位置，只需要：
p_world = T_world_to_base * T_base_to_end * p_end嗯，这里要注意：矩阵乘法的顺序不能搞反。我见过太多人把顺序写反，结果仿真出来的轨迹完全不对。
3.2 刚体运动学
搞定了坐标系，接下来就是刚体怎么动的问题。刚体运动学，说白了就是研究位置、速度、加速度之间的几何关系，不考虑力。
我记得有一次做六轴机器人的轨迹规划，客户要求末端走一个完美的圆弧。我一开始直接用关节空间插值，结果末端轨迹歪歪扭扭。后来老老实实回到运动学，先算末端在笛卡尔空间的位置，再逆解到关节空间，问题就解决了。
刚体运动学核心就两个公式：
- 线速度传递：
v_B = v_A + ω × r_AB - 角速度传递：
ω_B = ω_A（刚体上任意点角速度相同）
你想想看，如果两个刚体通过一个旋转关节连接，那关节处的角速度就是两个刚体角速度的叠加。这个在仿真里特别常用。
避坑指南：我曾经在仿真一个四足机器人时，忽略了「角速度不是向量」这个细节。角速度的加法不满足交换律！也就是说 ω₁ + ω₂ ≠ ω₂ + ω₁。如果你用欧拉角表示姿态，一定要小心万向锁问题。我个人建议仿真中统一用四元数或旋转矩阵。
3.3 动力学基础（牛顿-欧拉方程）
运动学只告诉你「怎么动」，动力学才告诉你「为什么动」。牛顿-欧拉方程，就是描述刚体受力和运动关系的核心工具。
对于单个刚体，牛顿-欧拉方程可以写成：
F = m * a_c （平动部分）
τ = I * α + ω × (I * ω) （转动部分）其中 F 是合力，m 是质量，a_c 是质心加速度；τ 是合力矩，I 是惯性张量，α 是角加速度，ω 是角速度。
为什么转动部分多了一项 ω × (I * ω)？这其实就是所谓的陀螺力矩。我刚开始做无人机仿真时，忽略了这一项，结果仿真出来的无人机一翻滚就失控。后来加上这一项，才和实际飞行数据对得上。
实际应用：在仿真中，我们通常用「递推牛顿-欧拉算法」来计算多刚体系统的动力学。从基座开始，向外递推速度和加速度，再向内递推力和力矩。这个算法在机器人控制中几乎是标配。
3.4 PID控制原理
好了，前面讲了怎么描述运动、怎么计算力，接下来就是怎么控制它。PID控制，可以说是运动控制领域最经典、最实用的算法了。
PID控制器的输出由三部分组成：
| 项 | 公式 | 作用 | 我的经验 |
|---|---|---|---|
| 比例（P） | Kp * e(t) | 快速响应误差 | P太大容易震荡，我一般先调P到临界稳定再减半 |
| 积分（I） | Ki * ∫e(t)dt | 消除稳态误差 | 积分饱和是个坑，我习惯加限幅或使用抗积分饱和算法 |
| 微分（D） | Kd * de(t)/dt | 预测误差趋势，抑制震荡 | D对噪声敏感，我通常在仿真里加个低通滤波器再微分 |
为什么PID这么管用？说白了，它不需要知道系统的精确模型。你只要把误差反馈回去，调好三个参数，大部分系统都能控制住。
但要注意，PID不是万能的。我曾经在一个柔性关节机器人项目上，直接用PID控制位置，结果末端一直抖。后来分析才发现，柔性关节有谐振频率，PID的微分项反而放大了谐振。最后改用陷波滤波器 + PID才解决。
仿真中的调参技巧：我建议在仿真环境里先做「阶跃响应测试」。给一个目标位置阶跃，观察系统的响应曲线。理想的响应是：快速上升、小超调、无稳态误差。如果超调太大，减小P或增大D；如果响应太慢，增大P或减小I。
最后，给大家一个简单的离散PID实现代码，在仿真中可以直接用：
class PID:
def __init__(self, Kp, Ki, Kd, dt):
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.dt = dt
self.integral = 0
self.prev_error = 0
def compute(self, setpoint, measurement):
error = setpoint - measurement
self.integral += error * self.dt
derivative = (error - self.prev_error) / self.dt
output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
self.prev_error = error
return output嗯，代码很简单，但实际用的时候要注意：积分项一定要加限幅，不然积分饱和会让你控制量飞上天。
好了，这一章的内容就到这里。数学基础是枯燥的，但它是你后续所有仿真工作的基石。下一章我们会开始搭建实际的仿真环境，到时候这些知识都会用上。
公众号:蓝海资料掘金营，微信deep3321