前阵子帮一位师弟调四旋翼的悬停效果PID参数在真机上反反复复折腾了一周一会儿低头一会儿抖舵最后把问题定位到机架的重心偏置和推力系数不准确上。那时候我就跟他说如果你手里有一个靠谱的四旋翼无人机数学模型很多参数可以先在仿真里跑明白而不是拿着真机一遍遍试错。这篇博客我打算把四旋翼无人机的数学模型从头到尾推一遍用最朴素的牛顿-欧拉方法覆盖从坐标系选择、旋转矩阵推导到线运动方程、角运动方程最后整理成可以直接用于仿真的状态空间模型并给出常见参数和验证方法。这是做飞控、做仿真、写毕业论文的人必备的一块基础工作。尤其是如果你打算做控制器设计、状态估计或者故障诊断这个模型就是你所有工作的地基。我会尽量把每一步都写清楚包括那些教材里经常一笔带过、但实际推导时特别容易卡壳的细节。1. 建模前必须想清楚的三件事坐标系、方法、姿态1.1 为什么先定坐标系机体坐标系与地面坐标系的物理意义很多新手拿到四旋翼的第一反应是直接列运动方程。但我在实际推导中最大的体会是坐标系没定清楚后面全是坑。四旋翼的动力学天然涉及两套坐标系。一套是地面坐标系通常叫NED坐标系也就是北东地North-East-Downx轴指向北y轴指向东z轴指向下。这是导航和航线规划用的坐标系。另一套是机体坐标系固定在小飞机上原点在重心x轴指向机头y轴指向右翼z轴指向机腹方向。为什么需要两套坐标系因为力的产生和运动状态的测量分散在这两套坐标系里。陀螺仪、加速度计装在机身上测量的是体轴系下的角速度和加速度而GPS、高度计、航线规划都在地面坐标系里。你要描述飞机的运动轨迹必须把机体坐标系下的力投影到地面坐标系或者把地面坐标系下的位置换算到机体系下。没有谁好谁坏这就是一个坐标系变换问题。这里要特别强调很多文献里z轴是向上的East-North-UpENU但在四旋翼飞控里NED是绝对主流。Pixhawk、PX4、APM这些开源飞控全部用NED。第一次建模的时候如果没注意到这个细节你推导出来的重力符号会是反的姿态解算也会出问题。我的建议是如果你做四旋翼直接跟NED走别在这种地方标新立异。1.2 建模方法选型为什么我选牛顿-欧拉而不是拉格朗日建立四旋翼动力学模型主要有两条路线牛顿-欧拉法和拉格朗日法。拉格朗日法的核心是写出系统的动能和势能然后通过拉格朗日方程推导运动方程。它的好处是从能量角度出发不需要处理约束力非常适合带关节约束的机械系统比如机械臂。但四旋翼本质上是一个无约束的自由刚体在三维空间中飞行它的所有运动都由合外力和合外力矩决定。这个时候用牛顿-欧拉法是最直观的把一个刚体拿出来把每个旋翼产生的推力、反扭矩、重力、陀螺力矩全部画出来然后直接写运动方程。两套方法推出来的结果当然是一样的但牛顿-欧拉法在工程上更友好。因为你在后面做控制器设计、做故障诊断时需要明确知道“这个力矩是由哪几个旋翼贡献的”牛顿-欧拉法的每一项都有明确的物理对应。拉格朗日法虽然数学上很优雅但中间会引入广义坐标和广义力反而增加了理解的负担。我这几年看过的论文里真正做四旋翼建模和控制的基本都用牛顿-欧拉法这是个经验规律不是数学上的必然。所以我强烈建议入门者从牛顿-欧拉法入手。1.3 姿态描述选型欧拉角的适用边界在哪里姿态描述方式有三个选项欧拉角、方向余弦矩阵、四元数。方向余弦矩阵就是旋转矩阵用来做坐标变换很方便但9个变量加上正交约束不适合直接作为控制器变量。四元数是飞控里最常用的姿态表示因为它只有4个参数没有奇异性计算效率高。但四元数的物理意义不够直观你很难直接说“四元数(0.707, 0, 0.707, 0)对应多大的俯仰角”。欧拉角的优势是直观三个角度分别对应横滚、俯仰、偏航跟飞手遥控器的通道一一对应。所以在建模阶段几乎所有教程和论文都用欧拉角来表达姿态。但欧拉角有个著名的坑叫万向节锁。当俯仰角达到正负90度时横滚和偏航的旋转轴重合会丢失一个自由度。对四旋翼来说正常飞行时俯仰角很少超过30度所以用欧拉角建模完全够用。但如果以后做特技飞行、翻滚动作就必须切换到四元数模型。我的思路是建模和基础控制用欧拉角方便理解和调参等到做高级飞行动作时再升级成四元数。这两份模型我都维护过转换的坑后面我会专门写一节。2. 运动学建模旋转矩阵和姿态角速度的换算2.1 旋转矩阵完整推导Z-Y-X顺序下的欧拉角变换运动学的第一个核心任务是建立机体坐标系和地面坐标系之间的变换关系也就是求旋转矩阵。我采用的欧拉角旋转顺序是Z-Y-X这是航空领域的标准顺序先绕z轴旋转偏航角ψ再绕新的y轴旋转俯仰角θ最后绕最新的x轴旋转横滚角φ。注意这里的每次旋转都是绕旋转后的新轴进行的这叫内旋方式。三个单轴旋转矩阵分别是这样的绕z轴旋转ψ Rz [cosψ, -sinψ, 0; sinψ, cosψ, 0; 0, 0, 1]绕y轴旋转θ Ry [cosθ, 0, sinθ; 0, 1, 0; -sinθ, 0, cosθ]绕x轴旋转φ Rx [1, 0, 0; 0, cosφ, -sinφ; 0, sinφ, cosφ]总的旋转矩阵按照Z-Y-X顺序相乘得到从机体坐标系到地面坐标系的变换矩阵R_eb Rz(ψ) * Ry(θ) * Rx(φ)展开之后就是R_eb [ 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θ ]这里我想多说一句旋转矩阵的列向量有明确的物理意义。第一列是机体x轴在地面系中的投影第二列是机体y轴在地面系中的投影第三列是机体z轴在地面系中的投影。这个理解非常有用因为后面计算推力在NED系下的投影时就是直接取第三列。旋转顺序不是可以随便选的。旋转不满足交换律先绕y轴转30度再绕x轴转30度和先绕x轴转30度再绕y轴转30度结果是完全不同的。你看生活里手机横竖屏切换就知道先横着转再竖着转跟先竖着转再横着转最终屏幕朝向不一样。所以推导之前一定要在文档里明确标注旋转顺序否则模型和别人对不上。2.2 姿态角速度到机体角速度的变换欧拉角速率那点事运动学的第二个任务是把欧拉角的变化率换算成机体的滚转角速度p、俯仰角速度q和偏航角速度r。初学者最容易在这里犯错以为欧拉角导数就是机体角速度。真不是。欧拉角是绕三个不同的中间坐标系轴旋转的而p、q、r是绕机体坐标系三个轴的角速度分量两者之间隔着一个变换矩阵。推导的思路是把三个欧拉角速度分别投影到机体坐标系上然后叠加。绕z轴的偏航角速度ψ̇方向是地面系z轴投影到机体系时需要经过Rx和Ry的旋转绕y轴的俯仰角速度θ̇方向是中间坐标系的y轴投影到机体系时只需要经过Rx绕x轴的横滚角速度φ̇方向和机体x轴重合直接就是φ̇。整理之后得到p φ̇ - ψ̇ sinθ q θ̇ cosφ ψ̇ sinφ cosθ r -θ̇ sinφ ψ̇ cosφ cosθ写成矩阵形式[p, q, r]^T [1, 0, -sinθ; 0, cosφ, sinφcosθ; 0, -sinφ, cosφcosθ] * [φ̇, θ̇, ψ̇]^T这个变换矩阵在俯仰角θ等于正负90度时会奇异这就是欧拉角的万向节锁问题在数学上的体现。做姿态解算时如果飞机做了大角度俯仰动作这个矩阵会变成奇异矩阵导致欧拉角速率求解失败。这也是为什么飞控内部姿态解算要用四元数。2.3 运动学方程的完整状态表达把位置和姿态的运动学放到一起就得到运动学方程组ẋ u ẏ v ż w φ̇ p q sinφ tanθ r cosφ tanθ θ̇ q cosφ - r sinφ ψ̇ q sinφ / cosθ r cosφ / cosθ前面三个是位置速度关系后面三个是角速度关系。这组方程本身不包含力只描述几何关系所以叫运动学。在实际工程中这组方程经常作为状态方程的前半部分写在系统里。我要提醒你一件事当你看到角速度转换矩阵里出现tanθ和1/cosθ这些项时就要意识到俯仰角接近90度时数值会爆炸。所以飞行控制在大角度状态下一般都会切换到四元数姿态表示。3. 动力学建模力和力矩如何变成加速度3.1 四旋翼的力和力矩是怎么产生的动力学建模的前提是搞清楚有哪几个力和力矩作用在机架上。首先是旋翼推力。单个旋翼旋转时产生的推力近似与转速的平方成正比写成T_i k * ω_i²其中k是推力系数和桨的形状、直径、空气密度有关。四个旋翼产生四个推力方向都沿机体坐标系的z轴负方向。其次是反扭矩。旋翼旋转时会带动空气空气反过来对机身产生一个反扭矩大小近似为M_i d * ω_i²d是反扭矩系数。反扭矩的方向与旋翼旋转方向相反这就是为什么四旋翼要使用反转对1号电机和3号电机逆时针旋转2号和4号电机顺时针旋转这样悬停时四个反扭矩可以相互抵消。第三是陀螺力矩。当电机高速旋转时如果把机架转动一个方向电机的角动量变化会产生一个作用于机架的力矩这叫陀螺效应。快速做俯仰或横滚动作时这个力矩会很明显。第四是重力作用在地面系z轴正方向因为NED下z向下。最后是气动阻力在低速飞行时通常被忽略但高速飞行时必须考虑。3.2 线动量方程重力、推力和加速度的关系线动量方程的本质是牛顿第二定律ma F。但这里的加速度必须是地面坐标系下的惯性加速度而推力是沿机体坐标系z轴的所以需要通过旋转矩阵做投影。假设四个旋翼的总推力为T T1 T2 T3 T4在机体坐标系下推力向量为[0, 0, -T]^T。把它通过旋转矩阵投影到地面坐标系[0, 0, -T]^T 经过R_eb变换后等于取R_eb的第三列乘以-T F_thrust_e [-T(cosφ sinθ cosψ sinφ sinψ)] [-T(cosφ sinθ sinψ - sinφ cosψ)] [-T cosφ cosθ]重力在地面系下是[0, 0, mg]^T其中g是重力加速度。于是得到线运动方程m ẍ -T(cosφ sinθ cosψ sinφ sinψ) m ÿ -T(cosφ sinθ sinψ - sinφ cosψ) m z̈ mg - T cosφ cosθ这里最让人不习惯的是z向下时重力是正的mg推力是负方向。所以悬停时T mg / (cosφ cosθ)在水平姿态下就是T mg四个旋翼各分mg/4。这个式子非常常用做新手调参时先把每个旋翼的目标推力算出来就能估计悬停油门在哪。3.3 角动量方程机体坐标系下的欧拉方程角动量方程是动力学里最复杂的部分。惯性坐标系下角动量定理很简单麻烦的是惯性张量在惯性系下是变化的所以通常在机体坐标系下研究角运动。四旋翼结构近似对称所以惯性张量可以近似为对角阵J diag(Ixx, Iyy, Izz)机体坐标系下的欧拉方程为J ω̇ ω × (J ω) M写成三个分量方程Ixx ṗ (Iyy - Izz) q r Mx Iyy q̇ (Izz - Ixx) p r My Izz ṙ (Ixx - Iyy) p q Mz这里Mx, My, Mz分别是作用在机体系三个轴上的外力矩。对十字形四旋翼当1号、3号电机逆时针2号、4号顺时针时Mx k l (ω2² - ω4²) My k l (ω3² - ω1²) Mz d (ω1² - ω2² ω3² - ω4²)其中l是机臂长度也就是从重心到电机转轴的距离。Mx由左右两边的推力差产生My由前后两边的推力差产生Mz由四个反扭矩的代数差产生。如果你想建立更完整的模型可以在角动量方程中加入电机转子的陀螺项。每个电机的角动量为J_rot * ω_i当机体旋转时这个角动量变化会产生陀螺力矩。但很多控制论文在悬停工作点附近会把这项忽略掉实测下来影响也确实不大。3.4 完整非线性动力学方程组把运动学和动力学合在一起我习惯把完整的非线性模型写成以下形式ẍ - (k/m)(ω1² ω2² ω3² ω4²)(cosφ sinθ cosψ sinφ sinψ) ÿ - (k/m)(ω1² ω2² ω3² ω4²)(cosφ sinθ sinψ - sinφ cosψ) z̈ g - (k/m)(ω1² ω2² ω3² ω4²) cosφ cosθ φ̈ [ (Iyy - Izz) θ̇ψ̇ k l (ω2² - ω4²) ] / Ixx θ̈ [ (Izz - Ixx) φ̇ψ̇ k l (ω3² - ω1²) ] / Iyy ψ̈ [ (Ixx - Iyy) φ̇θ̇ d (ω1² - ω2² ω3² - ω4²) ] / Izz这里的变量是六个位置x, y, z和姿态角φ, θ, ψ输入是四个旋翼转速ω1, ω2, ω3, ω4。这就是四旋翼最典型的欠驱动特性六个自由度只有四个输入所以四旋翼不能独立控制每个自由度必须有耦合。这也是为什么无人机控制比固定翼更复杂、也更需要数学模型支撑。方程里有一个很容易被忽视的点是下标顺序必须跟你画的电机布局保持一致。假设换一种电机转向布局Mz那项的加减号就会全部反过来。这一点在复现论文时尤其要小心很多论文不画电机转向图直接给公式你抄过来就会符号反着。4. 最终模型长什么样状态空间方程与参数实例4.1 状态变量怎么选输入怎么定义数学模型最终要整理成适合仿真和控制的形式。控制里最常用的是状态空间方程先定义状态向量。状态量选12个维度x_vec [x, y, z, φ, θ, ψ, ẋ, ẏ, ż, p, q, r]^T其中[x, y, z]是NED系下的位置[φ, θ, ψ]是姿态角[ẋ, ẏ, ż]是NED系下的速度[p, q, r]是体轴系下的角速度。输入向量一般有两种取法。一种直接用四个旋翼转速的平方U [ω1², ω2², ω3², ω4²]好处是和底层电机控制直接对应。另一种先合成虚拟控制量U1 k (ω1² ω2² ω3² ω4²) // 总推力 U2 k l (ω2² - ω4²) // 横滚力矩 U3 k l (ω3² - ω1²) // 俯仰力矩 U4 d (ω1² - ω2² ω3² - ω4²) // 偏航力矩这个交换矩阵的好处是位置和姿态方程可以解耦成四个通道做PID控制器时一目了然。确实实际做控制器时姿态环基本都用U1到U4这四个虚拟控制量来设计。需要注意的是U1要除以m之后再代入加速度方程而U2到U4代入角加速度方程时还要除以对应的转动惯量。4.2 一套可以直接动手仿真的参数实例建模完成之后参数怎么来理论计算加实验拟合。推力系数k和反扭矩系数d可以通过拉力测试台实测转动惯量可以通过摆锤法或者CAD模型估算。这里给一套比较典型的四旋翼参数适合做第一版仿真验证参数符号数值单位质量m1.3kg重力加速度g9.81m/s²机臂长度l0.23mx轴惯量Ixx7.7e-3kg·m²y轴惯量Iyy7.7e-3kg·m²z轴惯量Izz1.3e-2kg·m²推力系数k1.4e-5N/(rad/s)²反扭矩系数d2.0e-7N·m/(rad/s)²这套参数参考了常见350级别四旋翼的数据虽然不是某个具体机型的精确值但数量级是对的。你拿到自己的真机后建议用拉力计重新测量k和d这两个参数对模型的准确性影响最大。拿到参数后的第一件事是验证悬停工作点。用简单脚本算一下悬停时每个旋翼的角速度import numpy as np m 1.3 g 9.81 k 1.4e-5 # 悬停时总推力等于重力四个旋翼均分 T_hover m * g omega_hover np.sqrt(T_hover / (4 * k)) print(omega_hover) # 大约 477 rad/s print(T_hover) # 12.753 N和 m*g 一致算出来大概是477 rad/s。如果你实测的悬停油门对应的角速度和这个值差距很大那说明你的推力系数k可能标定得有问题。这种交叉验证非常有用可以帮助你在建模阶段就发现参数异常。4.3 状态空间方程的完整矩阵形式把上面的方程整理成标准状态空间形式。完整12维状态方程虽然看着长但对后续做线性化、做LQR、做卡尔曼滤波都很有用。下面是整理好的标准形式状态方程ẋ ẋẏ ẏż żφ̇ pθ̇ qψ̇ rẍ -U1 (cosφ sinθ cosψ sinφ sinψ) / mÿ -U1 (cosφ sinθ sinψ - sinφ cosψ) / mz̈ g - U1 cosφ cosθ / mṗ [ (Iyy - Izz) q r U2 ] / Ixxq̇ [ (Izz - Ixx) p r U3 ] / Iyyṙ [ (Ixx - Iyy) p q U4 ] / Izz这个模型就是后续所有工作的基础。做姿态控制时重点关注后六个方程做导航控制时前六个方程负责把姿态和位置接起来。顺便回应一个经常被问到的问题数学模型必须有最终图吗有些人搜“数学模型应该有什么图”其实最终模型是一组方程不一定非要画成结构框图。但工程上我们通常补一张系统结构图把“位置环→姿态环→电机→机体动力学→传感器”这条链路画出来让读者知道模型在系统里的位置。这张图不是模型本身而是模型的说明书。4.4 工作点线性化与简化模型完整的非线性模型适合仿真但控制器设计往往需要线性模型。四旋翼最常见的线性化是在悬停工作点做的也就是把φ、θ、ψ、角速度都当作小量忽略二阶项。在悬停点附近做小扰动线性化可以得到非常经典的简化模型ẍ ≈ -g (θ cosψ φ sinψ) ÿ ≈ -g (θ sinψ - φ cosψ) z̈ ≈ g - U1 cosφ cosθ / m ≈ g - U1 / m φ̈ ≈ U2 / Ixx θ̈ ≈ U3 / Iyy ψ̈ ≈ U4 / Izz这几个简化公式几乎是所有四旋翼控制论文的起步点也是PID整定时最常用的近似模型。设计高度控制器时直接用z̈ g - U1/m再做个比例微分控制就已经能飞得不错了。这里有个细节值得提醒水平位置加速度其实是通过姿态来间接控制的。从式子可以看出来ẍ和ÿ主要由θ和φ决定也就是说你想往北飞得先有一个朝北的推力分量那就得先把机头俯仰下去。这就是四旋翼位置控制的本质逻辑。5. 建模过程中踩过的坑与模型验证方法5.1 欧拉角定义不一致导致的符号灾难我在建模和阅读论文时遇到过最多的坑就是欧拉角旋转顺序不一致。有的论文用Z-X-Y有的用Y-X-Z还有的用内旋有的用外旋最后推出来的旋转矩阵看起来完全不一样但其实是同一套物理关系的不同表达。踩了几次坑之后我的经验是拿到任何一篇文献先去看它的坐标系定义和旋转顺序再去看它的旋转矩阵长什么样。如果这些都对不上那它的姿态方程和你的对不上是正常的不代表谁错了。你只需要把你的模型固定成一套标准约定比如Z-Y-X内旋加NED坐标系然后用这个约定去统一所有工作。还有一个小技巧建模时先在纸上画出三个坐标轴和旋转顺序放在手边。推导过程中每写一个矩阵都对一下轴的指向。这个习惯帮我避免了很多粗心错误。5.2 旋转矩阵方向搞反这是最隐蔽的错误另一个高频错误是旋转矩阵方向反了。R_eb表示从机体系到地面系的变换R_be表示从地面系到机体系的变换。这两者互为转置因为是正交矩阵逆等于转置。如果你把R_eb写成了R_be那么推力投影的公式也会反过来整个模型就和真实运动相反。一个典型的特征是如果你仿真出来的飞机油门一加反而往下掉大概率就是这里反了。验证方向是否正确的方法很简单取一个只有偏航角ψ的特殊情况让φθ0然后看推力投影公式。此时R_eb Rz(ψ)你把机体系z轴的推力[0,0,-T]投影到地面系如果飞机机头朝北ψ0推力应该正好朝向正下方也就是z轴正方向如果机头朝东ψ90度推力投影应该也转90度。假如仿真结果不符合这个直觉就说明矩阵方向写反了。5.3 推力系数和反扭矩系数的标定误差模型参数中质量和惯量比较容易精确获得真正难的是推力系数k和反扭矩系数d。这两个系数不是恒定常数它和桨的转速、空气密度、电机温度都有关系。如果在仿真里用的k和实际用的差20%那么模型预测的悬停油门就会差很多。我的经验是拿到一个新机架后第一件事就是做静拉力测试。把电机桨装到拉力台上从低油门逐步加到高油门记录转速和拉力然后拟合k值。有条件的话测一下不同电压下的k值你会发现电压低时同样的转速拉力会略小。反扭矩系数d更难测因为它不是直接拉力而是让机体旋转的力矩。工程上常用一种间接方法让四旋翼悬停然后逐步加大偏航控制量记录达到稳定偏航角速率时的输入再反推d。这个方法精度不算高但足够用于仿真和控制初调。5.4 模型验证的三板斧模型建完不验证就等于白建模。我自己常用的验证手段有三个。第一是悬停验证。在零姿态角下总推力等于重力此时z̈应为0。这是最简单、最基础的验证先做这一步能排除大部分符号和参数问题。第二是开环响应验证。在仿真中给一个小阶跃输入看加速度或者角加速度的方向和大小是否符合物理直觉。比如给正U2横滚力矩飞机应该产生正的横滚角加速度如果反了旋翼布局的下标肯定有问题。第三是和飞行日志对比。这个比较进阶就是把真实飞行时采集的转速数据输入到模型里仿真出对应的位置和姿态轨迹然后和真实飞行的GPS、姿态数据进行对比。如果趋势一致、误差在可接受范围内那模型就可以用于控制器设计和硬件在环仿真。这一步做完了你的模型才算真正闭环验证通过。5.5 “最终图”和“参考答案”模型没有唯一形式最后想聊一个很多人会困惑的点尤其是做课程设计或者毕业论文的同学总觉得数学模型应该有一个“标准答案”或者一张最终的图。实际上四旋翼的数学模型没有唯一形式。同样一架飞机你可以写出非线性模型、线性化模型、面向控制的简化模型也可以写成频域传递函数形式。哪种形式算“最终答案”取决于你要干什么。做仿真用非线性模型做姿态PID用线性化模型做系统辨识还要再变形。所以别再纠结数学模型应该有什么最终图真正应该关注的是这组方程能不能准确描述你关心的那部分运动特性。类似地网上搜“数学模型习题参考解答pdf”这种资料时也要带着批判眼光去看。四旋翼建模的变量定义、符号习惯、坐标方向千差万别一份参考答案很可能只是某一套约定下的结果。你能把物理过程搞清楚用自己的推导自洽地得到一组方程那它就是你的正确答案。写在最后的一点心得从我个人的体会来说四旋翼数学模型的推导在逻辑上并不难真正的门槛是细节约定多、符号体系乱、坐标系方向容易混淆。我第一次完整推导的时候光旋转矩阵的正负号就检查了三遍最后还是靠仿真数据才定位到问题。后来养成了一个习惯建模之前先把坐标系、旋转顺序、电机转向画在一张纸上然后所有的推导和代码都对照这张纸来写。这个小习惯帮我省掉了大量排查时间以后可以试试看。如果你准备自己动手建模最后的建议是别在文档里推导完就收工一定把它写成代码跑起来。哪怕只是算一下悬停角速度、算一下小角度响应都能帮你发现很多纸面上看不出来的问题。模型跑通了后面的姿态控制和位置控制才有支撑。