第二节 基础数学回顾：向量、矩阵、复数、四元数的定义、运算规则及在飞控中的物理意义

嵌入式飞控算法本质上是在有限的计算资源下，对飞行器姿态、位置、速度等物理量进行实时估计与控制。无论是姿态解算、惯性导航、传感器融合还是控制律设计，其底层运算都离不开向量、矩阵、复数和四元数这四类数学工具。本节将系统回顾它们的定义与运算规则，并重点剖析其在飞控系统中的物理映射——即每一个数学符号背后对应的真实物理含义。理解这些映射关系，是后续学习姿态解算、卡尔曼滤波和PID控制的前提。

2.1 向量：飞控中最基本的物理量载体

2.1.1 核心原理：向量的定义与物理映射

向量是既有大小又有方向的量。在飞控系统中，几乎所有传感器原始数据都以向量形式呈现：加速度计输出的三轴比力向量 a = [a_x, a_y, a_z]^T，陀螺仪输出的三轴角速度向量 ω = [p, q, r]^T，磁力计输出的地磁场向量 m = [m_x, m_y, m_z]^T。这些向量均定义在机体坐标系（Body Frame）下，其原点位于飞行器重心，x轴指向机头，y轴指向右翼，z轴指向下方（右手定则）。

向量的点积（内积）a·b = |a||b|cosθ 在飞控中用于计算投影分量。例如，当我们需要将重力加速度在机体轴上的分量分离出来时，就是通过重力向量与机体轴向量的点积实现的。向量的叉积（外积）a×b 则产生一个垂直于两向量所在平面的新向量，其模长等于两向量构成的平行四边形面积。在姿态解算中，叉积被广泛用于计算两个坐标系之间的误差修正量——例如加速度计测量的重力方向与当前姿态推算出的重力方向之间的偏差，正是通过叉积得到的。

2.1.2 判断方法：向量运算在飞控中的典型应用场景

2.1.3 案例拆解：加速度计数据预处理中的向量运算

假设某四旋翼在悬停状态下，加速度计原始输出为 a = [0.12, -0.08, 9.78]^T m/s²。首先计算模长：|a| = √(0.12² + 0.08² + 9.78²) ≈ 9.781 m/s²，与当地重力加速度9.79 m/s²接近，说明传感器工作正常。接着归一化得到单位向量 â = [0.0123, -0.0082, 0.9999]^T。在姿态解算中，这个归一化向量将与由当前姿态矩阵推算出的重力方向单位向量做叉积，得到姿态误差修正量。若飞行器发生倾斜，例如俯仰角增加5°，则加速度计测得的重力方向在机体坐标系中的投影会发生变化，叉积结果将产生一个非零修正向量，驱动姿态估计器向正确方向收敛。

2.2 矩阵：坐标变换与系统建模的核心工具

2.2.1 核心原理：旋转矩阵与方向余弦阵

矩阵在飞控中最核心的应用是描述坐标系之间的旋转关系。从机体坐标系到世界坐标系的旋转可以用一个3×3的正交矩阵 R 表示，称为方向余弦阵（DCM）。该矩阵的每一列表示机体坐标轴在世界坐标系中的方向余弦。由于旋转矩阵是正交矩阵，满足 R^TR = I，且det(R) = 1，因此其逆矩阵等于转置矩阵，这为坐标变换的逆运算提供了极大便利。

矩阵乘法在飞控中用于连续旋转的合成。例如，飞行器先绕z轴旋转偏航角ψ，再绕新y轴旋转俯仰角θ，最后绕新x轴旋转横滚角φ，则总的旋转矩阵为 R = R_x(φ)·R_y(θ)·R_z(ψ)。注意矩阵乘法不满足交换律，旋转顺序至关重要。在飞控代码中，常见的旋转顺序为Z-Y-X（偏航-俯仰-横滚），这与航空惯例一致。

2.2.2 操作流程：矩阵在飞控中的典型运算链

