搞四旋翼的兄弟应该都经历过这种场面——Simulink里模型搭了一半PID参数随手填了个Kp1, Ki0.1, Kd0.01满怀信心地点下运行按钮结果 Scope 里的曲线直接冲出天际数值飙到十的几十次方最后仿真直接停下报错“Input contains NaN or Inf”。这太正常了。四旋翼无人机控制系统里最难的不是写代码而是把物理模型、控制器设计和仿真调试完整地串起来让每个环节都能对得上号。这篇文章就围绕一个很典型的项目——基于PID控制的四旋翼无人机系统Matlab仿真把这个流程从头到尾拆开讲。从动力学方程怎么搭、级联PID为什么比单环PID靠谱、PID参数怎么一点点调到稳定到仿真发散怎么定位问题都会聊到。内容既是写给课程设计、毕业设计卡住的同学也是给准备上手飞控、想搞明白“PID和无人机到底怎么结合”的工程师看的目标是让你看完就能打开Matlab自己搭一个能收敛的仿真模型。1. 项目概述与整体设计思路1.1 这个项目到底做了什么这个项目本质上是一套四旋翼无人机的“虚拟飞控验证平台”。你要做的事情是在Matlab/Simulink里建立四旋翼的六自由度动力学模型设计PID控制律姿态、高度甚至位置然后通过仿真曲线验证控制器能不能让无人机稳定悬停、按指令飞行。整个系统的信息流是这样走的给定期望姿态角比如“横滚5度”控制器根据当前姿态和期望姿态的偏差计算控制量控制量映射到四个电机的转速电机转速产生推力和力矩推力和力矩输入给动力学模型动力学模型更新位置和姿态再反馈给控制器如此循环。把这个流程跑通之后你能做的验证就很多了比如让无人机从初始姿态20度俯仰角开始看它能不能自己“摆正”给它一个高度阶跃信号看它能不能快速爬升到目标高度且不超调或者给模型加一点传感器噪声看PID的抗干扰能力如何。这些在真机上测试成本高、风险大在仿真里却是几分钟的事。这正是做这个项目价值的核心——在烧钱摔机之前先用仿真把控制器“磨”到基本可用。1.2 为什么选择四旋翼、PID和Simulink这个组合先说四旋翼。四旋翼是典型的开环不稳定系统你把它放空中不施加控制它很快就会因为力矩不平衡而翻转坠地。这种“看起来简单、控制起来很难”的特性让它成为学习控制理论的最佳试验田。相比固定翼四旋翼的动力学耦合没那么复杂但足够体现出控制算法的作用相比倒立摆四旋翼又有更实际的工程背景做完还能往飞控方向延伸。然后是PID。说实话现在很多论文里都在上LQR、H∞、滑模控制、自抗扰这些高级算法但真正常见飞控固件里用的、能在万里挑一的复杂环境中稳定跑起来的还是级联PID。PID的好处是物理意义极其清晰——比例项管“现在偏差多大”、积分项管“历史积累了多少偏差”、微分项管“偏差变化得多快”这三个词就能解释清楚调试起来也直观。很多人觉得PID“太简单、没水平”但真能把这个简单算法在非线性、强耦合的四旋翼上调到稳定飞行你对控制系统的理解绝对不会浅。最后是Simulink。用Simulink搭建这个系统最大的优势是模块化。动力学模型可以做成一个子系统控制器逻辑做成另一个子系统输入信号、示波器都可视化哪条信号线连错了、哪个量发散了一眼就能看出来。相比纯写代码Simulink更适合验证控制思路也特别适合做课程汇报和答辩演示。而且Matlab的Control System Toolbox提供了一堆分析工具波特图、根轨迹、阶跃响应曲线都能直接看这比自己在纸上推公式直观得多。2. 四旋翼动力学模型仿真的地基2.1 坐标系与运动方程先别急着写代码任何四旋翼仿真的第一步都是建立数学模型。不是找个现成方程抄进去就完事而是要知道每个项背后的物理含义否则后面仿真出问题你根本无从排查。我们一般定义两个坐标系地面坐标系惯性系和机体坐标系。地面坐标系固定在地面用来描述无人机的位置x, y, z和三轴姿态角——横滚角φ、俯仰角θ、偏航角ψ机体坐标系固定在无人机机身上原点在质心x轴指向机头y轴指向右侧z轴向下有些定义里z轴向上注意统一。四旋翼的动力学模型大体分为两部分。第一部分是线运动方程由牛顿第二定律得到作用在机身上的总外力包含重力、四个旋翼产生的升力外力导致无人机产生线加速度第二部分是角运动方程由欧拉方程得到旋翼产生的力矩导致无人机产生角加速度。如果把模型写全是一组12阶的非线性微分方程——位置3个、速度3个、姿态角3个、角速度3个。实际搭建仿真模型时大多数人会做几个简化假设机体视为刚体、质量分布对称、旋翼的推力和力矩与转速平方成正比、忽略空气阻力或简化为线性阻尼。这些假设在低速悬停场景下误差不大但能让模型复杂度大幅下降。记住简化假设的前提很重要你简化到哪里模型的适用范围就到哪后面分析结果时心里要有数。2.2 Simulink模型搭建的模块划分在Simulink里建模型我强烈建议按物理意义拆分成几个子系统而不是把所有方程写成一坨。一个清晰的模块划分是输入接口接收四个电机转速或油门指令。力/力矩计算模块根据电机转速换算升力和力矩。姿态动力学模块输出角速度和姿态角。位置动力学模块输出速度和位置。反馈输出把当前姿态、角速度、位置送回控制器。划分好模块后每个子系统内部用积分器Integrator串联就能搭建状态方程。比如姿态角的更新流程是角加速度积分得到角速度角速度再积分通过旋转矩阵转换得到姿态角。很多新手爱犯的一个错误是直接对角度求导再积分忽略了坐标变换关系这会导致角度计算在非小角度时出错。直接在Simulink里用积分器和数学模块搭会比较繁琐我通常用MATLAB Function模块写动力学方程输入是控制量输出是状态量的一阶导数然后接积分器。这样做的好处是方程修改起来非常方便也便于检查和对照公式。核心的动力学函数核心框架大概是这个样子function [dotPos, dotVel, dotEuler, dotOmega] droneDynamics(omega, state, params) % 输入: % omega - 四旋翼电机转速向量 [w1 w2 w3 w4] % state - 当前状态 [x y z vx vy vz phi theta psi p q r] % params - 模型参数结构体 % 输出: % 各状态量的一阶导数 phi state(7); theta state(8); psi state(9); p state(10); q state(11); r state(12); % 算总升力与三轴力矩 F params.k * sum(omega.^2); tau_phi params.k * params.l * (omega(4)^2 - omega(2)^2); tau_theta params.k * params.l * (omega(3)^2 - omega(1)^2); tau_psi params.d * (omega(1)^2 - omega(2)^2 omega(3)^2 - omega(4)^2); % 线运动方程地面系 R rotationMatrix(phi, theta, psi); accel [0; 0; -params.g] R * [0; 0; F / params.m]; % 角运动方程机体系 Ix params.Ix; Iy params.Iy; Iz params.Iz; dotp (tau_phi (Iy - Iz) * q * r) / Ix; dotq (tau_theta (Iz - Ix) * p * r) / Iy; dotr (tau_psi (Ix - Iy) * p * q) / Iz; % 欧拉角变化率与机体角速度的转换 dotPhi p q * sin(phi) * tan(theta) r * cos(phi) * tan(theta); dotTheta q * cos(phi) - r * sin(phi); dotPsi q * sin(phi) / cos(theta) r * cos(phi) / cos(theta); dotPos [state(4); state(5); state(6)]; dotVel accel; dotEuler [dotPhi; dotTheta; dotPsi]; dotOmega [dotp; dotq; dotr]; end这里我想特别提醒一点注意欧拉角更新公式在俯仰角θ接近±90度时会出现奇异值因为tan(θ)和cos(θ)在分母上。如果你只做小角度姿态控制仿真不会碰到这个问题但如果做特技动作或大角度仿真就必须用四元数替代欧拉角。我也见过课程设计里因为推杆太猛导致俯仰角接近90度、仿真直接发散的案例排查半天发现是模型奇异而不是控制器的问题。2.3 模型参数怎么来拍脑袋不如实测很多同学拿到仿真任务第一件事是问“参数从哪里找”其实四旋翼的模型参数不需要很精确重点是量级要对。标准参考值大概是这样的一架轴距450mm的入门级四旋翼质量1kg左右力臂0.225m绕x/y轴的转动惯量在0.005~0.01 kg·m²的量级绕z轴的转动惯量大约是x轴的两倍。如果你只是要做控制算法验证用这个量级的数据就够了。如果你是做毕业设计要求参数尽量贴近真实可以自己算或实测。质量用电子秤直接称力臂用尺子量转动惯量可以用两种方式得到一个参考值一种是把四旋翼简化成“中心质量四个电机质量”的模型用CAD软件测量另一种是悬挂法把机体用细绳吊起来做扭转摆动测量周期反算转动惯量。推力系数和阻力系数则可以通过台架测力实验拟合把电机装在一个固定在台架上的力传感器上给不同油门测出推力和转速再做二次拟合。这些方法都不复杂但拿到的数据比网上随便抄的参数靠谱得多。3. PID控制器设计从单环到级联3.1 单环PID为什么搞不定四旋翼如果只给四旋翼的横滚角设计一个单环PID——输入期望角度输出控制量也不是完全不能飞但效果会很差而且参数极难调。原因在于四旋翼姿态通道本身就像一个“积分型欠阻尼系统”你通过PID直接控制角度但这个环节里包含了角度到角速度的积分以及角速度到力矩的惯性环节等效后的被控对象阶次高、阻尼不足单环的P增益稍微调大就容易振荡调小了又反应迟钝。打个比方这就好比你想让车停到指定位置但你只能控制油门却不知道当前车速是多少也没法感知车速变化。你只能根据“离目标还差多远”来踩油门或刹车那结果必然是近了就急刹车、远了就猛加速很难平稳停下来。单环PID控制四旋翼姿态就是这个状态它缺少对中间物理量——角速度的反馈。3.2 级联PID内环角速度、外环角度实际飞控里用的都是级联PID也叫串级PID。以横滚通道为例控制结构分内外两层外环角度环输入期望横滚角φ_des反馈当前横滚角φ输出期望横滚角速度p_des。内环角速度环输入期望角速度p_des反馈当前角速度p输出横滚力矩指令。外环通常只用P控制就够了因为角度误差本身没有累积效应内环一般用PD或者PIDP项提供转动角速度的“拉力”D项抑制角速度变化率起到阻尼作用。内环带宽要明显高于外环工程上一般要求内环响应速度是外环的3到5倍也就是内环“反应快”外环“反应慢”这样两个环才不会互相打架。如果用一句话总结调参方向先调内环让角速度响应快又稳再调外环让角度精准平滑。高度通道的控制结构类似可以设计成“外环高度内环垂直速度”的级联形式。另外还有一个细节如果你希望无人机在倾斜时保持位置不漂移位置环水平方向也需要控制通常是外环位置、中间环速度、内环姿态。位置环的输出是期望姿态角这也是很多初学者容易绕晕的地方——位置和姿态两个通道不是孤立的而是级联在一起。3.3 控制量输出与执行器限幅PID控制器输出的不是“油门”而是“期望力矩”。要把力矩变成四个电机的转速指令需要做控制分配。四旋翼的标准分配方式是这样的总升力由四个电机共同承担横滚力矩通过两侧电机差动实现俯仰力矩通过前后电机差动实现偏航力矩则利用“正反桨反转方向不同”的特性让某一对电机转速升高、另一对降低产生扭转力矩。实际在Simulink里我会先让控制器输出[F_des, tau_phi, tau_theta, tau_psi]然后通过一个控制分配矩阵计算出四个电机的期望拉力再根据“拉力与转速平方成正比”的关系换算成电机转速指令最后接一个饱和模块限制转速的最小值和最大值。这一步不做的话仿真中PID很容易输出一个天文数字般的控制量模型跑出来也是完全失真。限幅之后你会发现模型的行为更接近真实系统也更容易调参。另外还要注意积分饱和问题。当执行器已经到达物理极限比如油门已经100%PID的误差还在持续累加积分项会越积越大导致执行器回到正常范围后控制系统出现明显超调。解决办法很直接积分分离、对积分项做限幅或者用条件积分——只在执行器未饱和时累加积分项。这段逻辑虽然简单但能明显改善大角度机动后的恢复表现。4. Matlab/Simulink仿真实现全流程4.1 环境准备与工具箱这个项目对Matlab版本没有太苛刻的要求R2018b之后的版本基本都能流畅运行我这边用的是R2021a。只需要基础Simulink和Matlab本体如果你装了Control System Toolbox或者Aerospace Blockset分析波特图和集成的飞行动力学模块会更方便但没有也不影响你把模型跑通。安装Matlab本身有个很容易踩的坑安装路径不要带中文甚至最好不要带空格。我见过不少同学因为装在D:\软件\Matlab或者C:\Program Files (x86)\MATLAB...下导致Simulink里部分工具箱加载异常或者编译C代码时找不到路径。另外Simulink的模型文件最好也放在纯英文路径下避免后续做代码生成、处理数据时出现莫名其妙的字符编码问题。这类问题看着是小事但排查起来能浪费你两小时。4.2 从零搭建Simulink模型的完整步骤我建议按照以下顺序搭建每一步都验证不要等全部搭完再跑否则出问题都不知道是控制器错还是模型错先建一个空白模型添加Constant模块和Scope模块验证信号通路是通的。搭动力学子系统。用MATLAB Function模块写入上节的动力学函数外部接4路电机转速输入输出12个状态量。用Demux把状态向量分出来方便后续接示波器观察。搭控制器子系统。控制器输入是期望量和反馈量的误差输出是四路电机转速指令。先用最简单的定值期望比如横滚角期望为0、高度期望为1米。连接闭环。把动力学子系统的姿态角、角速度反馈给控制器把控制器的电机转速接回动力学子系统。加信号观察。把期望角度、实际角度、高度、转速等关键信号用Mux汇总到同一个Scope方便对比。配置仿真参数。求解器用变步长ode45仿真时长根据场景设置悬停验证10秒就够轨迹跟踪可能需要30秒以上。这里有一个Simulink用户容易忽略的点如果出现“代数环Algebraic Loop”警告你需要检查是不是有信号在同一个步长内既作为输入又作为输出。解决方法是在反馈回路里加一个Memory模块或者Unit Delay模块人为延迟一拍。虽然这会让模型和真机有一点差异真机也有传感器延迟嘛但总比解不出来或者解出来不收敛好。搭建完的Simulink模型整体数据流应该是期望 → 误差 → 外环P → 期望角速度 → 内环PD → 期望力矩 → 控制分配 → 电机转速限幅 → 动力学模型 → 状态输出 → 反馈。这条链路清晰了你调参时就能直接看出是哪一环出了问题。4.3 级联PID参数整定的实操方法参数整定可能是整个项目里最劝退人的环节但其实你按照“先内环、后外环”“先P、后D、再I”的顺序来会顺利得多。我具体说一下横滚通道的整定过程。先把外环P设成一个很小的值比如1.0内环P从0开始慢慢加。给一个小的角速度阶跃指令比如每秒30度观察内环角速度响应。当内环P逐渐增大角速度响应会变快但会开始振荡记下刚好出现持续振荡的增益取它的50%~70%作为内环P的初值。加上内环D你会看到角速度超调逐渐被抑制曲线变得平滑。接下来调外环P给角度阶跃指令增大外环P让角度响应加快但绝不能让外环输出超过内环能跟上的速度否则内外环“打架”姿态会抖动。最后如果角度响应存在稳态误差再在内环加一点积分项。很多人一上来就同时调四个环的参数或者直接抄别人论文里的PID数值跑出来发散就崩溃。实际上PID参数是跟你的模型参数强相关的换一架四旋翼、换一套推力系数最优参数就变了。抄来的参数最多给你一个量级参考真正好用的参数必须自己一点点试出来。调参过程我会配合阶跃响应Scope曲线来看上升时间是否太快、超调是否太大、响应是否振荡衰减、稳态误差是否为零这些都是直观看得到的。如果你不想完全手动试参Matlab的Control System Designer或者Response Optimizer可以做自动整定。把被控对象是线性传递函数时用起来最顺手但四旋翼模型是非线性的自动整定不一定能直接套用。我的做法是先用线性化模型粗调一组参数再放到非线性模型里细调。这一步如果能做对你的参数整定效率会比别人快一倍。4.4 让仿真更真实的几个细节一个只有“理想动力学理想控制器”的仿真模型调试起来虽然容易但一旦脱离仿真上真机基本会摔得很惨。原因是仿真忽略了太多真实因素。我建议至少给模型加三个“不理想”环节第一是传感器噪声。用Random Number模块给姿态和角速度测量加上白噪声噪声方差参考真实MPU6050的量级角速度噪声方差大约在0.01~0.05 (rad/s)²。加了噪声之后你会马上发现纯微分项对噪声极其敏感D增益稍大一点输出控制量就“布满毛刺”这时候你就会理解为什么飞控里要做滤波为什么D项不能盲目加大。第二是执行器饱和与延迟。前面说的电机限幅是一个层面执行器延迟则是另一个层面。电机的转速响应不是瞬间完成的可以用一阶惯性环节来近似电机响应时间常数在0.05到0.1秒左右。把这一环节加上你会发现之前调好的参数可能需要重新微调因为延迟会降低系统相位裕度。第三是传感器采样和控制器计算周期。真实飞控不是连续系统而是离散系统控制频率一般在250Hz到1kHz。Simulink里可以用Rate Transition模块模拟采样周期。我发现很多课程设计完全忽略这一点仿真步长用了ode45变步长也没有采样的概念出来结果“完美无瑕”但写进单片机一看控制频率跟不上姿态早就发散了。仿真里加入离散化处理才是真正在向工程落地靠近。5. 仿真结果分析与调试实录5.1 阶跃响应的几种典型表现给期望高度一个从0到1米的阶跃信号是测试高度控制器最基本也是最有信息量的做法。观察响应曲线你能立刻判断控制器状态无控制时高度曲线不上升或者姿态发散导致高度数据乱飞。纯比例控制高度会上升但可能存在稳态误差比如最终稳定在0.85米而不是1米这是因为消除这个误差需要非零的电机转速补偿重力而纯P控制无法提供这个持续输出。加入积分项后稳态误差消失但可能出现超调比如冲到1.15米再回落。级联高度速度控制响应曲线更平滑上升过程中速度先快后慢接近目标时平稳刹车。姿态角阶跃响应比如期望横滚角从0到10度是另一个必看的指标。理想情况下应该做到上升时间在0.5到1.5秒之间超调小于5%调节时间小于3秒稳态误差为零。如果你的曲线像“弹簧一样来回弹”说明阻尼不足加内环D如果响应慢吞吞说明P太小或者外环限制了响应速度。5.2 仿真发散问题排查最常踩的坑排名仿真发散可以说是这个项目里出现频率最高的故障而且新手遇到之后第一反应往往是疯狂调PID参数结果越调越发散。其实发散的原因可能根本和参数无关。我按出现概率排个序第一位PID输出符号反了。这听着很蠢但真的特别常见。期望角度减去反馈角度如果写成反馈减期望那整个控制就变成正反馈系统必然发散。排查方法很简单控制器输入给一个正的角度偏差看控制量输出来的是正还是负再对照动力学模型里力矩的正方向定义符号错一次就发现了。第二位求解器步长太大或者模型代数环导致数值不稳定。Simulink默认的变步长ode45大多数情况下能正确解算但如果你发现仿真在某个瞬间指数发散步长曲线骤减很可能是模型存在快动态刚性问题。这时候可以尝试把最大步长限制在0.01秒或者0.001秒或改换ode15s刚性求解器往往立竿见影。第三位控制器增益数量级不对。常见问题是P和D的数值没有按物理单位匹配角速度用rad/s还是deg/s没统一造成D项巨大或微小。做仿真前先确认所有单位一致我习惯全用国际单位弧度、米、秒只在最后显示特性和画图时转成角度。第四位模型里的某条信号线接错了。比如把p横滚角速度接到了俯仰通道的输入端控制器符号全对但通道接错照样发散。排查方式是分别给横滚、俯仰、偏航通道单独输入阶跃看对应输出通道是不是对应变化。5.3 调试实录起飞瞬间姿态发散以一个我调过的案例为例。模型和控制器都搭好了悬停仿真前2秒姿态还能稳住但第三个电机转速指令一增大横滚角就开始振荡振幅越来越大最后发散。我按经验没有先动PID而是把积分项清零重新仿真振荡立刻大幅缓解。再检查积分限幅发现根本没做限幅积分项在姿态误差增大时不断累积形成了“误差大→积分大→控制过冲→误差反相→积分继续累积”的恶性循环。加上积分限幅之后系统立刻稳定下来。这个案例说明很多发散问题不是PID“不够强”而是“过于强且没有保护机制”尤其在执行器饱和的情况下积分项就是发散的第一嫌疑人。5.4 阶跃响应指标怎么看完成一组控制参数整定后我用下面的表格记录关键指标方便对比不同参数组的效果控制通道上升时间超调量调节时间5%稳态误差结论横滚角度环0.82s4.2%1.65s0°优俯仰角度环0.85s4.8%1.70s0°优高度环1.20s6.5%2.10s0m良偏航角速度环0.30s0%0.55s0°/s优这些指标对四个通道的要求不一样。姿态通道要求快速但严格控制超调高度通道可以接受略慢的响应但要求无稳态误差偏航通道通常只做角速度控制或简单角度控制响应相对较快。记录指标还有一个用处答辩或者汇报时能直接展示“调节前超调25%、调节后4.2%”的对比比说一句“我调了参数”有说服力得多。6. 常见问题与避坑指南速查表6.1 高频问题与对应解决方案问题现象可能原因排查与解决方向仿真报错“Input contains NaN or Inf”控制器输出极大值、模型里出现除以零加积分限幅、执行器饱和检查欧拉角奇异、初值是否合理姿态响应持续振荡、不衰减内环阻尼不足、增益过大加内环D、降低内环P检查采样周期高度始终达不到设定值积分项不足或未启用增加积分项或检查重力补偿是否正确仿真很慢、卡住代数环、刚性系统检查代数环警告尝试ode15s、限制最大步长曲线看起来“毛刺”很多传感器噪声过大、D项过大降低噪声方差、加低通滤波、减D增益小角度正常大角度发散欧拉角奇异、线性化假设失效改用四元数姿态表示或限制最大指令角度6.2 仿真到实物过渡的几条经验最后如果你做完仿真以后真的要往飞控上移植有几点经验必须先说清楚。仿真和实物的差距主要来自四个方面模型误差仿真忽略了空气阻力、桨叶形变等、执行器延迟电机响应不是一阶惯性那么简单、传感器噪声与延迟、离散化精度。你在仿真里调出来的参数放到真机上大概率要往小里调尤其D项和积分项在真机上最容易引发高频振荡。我刚接触飞控时在仿真里把D加得很大姿态曲线“笔直”结果上真机后电机嗡嗡作响、机体抖动得厉害这就是D项放大传感器噪声在高频段造成的影响。如果你已经在做“裸机PID控制”也就是在STM32这类单片机上直接用C语言实现PID而不依赖现成飞控框架那仿真阶段多准备一份离散化PID就很有价值。仿真里用的是连续PID移植到单片机时要改成位置式或增量式离散PID控制周期一般在2~4毫秒对应250~500Hz。建议在Simulink里先按这个周期做离散化仿真确认稳定性后再写进C代码能省掉不少烙铁和电池的费用。另外如果你以后要接霍尔编码器电机做小车或者自稳台仿真里积累的“先内环后外环”“先P后D再I”那套整定方法论是完全通用的。我在调编码器电机速度环时几乎就是把四旋翼角速度环的整定步骤原封不动搬过去从零开始加P到临界振荡退回一半再加D最后少量I消除静差。方法论一旦学会了换什么被控对象都不会慌。7. 写在最后的一点体会做完这套四旋翼PID仿真之后我个人最大的体会是控制系统的设计能力不是靠背公式堆出来的而是在“建模—仿真—调参—发现发散—再改模型—再调参”这个循环里磨出来的。第一次把一个看起来毫无希望的振荡模型调到稳稳定定地悬停在1米高度那个成就感确实很值。最后分享一个建议别偷懒用别人已经调好的模型——哪怕是GitHub上开源的也建议自己照着搭一遍。你亲手搭过一遍、调过一遍、发散过一遍又解决了才能真正说出“我掌握PID控制四旋翼”这句话。按照上面这套流程走完你得到的不仅是一个能跑的仿真还有一整套调试方法论后面无论是写论文、做比赛还是上真机飞控都够用。
搞四旋翼的兄弟应该都经历过这种场面——Simulink里模型搭了一半PID参数随手填了个Kp1, Ki0.1, Kd0.01满怀信心地点下运行按钮结果 Scope 里的曲线直接冲出天际数值飙到十的几十次方最后仿真直接停下报错“Input contains NaN or Inf”。这太正常了。四旋翼无人机控制系统里最难的不是写代码而是把物理模型、控制器设计和仿真调试完整地串起来让每个环节都能对得上号。这篇文章就围绕一个很典型的项目——基于PID控制的四旋翼无人机系统Matlab仿真把这个流程从头到尾拆开讲。从动力学方程怎么搭、级联PID为什么比单环PID靠谱、PID参数怎么一点点调到稳定到仿真发散怎么定位问题都会聊到。内容既是写给课程设计、毕业设计卡住的同学也是给准备上手飞控、想搞明白“PID和无人机到底怎么结合”的工程师看的目标是让你看完就能打开Matlab自己搭一个能收敛的仿真模型。1. 项目概述与整体设计思路1.1 这个项目到底做了什么这个项目本质上是一套四旋翼无人机的“虚拟飞控验证平台”。你要做的事情是在Matlab/Simulink里建立四旋翼的六自由度动力学模型设计PID控制律姿态、高度甚至位置然后通过仿真曲线验证控制器能不能让无人机稳定悬停、按指令飞行。整个系统的信息流是这样走的给定期望姿态角比如“横滚5度”控制器根据当前姿态和期望姿态的偏差计算控制量控制量映射到四个电机的转速电机转速产生推力和力矩推力和力矩输入给动力学模型动力学模型更新位置和姿态再反馈给控制器如此循环。把这个流程跑通之后你能做的验证就很多了比如让无人机从初始姿态20度俯仰角开始看它能不能自己“摆正”给它一个高度阶跃信号看它能不能快速爬升到目标高度且不超调或者给模型加一点传感器噪声看PID的抗干扰能力如何。这些在真机上测试成本高、风险大在仿真里却是几分钟的事。这正是做这个项目价值的核心——在烧钱摔机之前先用仿真把控制器“磨”到基本可用。1.2 为什么选择四旋翼、PID和Simulink这个组合先说四旋翼。四旋翼是典型的开环不稳定系统你把它放空中不施加控制它很快就会因为力矩不平衡而翻转坠地。这种“看起来简单、控制起来很难”的特性让它成为学习控制理论的最佳试验田。相比固定翼四旋翼的动力学耦合没那么复杂但足够体现出控制算法的作用相比倒立摆四旋翼又有更实际的工程背景做完还能往飞控方向延伸。然后是PID。说实话现在很多论文里都在上LQR、H∞、滑模控制、自抗扰这些高级算法但真正常见飞控固件里用的、能在万里挑一的复杂环境中稳定跑起来的还是级联PID。PID的好处是物理意义极其清晰——比例项管“现在偏差多大”、积分项管“历史积累了多少偏差”、微分项管“偏差变化得多快”这三个词就能解释清楚调试起来也直观。很多人觉得PID“太简单、没水平”但真能把这个简单算法在非线性、强耦合的四旋翼上调到稳定飞行你对控制系统的理解绝对不会浅。最后是Simulink。用Simulink搭建这个系统最大的优势是模块化。动力学模型可以做成一个子系统控制器逻辑做成另一个子系统输入信号、示波器都可视化哪条信号线连错了、哪个量发散了一眼就能看出来。相比纯写代码Simulink更适合验证控制思路也特别适合做课程汇报和答辩演示。而且Matlab的Control System Toolbox提供了一堆分析工具波特图、根轨迹、阶跃响应曲线都能直接看这比自己在纸上推公式直观得多。2. 四旋翼动力学模型仿真的地基2.1 坐标系与运动方程先别急着写代码任何四旋翼仿真的第一步都是建立数学模型。不是找个现成方程抄进去就完事而是要知道每个项背后的物理含义否则后面仿真出问题你根本无从排查。我们一般定义两个坐标系地面坐标系惯性系和机体坐标系。地面坐标系固定在地面用来描述无人机的位置x, y, z和三轴姿态角——横滚角φ、俯仰角θ、偏航角ψ机体坐标系固定在无人机机身上原点在质心x轴指向机头y轴指向右侧z轴向下有些定义里z轴向上注意统一。四旋翼的动力学模型大体分为两部分。第一部分是线运动方程由牛顿第二定律得到作用在机身上的总外力包含重力、四个旋翼产生的升力外力导致无人机产生线加速度第二部分是角运动方程由欧拉方程得到旋翼产生的力矩导致无人机产生角加速度。如果把模型写全是一组12阶的非线性微分方程——位置3个、速度3个、姿态角3个、角速度3个。实际搭建仿真模型时大多数人会做几个简化假设机体视为刚体、质量分布对称、旋翼的推力和力矩与转速平方成正比、忽略空气阻力或简化为线性阻尼。这些假设在低速悬停场景下误差不大但能让模型复杂度大幅下降。记住简化假设的前提很重要你简化到哪里模型的适用范围就到哪后面分析结果时心里要有数。2.2 Simulink模型搭建的模块划分在Simulink里建模型我强烈建议按物理意义拆分成几个子系统而不是把所有方程写成一坨。一个清晰的模块划分是输入接口接收四个电机转速或油门指令。力/力矩计算模块根据电机转速换算升力和力矩。姿态动力学模块输出角速度和姿态角。位置动力学模块输出速度和位置。反馈输出把当前姿态、角速度、位置送回控制器。划分好模块后每个子系统内部用积分器Integrator串联就能搭建状态方程。比如姿态角的更新流程是角加速度积分得到角速度角速度再积分通过旋转矩阵转换得到姿态角。很多新手爱犯的一个错误是直接对角度求导再积分忽略了坐标变换关系这会导致角度计算在非小角度时出错。直接在Simulink里用积分器和数学模块搭会比较繁琐我通常用MATLAB Function模块写动力学方程输入是控制量输出是状态量的一阶导数然后接积分器。这样做的好处是方程修改起来非常方便也便于检查和对照公式。核心的动力学函数核心框架大概是这个样子function [dotPos, dotVel, dotEuler, dotOmega] droneDynamics(omega, state, params) % 输入: % omega - 四旋翼电机转速向量 [w1 w2 w3 w4] % state - 当前状态 [x y z vx vy vz phi theta psi p q r] % params - 模型参数结构体 % 输出: % 各状态量的一阶导数 phi state(7); theta state(8); psi state(9); p state(10); q state(11); r state(12); % 算总升力与三轴力矩 F params.k * sum(omega.^2); tau_phi params.k * params.l * (omega(4)^2 - omega(2)^2); tau_theta params.k * params.l * (omega(3)^2 - omega(1)^2); tau_psi params.d * (omega(1)^2 - omega(2)^2 omega(3)^2 - omega(4)^2); % 线运动方程地面系 R rotationMatrix(phi, theta, psi); accel [0; 0; -params.g] R * [0; 0; F / params.m]; % 角运动方程机体系 Ix params.Ix; Iy params.Iy; Iz params.Iz; dotp (tau_phi (Iy - Iz) * q * r) / Ix; dotq (tau_theta (Iz - Ix) * p * r) / Iy; dotr (tau_psi (Ix - Iy) * p * q) / Iz; % 欧拉角变化率与机体角速度的转换 dotPhi p q * sin(phi) * tan(theta) r * cos(phi) * tan(theta); dotTheta q * cos(phi) - r * sin(phi); dotPsi q * sin(phi) / cos(theta) r * cos(phi) / cos(theta); dotPos [state(4); state(5); state(6)]; dotVel accel; dotEuler [dotPhi; dotTheta; dotPsi]; dotOmega [dotp; dotq; dotr]; end这里我想特别提醒一点注意欧拉角更新公式在俯仰角θ接近±90度时会出现奇异值因为tan(θ)和cos(θ)在分母上。如果你只做小角度姿态控制仿真不会碰到这个问题但如果做特技动作或大角度仿真就必须用四元数替代欧拉角。我也见过课程设计里因为推杆太猛导致俯仰角接近90度、仿真直接发散的案例排查半天发现是模型奇异而不是控制器的问题。2.3 模型参数怎么来拍脑袋不如实测很多同学拿到仿真任务第一件事是问“参数从哪里找”其实四旋翼的模型参数不需要很精确重点是量级要对。标准参考值大概是这样的一架轴距450mm的入门级四旋翼质量1kg左右力臂0.225m绕x/y轴的转动惯量在0.005~0.01 kg·m²的量级绕z轴的转动惯量大约是x轴的两倍。如果你只是要做控制算法验证用这个量级的数据就够了。如果你是做毕业设计要求参数尽量贴近真实可以自己算或实测。质量用电子秤直接称力臂用尺子量转动惯量可以用两种方式得到一个参考值一种是把四旋翼简化成“中心质量四个电机质量”的模型用CAD软件测量另一种是悬挂法把机体用细绳吊起来做扭转摆动测量周期反算转动惯量。推力系数和阻力系数则可以通过台架测力实验拟合把电机装在一个固定在台架上的力传感器上给不同油门测出推力和转速再做二次拟合。这些方法都不复杂但拿到的数据比网上随便抄的参数靠谱得多。3. PID控制器设计从单环到级联3.1 单环PID为什么搞不定四旋翼如果只给四旋翼的横滚角设计一个单环PID——输入期望角度输出控制量也不是完全不能飞但效果会很差而且参数极难调。原因在于四旋翼姿态通道本身就像一个“积分型欠阻尼系统”你通过PID直接控制角度但这个环节里包含了角度到角速度的积分以及角速度到力矩的惯性环节等效后的被控对象阶次高、阻尼不足单环的P增益稍微调大就容易振荡调小了又反应迟钝。打个比方这就好比你想让车停到指定位置但你只能控制油门却不知道当前车速是多少也没法感知车速变化。你只能根据“离目标还差多远”来踩油门或刹车那结果必然是近了就急刹车、远了就猛加速很难平稳停下来。单环PID控制四旋翼姿态就是这个状态它缺少对中间物理量——角速度的反馈。3.2 级联PID内环角速度、外环角度实际飞控里用的都是级联PID也叫串级PID。以横滚通道为例控制结构分内外两层外环角度环输入期望横滚角φ_des反馈当前横滚角φ输出期望横滚角速度p_des。内环角速度环输入期望角速度p_des反馈当前角速度p输出横滚力矩指令。外环通常只用P控制就够了因为角度误差本身没有累积效应内环一般用PD或者PIDP项提供转动角速度的“拉力”D项抑制角速度变化率起到阻尼作用。内环带宽要明显高于外环工程上一般要求内环响应速度是外环的3到5倍也就是内环“反应快”外环“反应慢”这样两个环才不会互相打架。如果用一句话总结调参方向先调内环让角速度响应快又稳再调外环让角度精准平滑。高度通道的控制结构类似可以设计成“外环高度内环垂直速度”的级联形式。另外还有一个细节如果你希望无人机在倾斜时保持位置不漂移位置环水平方向也需要控制通常是外环位置、中间环速度、内环姿态。位置环的输出是期望姿态角这也是很多初学者容易绕晕的地方——位置和姿态两个通道不是孤立的而是级联在一起。3.3 控制量输出与执行器限幅PID控制器输出的不是“油门”而是“期望力矩”。要把力矩变成四个电机的转速指令需要做控制分配。四旋翼的标准分配方式是这样的总升力由四个电机共同承担横滚力矩通过两侧电机差动实现俯仰力矩通过前后电机差动实现偏航力矩则利用“正反桨反转方向不同”的特性让某一对电机转速升高、另一对降低产生扭转力矩。实际在Simulink里我会先让控制器输出[F_des, tau_phi, tau_theta, tau_psi]然后通过一个控制分配矩阵计算出四个电机的期望拉力再根据“拉力与转速平方成正比”的关系换算成电机转速指令最后接一个饱和模块限制转速的最小值和最大值。这一步不做的话仿真中PID很容易输出一个天文数字般的控制量模型跑出来也是完全失真。限幅之后你会发现模型的行为更接近真实系统也更容易调参。另外还要注意积分饱和问题。当执行器已经到达物理极限比如油门已经100%PID的误差还在持续累加积分项会越积越大导致执行器回到正常范围后控制系统出现明显超调。解决办法很直接积分分离、对积分项做限幅或者用条件积分——只在执行器未饱和时累加积分项。这段逻辑虽然简单但能明显改善大角度机动后的恢复表现。4. Matlab/Simulink仿真实现全流程4.1 环境准备与工具箱这个项目对Matlab版本没有太苛刻的要求R2018b之后的版本基本都能流畅运行我这边用的是R2021a。只需要基础Simulink和Matlab本体如果你装了Control System Toolbox或者Aerospace Blockset分析波特图和集成的飞行动力学模块会更方便但没有也不影响你把模型跑通。安装Matlab本身有个很容易踩的坑安装路径不要带中文甚至最好不要带空格。我见过不少同学因为装在D:\软件\Matlab或者C:\Program Files (x86)\MATLAB...下导致Simulink里部分工具箱加载异常或者编译C代码时找不到路径。另外Simulink的模型文件最好也放在纯英文路径下避免后续做代码生成、处理数据时出现莫名其妙的字符编码问题。这类问题看着是小事但排查起来能浪费你两小时。4.2 从零搭建Simulink模型的完整步骤我建议按照以下顺序搭建每一步都验证不要等全部搭完再跑否则出问题都不知道是控制器错还是模型错先建一个空白模型添加Constant模块和Scope模块验证信号通路是通的。搭动力学子系统。用MATLAB Function模块写入上节的动力学函数外部接4路电机转速输入输出12个状态量。用Demux把状态向量分出来方便后续接示波器观察。搭控制器子系统。控制器输入是期望量和反馈量的误差输出是四路电机转速指令。先用最简单的定值期望比如横滚角期望为0、高度期望为1米。连接闭环。把动力学子系统的姿态角、角速度反馈给控制器把控制器的电机转速接回动力学子系统。加信号观察。把期望角度、实际角度、高度、转速等关键信号用Mux汇总到同一个Scope方便对比。配置仿真参数。求解器用变步长ode45仿真时长根据场景设置悬停验证10秒就够轨迹跟踪可能需要30秒以上。这里有一个Simulink用户容易忽略的点如果出现“代数环Algebraic Loop”警告你需要检查是不是有信号在同一个步长内既作为输入又作为输出。解决方法是在反馈回路里加一个Memory模块或者Unit Delay模块人为延迟一拍。虽然这会让模型和真机有一点差异真机也有传感器延迟嘛但总比解不出来或者解出来不收敛好。搭建完的Simulink模型整体数据流应该是期望 → 误差 → 外环P → 期望角速度 → 内环PD → 期望力矩 → 控制分配 → 电机转速限幅 → 动力学模型 → 状态输出 → 反馈。这条链路清晰了你调参时就能直接看出是哪一环出了问题。4.3 级联PID参数整定的实操方法参数整定可能是整个项目里最劝退人的环节但其实你按照“先内环、后外环”“先P、后D、再I”的顺序来会顺利得多。我具体说一下横滚通道的整定过程。先把外环P设成一个很小的值比如1.0内环P从0开始慢慢加。给一个小的角速度阶跃指令比如每秒30度观察内环角速度响应。当内环P逐渐增大角速度响应会变快但会开始振荡记下刚好出现持续振荡的增益取它的50%~70%作为内环P的初值。加上内环D你会看到角速度超调逐渐被抑制曲线变得平滑。接下来调外环P给角度阶跃指令增大外环P让角度响应加快但绝不能让外环输出超过内环能跟上的速度否则内外环“打架”姿态会抖动。最后如果角度响应存在稳态误差再在内环加一点积分项。很多人一上来就同时调四个环的参数或者直接抄别人论文里的PID数值跑出来发散就崩溃。实际上PID参数是跟你的模型参数强相关的换一架四旋翼、换一套推力系数最优参数就变了。抄来的参数最多给你一个量级参考真正好用的参数必须自己一点点试出来。调参过程我会配合阶跃响应Scope曲线来看上升时间是否太快、超调是否太大、响应是否振荡衰减、稳态误差是否为零这些都是直观看得到的。如果你不想完全手动试参Matlab的Control System Designer或者Response Optimizer可以做自动整定。把被控对象是线性传递函数时用起来最顺手但四旋翼模型是非线性的自动整定不一定能直接套用。我的做法是先用线性化模型粗调一组参数再放到非线性模型里细调。这一步如果能做对你的参数整定效率会比别人快一倍。4.4 让仿真更真实的几个细节一个只有“理想动力学理想控制器”的仿真模型调试起来虽然容易但一旦脱离仿真上真机基本会摔得很惨。原因是仿真忽略了太多真实因素。我建议至少给模型加三个“不理想”环节第一是传感器噪声。用Random Number模块给姿态和角速度测量加上白噪声噪声方差参考真实MPU6050的量级角速度噪声方差大约在0.01~0.05 (rad/s)²。加了噪声之后你会马上发现纯微分项对噪声极其敏感D增益稍大一点输出控制量就“布满毛刺”这时候你就会理解为什么飞控里要做滤波为什么D项不能盲目加大。第二是执行器饱和与延迟。前面说的电机限幅是一个层面执行器延迟则是另一个层面。电机的转速响应不是瞬间完成的可以用一阶惯性环节来近似电机响应时间常数在0.05到0.1秒左右。把这一环节加上你会发现之前调好的参数可能需要重新微调因为延迟会降低系统相位裕度。第三是传感器采样和控制器计算周期。真实飞控不是连续系统而是离散系统控制频率一般在250Hz到1kHz。Simulink里可以用Rate Transition模块模拟采样周期。我发现很多课程设计完全忽略这一点仿真步长用了ode45变步长也没有采样的概念出来结果“完美无瑕”但写进单片机一看控制频率跟不上姿态早就发散了。仿真里加入离散化处理才是真正在向工程落地靠近。5. 仿真结果分析与调试实录5.1 阶跃响应的几种典型表现给期望高度一个从0到1米的阶跃信号是测试高度控制器最基本也是最有信息量的做法。观察响应曲线你能立刻判断控制器状态无控制时高度曲线不上升或者姿态发散导致高度数据乱飞。纯比例控制高度会上升但可能存在稳态误差比如最终稳定在0.85米而不是1米这是因为消除这个误差需要非零的电机转速补偿重力而纯P控制无法提供这个持续输出。加入积分项后稳态误差消失但可能出现超调比如冲到1.15米再回落。级联高度速度控制响应曲线更平滑上升过程中速度先快后慢接近目标时平稳刹车。姿态角阶跃响应比如期望横滚角从0到10度是另一个必看的指标。理想情况下应该做到上升时间在0.5到1.5秒之间超调小于5%调节时间小于3秒稳态误差为零。如果你的曲线像“弹簧一样来回弹”说明阻尼不足加内环D如果响应慢吞吞说明P太小或者外环限制了响应速度。5.2 仿真发散问题排查最常踩的坑排名仿真发散可以说是这个项目里出现频率最高的故障而且新手遇到之后第一反应往往是疯狂调PID参数结果越调越发散。其实发散的原因可能根本和参数无关。我按出现概率排个序第一位PID输出符号反了。这听着很蠢但真的特别常见。期望角度减去反馈角度如果写成反馈减期望那整个控制就变成正反馈系统必然发散。排查方法很简单控制器输入给一个正的角度偏差看控制量输出来的是正还是负再对照动力学模型里力矩的正方向定义符号错一次就发现了。第二位求解器步长太大或者模型代数环导致数值不稳定。Simulink默认的变步长ode45大多数情况下能正确解算但如果你发现仿真在某个瞬间指数发散步长曲线骤减很可能是模型存在快动态刚性问题。这时候可以尝试把最大步长限制在0.01秒或者0.001秒或改换ode15s刚性求解器往往立竿见影。第三位控制器增益数量级不对。常见问题是P和D的数值没有按物理单位匹配角速度用rad/s还是deg/s没统一造成D项巨大或微小。做仿真前先确认所有单位一致我习惯全用国际单位弧度、米、秒只在最后显示特性和画图时转成角度。第四位模型里的某条信号线接错了。比如把p横滚角速度接到了俯仰通道的输入端控制器符号全对但通道接错照样发散。排查方式是分别给横滚、俯仰、偏航通道单独输入阶跃看对应输出通道是不是对应变化。5.3 调试实录起飞瞬间姿态发散以一个我调过的案例为例。模型和控制器都搭好了悬停仿真前2秒姿态还能稳住但第三个电机转速指令一增大横滚角就开始振荡振幅越来越大最后发散。我按经验没有先动PID而是把积分项清零重新仿真振荡立刻大幅缓解。再检查积分限幅发现根本没做限幅积分项在姿态误差增大时不断累积形成了“误差大→积分大→控制过冲→误差反相→积分继续累积”的恶性循环。加上积分限幅之后系统立刻稳定下来。这个案例说明很多发散问题不是PID“不够强”而是“过于强且没有保护机制”尤其在执行器饱和的情况下积分项就是发散的第一嫌疑人。5.4 阶跃响应指标怎么看完成一组控制参数整定后我用下面的表格记录关键指标方便对比不同参数组的效果控制通道上升时间超调量调节时间5%稳态误差结论横滚角度环0.82s4.2%1.65s0°优俯仰角度环0.85s4.8%1.70s0°优高度环1.20s6.5%2.10s0m良偏航角速度环0.30s0%0.55s0°/s优这些指标对四个通道的要求不一样。姿态通道要求快速但严格控制超调高度通道可以接受略慢的响应但要求无稳态误差偏航通道通常只做角速度控制或简单角度控制响应相对较快。记录指标还有一个用处答辩或者汇报时能直接展示“调节前超调25%、调节后4.2%”的对比比说一句“我调了参数”有说服力得多。6. 常见问题与避坑指南速查表6.1 高频问题与对应解决方案问题现象可能原因排查与解决方向仿真报错“Input contains NaN or Inf”控制器输出极大值、模型里出现除以零加积分限幅、执行器饱和检查欧拉角奇异、初值是否合理姿态响应持续振荡、不衰减内环阻尼不足、增益过大加内环D、降低内环P检查采样周期高度始终达不到设定值积分项不足或未启用增加积分项或检查重力补偿是否正确仿真很慢、卡住代数环、刚性系统检查代数环警告尝试ode15s、限制最大步长曲线看起来“毛刺”很多传感器噪声过大、D项过大降低噪声方差、加低通滤波、减D增益小角度正常大角度发散欧拉角奇异、线性化假设失效改用四元数姿态表示或限制最大指令角度6.2 仿真到实物过渡的几条经验最后如果你做完仿真以后真的要往飞控上移植有几点经验必须先说清楚。仿真和实物的差距主要来自四个方面模型误差仿真忽略了空气阻力、桨叶形变等、执行器延迟电机响应不是一阶惯性那么简单、传感器噪声与延迟、离散化精度。你在仿真里调出来的参数放到真机上大概率要往小里调尤其D项和积分项在真机上最容易引发高频振荡。我刚接触飞控时在仿真里把D加得很大姿态曲线“笔直”结果上真机后电机嗡嗡作响、机体抖动得厉害这就是D项放大传感器噪声在高频段造成的影响。如果你已经在做“裸机PID控制”也就是在STM32这类单片机上直接用C语言实现PID而不依赖现成飞控框架那仿真阶段多准备一份离散化PID就很有价值。仿真里用的是连续PID移植到单片机时要改成位置式或增量式离散PID控制周期一般在2~4毫秒对应250~500Hz。建议在Simulink里先按这个周期做离散化仿真确认稳定性后再写进C代码能省掉不少烙铁和电池的费用。另外如果你以后要接霍尔编码器电机做小车或者自稳台仿真里积累的“先内环后外环”“先P后D再I”那套整定方法论是完全通用的。我在调编码器电机速度环时几乎就是把四旋翼角速度环的整定步骤原封不动搬过去从零开始加P到临界振荡退回一半再加D最后少量I消除静差。方法论一旦学会了换什么被控对象都不会慌。7. 写在最后的一点体会做完这套四旋翼PID仿真之后我个人最大的体会是控制系统的设计能力不是靠背公式堆出来的而是在“建模—仿真—调参—发现发散—再改模型—再调参”这个循环里磨出来的。第一次把一个看起来毫无希望的振荡模型调到稳稳定定地悬停在1米高度那个成就感确实很值。最后分享一个建议别偷懒用别人已经调好的模型——哪怕是GitHub上开源的也建议自己照着搭一遍。你亲手搭过一遍、调过一遍、发散过一遍又解决了才能真正说出“我掌握PID控制四旋翼”这句话。按照上面这套流程走完你得到的不仅是一个能跑的仿真还有一整套调试方法论后面无论是写论文、做比赛还是上真机飞控都够用。
