四旋翼无人机搞到一半最容易被劝退的点往往不是PID调参也不是选型而是数学模型。尤其当你发现网上教程各写各的坐标系朝向不一样、欧拉角顺序不一样、推力表达式正负号也不一样照着抄进Simulink里模型直接飞上天花板或者原地抽搐。这个坑我踩过太多次所以这篇把整个四旋翼数学模型的推导过程完整捋一遍从坐标系定义开始一路推到状态空间方程和悬停线性化顺便把转动惯量、推力系数这些参数的实测方法也放进来当作一份可以直接复现的参考。这篇文章适合正在做飞控算法、仿真系统或者毕业设计相关工作的朋友尤其是那些已经过了“会飞”阶段、开始琢磨“为什么这样飞”的开发者。读完之后你至少能回答三个问题动力学方程的每一项物理含义是什么怎样从电机转速得到整机的力和力矩以及拿到一个非线性模型之后怎么把它变成能设计PID控制器的线性模型。1. 坐标系与符号约定建模第一步错了后面全白搭1.1 我采用的NED与前右下约定建模第一步不是写公式而是先把坐标系说死。很多推导混乱根源就是坐标系约定不一致。我习惯采用航空航天领域常用的NED坐标系也就是北东地North-East-Down作为地面惯性系记为E系。机体坐标系B系则采用“前右下”x轴指向机头方向y轴指向机身右侧z轴指向下方。这样B系和E系在初始水平状态时完全重合后续推导不需要额外转换符号。需要说明的是NED坐标系的“地轴朝下”初看反直觉但它有个实际好处重力加速度可以直接写成[0, 0, g]的正值形式不用每次都在公式里加负号。很多ENU东北天体系的用户推导时会因为重力方向符号反复出问题NED体系下这类问题少很多。这个约定确定后所有向量和矩阵都基于它展开。惯性系E的位置向量写作[ \boldsymbol{\xi} [x, y, z]^T ]机体系的线速度写作[ \mathbf{v}_b [u, v, w]^T ]机体角速度写作[ \boldsymbol{\omega}_b [p, q, r]^T ]这三个是模型的基本量。后面的推导全部围绕它们展开。1.2 旋转矩阵与欧拉角顺序ZYX也好YXZ也好必须写明白四旋翼的姿态一般用欧拉角表示横滚角φ、俯仰角θ、偏航角ψ。但欧拉角的旋转顺序不同旋转矩阵的表达式就不同。最常用的是ZYX顺序即先绕z轴偏航、再绕y轴俯仰、最后绕x轴横滚。这个顺序也是很多飞控教科书和开源飞控默认使用的。从机体系到地面系的旋转矩阵定义为[ \mathbf{R} \mathbf{R}_z(\psi) \mathbf{R}_y(\theta) \mathbf{R}_x(\phi) ]展开后得到[ \mathbf{R} \begin{bmatrix} c\theta c\psi s\phi s\theta c\psi - c\phi s\psi c\phi s\theta c\psi s\phi s\psi \ c\theta s\psi s\phi s\theta s\psi c\phi c\psi c\phi s\theta s\psi - s\phi c\psi \ -s\theta s\phi c\theta c\phi c\theta \end{bmatrix} ]其中c表示coss表示sin。这个矩阵把机体系的向量的分量变换到地面系[ \mathbf{v}_e \mathbf{R} \mathbf{v}_b ]反过来从地面系到机体系用它的转置[ \mathbf{v}_b \mathbf{R}^T \mathbf{v}_e ]因为旋转矩阵是正交矩阵所以逆等于转置这个性质很实用。这个矩阵是后续一切推导的基石。推力向量、重力向量、速度向量的坐标变换都要通过它完成。建议你把自己选择的旋转顺序和矩阵形式固定下来写入自己的算法文档头部免得三个月后回来看代码时对着满屏的矩阵符号怀疑人生。2. 运动学方程机体角速度与欧拉角速率的关系2.1 角速度到欧拉角速率的变换矩阵很多刚开始做仿真的人直接用角速度积分欧拉角这是个典型错误。因为欧拉角的导数不等于机体角速度分量两者之间有一个变换矩阵。设欧拉角速率为(\dot{\boldsymbol{\eta}} [\dot{\phi}, \dot{\theta}, \dot{\psi}]^T)机体角速度为(\boldsymbol{\omega}_b [p, q, r]^T)它们的关系是[ \boldsymbol{\omega}_b \begin{bmatrix} 1 0 -s\theta \ 0 c\phi s\phi c\theta \ 0 -s\phi c\phi c\theta \end{bmatrix} \begin{bmatrix} \dot{\phi} \ \dot{\theta} \ \dot{\psi} \end{bmatrix} ]反过来如果用机体角速度求欧拉角速率[ \dot{\phi} p q \cdot s\phi \cdot t\theta r \cdot c\phi \cdot t\theta ][ \dot{\theta} q \cdot c\phi - r \cdot s\phi ][ \dot{\psi} \frac{q \cdot s\phi r \cdot c\phi}{c\theta} ]其中t表示tan。这个矩阵推导过程不复杂本质是把机体角速度的旋转轴依次反向投影到欧拉角的旋转轴上你可以自己从头推一遍手感完全不一样。2.2 俯仰角90度的奇异性问题上面这个表达式里有个明显的隐患当俯仰角θ接近±90度时分母上的cosθ趋于零偏航角速率ψ的数值会爆炸。这就是万向锁问题。实际飞控系统中极少直接用这个公式做姿态解算。在PX4这类主流飞控里姿态估计用四元数四元数微分方程没有奇异点更新也稳定最终需要欧拉角的时候再从四元数转换输出。但在仿真建模时很多人图省事直接用欧拉角微分方程。如果你的仿真场景只做悬停附近的小机动问题不大一旦做大机动仿真比如翻滚动作模型就会出NaN。我的建议是仿真模型也尽量用四元数做状态量的姿态表示推导时用欧拉角展示物理意义实现时切到四元数。这个过渡本身很简单四元数(\mathbf{q} [q_0, q_1, q_2, q_3]^T)和机体角速度的关系是[ \dot{\mathbf{q}} \frac{1}{2} \mathbf{Q}(\boldsymbol{\omega}_b) \mathbf{q} ]其中(\mathbf{Q}(\boldsymbol{\omega}_b))是由机体角速度构造的四元数乘法矩阵。工程上这行代码就能写完但能把你的仿真从“只能小角度”升级为“全姿态飞行”。3. 动力学方程力与力矩从哪里来3.1 平移动力学牛顿第二定律在机体系中的应用四旋翼的平动动力学方程可以用牛顿第二定律在机体坐标系下写出[ m \dot{\mathbf{v}}_b m (\boldsymbol{\omega}_b \times \mathbf{v}_b) \mathbf{f}_b ]其中交叉项(\boldsymbol{\omega}_b \times \mathbf{v}_b)来自非惯性旋转坐标系是机体旋转对平移加速度的影响。如果把方程变换到地面系则可以写成更直观的形式[ m \ddot{\boldsymbol{\xi}} m \mathbf{g}_e \mathbf{R} \mathbf{T}b \mathbf{F}{d} ]这里(\mathbf{g}_e)是重力加速度在地面系下的向量NED约定下为([0, 0, g]^T)。(\mathbf{T}b)是升力在机体系下的向量。(\mathbf{F}{d})是气动阻力。四旋翼的升力由四个螺旋桨产生方向沿机体z轴负方向NED坐标系下就是向上。于是推力向量可写为[ \mathbf{T}b [0, 0, -T{total}]^T ]其中[ T_{total} T_1 T_2 T_3 T_4 k_T \sum_{i1}^{4} \omega_i^2 ](k_T)是推力系数(\omega_i)是第i个电机的转速。气动阻力项我一般用线性模型近似[ \mathbf{F}_d -\mathbf{k}_d \dot{\boldsymbol{\xi}} ]即阻力与飞行速度成正比。这个模型在低速场景下够用高速飞行时阻力会呈平方关系变化但作为控制设计用的简化模型线性阻力足够支撑前期的仿真准确性。将各项代入地面系平动方程就得到三个位置分量的二阶微分方程。这些方程与控制输入T和姿态角直接相关是外环位置控制器的建模基础。3.2 转动动力学欧拉方程与力矩来源转动动力学采用刚体欧拉方程[ \mathbf{J} \dot{\boldsymbol{\omega}}_b \boldsymbol{\omega}_b \times (\mathbf{J} \boldsymbol{\omega}_b) \mathbf{M}_b ]其中(\mathbf{J})是机体转动惯量矩阵。假设机体关于xOz和yOz平面对称惯量矩阵可以近似为对角阵[ \mathbf{J} \text{diag}(I_{xx}, I_{yy}, I_{zz}) ]交叉项在小四旋翼上影响很小通常直接忽略。力矩向量(\mathbf{M}_b)由三部分组成推力差产生的滚转与俯仰力矩反扭矩产生的偏航力矩螺旋桨陀螺效应产生的陀螺力矩3.3 各电机的力矩分配关系以“”构型为例假设四个电机分别位于机体坐标系的前、右、后、左方向力臂长度为(l)。电机编号与位置如下电机编号位置旋转方向推力方向电机1前x逆时针向上-z电机2右y顺时针向上-z电机3后-x顺时针向上-z电机4左-y逆时针向上-z相邻电机旋转方向相反用来抵消反扭矩。这是四旋翼能稳定偏航控制的前提条件。每个电机的推力在机体上产生的力矩是[ \mathbf{M}_i \mathbf{r}_i \times \mathbf{F}_i ]其中(\mathbf{r}_i)是电机位置向量(\mathbf{F}_i [0, 0, -T_i]^T)。展开计算后滚转、俯仰、偏航力矩分别为[ M_\phi l (T_2 - T_4) ][ M_\theta l (T_1 - T_3) ][ M_\psi k_M (\omega_1^2 - \omega_2^2 \omega_3^2 - \omega_4^2) ]其中(k_M)是反扭矩系数。这里的符号对应上面表格的旋转方向如果你采用顺时针和逆时针定义相反则偏航力矩表达式中的加减号也要相应调整。以上是“”构型的表达式。如果是“X”构型滚转和俯仰力矩是两个对角电机的合成表达式变为[ M_\phi \frac{\sqrt{2}}{2} l (T_1 T_2 - T_3 - T_4) ][ M_\theta \frac{\sqrt{2}}{2} l (-T_1 T_2 T_3 - T_4) ]X构型的力矩分配矩阵看起来更复杂但本质一样都是把推力差投影到对应轴上。我先讲清楚构型的推导逻辑后面换构型时只要重新列几何关系即可。3.4 陀螺力矩与被忽视的附加效应旋转的螺旋桨相当于陀螺当机体转动时螺旋桨会产生阻碍机体转动的陀螺力矩。这个力矩的来源是角动量守恒方向和机体角速度、螺旋桨转速有关。单桨陀螺力矩为[ \mathbf{M}{gyro,i} \mathbf{J}{rp} (\boldsymbol{\omega}_b \times [0, 0, \omega_i]^T) ]四个电机叠加后总陀螺力矩可以合并为[ \mathbf{M}{gyro} -\mathbf{J}{rp} (\boldsymbol{\omega}_b \times [0, 0, \omega_1 - \omega_2 \omega_3 - \omega_4]^T) ]其中(\mathbf{J}_{rp})是螺旋桨和电机转子的等效转动惯量。在实际悬停附近的小机动中陀螺力矩的量级远小于推力气动产生的力矩很多人建模时直接忽略。但做大机动仿真或者玩滑模控制时陀螺力矩是影响性能的细节项建议完整保留。4. 完整非线性模型与悬停线性化4.1 12维状态空间方程把前面所有方程拼到一起就得到完整的状态空间模型。状态向量取[ \mathbf{X} [x, y, z, u, v, w, \phi, \theta, \psi, p, q, r]^T ]输入向量为四个电机转速平方[ \mathbf{U} [\omega_1^2, \omega_2^2, \omega_3^2, \omega_4^2]^T ]完整非线性方程如下[ \begin{aligned} \dot{x} u \ \dot{y} v \ \dot{z} w \ \dot{u} \frac{1}{m} (c\psi s\theta c\phi s\psi s\phi) T_{total} - \frac{k_d}{m} u \ \dot{v} \frac{1}{m} (s\psi s\theta c\phi - c\psi s\phi) T_{total} - \frac{k_d}{m} v \ \dot{w} \frac{1}{m} (c\theta c\phi) T_{total} - g - \frac{k_d}{m} w \ \dot{\phi} p q s\phi t\theta r c\phi t\theta \ \dot{\theta} q c\phi - r s\phi \ \dot{\psi} \frac{q s\phi r c\phi}{c\theta} \ \dot{p} \frac{I_{yy} - I_{zz}}{I_{xx}} q r \frac{M_\phi}{I_{xx}} \ \dot{q} \frac{I_{zz} - I_{xx}}{I_{yy}} p r \frac{M_\theta}{I_{yy}} \ \dot{r} \frac{I_{xx} - I_{yy}}{I_{zz}} p q \frac{M_\psi}{I_{zz}} \end{aligned} ]注意(\dot{w})表达式中用了(-g)而前面说重力是(g)这里的区别在于推力分量已经包含了姿态投影而z轴是向下的。距离权衡之后垂直方向加速度等于推力向上分量减去重力NED坐标下数值上表现为((c\theta c\phi)T/m - g)。很多人在这一步纠结符号我建议直接根据物理直觉去核对量纲和极限情况悬停时(T mg)、(\phi\theta0)则(\dot{w}0)模型才是自洽的。4.2 小角度近似从非线性到可设计控制器的线性模型完整非线性模型适合仿真但不适合直接用经典控制理论设计。工程上最常用的做法是在悬停工作点附近做小角度线性化。悬停时各状态量均处于配平值即[ \bar{\mathbf{X}} [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]^T ]配平推力为[ \bar{T}_{total} mg ]在小角度假设下[ s\phi \approx \phi, \quad s\theta \approx \theta, \quad c\phi \approx 1, \quad c\theta \approx 1 ]忽略二阶小量和交叉耦合项水平位置的近似方程为[ \ddot{x} \approx g \theta ][ \ddot{y} \approx -g \phi ][ \ddot{z} \approx \frac{T_{total}}{m} - g ]姿态角通道近似为三个相互独立的二阶系统[ I_{xx} \ddot{\phi} \approx l (T_2 - T_4) ][ I_{yy} \ddot{\theta} \approx l (T_1 - T_3) ][ I_{zz} \ddot{\psi} \approx k_M (\omega_1^2 - \omega_2^2 \omega_3^2 - \omega_4^2) ]这个线性模型虽然粗糙却是整个串级PID设计的主干。你现在看到的几乎所有开源飞控的调参教程其理论依据都是这个线性化模型。如果要设计LQR这类现代控制器通常会进一步把上述模型写成标准状态空间形式[ \dot{\mathbf{x}} A \mathbf{x} B \mathbf{u} ]以水平和垂直通道为例设计LQR时的状态加权矩阵Q和输入加权矩阵R就需要根据这个线性模型中的物理参数去调节。模型推错一个符号仿真里的LQR矩阵再漂亮上真机也是炸机。4.3 非线性控制方法滑模控制为何需要完整模型近年来滑模控制SMC在四旋翼中讨论度很高。滑模控制的优点是抗扰动能力强在处理风扰、负载变化等场景下比线性PID有优势。但滑模控制的设计非常依赖完整非线性模型。因为它需要在控制律中显式包含系统的非线性项比如((I_{yy}-I_{zz})/I_{xx} \cdot qr)这类项。如果模型里把这些项抹掉滑模面设计会失真系统在高速旋转时容易出现抖振甚至失稳。所以如果你做的是滑模控制方向前面4.1节的完整模型一个字都不能省。这一步省下的功夫后面会在调参和仿真阶段加倍还回来。5. 模型参数怎么定转动惯量与推力系数的实测方法5.1 三线摆法测量转动惯量模型推完之后里面的参数(I_{xx}, I_{yy}, I_{zz}, k_T, k_M, J_{rp})不能靠猜。转动惯量最容易想到的方法是拆机建模用SolidWorks或者Fusion 360把机架、电调、电池、各零件建模后自动计算惯量。但装配体的零部件精度和实际安装位置存在误差算出来的效果通常差一点意思。更可靠的实测方案是三线摆法。原理是利用圆盘悬挂系统的小角度扭转周期与转动惯量的关系间接测量。具体做法制作上下两个同心圆盘用三根等长细绳对称悬挂。下圆盘装待测机体轻扭转后释放记录摆动周期。分别测空盘和装机的周期。根据周期差计算出机体绕垂直轴的转动惯量。计算公式为[ I \frac{m g R r}{4\pi^2 L} T^2 ]其中m是下盘及负载质量R是上盘吊点半径r是下盘吊点半径L是绳长T是小角摆动周期。绕x轴和y轴的惯量测试方法是把机体侧过来安装重复测量。这个过程比较费时间但做一次之后数据可以反复用。注意确保每次安装时绳长一致不然数据一致性会差。5.2 推力系数与反扭矩系数的标定推力系数(k_T)的标定比较直接。把电机和螺旋桨安装到一个固定测力台架上给电调一个固定油门等转速稳定后同时记录拉力和转速。从低油门到高油门逐点测试每组数据重复三次取平均。将记录的N组数据拟合曲线[ T k_T \omega^2 ]用最小二乘法拟合得到(k_T)。反扭矩系数(k_M)的标定稍微麻烦些。常见做法是把电机安装在一个带力矩臂的底座上电机旋转时测量底座对测力计的推力乘以臂长就是反扭矩。另一个办法是直接查螺旋桨厂商提供的拉力-扭矩表不过通常需要自己实测才可靠。这两个系数会因螺旋桨型号、电机磁铁特性、桨叶磨损状态而变化。我的建议是每次换桨后至少复测一次避免参数漂移导致控制性能变化。5.3 电机时间常数与执行机构动态很多初版模型只写了静态的(T k_T \omega^2)忽略了电机的动态响应也就是油门指令变化到实际转速变化之间存在延迟。电机和电调可以近似为一阶惯性系统[ \dot{\omega} -\frac{1}{\tau_m} \omega \frac{1}{\tau_m} \omega_{des} ]其中(\tau_m)是电机时间常数一般在0.01s到0.05s之间。测量方法很简单给电调一个阶跃油门用转速计或飞控日志记录转速从10%到90%的时间也就是上升时间再换算出一阶惯性时间常数。说一个我踩过的坑早期仿真时电机时间常数设成0.001s结果仿真里PID参数调得很欢上真机后姿态却震荡得厉害。原因就是真机电机响应没有仿真里那么快相位延迟让调好的增益直接失效。做仿真时一定把执行机构动态加进去否则仿真结论没有参考价值。6. 从推导到飞控串级PID结构与模型如何使用6.1 串级PID的本质是在利用模型的分层结构串级PID是所有四旋翼飞控中最经典的控制结构它的本质就是利用前面推导的线性化模型的分层特性。姿态环分两层外环角度环输入期望欧拉角输出期望角速度。内环角速度环输入期望角速度输出期望力矩。角速度环对应的是模型中的转动动力学即从力矩到角速度的传递函数[ \frac{p(s)}{M_\phi(s)} \frac{1}{I_{xx} s} ]角度环对应的是运动学积分关系[ \frac{\phi(s)}{p(s)} \frac{1}{s} ]所以每个通道实际是对一个双积分器系统的串联校正。角度环作用是抑制角度误差角速度环作用是抑制角速度变化和外扰。为什么内环要更快因为内环是被控对象动态最直接的部分带宽分配通常遵循“内环比外环快5到10倍”的经验。以内环50-200Hz更新率、外环10-50Hz更新率为常见配置。这个倍数关系来源于时间尺度分离原则外环控制周期内内环已经基本稳定外环看到的是内环建立起来的等效静态增益。LQR控制器的设计同样依赖这个模型结构只不过它把两个环的状态放进同一个性能指标里优化得到的状态反馈增益矩阵代替了手工整定的PID参数。前提仍然是你有准确的模型参数。6.2 仿真验证时的经典错误与排查链我在仿真验证阶段遇到过几次很典型的模型问题逐一排查过来后基本可以归纳成几类。第一类是重力方向符号错误。表现是模型在零输入时垂直方向就开始加速漂移悬停时需要对Hover油门做一个明显偏移才能稳住。查这个问题的思路很简单清空所有控制和输入只保留重力检查模型是否以重力加速度向下加速。不是的话说明重力变换方向写反了。第二类是欧拉角导数和机体角速度的变换矩阵写错。表现是姿态回路的仿真数据里角速度环输出与角度环不匹配角度变化率与角速度积分结果对不上。建议做一个纯姿态自由运动测试给定初始角速度观察欧拉角变化是否和解析解一致。第三类是偏航力矩符号反了。表现是混合输入中的偏航通道与其他通道耦合严重偏航指令响应后横滚和俯仰也被带偏。排查方法是单通道测试只给偏航力矩输入观察系统是否只产生偏航角速度变化而滚转俯仰方向没有响应。若耦合明显就去检查反扭矩和电机旋转方向的对应关系。还有一个高频问题出现在Simulink中模型数值积分步长设置过大导致高频姿态动态被数值截断。四旋翼姿态动态的固有频率一般在5到20Hz仿真步长需要至少小于0.005s推荐用0.001s到0.002s的固定步长。用变步长求解器也有风险模型非线性强时容易出现步长自适应失败最后结果看起来像发散实际是数值问题。6.3 模型在编队和任务规划场景中的延伸把单机数学模型稳定跑通之后多机编队、调度系统、视觉拼接这些应用才有了共同的基础。编队控制中每架无人机用同样的模型只是初始状态和目标轨迹不同任务调度系统需要根据模型预测每架飞机的可达性和时间窗变化。没有准确的单机模型这些上层控制都只是空中楼阁。值得提醒的是编队环境下气流干扰和尾流效应会让每架飞机的推力系数等参数发生偏移所以编队用的控制算法最好预留参数在线辨识的接口。滑模控制和自适应控制在编队场景中表现更好原因就在于此。7. 最后的调试心得模型、仿真与真机之间的三重奏从数学模型推导到真机飞行整个过程如果只给一句话建议那就是永远保持怀疑永远用实验验证模型。模型只代表你的假设真机才是唯一答案。我自己现在做一套模型时会先把非线性模型放进Simulink里跑纯开环测试确认力、力矩、陀螺效应各项响应都是物理可解释的然后再叠加串级PID做闭环跟踪仿真检查不同指令下的响应速度、超调量和抗扰动能力最后才把同一套参数搬到PX4 SITL仿真里验证。这样分层验证能最大程度缩短模型排错周期。另外关于参数的复用性不同姿态角的转动惯量不会变但推力系数和电机时间常数会随着电池电压下降而轻微变化或者随着桨叶变形而明显变化。如果你的仿真和真机差距逐渐变大第一个检查项就是重新标定动力系统参数而不是改PID。遇到模型问题卡住的时候不要太执着于纯数学推导。把模型丢到仿真里看它在哪里发散从这里反推物理意义往往比坐在椅子上多写三页公式效率高得多。四旋翼数学模型推导这件事本质上不是数学练习而是对飞行过程逐层拆解再重组的过程。拆得越细你对自己飞机每个动作背后的因果链路就越清楚。