2. 运动学方程：位置运动学、姿态运动学（欧拉角法）、姿态运动学（四元数法）、运动学方程离散化
各位同学，欢迎来到运动学方程这一节。
说实话，动力学方程讲的是「力怎么让飞机动」，而运动学方程讲的是「动起来之后，位置和姿态怎么变」。两者是递进关系。我个人习惯把运动学拆成两部分：位置运动学和姿态运动学。位置运动学相对简单，姿态运动学才是真正的「坑」。今天咱们就把这两个坑填平。
核心思想：运动学方程不关心力，只关心速度、角速度与位置、姿态之间的几何关系。
2.1 位置运动学
位置运动学说白了就是：速度怎么积分成位置。
我们定义机体坐标系下的速度向量为 V_b = [u, v, w]^T，分别代表前向、侧向、垂向速度。但我们要的是地面坐标系下的位置 P_n = [x, y, z]^T。所以需要做一个坐标变换。
公式很简单：
P_n_dot = R_b^n * V_b其中 R_b^n 是从机体坐标系到地面坐标系的旋转矩阵。展开写就是：
[x_dot] [cosθ cosψ sinφ sinθ cosψ - cosφ sinψ cosφ sinθ cosψ + sinφ sinψ] [u]
[y_dot] = [cosθ sinψ sinφ sinθ sinψ + cosφ cosψ cosφ sinθ sinψ - sinφ cosψ] [v]
[z_dot] [ -sinθ sinφ cosθ cosφ cosθ ] [w]嗯，看着有点吓人。但实际用的时候，我们通常只关心高度通道。我记得有一次做室内定位项目，水平位置用光流，高度用气压计。当时我就在想：位置运动学其实就干了一件事——把机体速度投影到地面系，然后积分。
小技巧：实际工程中，位置运动学通常和GPS/IMU融合一起做。纯积分会漂，你想想看，IMU的零偏一累积，位置就跑飞了。所以一定要加反馈修正。
2.2 姿态运动学（欧拉角法）
姿态运动学要解决的是：角速度怎么积分成姿态角。
欧拉角法是最直观的方法。我们用 Φ = [φ, θ, ψ]^T 表示滚转、俯仰、偏航角。机体角速度 ω_b = [p, q, r]^T 和欧拉角速率之间有一个转换关系：
[φ_dot] [1 sinφ tanθ cosφ tanθ] [p]
[θ_dot] = [0 cosφ -sinφ ] [q]
[ψ_dot] [0 sinφ/cosθ cosφ/cosθ] [r]这个矩阵叫欧拉角运动学矩阵。注意看，当 θ = ±90° 时，tanθ 和 1/cosθ 会趋于无穷大。这就是著名的万向锁问题。
警告：欧拉角法在俯仰角接近 ±90° 时会出现奇异。我曾在做特技飞行控制时踩过这个坑——飞机一翻过来，姿态解算直接炸了。所以，如果你做的是全姿态飞行（比如穿越机、特技无人机），千万别只用欧拉角法。
另外，欧拉角法还有一个缺点：计算量大。每次更新都要算一堆三角函数。在早期的单片机（比如STM32F1）上，这玩意儿挺吃资源的。
2.3 姿态运动学（四元数法）
四元数法就是为了解决欧拉角的两个痛点：奇异性和计算效率。
四元数 q = [q0, q1, q2, q3]^T 是一个超复数，它用四个参数表示旋转，没有奇点。四元数运动学方程如下：
q_dot = 0.5 * q ⊗ ω_b写成矩阵形式：
[q0_dot] [ 0 -p -q -r] [q0]
[q1_dot] = [ p 0 r -q] [q1]
[q2_dot] [ q -r 0 p] [q2]
[q3_dot] [ r q -p 0] [q3]你看，全是加减乘除，没有三角函数。效率高多了。
个人经验：我现在所有项目都用四元数法做姿态解算。欧拉角只用在最后输出给地面站显示，或者做角度环控制时用。内部积分、滤波、预测，全部用四元数。省心，不出幺蛾子。
但四元数也有一个坑：归一化。由于数值积分误差，四元数的模会慢慢偏离1。如果不做归一化，姿态会慢慢扭曲。我建议每次更新后都做一次归一化：
q = q / norm(q)就这么一行代码，能救你命。
2.4 运动学方程离散化
连续方程写完了，但飞控是数字系统，得离散化。常用的方法有两种：欧拉法和龙格-库塔法。
2.4.1 欧拉法（一阶）
最简单，也最常用：
x(k+1) = x(k) + dt * f(x(k), u(k))对于位置运动学：
P_n(k+1) = P_n(k) + dt * R_b^n(k) * V_b(k)对于四元数运动学：
q(k+1) = q(k) + dt * 0.5 * q(k) ⊗ ω_b(k)
q(k+1) = q(k+1) / norm(q(k+1)) // 归一化注意：欧拉法在dt较大时误差会累积。我一般把姿态环跑在1kHz（dt=0.001s），位置环跑在100Hz（dt=0.01s）。这样欧拉法完全够用。
2.4.2 四阶龙格-库塔法（RK4）
如果你对精度要求高，或者dt比较大（比如50Hz），可以用RK4。以四元数为例：
k1 = 0.5 * q(k) ⊗ ω_b(k)
k2 = 0.5 * (q(k) + dt/2 * k1) ⊗ ω_b(k + dt/2)
k3 = 0.5 * (q(k) + dt/2 * k2) ⊗ ω_b(k + dt/2)
k4 = 0.5 * (q(k) + dt * k3) ⊗ ω_b(k + dt)
q(k+1) = q(k) + dt/6 * (k1 + 2*k2 + 2*k3 + k4)
q(k+1) = q(k+1) / norm(q(k+1))嗯，代码量翻倍了。但精度确实好。我曾经在PX4的仿真里对比过，RK4比欧拉法在相同dt下误差小一个数量级。
避坑指南：我曾经在低算力芯片上硬跑RK4，结果CPU占用率飙到80%。后来发现，对于大多数多旋翼应用，欧拉法+小dt完全够用。RK4更适合仿真或者高精度惯性导航。别盲目追求高阶算法，够用就好。
2.5 知识体系总览
下面这张图总结了本章的核心逻辑：
从图中可以看到：速度→运动学→离散化→位置/姿态，这就是运动学方程的全链路。我个人建议你先把四元数法吃透，因为它是现代飞控的标配。欧拉角法可以作为理解工具，但别在工程里依赖它。
好了，运动学方程就讲到这里。记住：位置运动学是投影+积分，姿态运动学是旋转+积分。离散化时，选对方法比选高阶方法更重要。下一节我们会把这些方程和动力学方程联立，构建完整的六自由度模型。
课后练习：写一个Python脚本，分别用欧拉法和RK4离散化四元数运动学方程，对比在dt=0.01s和dt=0.001s下的姿态误差。你会发现，dt越小，两种方法越接近。