数学基础（一）：向量与矩阵运算、点积与叉积的物理意义、旋转矩阵的推导
各位同学，欢迎来到第二讲。
说实话，很多做飞控的朋友一上来就啃四元数、欧拉角，结果越学越晕。为什么？因为底层的数学工具没拿稳。今天这堂课，咱们就把向量、矩阵、点积、叉积这些基本功彻底捋一遍。这些东西搞明白了，后面姿态解算的代码你写起来会非常顺手。
2.1 向量：飞控里的“方向与大小”
向量是什么？说白了，就是既有大小又有方向的量。在无人机里，它无处不在：
- 位置向量：飞机相对于原点的位置
- 速度向量：飞机飞行的方向和快慢
- 加速度向量：IMU（惯性测量单元）输出的原始数据
- 磁场向量：磁力计感知的地球磁场方向
一个三维向量通常写成这样：
v = [vx, vy, vz]^T上标 T 表示转置，其实就是把行向量变成列向量。我个人习惯在代码里用列向量，因为矩阵乘法时列向量更自然。
向量的基本运算，咱们快速过一遍：
- 加法：对应分量相加。比如飞机同时受到两个力，合力就是向量相加。
- 数乘：每个分量乘以一个标量。比如把速度向量放大两倍。
- 模长：sqrt(vx² + vy² + vz²)。在飞控里，模长就是加速度的大小、速度的大小。
- 单位向量：向量除以模长。只保留方向，去掉大小。这在姿态解算中极其常用。
2.2 点积：判断“方向是否一致”
点积，也叫内积。两个向量的点积公式是：
a · b = |a| * |b| * cos(θ)其中 θ 是两个向量之间的夹角。
物理意义是什么？ 说白了，点积衡量的是两个向量在方向上的“重合程度”。
- 如果两个向量方向完全一致（θ=0°），点积 = |a|*|b|，最大。
- 如果两个向量垂直（θ=90°），点积 = 0。
- 如果两个向量方向相反（θ=180°），点积 = -|a|*|b|，最小。
在飞控里有什么用？ 我举两个实际例子：
- 判断重力方向：加速度计测到的向量是重力加速度 g。如果你知道机体的坐标系，点积可以告诉你机头是否朝上。
- 投影计算：把一个向量投影到另一个向量上，用的就是点积。比如把速度向量投影到机体轴线上，得到“沿机头方向的速度分量”。
2.3 叉积：找到“旋转轴”
叉积，也叫外积。两个向量的叉积结果是一个新的向量，这个向量垂直于原来的两个向量。公式是：
a × b = |a| * |b| * sin(θ) * n其中 n 是垂直于 a 和 b 的单位向量，方向由右手定则决定。
物理意义是什么？ 叉积衡量的是两个向量的“垂直程度”和“旋转趋势”。
- 如果两个向量平行（θ=0°或180°），叉积 = 0。
- 如果两个向量垂直（θ=90°），叉积的模最大。
- 叉积的方向就是旋转轴的方向。
在飞控里，叉积是姿态解算的核心。你想想看：
- 姿态误差计算：加速度计测到的重力方向，和陀螺仪积分得到的预估重力方向，两者之间的误差怎么算？用叉积！叉积的结果就是旋转轴和旋转角度（小角度近似下）。
- 互补滤波：经典的 Mahony 滤波算法，核心就是利用叉积作为误差信号，去修正陀螺仪的漂移。
2.4 矩阵：旋转的“数学翻译器”
矩阵说白了就是一个数字表格。但在飞控里，矩阵最重要的作用就是描述旋转。
矩阵乘法：一个 3x3 的旋转矩阵乘以一个 3x1 的向量，结果是一个新的向量。这个新向量就是原向量经过旋转后的结果。
v' = R * v其中 R 是旋转矩阵，v 是原始向量，v' 是旋转后的向量。
矩阵乘法的规则：左矩阵的行乘以右矩阵的列。这个规则虽然简单，但写代码时容易搞错维度。我建议你写代码前先画个矩阵形状图，确认维度匹配。
2.5 旋转矩阵的推导：从基础到实战
旋转矩阵怎么来的？咱们从最简单的二维旋转开始。
假设一个点 P 在二维平面上，坐标为 (x, y)。把它绕原点逆时针旋转 θ 角度，新坐标 (x', y') 是多少？
用三角函数推导：
x' = x * cos(θ) - y * sin(θ)
y' = x * sin(θ) + y * cos(θ)写成矩阵形式：
[x'] [cosθ -sinθ] [x]
[y'] = [sinθ cosθ] [y]这就是二维旋转矩阵。
扩展到三维，绕三个轴旋转的矩阵分别是：
| 旋转轴 | 旋转矩阵 |
|---|---|
| 绕 X 轴（滚转） |
[1, 0, 0] [0, cosφ, -sinφ] [0, sinφ, cosφ] |
| 绕 Y 轴（俯仰） |
[cosθ, 0, sinθ] [0, 1, 0] [-sinθ, 0, cosθ] |
| 绕 Z 轴（偏航） |
[cosψ, -sinψ, 0] [sinψ, cosψ, 0] [0, 0, 1] |
注意：绕 Y 轴的矩阵里，sinθ 的符号和绕 X、Z 轴不同。这是因为坐标系的右手定则导致的。嗯，这里很容易搞混，我建议你推导一遍，不要死记硬背。
完整的旋转矩阵：实际飞行中，飞机是同时绕三个轴旋转的。完整的旋转矩阵是三个基本旋转矩阵的乘积。但顺序很重要！不同的旋转顺序（比如 ZYX 还是 XYZ）会得到不同的矩阵。飞控里最常用的是 ZYX 顺序（先偏航、再俯仰、最后滚转）。
2.6 代码实战：用 Python 实现基础运算
光说不练假把式。咱们用 Python 把今天学的知识实现一遍：
import numpy as np
# 定义两个向量
a = np.array([1, 2, 3])
b = np.array([4, 5, 6])
# 点积
dot_product = np.dot(a, b)
print(f"点积: {dot_product}")
# 叉积
cross_product = np.cross(a, b)
print(f"叉积: {cross_product}")
# 向量模长
norm_a = np.linalg.norm(a)
print(f"向量 a 的模长: {norm_a}")
# 单位向量
unit_a = a / norm_a
print(f"单位向量 a: {unit_a}")
# 绕 Z 轴旋转 30 度
theta = np.radians(30)
R_z = np.array([
[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0],
[0, 0, 1]
])
v = np.array([1, 0, 0])
v_rotated = R_z @ v # @ 是矩阵乘法
print(f"旋转后的向量: {v_rotated}")这段代码跑出来，你会看到点积、叉积、旋转后的向量。我建议你亲手敲一遍，把角度改一改，看看结果怎么变。
2.7 小结
今天咱们把向量、点积、叉积、旋转矩阵这些基础工具过了一遍。这些东西看着简单，但它们是姿态解算的基石。你想想看：
- 点积帮你判断方向是否一致
- 叉积帮你找到旋转轴和误差
- 旋转矩阵帮你描述任意姿态
下一讲，咱们会深入欧拉角和四元数。到时候你会发现，今天学的这些知识全都会用上。所以，别偷懒，把代码跑一遍，把公式推一遍。相信我，这会让你后面的学习轻松很多。