第四章：三维空间数学基础
各位同学，欢迎来到《数字孪生运动系统入门宝典》的第四课。今天我们要啃的这块骨头，是三维空间数学基础。说实话，这可能是整个课程里最“劝退”的一章，但也是最重要的一章。我当年刚入行时，就因为没吃透这些概念，在项目里栽过跟头。所以，咱们今天慢慢来，把这块硬骨头啃下来。
核心观点：三维空间数学是数字孪生运动系统的“语法”。不懂语法，你写不出正确的“句子”。
4.1 向量与矩阵运算
先说说向量。向量是什么？说白了，就是带方向和大小的量。在三维空间里，我们用三个数字表示一个向量，比如 (x, y, z)。它既可以表示一个点，也可以表示一个方向。
我个人习惯把向量想象成“箭头”。这个箭头从原点出发，指向 (x, y, z) 那个点。箭头有长度（模长），也有方向。
向量运算，其实就几种：
- 加法： 两个向量头尾相接。比如你往前走一步，再往右走一步，最终位置就是两个向量的和。
- 减法： 从一个点指向另一个点的向量。这个在运动控制里特别常用，比如计算“从A点到B点”的位移。
- 点积： 结果是一个数。它告诉你两个向量的“对齐程度”。如果点积为0，说明两个向量垂直。我在做碰撞检测时经常用这个。
- 叉积： 结果是一个新向量，垂直于原来的两个向量。这个在计算法线、旋转轴时特别有用。
举个例子，假设你有两个向量 a = (1, 2, 3) 和 b = (4, 5, 6)：
// 向量加法
a + b = (1+4, 2+5, 3+6) = (5, 7, 9)
// 向量点积
a · b = 1*4 + 2*5 + 3*6 = 4 + 10 + 18 = 32
// 向量叉积
a × b = (2*6 - 3*5, 3*4 - 1*6, 1*5 - 2*4) = (12-15, 12-6, 5-8) = (-3, 6, -3)
我的小技巧： 写代码时，我习惯用右手定则来判断叉积的方向。拇指指向第一个向量，食指指向第二个向量，中指指向的就是叉积结果的方向。嗯，这个在调试旋转时特别管用。
再说矩阵。矩阵就是一堆数字排成的矩形。在三维空间里，最常用的是 3x3 矩阵（用于旋转）和 4x4 矩阵（用于齐次坐标变换）。
矩阵乘法是核心操作。注意，矩阵乘法不满足交换律，也就是说 A×B 不等于 B×A。我曾经在项目里因为搞反了矩阵乘法的顺序，导致机械臂转到了完全错误的方向……嗯，从那以后我再也不敢马虎了。
4.2 欧拉角与四元数
接下来是旋转的两种表示方式：欧拉角和四元数。
欧拉角，就是用三个角度来表示旋转：绕X轴转多少度（滚转）、绕Y轴转多少度（俯仰）、绕Z轴转多少度（偏航）。听起来很直观，对吧？
但欧拉角有个大坑——万向锁。当某个轴旋转到90度时，另外两个轴会变得“重叠”，导致丢失一个自由度。我在做无人机姿态控制时遇到过这个问题，飞机突然“卡住”了，怎么调都调不回来。后来改用四元数才解决。
四元数，用四个数字表示旋转：一个实部和三个虚部。形式是 q = w + xi + yj + zk，其中 w 是实部，(x, y, z) 是虚部。
四元数的好处是：
- 没有万向锁问题
- 插值平滑（球面线性插值，SLERP）
- 计算效率高
举个例子，绕Y轴旋转90度的四元数是：
// 绕Y轴旋转90度（弧度制）
angle = π/2
q = (cos(angle/2), 0, sin(angle/2), 0)
= (cos(π/4), 0, sin(π/4), 0)
= (0.707, 0, 0.707, 0)
注意： 四元数虽然强大，但理解起来有点反直觉。我建议你先把欧拉角搞明白，再过渡到四元数。别一上来就啃四元数，容易劝退。
4.3 坐标变换与齐次矩阵
在数字孪生系统里，我们经常需要在不同坐标系之间转换。比如，传感器测得的数据是在传感器坐标系下的，但我们需要把它转换到世界坐标系下。
齐次矩阵就是干这个的。它是一个 4x4 的矩阵，同时包含了旋转和位移信息。形式如下：
| R11 R12 R13 Tx |
| R21 R22 R23 Ty |
| R31 R32 R33 Tz |
| 0 0 0 1 |
其中，左上角的 3x3 子矩阵 R 是旋转矩阵，右上角的 (Tx, Ty, Tz) 是位移向量。
使用齐次矩阵的好处是，你可以把多次变换“串联”起来。比如，从A到B的变换矩阵是 T_AB，从B到C的变换矩阵是 T_BC，那么从A到C的变换就是 T_AC = T_AB × T_BC。
我个人习惯把齐次矩阵想象成“坐标系的说明书”。它告诉你，一个点在这个坐标系里怎么表示，在另一个坐标系里又怎么表示。
4.4 刚体运动学基础
最后，我们聊聊刚体运动学。刚体，就是不会变形的物体。在数字孪生里，我们处理的机械臂、无人机、车辆，都可以近似看作刚体。
刚体运动学研究的是刚体的位置、速度和加速度，不考虑力。说白了，就是“怎么动”，而不是“为什么动”。
刚体的运动可以分解为两部分：
- 平移： 刚体上所有点都沿着同一个方向移动相同的距离。
- 旋转： 刚体绕某个轴转动。
在三维空间里，刚体的位姿（位置+姿态）可以用一个齐次矩阵来表示。刚体的速度则用线速度和角速度来表示。
举个例子，一个机械臂的末端执行器，它的位姿就是由各个关节的角度决定的。每个关节的旋转，都会影响末端的位置和姿态。这就是所谓的正运动学。
// 一个简单的2关节机械臂正运动学
// 关节1：绕Z轴旋转 θ1
// 关节2：绕Z轴旋转 θ2
// 连杆长度：L1, L2
// 末端位置
x = L1*cos(θ1) + L2*cos(θ1 + θ2)
y = L1*sin(θ1) + L2*sin(θ1 + θ2)
z = 0 // 平面机械臂
我的经验： 在做运动学仿真时，我习惯先用简单的2D模型验证算法，再扩展到3D。这样调试起来快很多。你想想看，如果2D都跑不对，3D肯定更乱。
知识体系总览
为了让你更直观地理解本章的知识结构，我画了一张图：
这张图把本章的四个核心模块串起来了。你看，向量与矩阵是基础工具，欧拉角和四元数是旋转的两种表示，坐标变换解决“怎么在不同坐标系间转换”，刚体运动学则把这些知识应用到实际运动分析中。
好了，第四章的内容就到这里。记住，这些数学基础是数字孪生运动系统的“内功”。内功练好了，后面学什么招式都快。如果你在项目中遇到具体问题，欢迎随时交流。
公众号:蓝海资料掘金营，微信deep3321