复现论文这件事听起来像是把公式抄进代码跑一跑就完事实际上我第一晚就翻了车。那篇某期刊的四旋翼姿态控制论文我把姿态运动学方程、PID控制器原样搬进仿真脚本点击运行零输入状态下模型愣是在0.6秒内原地翻滚了两周。后来才意识到复现的核心根本不是“抄公式”而是把论文里省略掉的坐标系定义、控制信号流向、执行器模型、仿真离散化方式逐一补全。这篇内容就是按我完整复现一次四旋翼姿态控制论文的过程整理出来的从建模、PID设计到仿真验证全覆盖适合正在做课程设计、毕设或期刊复现的开发者参考。1. 复现前必须先想清楚的模型假设与控制架构1.1 模型简化假设决定你能不能和论文曲线对得上四旋翼的飞行动力学模型在教科书里长得非常复杂但大多数姿态控制论文只取了其中一小部分。我复现时第一件事不是写代码而是拿一张纸把论文里的公式全部抄一遍然后标注它背后用了哪些假设。最常见的假设有这么几条把四旋翼当成刚体认为桨叶形变、气动弹性效应可以忽略认为结构上严格对称因此转动惯量矩阵近似为对角阵认为电机响应可以简化为一阶惯性环节或者干脆理想化即力矩输出不做延迟认为小角度姿态变化于是把欧拉角速率方程线性化处理。这些假设听起来很“常规”但它们直接影响你和论文的匹配度。比如论文里如果用了小角度近似 sinθ≈θ你用这套控制器去仿真30度姿态阶跃必然和原论文曲线产生明显偏差因为原论文本来就没有在你这个工况下保证精度。另一个容易忽略的是重力在姿态动力学中的处理方式。四旋翼的重力作用线过质心时它不会产生机体轴上的力矩所以在姿态动力学方程里经常被整体忽略。这个前提是质心与机体坐标原点重合但你如果看了论文附录里的模型实物参数布局未必对称那就得自己判断要不要补重力引起的力矩项。我的习惯是保留一个开关变量默认按论文来但跑对比试验时可以随时打开关闭。1.2 坐标系和符号约定最容易埋雷的地方坐标系定义是整个复现过程中最容易埋雷的地方几乎没有之一。同一套物理量世界系用北东地NED还是东北天ENU定义姿态角正负规则就完全不同机体系原点取在重心还是取在几何中心转动惯量的数值也会跟着变。论文里通常用一串符号说明坐标系但很多作者不会强调它到底用的是 NED 还是 ENU你得从受力分析图或者重力向量方向去反推。我复现时把这个当成一个单独的检查项来做先建立一个三维坐标示意标注世界系X、Y、Z轴方向再标机体系的X、Y、Z轴方向然后检查欧拉角旋转顺序。四旋翼论文里常用的旋转顺序是Z-Y-X偏航、俯仰、滚转对应绕Z轴偏航、绕新Y轴俯仰、绕新X轴滚转。同一个角度如果旋转顺序换成了Z-X-Y后面所有姿态运动学方程全部要重写。判断方法很简单看论文正文给出的欧拉角速率到角速度的转换矩阵如果矩阵第1行第2列是 sinφ tanθ那大概率就是Z-Y-X顺序。我还习惯给自己做一张“符号自检表”把论文里的每个符号、含义、单位、我代码里的变量名全部列出来逐项对照。表格里这一行写的是什么、代码里用的是哪个变量一眼就能看出来避免出现扭矩和转动惯量量纲对不上这类低级问题。论文符号含义单位代码变量备注φ, θ, ψ滚转、俯仰、偏航角radphi, theta, psi世界系到机体系p, q, r机体角速度rad/sp, q, r机体轴上分量Jx, Jy, Jz转动惯量对角元素kg·m²Jx, Jy, Jz假设惯性积为0τx, τy, τz控制力矩N·mtau控制器输出Ωi电机转速rad/somega_motor控制分配前1.3 先画出信号流再写代码模型假设和符号问题理清之后下一步是把论文里的控制框图还原成信号流。很多复现者的第一个错误是直接找论文里的控制器公式却不知道控制器输入到底是什么信号、输出又流向哪里。姿态控制论文里最常见的信号流是期望姿态角 → 角度误差 → 角度环PID → 期望角速度 → 角速度误差 → 角速度环PID → 期望力矩 → 控制分配 → 电机转速 → 四旋翼动力学 → 姿态角。这套串级结构你可以在绝大部分论文里找到。但也有单环结构的论文直接由角度误差生成力矩这种情况下没有角速度环调参思路完全不同。你还需要确认期望姿态角是从哪里来的。有些论文直接给定常值期望姿态有些则把期望姿态角看作是位置控制器的输出也就是整机级联控制的一环。如果只复现姿态控制器那期望姿态角就是外部输入通常给一个阶跃或者正弦信号就够了。信号流里最容易被忽略的是传感器模型。原始论文可能直接在连续域里做控制器设计仿真时也没有加入IMU测量噪声和延迟但有些对比实验里会加。我的复现原则是先做一个完全理想传感器的版本保证能复现论文的阶跃响应然后再决定要不要加噪声。否则一开始就带噪声你可能分不清曲线抖动是控制器问题还是传感器问题。2. 姿态运动学与动力学建模把公式变成可以跑起来的代码2.1 欧拉角速率与机体角速度的转换关系姿态控制的被控对象有两个核心方程第一个是姿态运动学方程描述欧拉角随时间变化和机体角速度之间的关系。在Z-Y-X旋转顺序下滚转角速率、俯仰角速率、偏航角速率与机体角速度p、q、r之间有如下关系φ_dot p q·sinφ·tanθ r·cosφ·tanθθ_dot q·cosφ − r·sinφψ_dot q·sinφ / cosθ r·cosφ / cosθ这个公式看起来不复杂但有几个隐含的坑。第一个坑是θ接近90度时tanθ趋向无穷方程出现奇异。这在四旋翼正常飞行范围内不会触发但如果你测试大角度翻转动作就会在俯仰90度附近看到数值爆炸。期刊复现一般不会测这种工况但自己扩展测试时要知道这个边界。第二个坑是符号方向。我复现时因为坐标系里Y轴方向定义和论文不一致导致θ_dot这一行正负号反了仿真里俯仰角疯狂正向漂移。检查方法是把p、q、r中的某一个单独设成非零比如只给q0.1 rad/s其他角速度为零看θ是否以预期速率变化同时φ、ψ保持不变。这个验证步骤不需要跑完整仿真单独写一个小测试脚本几十秒就能出结果。第三个坑是欧拉角与角速度的积分方向。有些初学者会在代码里直接对期望角速度积分来得到期望欧拉角这是不对的。期望欧拉角应该由轨迹规划或位置控制器给出角速度环只负责消除角速度误差不要混在一起。代码里实现运动学方程时我一般先写一个独立的姿态导数函数参数是当前欧拉角和当前机体角速度import numpy as np def euler_dynamics(phi, theta, psi, p, q, r): # Z-Y-X 旋转顺序下的欧拉角速率 phi_dot p q * np.sin(phi) * np.tan(theta) r * np.cos(phi) * np.tan(theta) theta_dot q * np.cos(phi) - r * np.sin(phi) psi_dot q * np.sin(phi) / np.cos(theta) r * np.cos(phi) / np.cos(theta) return np.array([phi_dot, theta_dot, psi_dot])注意这里的theta在cos(theta)接近0时会出问题实际工程里可以加一个判断当cos(theta)绝对值小于某个阈值时用四元数接替欧拉角。不过在普通姿态控制复现里这一步基本用不到。2.2 刚体旋转动力学与力矩模型第二个核心方程是刚体旋转动力学方程。在机体系下角速度ω对时间的导数由惯性力矩、陀螺力矩和外部控制力矩共同决定J·ω_dot −ω × (J·ω) τ其中τ是控制力矩向量。展开成标量形式就是p_dot (τx − (Jz − Jy)·q·r) / Jxq_dot (τy − (Jx − Jz)·r·p) / Jyr_dot (τz − (Jy − Jx)·p·q) / Jz这里的交叉项 (Jz−Jy)·q·r 就是陀螺效应它在高速旋转时会产生明显的耦合力矩。如果论文里忽略了这一项那你的仿真结果在快速偏航和滚转耦合时会和论文不一致。我复现的那个案例里论文明确说转动惯量三轴近似相等于是交叉项约等于0但这个近似在机体左右不对称的实物模型上是站不住脚的。转动惯量参数的选择也很关键。如果原论文给出了参数表直接使用如果只给了控制器增益和模型参数范围那就得自己估算。四旋翼转动惯量通常在0.002到0.02 kg·m²量级电机轴距越长、总质量越大转动惯量越大。可以先用简单几何公式估算比如把机体近似为质量均匀分布的十字架然后做一次参数敏感性测试看控制器在参数变化下是否仍然稳定。控制力矩τ是从哪里来的在电机层面上每个旋翼产生的升力大致与转速平方成正比每个旋翼在机体轴上产生反扭矩也与转速平方相关。忽略电机动态时可以直接把期望力矩当作输入考虑电机动态时通常给一个一阶惯性环节τ_actual_dot (τ_desired − τ_actual) / T_motor其中T_motor是电机时间常数通常在0.02到0.1秒之间。我复现时把这个时间常数先设成0也就是理想执行器等控制器参数全部调通之后再打开观察它对系统带宽的影响。2.3 完整的姿态受控模型代码把运动学和动力学方程合在一起就得到一个状态为 [φ, θ, ψ, p, q, r] 的系统。控制器输出期望力矩经过动力学方程得到角速度再通过运动学方程得到欧拉角。下面这段代码是我复现用的基础框架没有仿真平台依赖纯Python几秒钟就能跑完import numpy as np # 模型参数 Jx, Jy, Jz 0.005, 0.005, 0.010 # kg·m^2 Tm 0.0 # 电机时间常数先设0 # 控制器参数先随便给后面再整定 ang_Kp, ang_Ki, ang_Kd 2.0, 0.0, 0.05 rate_Kp, rate_Ki, rate_Kd 0.1, 0.0, 0.005 # 限幅 max_angle_cmd 0.5 # 角度环输出的期望角速度限幅 rad/s max_torque 0.5 # 角速度环输出的力矩限幅 N·m state np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # phi, theta, psi, p, q, r phi_des 0.3 # 滚转角阶跃目标 def attitude_dynamics(state, tau): phi, theta, psi, p, q, r state # 运动学 euler_dot euler_dynamics(phi, theta, psi, p, q, r) # 动力学 p_dot (tau[0] - (Jz - Jy) * q * r) / Jx q_dot (tau[1] - (Jx - Jz) * r * p) / Jy r_dot (tau[2] - (Jy - Jx) * p * q) / Jz return np.concatenate([euler_dot, [p_dot, q_dot, r_dot]]) def rate_controller(p, q, r, p_des, q_des, r_des, dt, integ): err np.array([p_des - p, q_des - q, r_des - r]) integ err * dt integ np.clip(integ, -0.5, 0.5) out np.array([ rate_Kp * err[0] rate_Ki * integ[0] rate_Kd * (err[0] - rate_err_prev[0]) / dt, rate_Kp * err[1] rate_Ki * integ[1] rate_Kd * (err[1] - rate_err_prev[1]) / dt, rate_Kp * err[2] rate_Ki * integ[2] rate_Kd * (err[2] - rate_err_prev[2]) / dt ]) rate_err_prev[:] err return np.clip(out, -max_torque, max_torque)控制器积分项的变量初始化在这段略掉了完整版本里要放在循环外部。这个框架的好处是动力学、控制器、执行器三者分得很开后面加电机模型、加传感器噪声、改控制器结构都是从某个独立函数入手不会牵一发动全身。3. 串级PID结构角度环与角速度环为什么必须分开3.1 单环PID和串级PID在四旋翼上的本质差别初学复现的人最容易提出一个问题姿态控制不就是三个通道各用一套PID吗为什么要分角度环和角速度环两层这里的关键是物理量属性不同。角度环控制的输出是期望角速度角速度环控制的输出才是力矩。从期望角度到期望角速度是一个运动学关系从期望角速度到期望力矩才涉及动力学。如果你只对角度误差做PID那么角度误差大时输出一个大力矩却完全不知道当前机体已经在等速旋转这个力矩是去抵消角速度还是去改变角速度控制器的阻尼特性完全没有发挥出来。串级结构把这个问题拆开了内环角速度控制器像一个“阻尼器”它负责建立角速度到力矩的响应关系让系统在角速度层面表现得像一个有确定带宽的惯性环节外环角度控制器只负责生成角速度指令相当于指挥内环完成角度跟踪。这样每个环的控制目标单纯参数整定也更容易。从频域角度看内环带宽应该比外环高3到5倍。比如角度环的期望穿越频率是2 rad/s那角速度环的穿越频率最好做到6到10 rad/s。这个比例在仿真里可以通过波特图验证实际操作中就是调内环增益让内环响应明显比外环快。如果两个环的带宽接近串级结构退化成单环调参时会发现无论怎么调总有一段频率范围是抖动的。我在复现时非常推荐用这个判断标准去评估参数是否合理给一个期望角度看角速度指令是否比角度响应快得多给一个期望角速度看力矩输出是否足够迅速且平滑。如果角度环和角速度环的响应速度混在一起说明参数比例需要调整。3.2 PID参数的初值估算与整定顺序串级PID的参数整定顺序只有一个正确方向先内环后外环。先调外环有一个非常现实的问题——角度环输出的是角速度参考如果内环本身不收敛你根本没法评估外环参数的作用效果。角速度环的P参数初值可以从物理意义出发估算。角速度环控制的是力矩力矩除以转动惯量是角加速度所以角速度环的P参数大致可以理解为“期望每秒产生多大的角加速度来消除1 rad/s的角速度误差”。比如Jx0.005 kg·m²希望1 rad/s误差产生10 rad/s²的角加速度那P大约就是 0.005 × 10 0.05。这个估算结果不一定是最优值但至少量级不会错。I参数一般先取P的10%到20%D参数先取0让系统稳定后再一点一点加。角度环的P参数处理思路类似但要注意它的物理单位。角度环输出的是角速度命令输入是角度误差rad所以P的量纲是1/s。期望1 rad角度误差产生1 rad/s角速度修正P就可以从1开始试然后根据响应速度调整。I参数用来消除稳态误差但角度环的积分作用如果太强大角度阶跃下容易产生超调甚至发散我的习惯是先不着急加角度环的I先看纯P控制下稳态误差有多大再决定补偿强度。调参的时候我养成了一个“一参一跑”的习惯。就是每次只改变一个参数记录当前姿态响应曲线再改下一个。不要同时调两三个参数否则超调到底是哪个参数引起的根本说不清。下面这张表是我复现时的调参记录格式每轮测试只需要几分钟轮次修改参数现象结论1rate_Kp0.05角速度指令响应慢力矩输出小需要增大P2rate_Kp0.15角速度指令有明显超调P偏大回退到0.123ang_Kp2.0角度环有约0.02rad稳态误差加少量I4ang_Ki0.3稳态误差消失阶跃无超调参数基本可用3.3 工程细节限幅、积分分离与微分滤波复现论文时最容易忽略的是控制器输出限幅。很多论文只在推导里写PID控制器表达式不给执行器饱和范围。如果你的仿真模型里电机转速无限大、力矩输出没有饱和控制器可能给出一个毫无物理意义的力矩指令而仿真结果却看起来很好。这种“假收敛”在论文复现里非常迷惑人。我的做法是在角速度环输出处加一个力矩限幅数值参考四旋翼电机能提供的最大推力和力矩来设定。如果原论文没给可以按每轴最大力矩0.5 N·m这种量级先设然后用不同限幅值做对比看系统在大姿态变化时是否出现饱和现象。饱和不仅会让响应变慢还会引入积分饱和问题——积分项持续累积输出被限幅截断一旦误差反向积分项还要过一段时间才能退回来导致大幅度超调。一个有效的抗积分饱和方法是只有控制器输出没有到达限幅边界时才允许积分累积。实现起来也很简单在积分更新处加一个判断条件# 抗积分饱和写法 out_before_sat rate_Kp * err rate_Ki * integ rate_Kd * deriv if not (out_before_sat max_torque or out_before_sat -max_torque): integ err * dt微分项也是容易炸仿真的一环。角度环的微分项如果直接对测量角度求导测量噪声会被放大得很厉害。我在仿真里即使没加传感器噪声也会在微分项上加一阶低通滤波滤波截止频率取控制器频率的10倍左右。这样既保留了微分项的阻尼作用又不会因为数值噪声造成抖动。4. 仿真发散到收敛完整排错链路与实测记录4.1 第一个失败案例俯仰角在0.3秒内爆掉说回开头那个翻车场景。我第一天复现时的仿真现象非常典型初始俯仰角设了0.05 rad期望滚转角0.3 rad控制器输出正常但俯仰角在0.3秒内开始震荡随后直接冲到1.5 rad整个模型进入发散状态。我当时的排查链路是这样的。先关掉控制器把期望力矩设成零观察模型在无输入下的自由响应。结果发现俯仰角会自己慢慢增大说明模型本身有问题不是PID的问题。接着检查运动学方程发现我把欧拉角速率方程里的 −r·sinφ 项写成了 r·sinφ。这个正负号错误在滚转和偏航耦合时才会暴露但初始滚转角的微小偏差已经足以触发它。修正符号后模型在零输入下稳定了但加上控制又发散。这次我用同样的方法把角度环输出直接置零只保留角速度环给一个固定的期望角速度发现角速度环能够跟踪但没有阻尼——D参数是0系统在持续振荡。加上D参数后角速度环收敛然后把角度环逐步接回来最终整个回路才稳定下来。这个排查过程让我养成一个好习惯永远从内到外逐级验证。先验证运动学、再验证动力学、再验证内环、最后接通外环。每一步都确认这一段没问题再继续下一步。直接端到端跑通调试很难定位问题。4.2 仿真步长与数值积分方法为什么你的模型比论文“更会炸”数值积分方法对复现结果的影响比很多复现者想象的要大。同一个模型用不同的步长和积分方法结果可能一个收敛一个发散。我做过一组对比测试固定控制器参数不变只改变仿真步长和积分器结果如下积分步长积分方法仿真结果0.05s欧拉法明显发散0.01s欧拉法临界稳定小幅振荡0.01sRK4收敛曲线平滑0.001sRK4收敛结果与0.01s基本一致背后的原因不复杂四旋翼姿态动力学带宽通常在几赫兹到几十赫兹控制周期如果太慢控制器就是在用非常滞后的信息做控制等效于给系统引入了很大的纯延迟。欧拉法在步长较大时数值阻尼不够容易在刚性项上积累误差。我的建议是仿真步长至少取控制频率的5到10倍四阶龙格库塔几乎是标配别用欧拉法省这几行代码。如果你后期还加了一阶电机惯性环节那相当于引入了额外的非零极点系统对步长更敏感。我遇到的情况是在理想执行器下0.01s步长没问题加上0.05s电机时间常数后同样步长开始抖动把步长降到0.002s才恢复。4.3 控制对象建模误差期望力矩到电机转速的转化另一个常见的“隐性问题”是控制分配那一步。姿态控制器输出的是力矩但电机只能产生转速中间还隔着一层控制分配映射。论文里会把这层映射写成控制分配矩阵如果直接忽略它等于认为电机的响应是瞬时的四旋翼模型会变得比真实系统简单得多。控制分配的定义前右、前左、后右、后左四个电机转速分别为Ω1到Ω4各有升力系数和扭矩系数通过差动产生滚转、俯仰、偏航力矩。为了让姿态环的仿真更真实我通常先按力矩控制器设计等姿态环验证完毕后再加入分配矩阵和电机模型。这样做的原因是如果一开始就把所有环节都加进去遇到仿真发散时你很难判断是控制器的问题还是分配矩阵写错了。加入电机模型后还需要注意电机转速的范围。四旋翼电机转速一般有上下限转速不能为负也不能超过电机最大转速。控制器输出的期望力矩经过分配后若超过电机能力范围就会出现执行器饱和和前一节说的输出限幅是两回事。论文复现时如果你发现姿态响应慢于论文曲线先检查是不是无意识地把执行器饱和范围设得太小了。5. 如何判断复现成功曲线对比的量化方法与经验5.1 别拿肉眼看曲线量化指标才有说服力很多复现者把论文截图和仿真曲线放到一起肉眼看形状差不多就算复现成功。这种做法在初学时可以但严谨的复现应该用一组量化指标来对比。最常用的几个指标是稳态误差、超调量、调节时间2%准则和均方根误差RMSE。调节时间的定义是响应曲线进入并保持在期望值±2%范围内所需的最短时间。比如期望姿态角是0.3 rad2%就是0.006 rad从阶跃开始时刻算起到曲线第一次进入这个范围且不再越出为止。超调量是最大峰值减去期望值再除以期望值的百分比。稳态误差是充分长时间后姿态角与期望值之差的绝对值。RMSE则衡量整段响应曲线和期望值之间的总体偏差计算公式为 sqrt(mean((y_true - y_sim)²))其中y_true在阶跃响应场景下就是常值期望。我用的对比方法是把仿真数据存成数组按时间索引计算这些指标再和论文原文里的数据或从图中读取的数据做差。如果原论文只给了图没有给具体数据那就需要用图像识别工具从截图里提取曲线坐标或者直接在图上量取几个关键点。这一步麻烦但很有价值因为很多论文的曲线本身就不够理想你可能复现得比它还“好”这时候反而说明模型细节有差异。5.2 标准验证场景阶跃、正弦跟踪与抗扰动论文复现要有全局观不能只看一条阶跃响应。我在复现时至少搭建三个验证场景这样才能判断控制器是真的掌握还是只在某个特定工况下碰巧工作。第一个场景是姿态角阶跃也是最常见的。分别测试10度和30度阶跃观察超调和调节时间是否和论文一致。30度阶跃下小角度假设逐渐失效如果原论文的控制器在小角度假设下设计大角度阶跃响应会和理论预测有明显偏差这个偏差本身就是复现分析的一部分。第二个场景是正弦跟踪。期望滚转角设为相位幅值0.1 rad、频率0.5 Hz的正弦信号记录实际滚转角曲线与期望曲线的相位延迟和幅值衰减。这个场景能暴露串级PID的带宽特性如果内环带宽不够跟踪正弦信号时会看到明显的幅值缩小和相位滞后。第三个场景是抗扰动测试。在仿真进行到中间时刻时比如0.3秒时给模型叠加一个持续0.2秒的常值力矩扰动观察姿态角能否恢复到期望值以及恢复时间。这可以验证控制器对角速度环的阻尼能力和积分作用是否足够。论文里如果没有这个图你可以自己加它能帮助判断控制器鲁棒性。这三个场景跑完如果都和论文给出的曲线在指标上接近那基本可以认为复现成功了。如果只有阶跃匹配另外两个不匹配说明模型可能缺少了某个动力学细节比如陀螺力矩项或者电机延迟。5.3 我的对比流程与常见偏差来源我自己的对比流程是一个闭环跑完所有验证场景后回到论文重新阅读模型部分看是否有遗漏项。这个流程大概是这样先读取论文的所有测试曲线图记录关键指标然后用我自己的模型跑同样的测试条件包括同样的初始状态、同样的期望信号幅值、同样的控制器参数比较指标后把差异列成表格逐项分析原因。常见偏差来源我列一下都是我反复踩过的坑。第一个是初始状态不一致。论文测试可能从某个非零姿态角开始二阶系统初始状态不同响应自然不同。第二个是控制器参数符号和积分初始化方式。有些论文里的PID用的是增量式写法有些是位置式写法微分项是否包含测量值还是误差值也会影响曲线形态。第三个是执行器范围。前面反复强调过限幅范围不一致会让仿真结果产生显著差异。第四个是电机响应模型。有些论文直接忽略有些用一阶模型时间常数不同系统响应就不同。遇到差异之后不要盲目调PID参数去贴合曲线。参数的改变能掩盖模型缺失但会透支你对系统的理解。我更推荐的做法是回到建模层从模型细节里找差异的来源把模型补全后再和论文进行比对。经过这一轮完整的复现我把论文里的姿态控制器从公式变成了可运行的仿真模型。最后分享一个小技巧在仿真之前先把控制器关掉给模型一个初始角速度扰动观察它能否按照动力学方程自然运动。如果这一步就出现能量不守恒之类的奇怪现象那大概率是模型本身的问题不要急着调PID参数。这个习惯帮我省下的排查时间比任何仿真框架的选择都多。