★ FEATURED ARTICLE 干过论文复现的人都有同感最气人的不是公式推不出来而是论文里写着“易得”“显然”的那几行恰恰是后面全部仿真的前提。PID控制不新鲜多旋翼建模也不算冷门但“论文复现”这四个字意味着你不仅要看懂模型还得把一篇文章里没写透的坐标系、符号约定、控制器结构、电机模型、参数单位一条条从字缝里抠出来然后在Simulink里搭出一个能跑、能比、能复现论文曲线的完整系统。这篇博文就是我从头复现某期刊论文“多旋翼飞行器动力学建模与控制系统设计”的记录包括建模推导、控制器落地、Simulink实现细节、调参过程以及一堆论文不会告诉你的坑。如果你也正在做类似的东西或刚打算用Simulink做飞行器控制仿真这篇应该能帮你少走不少弯路。1. 项目背景与目标拆解1.1 复现论文到底在复现什么很多人以为复现论文就是照抄方程其实不是。一篇动力学建模与控制设计的文章至少包含三层信息每一层都需要单独还原第一层是数学模型。论文会给出六自由度运动方程、电机推力与力矩公式但坐标系用的是NED还是ENU、欧拉角转序是什么、重力方向朝哪这些往往只在某个不起眼的段落或某张图里带过。符号定义一旦搞错后面全是白搭。第二层是控制器结构。论文会给出控制框图但真实落地时是角度环加角速度环的串级PID还是加了前馈和积分限幅的变体图中不会标得那么细。尤其内环抑制比、外环带宽怎么拉开需要自己还原整定。第三层是仿真参数和工况。质量、惯量、力臂长度、电机时间常数、初始状态这些数值散落在参数表、图注和实验章节里。缺少任何一个仿真出来的响应速度、超调量都可能和论文差一个数量级。我做的这个项目目标就是把这三层全部还原成一个能在Simulink里直接跑的模型并用阶跃响应和轨迹跟踪结果对比论文公开曲线。适合正在写毕设、做飞行器基础研究或者刚开始接触Simulink控制仿真的人参考。1.2 复现成果如何验收复现不是“跑起来”就算成功得有一套验收标准。我给自己定的指标是悬停状态下做滚转/俯仰5度阶跃超调不超过15%调节时间控制在1.5秒以内高度阶跃0.5米稳态误差为0水平位置跟踪时控制器输出不频繁打饱和姿态角始终在小角度范围内。这些指标不是论文原话而是我从论文里的响应曲线反推出来的。复现工作最忌讳只看“趋势差不多”一旦你开始量化比较很多隐患才会暴露出来。我最终得到的模型姿态响应和论文曲线在超调量、调节时间上的偏差控制在10%以内位置跟踪的误差也收敛到了厘米级。这个精度对纯仿真复现来说已经算不错后面我会逐个环节讲清楚怎么达到。1.3 参考文献信息的“五五开”法则复现到一半我才意识到期刊论文给出的信息量往往只够你恢复全部内容的五六成剩下三四成要靠合理假设。比如电机模型论文可能只写“电机动态视为一阶惯性环节”但时间常数是多少、油门映射是什么这类细节要么在实验辨识部分要么干脆没给。这时候我一般按小型多旋翼的常见范围取经验值并把取值的依据写进注释方便回来改。我给自己立了一个规矩凡是从论文里直接拿到的参数标“原文”凡是推断出来的参数标“假设”并在仿真说明里写清楚假设理由。这样做的好处是如果后续结果对不上我可以快速定位到底是建模错误还是参数偏差而不是把所有变量搅在一起猜。2. 多旋翼动力学建模的核心推导与简化2.1 坐标系与欧拉角约定第一步就决定生死大多数多旋翼论文用北东地NED坐标系原因为水平控制的方向定义很直观北向和东向分别对应X轴和Y轴重力沿Z轴正方向。机体坐标系的X轴指向机头Y轴指向机翼右侧Z轴向下推力沿Z轴负方向产生。欧拉角我用的是Z-Y-X内旋转序也就是先偏航、再俯仰、最后滚转。很多论文不给转序但转序不同旋转矩阵每一项的符号就不同姿态解算出来的油门分配也会跟着错。我在建模型时把旋转矩阵单独写成了一个子函数方便随时替换转序。这里强烈建议你也这么做别把坐标变换藏在整段方程里后面排查会非常痛苦。2.2 六自由度运动方程刚体动力学怎么落到状态空间多旋翼本质是一个六自由度刚体位置三自由度、姿态三自由度。在地球坐标系下位置方程可以写成m * ddot{x_E} R_B2E * F_B m * g * e3其中F_B [0, 0, -T]T是四个电机产生的总推力R_B2E把机体系力投影到地球系e3是NED下指向地心的单位向量。这个式子的意思是飞行器能“横着飞”不是因为发动机给它一个水平推力而是它先倾转机体让推力向量在水平方向出现分量。姿态部分我用角速度运动学加转动动力学。欧拉角变化率与机体角速度的关系由转序决定典型写法phi_dot p q * sin(phi) * tan(theta) r * cos(phi) * tan(theta) theta_dot q * cos(phi) - r * sin(phi) psi_dot (q * sin(phi) r * cos(phi)) / cos(theta)转动动力学保留陀螺力矩项不做简单解耦p_dot (Iyy - Izz) / Ixx * q * r Mx / Ixx q_dot (Izz - Ixx) / Iyy * p * r My / Iyy r_dot (Ixx - Iyy) / Izz * p * q Mz / Izz这些方程都是论文里会写但不会解释的“大致这么回事”。实际仿真里前两行欧拉角速率公式在theta接近90度时会产生奇异所以做特技飞行仿真不能用欧拉角要换四元数。但复现论文里的常规控制场景时飞行器始终在小角度附近运动欧拉角完全够用而且和论文的对比更直观。2.3 旋翼的力与力矩模型推力的平方律和混控矩阵每个电机的拉力近似正比于转速平方F_i b * omega_i^2。反扭矩也正比于转速平方M_i d * omega_i^2。这里的b是升力系数d是反扭矩系数量纲取决于omega单位是rad/s还是RPM。复现时最容易翻车的就是单位问题。我见过论文用rad/s算系数结果仿真里给了一个看似合理的油门量实际转速已经爆炸。力矩的产生分三路。滚转力矩由左右电机拉力差产生俯仰力矩由前后电机拉力差产生偏航力矩由对角方向的反扭矩差产生。把所有电机的拉力和力矩组合起来就得到混控矩阵。以我用的X型四旋翼为例假设电机1号左前、2号右前、3号左后、4号右后那么F_T b * (w1^2 w2^2 w3^2 w4^2) Mx L * b * (w1^2 - w2^2 - w3^2 w4^2) // 依布局定义 My L * b * (w1^2 w2^2 - w3^2 - w4^2) Mz d * (w1^2 - w2^2 w3^2 - w4^2)这块我不建议直接抄网上代码因为电机编号习惯不同符号很容易整体翻转。我的办法是写完混控矩阵后单独给一个电机加大油门看模型里对应的Mx、My、Mz符号是否与布局图吻合。这一步叫“开环自检”虽简单但能拦下80%的建模错误。2.4 电机响应模型一阶惯性环节不容忽略有些论文为了简化直接把控制器输出的油门量当作转速也就是完全忽略电机动态。如果只做低频悬停仿真这种简化还能忍一比阶跃响应的快速段你会发现实际仿真比论文曲线软得多就是因为电机延迟。小型多旋翼电机的响应特性近似一阶惯性环节时间常数通常在5到15毫秒之间我取10毫秒作为假设值tau * dot_omega omega omega_cmd把这个式子放进模型后姿态阶跃响应的快速段明显更贴近论文曲线了。仿真里把这个环节放在混控矩阵和力/力矩模型之间也就是控制器先给出期望转速经过一阶惯性后得到实际转速再经过平方关系生成拉力和力矩。3. 控制系统总体结构与PID整定3.1 内外环结构为什么是标配多旋翼控制没有复杂到要用现代控制理论打底一篇论文的稳定性证明和仿真往往靠的就是串级PID。原因很简单姿态动态比位置动态快得多把小时间尺度的姿态回路放在里面大时间尺度的位置回路放在外面两个回路可以分别整定。这叫时间尺度分离工程上被验证过无数遍。我的控制结构分成三层。最外面是位置环输入期望位置输出期望姿态角和总油门。中间是姿态角度环输入期望欧拉角输出期望机体角速度。最里面是角速度环输入期望角速度输出三轴力矩再经过混控矩阵分配电机转速。这种结构的直观好处是调试时可以一层一层往里加不会出现一上来就十几个参数全乱的局面。3.2 姿态环的串级PID角速度内环是灵魂我在姿态环里采用的是角度外环P、角速度内环PID的形式。角度环不用微分因为角速度已经包含姿态的变化率信息直接由内环反馈就把抑制作用给够了。期望角速度由角度误差生成p_des Kp_phi * (phi_des - phi) q_des Kp_theta * (theta_des - theta) r_des Kp_psi * (psi_des - psi)角速度环再对每个轴做一个PID。以滚转轴为例Mx_cmd Kp_p * (p_des - p) Ki_p * Integral(p_des - p) Kd_p * (p_des_dot - p_dot)这里积分项用来消除重力偏置和模型不准带来的稳态误差微分项用于抑制角速度超调。实际仿真中微分项对噪声非常敏感我会在差分之后加一个截止频率约为20赫兹的低通滤波器否则模型里的一点点数值噪声都能被放大成可见的力矩抖动。3.3 位置环与期望姿态解算水平移动全靠“倾斜”位置环直接输出三个轴期望加速度但飞行器没有水平推进器水平加速度只能靠倾转机体实现。小角度近似下NED系里水平加速度与欧拉角近似成线性关系。为了兼容任意偏航角我先做坐标旋转把地球系期望加速度投影到机体的水平前进轴和水平侧向轴a_x_B cos(psi) * a_x_E sin(psi) * a_y_E a_y_B -sin(psi) * a_x_E cos(psi) * a_y_E然后得到期望姿态角phi_des a_y_B / g theta_des -a_x_B / g这是从“推力倾斜产生水平加速度”反推出来的。高度通道单独处理总油门指令为T_cmd m * (g - a_z_E / (cos(phi)*cos(theta)))其中除以cos(phi)*cos(theta)是为了补偿大倾角时推力垂直分量的损失。具体符号以你的坐标系定义为准我在调试时特意做了一次正负测试给一个正x方向期望加速度观察模型里的theta_des是否为负如果相反就把公式整体翻转。3.4 PID参数整定的实用顺序调参顺序我固定为从内到外。第一步整定角速度环只给角速度指令阶跃让Kp_p从很小的值逐步增大直到响应出现轻微振荡然后回调到临界值的60%到70%。第二步整定角度环把角度环Kp_phi调出来让姿态阶跃超调控制在15%左右。第三步整定高度环和位置环位置环增益从非常小开始因为位置误差会通过姿态解算放大到姿态指令太激进容易激发姿态振荡。我使用的一组示例参数是角速度环Kp_p4.0、Ki_p0.6、Kd_p0.05角度环Kp_phi6.0位置环Kp_x1.0、Kd_x1.2、Kp_z2.0、Kd_z1.0。这些数值对应一个质量约1.4千克、机臂长度0.18米的小型四旋翼不要直接照搬但可以作为调参起点。4. Simulink仿真模型的搭建与实现4.1 顶层模型架构怎么把控制器和对象分开Simulink模型我分成了四个大块期望指令源、控制器、混控与电机、六自由度对象。期望指令源用Signal Editor给阶跃或轨迹控制器输出三轴力矩和总油门混控与电机部分把控制量变成四个转速最终生成力与力矩六自由度对象积分出位置、速度和姿态。我强烈建议把“控制器”和“对象”做成两个独立子系统中间不要混信号。这样排查问题时可以直接在控制器输出端断开单独给对象施加一个恒定力矩验证开环响应是否符合刚体力学的直觉。这种“先开环自检、后闭环整定”的流程比一边跑一边猜高效得多。4.2 手写六自由度运动方程比自带模块更可控Simulink自带Aerospace Blockset里有多旋翼模块但论文复现我不推荐直接用原因有两点。一是自带模块的坐标和输入输出约定是固定的很难和论文符号体系一一对应二是复现工作本身要求你把每个方程写清楚用黑盒模块会丢掉很多理解环节。我选择手写运动方程用MATLAB Function封装导数函数再连接Integrator模块做积分。状态量定义为一个12维向量x [x_E, y_E, z_E, vx_E, vy_E, vz_E, phi, theta, psi, p, q, r]积分器初始条件设为悬停平衡点比如高度为1米欧拉角全为0。MATLAB Function内部按前面推导的方程计算导数核心伪代码如下function xdot plantDerivative(x, u, p) % u [F_T, Mx, My, Mz] 来自控制器和电机模型 % p 参数结构体 % 解析状态 rE x(1:3); vE x(4:6); euler x(7:9); omega x(10:12); phi euler(1); theta euler(2); psi euler(3); % 旋转矩阵Z-Y-X RB2E rotation321(phi, theta, psi); % 位置方程 accE RB2E * [0; 0; -u(1)] / p.m [0; 0; p.g]; % 姿态运动学 T [1, sin(phi)*tan(theta), cos(phi)*tan(theta); 0, cos(phi), -sin(phi); 0, sin(phi)/cos(theta), cos(phi)/cos(theta)]; eulerDot T * omega; % 转动动力学 p_dot (p.Iyy - p.Izz)/p.Ixx * omega(2)*omega(3) u(2)/p.Ixx; q_dot (p.Izz - p.Ixx)/p.Iyy * omega(1)*omega(3) u(3)/p.Iyy; r_dot (p.Ixx - p.Iyy)/p.Izz * omega(1)*omega(2) u(4)/p.Izz; xdot [vE; accE; eulerDot; p_dot; q_dot; r_dot]; end这段代码看起来简单但要注意几个关键点。旋转矩阵函数必须单独验证我通常让欧拉角全为0时它退化为单位阵。状态量顺序必须和积分器的初始条件保持一致。还有重力方向NED系下重力加速度g取正数加在Z轴正方向很多人第一次写会在这里加反符号。4.3 控制器子系统的Simulink实现细节控制器部分我既用了Simulink自带PID Controller模块也手写了一部分逻辑。自带PID模块在“离散”模式下返回速度更快但论文复现需要自定义积分限幅和抗饱和所以更推荐自己搭。核心结构是减法器、增益、积分器、饱和模块和求和器。我遇到的第一个大坑是积分饱和。位置环和姿态环的PI控制器如果输出饱和后积分器继续累积飞行器会延迟响应甚至振荡。解决方法是在积分器后面加一个判断逻辑当控制器输出达到上限或下限时冻结积分增量。Simulink里可以用积分器模块的“limit output”和“state port”组合实现也可以用MATLAB Function写一个带抗饱和的离散积分器清晰且便于调整。另一个细节是微分项的近似实现。连续系统里直接用Derivative模块会产生大量的数值噪声尤其是当对象方程里包含限幅、死区这类非线性时。我用的是带一阶滤波的近似微分filteredDer (1 / Tf) * (input - state)Tf取0.05秒左右。这个滤波项同时起到了论文里没写但必须有“软硬件限制”的作用。4.4 仿真求解器与参数配置固定步长还是变步长求解器选择直接影响仿真速度与稳定性。第一次跑这个模型时我用了默认的变步长ODE45结果发现带电机限幅后模型在控制量切换瞬间出现微小的代数环变步长在零点附近会不断缩短步长导致仿真速度极慢甚至出现“数值奇异”提示。后来我改成固定步长ODE4步长取1毫秒。对四旋翼这种系统1毫秒足够刻画出姿态环0.1秒左右的时间尺度。如果你发现仿真频率还是太高可以先跑一个短暂时间窗口查看响应曲线的平滑度再决定要不要增大步长。此外Simulink里所有连续模块的采样时间最好都统一避免混用连续和离散模块导致步长不稳定的情况。初始化脚本我用一个MATLAB脚本统一维护所有参数放到模型的Callback里这样每次打开模型或开始仿真前都自动加载。参数结构体里同时保存了“来源”字段标注是论文原值还是经验假设值。这个习惯帮了大忙因为我在后期对比不同论文数据时能快速锁定哪些参数是假设项。5. 仿真数据验证与参数调整实录5.1 悬停姿态阶跃响应一切正常时的曲线长什么样模型搭完参数初设之后第一个验证动作是姿态阶跃。我给滚转通道一个5度阶跃指令同时保持俯仰和偏航期望为0。仿真出来的响应曲线大致是0.1秒左右开始有明显滚转角变化0.4秒到达峰值超调约10%随后在1秒内收敛到稳态。角速度p的曲线先快速冲高再回落控制器力矩输出没有出现大幅振荡。这里特别留意一个现象初始起飞前如果积分器初值设成0而悬停油门不为0模型会在第一个时刻产生一个“启动冲击”。我的处理方式是在初始化脚本里先做一次配平计算将期望高度设为1米在零姿态下计算T_cmd m*g并把稳态油门作为电机模型的输出初值避免启动瞬间出现跳变。5.2 高度阶跃与位置跟踪从悬停到动起来高度通道单独验证时给0.5米高度阶跃响应大约2秒稳定。这个通道的积分项很关键因为没有积分项由于重力估计偏差会残留一个几十厘米的高度误差。高度通道的响应速度不宜过快因为高度指令最终要变成总油门油门突变会激起姿态耦合力矩。水平位置跟踪我做了一个8米边长的方形轨迹期望速度采用梯形规划。观察结果是飞行器在拐角处有明显的超调最大横向误差约0.3米。这主要是因为位置环增益还不够高而姿态环带宽又限制了位置环继续加Kp。想进一步压轨迹误差要么提高姿态环带宽要么加入前馈加速度项。论文里通常会有前馈设计但从复现角度看先不加前馈把基线调稳也是一种保守但有效的方式。5.3 调参过程中碰到的怪异现象汇总调谐过程中有两次让我印象深刻的“灵异现象”。第一次是只给一个电机加大油门模型里四个转速都朝着同一方向飘了查了很久发现是混控矩阵中反扭矩的符号写反了。反扭矩方向取决于电机旋转方向对角电机的旋向相反因此相邻电机反扭矩应有一部分正向、一部分负向我全写成同号导致偏航通道直接失控。第二次是位置环期望姿态角公式写反结果给一个正向x位置误差飞行器竟然往负方向加速。排查时我故意把积分器和控制器全部断开给模型一个恒定的正向俯仰角观察x方向加速度方向这才定位到是姿态解算的正负号问题。从那以后我把“开环自检”列为模型完成后的必做项不给直接闭环保湿。6. 常见问题与避坑速查表现象可能原因排查思路解决方式仿真开始即报NaN或Inf重力方向符号错、质量单位用了克或千克混用、混控矩阵有零项检查初始化脚本的单位一致性断开闭环单给恒定力矩统一SI单位校正重力项方向姿态响应高频振荡角速度环增益过大或微分项噪声放大观察角速度反馈曲线是不是有高频毛刺调小Kp_p给微分加低通滤波悬停后缓慢漂移位置环积分项不足、期望姿态角有常值偏差记录长期稳态误差的极性增加高度和水平通道积分系数位置往期望相反方向加速姿态解算公式符号反了做正负测试给正向俯仰角看x加速度翻转phi_des、theta_des的公式符号仿真速度极慢变步长在非线性限幅处反复缩短步长查看步长曲线检查是否有代数环改固定步长ODE4步长取1e-3响应曲线比论文慢很多电机时间常数取值过大或被省略去掉一阶惯性环节看上限将电机时间常数调到5毫秒级别高度响应有稳态误差重力项和推力单位量纲不匹配配平时算理论油门并对比模型稳态油门用T_cmd m*g做配平检查输出限幅后恢复慢积分器未做抗饱和处理观察积分器状态是否持续累计加积分钳位或积分冻结逻辑我个人的经验是这类模型八成的问题不是原理看不懂而是符号约定、单位、积分器初始化这类“低级但致命”的细节。排查时不要相信感觉一定要先用开环测试验证每一个子系统的输入输出关系再闭环保湿。另外仿真模型的建造顺序也建议从内到外先电机和混控矩阵再刚体动力学再角度环再位置环每一层都验证过了再往上封装。按照这套流程走下来论文里的那些公式才会真正变成你自己的工具。我现在看一篇新的多旋翼论文第一件事已经不是去读它推导了而是先把它的坐标系画出来把电机编号标出来再决定要不要动手复现。这个过程本身就是论文复现最值钱的部分。
阅读完成 · 觉得有帮助？
