第2章：数学基础回顾：向量与矩阵运算、坐标变换、刚体运动学基础
各位同学，欢迎来到轨迹跟踪控制的第一道门槛——数学基础。
说实话，很多做控制的朋友一看到数学就头疼。我当年刚入行时也一样，觉得公式太抽象。但后来在调试一个六轴机械臂时，因为坐标变换算错了一个符号，导致末端执行器直接撞上了夹具……嗯，那次之后我彻底明白了：数学不是用来考试的，是用来保命的。
2.1 向量与矩阵：控制工程师的“扳手”
先聊向量。你想想看，机器人要运动，本质上就是描述“位置”和“方向”的变化。位置用向量表示，方向用旋转矩阵表示，就这么简单。
向量的基本运算
- 点积（内积）：
a·b = |a||b|cosθ。我习惯用它来判断两个向量是否垂直——如果点积为零，那就是垂直。在避障算法里，这个判断很常用。 - 叉积（外积）：
a×b得到一个新向量，方向垂直于a和b所在的平面。说白了，就是求法向量。我在做移动机器人路径规划时，经常用叉积来判断转弯方向。
避坑指南：我曾经在写代码时把叉积的顺序搞反了，结果机器人原地转圈。记住：a×b = -(b×a)，顺序很重要。
矩阵运算
矩阵说白了就是向量的“升级版”。一个3x3的旋转矩阵，本质上是三个列向量拼在一起。
// 矩阵乘法示例（C++风格伪代码）
Matrix3d R1, R2;
Matrix3d R = R1 * R2; // 注意：矩阵乘法不满足交换律
// R1*R2 ≠ R2*R1
我个人习惯把矩阵乘法理解为“线性变换的复合”。先旋转再平移，和先平移再旋转，结果完全不同。这个在后面的齐次变换里会反复用到。
2.2 坐标变换：旋转矩阵与齐次变换
坐标变换是轨迹跟踪的核心。为什么？因为机器人身上有多个坐标系：世界坐标系、基座坐标系、关节坐标系、工具坐标系……你得知道它们之间怎么换算。
旋转矩阵
旋转矩阵R是一个3x3的正交矩阵，满足 R^T R = I，且行列式为+1。它描述了一个坐标系相对于另一个坐标系的姿态。
| 旋转轴 | 旋转矩阵（绕X轴旋转θ角） |
|---|---|
| 绕X轴 |
|
| 绕Y轴 |
|
| 绕Z轴 |
|
我的小技巧：记不住旋转矩阵？别硬背。你只需要记住“绕哪个轴，哪个轴对应的行和列不变”，然后剩下的2x2子矩阵就是标准的二维旋转矩阵。
齐次变换矩阵
齐次变换矩阵T是一个4x4的矩阵，把旋转和平移统一起来：
T = [R t]
[0 1]
// 其中R是3x3旋转矩阵，t是3x1平移向量
为什么要用齐次？因为这样可以把连续的坐标变换写成矩阵连乘的形式。比如从世界坐标系到工具坐标系的变换：
T_world_to_tool = T_world_to_base * T_base_to_arm * T_arm_to_tool
你看，是不是很优雅？我当年第一次看到这个公式时，觉得这简直是数学之美。
注意：齐次变换的乘法顺序是从右往左读。也就是说，先应用右边的变换，再应用左边的。这个顺序搞反了，你的机器人就会飞到天上去。
2.3 刚体运动学基础
刚体运动学，说白了就是研究“一个不会变形的物体怎么动”。在机器人领域，每个连杆都可以看作一个刚体。
位置与姿态
一个刚体在空间中的位姿由6个自由度描述：3个位置（x, y, z）+ 3个姿态（roll, pitch, yaw 或 四元数）。
我个人更推荐用四元数表示姿态，因为它没有万向锁问题。但为了直观，很多教材还是用欧拉角。我的建议是：内部计算用四元数，人机交互用欧拉角。
速度与角速度
刚体的速度分为线速度v和角速度ω。它们之间通过一个重要的关系联系起来：
v_B = v_A + ω × r_AB
这个公式的意思是：刚体上B点的速度，等于A点的速度加上由于旋转产生的速度。我在做轨迹插补时，经常用这个公式来计算机器人末端的速度。
实战经验：有一次我在调试AGV（自动导引车）的轨迹跟踪，发现车体总是偏离路径。后来检查发现，是角速度的符号搞反了。记住：角速度的方向由右手定则确定，大拇指指向旋转轴，四指弯曲方向就是正方向。
2.4 知识体系总览
下面这张图是我自己整理的本章知识结构，你可以把它当作一个“导航地图”。
2.5 本章小结
这一章的内容，说白了就是三件事：
- 向量和矩阵——描述运动和变换的数学工具
- 坐标变换——在不同坐标系之间“翻译”位置和姿态
- 刚体运动学——理解刚体怎么动，速度怎么算
这些内容看起来基础，但我在实际项目中见过太多因为数学基础不牢而翻车的案例。比如有人把旋转矩阵的转置和逆搞混了，结果机器人姿态完全乱掉。所以，请务必亲手推导一遍公式，写几行代码验证一下。
我的建议：用Python的NumPy库写一个小脚本，实现旋转矩阵的乘法、齐次变换的连乘，然后打印结果看看。代码跑通了，概念就真正理解了。
公众号:蓝海资料掘金营，微信deep3321