1. 传感器数据读取：获取机体坐标系下的三轴加速度、角速度、磁场强度向量。
2. 姿态矩阵构建：根据当前姿态角（或四元数）构建旋转矩阵 R。
3. 坐标变换：将机体坐标系下的向量通过 v_world = R·v_body 变换到世界坐标系。
4. 控制量计算：在世界坐标系中计算期望位置/速度误差，再通过 R^T 变换回机体坐标系生成控制指令。
5. 矩阵更新：利用陀螺仪角速度通过矩阵微分方程 Ṙ = R·[ω]_× 更新姿态矩阵（其中[ω]_×为角速度的反对称矩阵）。

2.2.3 案例拆解：方向余弦阵在姿态解算中的数值漂移问题

使用DCM进行姿态解算时，由于积分误差和数值舍入，矩阵的正交性会逐渐丧失。具体表现为矩阵列向量不再严格正交，模长不再为1。解决方法是定期进行正交化修正：设 R 的列向量为 r₁, r₂, r₃，则修正后的向量为：

// 正交化修正（Gram-Schmidt过程）
// 假设R的列向量为r1, r2, r3
// 第一步：归一化r1
r1 = r1 / norm(r1);
// 第二步：将r2投影到r1上并减去
r2 = r2 - dot(r1, r2) * r1;
r2 = r2 / norm(r2);
// 第三步：r3 = r1 × r2（保证右手定则）
r3 = cross(r1, r2);

这种修正通常在每次姿态更新后执行一次，计算量约为几十次浮点运算，在STM32F4等主控上耗时不足1μs，完全可以接受。若不进行正交化修正，姿态误差会随时间累积，最终导致飞行器失控。

2.3 复数：二维旋转与频域分析的桥梁

2.3.1 核心原理：复数的旋转本质

复数 z = a + bi 可以看作二维平面上的一个点或向量。其极坐标形式 z = |z|(cosθ + i·sinθ) 揭示了复数的旋转本质：两个复数相乘，模长相乘、辐角相加。这意味着复数乘法天然实现了二维平面上的旋转与缩放。在飞控中，虽然姿态是三维的，但复数在以下场景中仍有重要应用：单轴陀螺仪的积分（如偏航角单独积分）、交流信号的相量分析、数字滤波器的频域设计等。

欧拉公式 e^iθ = cosθ + i·sinθ 将指数函数与三角函数联系起来，在飞控的频域分析中频繁出现。例如，互补滤波器的传递函数 H(s) = s/(s+ω_c) 在频域中可以用复数表示其幅频和相频特性，帮助工程师理解滤波器对不同频率信号的增益和相位延迟。

2.3.2 判断方法：复数运算在飞控中的典型应用

2.3.3 案例拆解：互补滤波器的复数域分析

在姿态解算中，互补滤波器将加速度计的低频信号与陀螺仪的高频信号进行融合。其核心思想可用复数域的传递函数描述：设加速度计传递函数为低通特性 G_acc(s) = ω_c/(s+ω_c)，陀螺仪传递函数为高通特性 G_gyro(s) = s/(s+ω_c)，则总输出为两者之和。在频域中，当频率远低于截止频率ω_c时，G_acc ≈ 1，G_gyro ≈ 0，此时主要信任加速度计；当频率远高于ω_c时，G_acc ≈ 0，G_gyro ≈ 1，此时主要信任陀螺仪。通过选择适当的ω_c（通常为0.5~2 rad/s），可以在抑制陀螺漂移的同时保证动态响应。

2.4 四元数：现代飞控姿态解算的基石

2.4.1 核心原理：四元数的定义与旋转表示

四元数 q = q₀ + q₁i + q₂j + q₃k 是复数的推广，其中i² = j² = k² = ijk = -1。一个单位四元数可以表示三维空间中的任意旋转：q = [cos(θ/2), sin(θ/2)·n]，其中n为旋转轴的单位向量，θ为旋转角度。相比旋转矩阵，四元数仅需4个参数，且不存在万向节锁问题，因此成为现代飞控姿态表示的标准选择。

