数学基础（二）：运动学基础

运动学，说白了就是研究物体怎么动、转到哪、速度多快，而不去管是什么力让它动的。做轨迹规划，你天天都得跟它打交道。我记得刚入行那会儿，总觉得运动学是理论课的东西，直到第一次在真机上跑轨迹，机器人直接撞了夹具……嗯，从那以后我再也不敢小看它了。

本章核心脉络：刚体运动描述 → 速度与角速度 → 雅可比矩阵 → 逆运动学。这四个模块是串联的，缺一个后面都推不下去。

刚体运动，说白了就是描述一个物体在空间里「在哪」和「朝哪」。位置用三维坐标 (x, y, z) 搞定，姿态用旋转矩阵 R ∈ SO(3) 搞定。合在一起就是 4×4 齐次变换矩阵 T ∈ SE(3)。

我个人习惯把齐次变换矩阵理解成「坐标系搬家工具」—— 它把一个点从 A 坐标系搬到 B 坐标系。你想想看，轨迹规划里最常干的事不就是把目标点从世界坐标系搬到机器人基坐标系，再搬到工具坐标系吗？

实用技巧：写代码时别手算旋转矩阵，直接用库。Python 用 scipy.spatial.transform.Rotation，C++ 用 Eigen::Quaterniond。我见过太多人自己手写旋转矩阵然后拼错符号的……

刚体不光有位置，还有速度。线速度 v 和角速度 ω 合在一起，就构成了一个六维向量，叫「空间速度」或「运动旋量」。公式很简单：

V = [v_x, v_y, v_z, ω_x, ω_y, ω_z]^T

这里有个坑，我踩过。不同文献对运动旋量的定义顺序不一样。有的把角速度放前面，有的把线速度放前面。你在对接不同团队的代码时，一定要先确认顺序。我曾经因为这个问题，调试了整整两天……

角速度还有一个重要的性质：它的反对称矩阵形式。ω 的叉乘矩阵 [ω]× 满足：

[ω]× · p = ω × p

这个性质在推导雅可比矩阵时非常有用。你记住一句话就行：叉乘可以写成矩阵乘法。

雅可比矩阵 J 是轨迹规划里最核心的数学工具之一。它建立了关节空间速度 q̇ 和操作空间速度 V 之间的线性映射：

V = J(q) · q̇

这里的 V 就是上面说的六维运动旋量。J 是一个 6×n 的矩阵，n 是关节数。对于六轴工业机器人，J 是 6×6 的方阵。

雅可比矩阵的每一列都有物理意义：第 i 列表示第 i 个关节以单位速度运动时，末端产生的速度。我在项目中经常用这个性质来调试——如果某个关节的雅可比列突然变得很小，说明这个关节对末端运动贡献不大，可能处于奇异位形附近。

避坑指南：雅可比矩阵是位形相关的，也就是说 J = J(q)。机器人在不同姿态下，同样的关节速度会产生完全不同的末端速度。所以轨迹规划时一定要实时更新雅可比矩阵，不能拿初始姿态的 J 算全程。

逆运动学（IK）是正运动学的逆过程：给定末端位姿 T_desired，求关节角 q。这是轨迹规划里最头疼的问题，因为：

IK 的求解方法分两类：

方法	原理	适用场景	优缺点
解析法	利用几何/代数推导闭式解	特定构型（如 PUMA 560）	快、精确；但通用性差
数值法	迭代优化（如牛顿-拉夫森）	通用构型	通用性强；但慢、可能不收敛

我个人习惯：能用解析法就用解析法。解析法快、稳定、没有迭代误差。但如果你用的是七轴冗余机器人或者非标准构型，那就只能上数值法了。

核心公式：数值 IK 的迭代更新公式：

Δq = J⁺ · Δx
q_{k+1} = q_k + Δq

其中 J⁺ 是雅可比矩阵的伪逆，Δx 是当前末端位姿与目标位姿的误差。迭代直到 Δx 小于阈值。

运动学基础是轨迹规划的数学地基。你想想看，没有刚体运动描述，你连目标点都表达不了；没有雅可比矩阵，你没法把关节速度映射到末端速度；没有逆运动学，你算不出关节角。这四个知识点环环相扣，缺一不可。

最后分享一个我的经验：多画坐标系。在纸上画、在代码里画、在仿真里画。坐标系画清楚了，运动学就学通了一半。