第2章 运动学与动力学建模：刚体运动学、牛顿-欧拉方程、四旋翼动力学模型、控制分配矩阵
各位同学，欢迎来到第二章。
说实话，建模这件事，很多人觉得枯燥。但我要告诉你——模型是算法的灵魂。你设计的控制器再花哨，如果模型不准，飞起来就是灾难。我在项目里见过太多这样的案例了。
2.1 刚体运动学基础
先聊聊刚体运动学。说白了，就是研究物体怎么动，但不关心是什么力让它动的。
四旋翼在空中飞，我们关心两件事：位置和姿态。位置好理解，就是xyz坐标。姿态呢？就是飞机朝哪个方向。
描述姿态，我习惯用欧拉角。为什么？直观啊！
- 滚转角 φ：绕x轴旋转，飞机左右倾斜
- 俯仰角 θ：绕y轴旋转，飞机抬头低头
- 偏航角 ψ：绕z轴旋转，飞机左右转向
但有个坑——万向锁。当俯仰角接近±90°时，滚转和偏航会耦合在一起。嗯，这里要注意，如果你做特技飞行，千万别只用欧拉角。我建议用四元数，虽然不直观，但没这毛病。
核心公式：从机体坐标系到世界坐标系的旋转矩阵
R = Rz(ψ) * Ry(θ) * Rx(φ)记住顺序：先滚转，再俯仰，最后偏航。这个顺序不能乱。
2.2 牛顿-欧拉方程
好，现在聊动力学。动力学关心的是：力怎么让物体动起来。
牛顿第二定律大家都会：F = ma。但别忘了，四旋翼是刚体，还有转动呢。所以我们需要欧拉方程。
完整的牛顿-欧拉方程长这样：
平动：m * v_dot = F_ext - m * g
转动：I * ω_dot = M_ext - ω × (I * ω)看着复杂？其实拆开看就明白了。
平动部分：飞机受到的合力，减去重力，等于质量乘以加速度。
转动部分：外力矩，减去陀螺效应项，等于惯量乘以角加速度。
个人经验：我在做第一架无人机时，忽略了陀螺效应项。结果飞机一转弯就抖得厉害。后来加上这一项，立马稳了。别小看这个叉积项。
2.3 四旋翼动力学模型
现在把牛顿-欧拉方程应用到四旋翼上。
四旋翼有四个电机，每个电机产生升力和反扭矩。升力方向沿机体z轴向上，反扭矩方向与旋翼旋转方向相反。
这里有个关键点：电机布局。我习惯用X型布局，前后左右四个电机。1号电机顺时针，2号逆时针，3号顺时针，4号逆时针。为什么要交替？为了抵消反扭矩。
完整的四旋翼动力学模型：
位置方程：
x_ddot = (cosφ sinθ cosψ + sinφ sinψ) * (F1+F2+F3+F4) / m
y_ddot = (cosφ sinθ sinψ - sinφ cosψ) * (F1+F2+F3+F4) / m
z_ddot = -g + (cosφ cosθ) * (F1+F2+F3+F4) / m
姿态方程：
φ_ddot = (Mx + (Iy - Iz) * θ_dot * ψ_dot) / Ix
θ_ddot = (My + (Iz - Ix) * φ_dot * ψ_dot) / Iy
ψ_ddot = (Mz + (Ix - Iy) * φ_dot * θ_dot) / Iz看着眼晕？别怕。实际用的时候，我们通常做小角度假设。当飞机悬停或慢速飞行时，角度变化很小，cos≈1，sin≈θ。模型一下子就简化了。
注意：小角度假设只适用于悬停和慢速飞行。如果你要做穿越机那种暴力飞行，必须用完整模型。我曾经吃过这个亏，用简化模型做特技，结果仿真和实际差了十万八千里。
2.4 控制分配矩阵
好，终于到了控制分配。这是从控制量到电机指令的桥梁。
我们想要四个控制量：
- U1：总升力（油门）
- U2：滚转力矩
- U3：俯仰力矩
- U4：偏航力矩
四个电机，每个输出升力Fi。怎么把U映射到F？用控制分配矩阵。
对于X型布局：
[U1] [ 1 1 1 1 ] [F1]
[U2] = [ -1 1 1 -1 ] [F2]
[U3] [ -1 -1 1 1 ] [F3]
[U4] [ 1 -1 1 -1 ] [F4]等等，这个矩阵怎么来的？
U1是四个电机升力之和，简单。
U2是滚转力矩。1号和4号电机在x轴两侧，产生滚转。符号相反是因为旋转方向不同。
U3是俯仰力矩。1号和2号在y轴两侧。
U4是偏航力矩。注意，1号和3号顺时针，2号和4号逆时针。所以反扭矩方向不同。
关键点：控制分配矩阵的逆矩阵，用来把电机指令转换成控制量。实际飞控中，我们经常需要做混控，就是根据遥控器输入，计算出每个电机该转多快。
2.5 实战中的坑与经验
聊点实际的。建模这件事，我踩过的坑不少。
第一个坑：惯量参数不准。 我曾经用CAD算的惯量，结果飞机飞起来姿态响应完全不对。后来用实验辨识，才发现CAD算的差了30%。
第二个坑：电机响应延迟。 模型里假设电机瞬间响应，但实际电机有延迟。这个延迟在高频控制时特别明显。我建议在模型里加一阶低通滤波，模拟电机响应。
第三个坑：空气动力学效应。 四旋翼在高速飞行时，会有桨叶失速、地面效应、涡环状态等。这些在基础模型里都没考虑。如果你要做高速飞行，必须加补偿。
我的建议：先做基础模型，让飞机能飞起来。然后逐步加复杂项。别一开始就想搞完美模型，那会把你逼疯的。
2.6 本章小结
这一章我们聊了：
- 刚体运动学：位置和姿态的描述
- 牛顿-欧拉方程：力和运动的关系
- 四旋翼动力学模型：从物理到数学
- 控制分配矩阵：从控制量到电机指令
记住，模型是基础。模型不准，后面所有算法都是空中楼阁。下一章，我们会基于这个模型，开始设计控制器。
嗯，今天就到这里。有问题随时找我。
公众号:蓝海资料掘金营，微信deep3321