四元数的乘法定义为：

q1 ⊗ q2 = [q1_0*q2_0 - q1_1*q2_1 - q1_2*q2_2 - q1_3*q2_3,
            q1_0*q2_1 + q1_1*q2_0 + q1_2*q2_3 - q1_3*q2_2,
            q1_0*q2_2 - q1_1*q2_3 + q1_2*q2_0 + q1_3*q2_1,
            q1_0*q2_3 + q1_1*q2_2 - q1_2*q2_1 + q1_3*q2_0]

四元数乘法表示旋转的合成：先进行q₂旋转再进行q₁旋转等价于一次旋转 q₁ ⊗ q₂。注意乘法顺序与旋转顺序的对应关系，这与矩阵乘法类似。

2.4.2 操作流程：四元数在飞控中的完整使用链路

1. 初始化：将初始欧拉角转换为四元数 q_init。
2. 陀螺仪积分：根据角速度 ω = [p, q, r]^T 更新四元数：q̇ = 0.5 · q ⊗ [0, ω_x, ω_y, ω_z]。
3. 加速度计/磁力计修正：计算参考向量（重力方向、磁场方向）与由当前四元数推算出的向量之间的误差，通过PI控制器修正陀螺仪漂移。
4. 四元数归一化：每次更新后执行 q = q/|q|，防止数值漂移导致非单位四元数。
5. 欧拉角提取：需要时从四元数提取欧拉角用于控制律或遥测。

2.4.3 案例拆解：四元数在姿态解算中的完整实现

以经典的Mahony互补滤波算法为例，其核心步骤如下：

// 1. 读取陀螺仪和加速度计数据
float gx, gy, gz;  // 陀螺仪角速度 (rad/s)
float ax, ay, az;  // 加速度计比力 (m/s²)

// 2. 归一化加速度计数据
float norm = sqrt(ax*ax + ay*ay + az*az);
ax /= norm; ay /= norm; az /= norm;

// 3. 由当前四元数推算重力方向
float vx = 2*(q1*q3 - q0*q2);
float vy = 2*(q0*q1 + q2*q3);
float vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

// 4. 叉积计算误差
float ex = ay*vz - az*vy;
float ey = az*vx - ax*vz;
float ez = ax*vy - ay*vx;

// 5. PI修正陀螺仪
gx += Kp*ex + Ki*exInt;
gy += Kp*ey + Ki*eyInt;
gz += Kp*ez + Ki*ezInt;

// 6. 四元数积分更新
q0 += 0.5f*(-q1*gx - q2*gy - q3*gz)*dt;
q1 += 0.5f*( q0*gx + q2*gz - q3*gy)*dt;
q2 += 0.5f*( q0*gy - q1*gz + q3*gx)*dt;
q3 += 0.5f*( q0*gz + q1*gy - q2*gx)*dt;

// 7. 归一化
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0/=norm; q1/=norm; q2/=norm; q3/=norm;

该算法在STM32F405上运行频率可达1kHz，CPU占用率不足5%。通过调整Kp和Ki参数，可以平衡响应速度与噪声抑制能力。Kp过大则高频噪声放大，Kp过小则动态响应迟缓；Ki用于消除稳态误差，但过大会导致超调。

2.5 四种数学工具的对比与选型

2.5.1 选型建议

在实际飞控开发中，四种工具并非互斥，而是协同工作：

• 传感器驱动层：使用向量表示原始数据，进行归一化、滤波等预处理。
• 姿态估计层：使用四元数作为姿态状态量，配合陀螺仪积分和向量观测修正。
• 控制律层：将四元数转换为旋转矩阵，用于误差计算和控制量映射。
• 调试分析层：使用复数进行频域分析，设计滤波器参数，评估系统稳定性。