4. 刚体动力学建模：牛顿-欧拉方程，力与力矩如何影响无人机的运动
各位同学，今天我们聊一个硬核话题——刚体动力学建模。
说白了，就是搞清楚无人机为什么会飞，以及它怎么按照我们的指令去飞。你想想看，遥控器一推，飞机就往前窜，这背后是谁在干活？就是牛顿和欧拉这两位老爷子。
4.1 牛顿-欧拉方程：飞控的“物理引擎”
我个人习惯把牛顿-欧拉方程比作飞控系统的“物理引擎”。就像游戏里的人物要受重力、碰撞影响一样，无人机在真实世界里也得遵守物理定律。
牛顿第二定律告诉我们：力 = 质量 × 加速度。欧拉方程则补充了旋转的部分：力矩 = 转动惯量 × 角加速度。
把这两者合在一起，就是完整的刚体运动方程：
m * (dV/dt) = F_gravity + F_thrust + F_drag + F_other
I * (dω/dt) = M_thrust + M_gyro + M_aero + M_other这里：
- m —— 无人机总质量
- V —— 速度向量
- I —— 转动惯量矩阵（3x3）
- ω —— 角速度向量
- F 和 M —— 各种力和力矩
核心要点：力决定平动，力矩决定转动。两者通过电机转速耦合在一起——你推油门，四个电机转速一起变，升力变了，力矩也可能跟着变。
4.2 力从哪来？力矩又怎么算？
我在项目中遇到过不少新手，一上来就写代码，结果飞机翻跟头。为什么？因为力的分解没搞明白。
无人机受到的力主要有三类：
- 重力 —— 始终指向地心，简单但别忽略
- 螺旋桨升力 —— 每个电机产生一个垂直于桨平面的力
- 空气阻力 —— 和速度的平方成正比，方向相反
力矩呢？更关键：
- 滚转力矩 —— 左右电机转速差产生
- 俯仰力矩 —— 前后电机转速差产生
- 偏航力矩 —— 对角电机转速差产生（反扭矩效应）
我的经验：写代码时，先把力和力矩分开算，最后再合成。我曾经把力矩算到力里面，结果仿真时飞机像火箭一样螺旋升天……排查了两天才发现。
4.3 坐标系转换：别让数学坑了你
嗯，这里要注意。力和力矩都是在机体坐标系下计算的，但重力是在世界坐标系下作用的。怎么转？用旋转矩阵。
我建议你记住这个公式：
F_world = R * F_body其中 R 是 3x3 的旋转矩阵，由欧拉角（横滚φ、俯仰θ、偏航ψ）构成：
R = [cosθ·cosψ, sinφ·sinθ·cosψ - cosφ·sinψ, cosφ·sinθ·cosψ + sinφ·sinψ;
cosθ·sinψ, sinφ·sinθ·sinψ + cosφ·cosψ, cosφ·sinθ·sinψ - sinφ·cosψ;
-sinθ, sinφ·cosθ, cosφ·cosθ ]避坑指南：我曾经因为欧拉角的旋转顺序搞反了，导致仿真时飞机横滚和俯仰总是反着来。记住：无人机通常用 Z-Y-X 顺序（偏航→俯仰→横滚）。
4.4 转动惯量：别猜，要测
很多同学问我：“老师，转动惯量能不能估算？”我的回答是：可以估算，但最好实测。
转动惯量 I 是一个 3x3 矩阵，对角线是绕各轴的惯量，非对角线是耦合项。对于对称的四旋翼，非对角线项很小，可以近似为0。
估算方法：
- 把无人机拆成几个简单几何体（电机、机臂、中心板）
- 分别计算每个部分的惯量
- 用平行轴定理合成
但说实话，我踩过这个坑。估算出来的惯量误差能到30%，导致PID参数怎么调都调不好。后来买了台惯量测试台，一测就准了。
4.5 知识体系总览
下面这张图，是我自己总结的刚体动力学建模逻辑，你一看就明白：
4.6 代码实现：从理论到实践
光说不练假把式。下面是我常用的刚体动力学仿真代码片段，用 Python 写的：
import numpy as np
class QuadrotorDynamics:
def __init__(self, mass, inertia_matrix):
self.m = mass # 质量 (kg)
self.I = inertia_matrix # 转动惯量矩阵
self.g = 9.81 # 重力加速度
# 状态: [x, y, z, vx, vy, vz, φ, θ, ψ, p, q, r]
self.state = np.zeros(12)
def compute_forces_and_moments(self, motor_speeds):
# motor_speeds: [ω1, ω2, ω3, ω4]
k_f = 1e-5 # 升力系数
k_m = 1e-6 # 力矩系数
l = 0.25 # 机臂长度 (m)
# 每个电机的升力
F = k_f * motor_speeds**2
# 总升力 (Z轴方向)
total_thrust = np.sum(F)
# 力矩
Mx = l * (F[1] - F[3]) # 滚转
My = l * (F[0] - F[2]) # 俯仰
Mz = k_m * (F[0] - F[1] + F[2] - F[3]) # 偏航
return total_thrust, np.array([Mx, My, Mz])
def step(self, motor_speeds, dt):
# 计算力和力矩
thrust, moments = self.compute_forces_and_moments(motor_speeds)
# 提取状态
x, y, z, vx, vy, vz, phi, theta, psi, p, q, r = self.state
# 旋转矩阵 (ZYX顺序)
R = self.euler_to_rotation_matrix(phi, theta, psi)
# 重力在世界坐标系
F_gravity = np.array([0, 0, -self.m * self.g])
# 推力在机体坐标系，转到世界坐标系
F_thrust_body = np.array([0, 0, thrust])
F_thrust_world = R @ F_thrust_body
# 合力
F_total = F_gravity + F_thrust_world
# 平动加速度
acc = F_total / self.m
# 角加速度 (欧拉方程)
omega = np.array([p, q, r])
alpha = np.linalg.inv(self.I) @ (moments - np.cross(omega, self.I @ omega))
# 更新状态 (简单欧拉积分)
self.state[3:6] += acc * dt # 速度
self.state[0:3] += self.state[3:6] * dt # 位置
self.state[9:12] += alpha * dt # 角速度
self.state[6:9] += self.state[9:12] * dt # 欧拉角
return self.state实用建议：实际项目中，别用欧拉积分，误差太大。我一般用四阶龙格-库塔法（RK4），精度高很多。另外，欧拉角在俯仰±90°时会奇异，这时候用四元数更稳。
4.7 常见坑与避坑指南
最后，分享几个我踩过的坑：
- 转动惯量搞错 —— 我曾经把电池放在机臂上，结果惯量分布变了，飞控参数全得重调。记住：质量分布直接影响转动惯量。
- 忽略空气阻力 —— 室内飞还好，室外有风时，阻力项不能省。我有个项目，飞机在三级风下直接飘走了……
- 坐标系搞混 —— 力在哪个坐标系算的，力矩在哪个坐标系算的，一定要写清楚注释。不然过两周你自己都看不懂。
- 积分步长太大 —— 仿真步长建议 1ms 以内，太大容易发散。我见过有人用 10ms 步长，结果飞机在仿真里疯狂震荡。
好了，刚体动力学建模就讲到这里。记住：力决定怎么飞，力矩决定怎么转。把这两个搞明白，后面的控制算法就好办了。