第2章数学基础回顾

第二讲：数学基础回顾——向量、矩阵与坐标系变换

各位同学好，我是你们的老朋友。

做运动控制这些年，我最大的体会就是：数学不是用来考试的，是用来解决实际问题的。今天我们要聊的向量、矩阵、坐标系变换，说白了就是三轴插补的“内功心法”。你想想看，电机要动，总得知道往哪动、动多远吧？这些信息，全靠数学来传递。

向量是什么？我习惯把它想象成一个带方向的箭头。在三维空间里，一个向量 v = (x, y, z) 就代表从原点出发，指向 (x, y, z) 这个点。

向量的基本运算，咱们做控制时天天用：

加法：v1 + v2 = (x1+x2, y1+y2, z1+z2) —— 说白了就是两个位移的叠加。比如先走X方向再走Y方向，结果就是对角线。
数乘：k * v = (k*x, k*y, k*z) —— 把箭头拉长或缩短。速度倍率调整就是这个道理。
点积：v1 · v2 = x1*x2 + y1*y2 + z1*z2 —— 判断两个方向是否垂直。我在做五轴防碰撞时，就用点积来算刀具轴向与工件表面的夹角。
叉积：v1 × v2 —— 得到垂直于两个向量的新向量。嗯，这个在计算旋转轴方向时特别有用。

避坑指南：我曾经在调试一台三轴雕刻机时，发现走圆弧总是不对。查了半天，原来是向量方向搞反了。记住：向量的方向比大小更重要。方向错了，电机就往反方向跑，轻则撞限位，重则撞工件。

单个向量好处理，但如果你有几十个点要同时旋转、缩放呢？这时候矩阵就派上用场了。

一个 3×3 矩阵可以看作一个线性变换的“配方”：

| a  b  c |   | x |   | a*x + b*y + c*z |
| d  e  f | * | y | = | d*x + e*y + f*z |
| g  h  i |   | z |   | g*x + h*y + i*z |

你看，一个矩阵乘法，就把三个坐标轴上的分量重新组合了。这就是旋转、缩放的本质。

单位矩阵：对角线全是1，其他是0。乘以任何向量都不变——相当于“什么都不做”。

逆矩阵：如果矩阵A把向量v变成了v'，那么A的逆矩阵就能把v'变回v。我在做正逆解时，经常要用到逆矩阵来从末端位姿反推各轴角度。

我的小技巧：写代码时别手算逆矩阵，容易出错。用NumPy的 np.linalg.inv() 或者 np.linalg.solve()，又快又准。我早期做项目时手算过一次，结果算错了，导致电机走了个奇怪的轨迹……从那以后，我就只相信库函数了。

这是今天的重头戏。三轴插补里，我们经常要在世界坐标系和工具坐标系之间来回切换。比如：

坐标系变换分两种：平移和旋转。

说白了就是“挪个地方”。如果坐标系B的原点相对于坐标系A的偏移是 t = (tx, ty, tz)，那么点P在A系下的坐标，就等于它在B系下的坐标加上这个偏移：

P_A = P_B + t

简单吧？但要注意：平移只改变位置，不改变方向。

旋转就稍微复杂一点。绕X轴、Y轴、Z轴的旋转矩阵分别是：

你想想看，绕Z轴旋转，其实就是让X和Y分量互相“交换”一部分。sin和cos就是那个交换的比例。

注意：旋转矩阵是正交矩阵，它的逆等于它的转置。这意味着：如果你把点旋转了θ角，再旋转-θ角，就能回到原位。但如果你先绕X转30°，再绕Y转60°，顺序不同结果完全不同！我见过有人把欧拉角的顺序搞反了，结果机械臂直接撞上了旁边的夹具……

平移用加法，旋转用乘法，混在一起很麻烦。所以有了齐次坐标：把一个三维点 (x, y, z) 写成 (x, y, z, 1)，然后用一个 4×4 矩阵同时表示旋转和平移：

| R  t |   | x |   | R*x + t |
| 0  1 | * | y | = | ...     |
           | z |   | ...     |
           | 1 |   | 1       |

其中R是3×3旋转矩阵，t是3×1平移向量。这样一来，一次矩阵乘法就完成了完整的坐标系变换。我在做三轴插补时，所有的点位数据都是用齐次矩阵存的，方便得很。

下面这张图，是我自己总结的本章知识脉络。你可以把它当作一个“地图”，随时回来查阅：

最后，分享几个我在项目中积累的经验：

好了，数学基础就聊到这儿。这些东西看着简单，但用好了，三轴插补的很多问题都能迎刃而解。下一讲我们就要开始真正的插补算法了，到时候你会发现，今天学的这些，全都会用上。

公众号:蓝海资料掘金营，微信deep3321