简介面向四旋翼无人机控制研究与开发者这套工程包提供了完整的自抗扰控制ADRC实现方案涵盖无人机动力学建模、扩展状态观测器设计、扰动抑制与稳定控制策略。压缩包共3个文件体积仅113KB包含两个Simulink模型文件和一个MATLAB脚本前者分别对应四旋翼物理模型与线性自抗扰控制器LADRC模型后者用于配置无人机质量、惯量等关键参数便于直接开展仿真调试与参数整定。LADRC部分基于扩展状态观测器与自适应策略能够动态补偿外部干扰为从传统PID到自抗扰控制的进阶理解提供了可运行示例。目前已有180人学习下载适合具备一定控制理论基础的无人机爱好者与相关专业学生可借助该资源快速验证算法效果、掌握Simulink建模方法并作为课程设计或科研实验的起步模板。1. 四旋翼ADRC把“总扰动”当成状态估计出来再消掉调试过quadcopter悬停的人都有体会PID参数在室内无风时好好的拿到室外一阵侧风过来就开始低频晃动换个载重又要从头调。ADRCActive Disturbance Rejection Control之所以在无人机圈子里受欢迎是因为它把模型失配、阵风、机体磨损这些不确定因素统一看成“总扰动”用扩展状态观测器实时估出来并补偿掉。这个包里给的不是教学demo而是一套能直接跑的四旋翼ADRC仿真工程Quadparameters.m负责物理参数PlantModel.slx是被控对象LADRC_GAO.slx是线性自抗扰控制器走的是高志强Gao的带宽参数化路线。适合刚把PID调明白、想往现代控制理论再走一步的飞控工程师。2. ADRC与LADRC_GAO从韩京清到带宽参数化2.1 韩京清提出的ADRC核心构成TD、ESO、NLSEF韩京清先生早年指出PID的“误差取微分”在实际系统中容易放大噪声于是重新设计了控制器结构。第一是跟踪微分器Tracking Differentiator, TD它给参考信号安排一个过渡过程同时提取出近乎无超调的微分信号第二是扩展状态观测器Extended State Observer, ESO这是整个ADRC的发动机它把未建模动态和外部扰动的总和作为“扩张状态”估计出来第三是非线性状态误差反馈NLSEF用非线性组合代替PID的线性加权提高收敛速度。这三块组装起来控制器不再依赖精确模型而是靠观测器实时感知系统在多大程度上偏离了标称模型。在四旋翼上这个思想非常直接。悬停时重力与推力平衡但桨叶老化、电池电压下降、突风都算作扰动。传统PID只能通过积分慢慢抵消稳态误差而ADRC把扰动当作一个虚拟状态在下一个控制周期就补偿掉大部分。这也是为什么ADRC在无人机抗风研究中经常被拿来和PID、LQR做对比。但韩京清原始版本里的非线性函数比如fal(e,α,δ)参数多整定靠试凑。高志强在2003年前后把它简化成线性形式让工程师只需要敲两个带宽数字就能用这就是后面LADRC_GAO.slx的理论根源。2.2 高志强的LADRCESO与控制律线性化带宽参数化LADRC做两点简化第一省掉跟踪微分器参考信号直接进入误差计算第二把ESO和非线性反馈都线性化。以二阶系统为例被控对象写成[ \ddot{y} f(y,\dot{y},w,t) bu ]其中f是总扰动b是控制增益的已知部分。LADRC把f也当作状态构造三阶线性ESO[ \begin{aligned} \dot{z}_1 z_2 \beta_1 (y-z_1) \ \dot{z}_2 z_3 b u \beta_2 (y-z_1) \ \dot{z}_3 \beta_3 (y-z_1) \end{aligned} ]z3就是总扰动f的估计。控制律写成[ u \frac{u_0 - z_3}{b} ]其中u0可以只是PD控制k_p(r-z1) k_d(-\dot{z}_1)。这样扰动被前馈抵消系统被拉回到纯积分的标称结构。高志强把观测器增益设计成带宽形式[ \beta_13\omega_o,\quad \beta_23\omega_o^2,\quad \beta_3\omega_o^3 ]控制器增益同理k_pω_c²k_d2ω_c。这样一来整个控制器只有两个参数ω_o观测器带宽越快则状态估计收敛越快但太高会把测量噪声放大和ω_c控制器带宽决定闭环响应速度。LADRC_GAO.slx这个文件名里的GAO就是高志强的姓模型里用的正是这套带宽参数化。对比一下传统PID和LADRC的调参工作量。PID至少需要调P、I、D三个参数而且对于姿态内环和位置外环两套PID耦合时互相牵制。LADRC每个通道只需要两个带宽而且物理意义明确ω_c针对参考跟踪速度ω_o针对抗扰能力。用这个仿真工程做实验时绝大多数时间是在来回调整这两个带宽而不是面对六七个互相影响的PID系数。2.3 Quadparameters.m参数与控制器参数的关系Quadparameters.m负责把四旋翼的物理参数灌进MATLAB工作区。典型的参数包括机体质量m旋翼力臂l转动惯量Ixx、Iyy、Izz以及螺旋桨的升力系数、扭矩系数等。下面是这个脚本里常见的一段%% Quadcopter physical parameters m 1.2; % mass kg g 9.81; % gravity m/s^2 l 0.25; % arm length m Ixx 0.018; % inertia around X axis kg.m^2 Iyy 0.018; Izz 0.034; Kf 1.2e-5; % motor force coefficient Km 2.0e-7; % motor torque coefficient这里m和I决定被控对象的响应快慢Kf和Km影响控制输入到实际力的换算。在仿真设计中这些参数被Simulink模型里的增益模块或MATLAB Function块引用。用LADRC调参时ω_c的初始值通常取开环穿越频率的2~3倍而ω_o要设计成ω_c的5倍左右如果Quadparameters.m里把Ixx改小转动惯量下降同样的ω_c会让角速度响应变快甚至振荡这时就需要适当降低ω_c。还有一点容易被忽略Quadparameters.m里如果定义了电机响应时间常数T_motor那么LADRC的设计必须把它算进总扰动里。ESO虽然能估计掉未建模动态但电机延迟太大时观测器的补偿会出现相位滞后。所以在仿真里我一般会把电机一阶惯性环节的带宽写进PlantModel.slx然后根据它的时间常数把ω_o限制在电机带宽的1/3以内否则高频段抖得厉害。3. 在Simulink里搭PlantModel.slx动力与测量回路3.1 四旋翼动力学模型位置环与姿态环的耦合四旋翼是一个典型的内外环结构。外环是水平位置状态是[x, y, z, vx, vy, vz]内环是姿态角状态是[φ, θ, ψ, p, q, r]。水平位置的变化由机体倾斜产生的水平推力分量决定[ m\ddot{x} (c_{\psi}s_{\theta}c_{\phi} s_{\psi}s_{\phi}) \cdot T ][ m\ddot{y} (s_{\psi}s_{\theta}c_{\phi} - c_{\psi}s_{\phi}) \cdot T ][ m\ddot{z} c_{\theta}c_{\phi} \cdot T - mg ]姿态角速度方程则跟转动惯量和力矩直接相关[ \dot{p} \frac{I_{yy}-I_{zz}}{I_{xx}} q r \frac{\tau_\phi}{I_{xx}} ]PlantModel.slx通常把这些公式用积分器和乘法器搭出来而不是用一个大的MATLAB Function块。这样做的好处是能直观看到每个积分环节的初始值设置。比如仿真开始时要让四旋翼稳定悬停在一个点上那么姿态角的初始值要和初始油门匹配否则一启动就会先冲出去一段。常见做法是把高度通道的期望值设成1m初始z从0开始控制器内部会通过积分把自己拉起来。位置环和姿态环的耦合在这里体现得很明显。外环控制器给出期望横滚角和俯仰角内环再把角度跟踪到给定值。如果内外环都用LADRC来设计两个ESO会同时工作。外环估计的是风、推力衰减等慢变扰动内环估计的是力矩不平衡、重心偏移等快变扰动。如果两个观测器带宽设置得太接近内外环之间可能出现频率打架具体表现是位置响应缓慢但姿态高频抖动。我在调参时通常保持外环ω_c 1~2 Hz内环ω_c 8~12 Hz差距拉开到5倍以上。3.2 从Quadparameters.m自动加载模型参数到工作区打开Quadparameters.m直接运行它会把参数写入base workspace。需要注意的是Simulink模型里的变量名必须和脚本里完全一致否则模型里引用的变量会显示为undefined。为了保证仿真可复现我习惯在脚本末尾加上一段参数校验%% sanity check assert(Ixx 0 Iyy 0 Izz 0, Inertia must be positive); assert(kf 0 km 0, Force/torque coefficients must be positive); disp(Quadcopter parameters loaded successfully);运行这个脚本后在命令行输入who就能看到所有变量。这一招在调试PlantModel.slx时特别有用。当你双击模型里某个Gain模块看到它的参数是一个变量名比如l但脚本没定义模型就会在仿真启动时报错。提前做校验能少踩很多坑。如果用的是R2019b之后的版本建议在模型初始化回调里直接调用Quadparameters脚本。做法是在Simulink模型窗口中进入Model Properties在InitFcn回调中写run(Quadparameters.m)。这样每次点“运行”按钮都会自动刷新参数不会出现改了脚本但工作区还是旧参数的尴尬情况。3.3 将PlantModel.slx与LADRC_GAO.slx组成闭环几个关键连线这个压缩包里的两个Simulink模型是分离的需要自己组成闭环。一般我会新建一个顶层模型把PlantModel.slx和LADRC_GAO.slx各用子系统引进来。连线时需要明确三组端口参考输入位置和偏航角的期望值通常是一个4维向量xe, ye, ze, psi_e测量反馈PlantModel输出的实际位置、速度和姿态角控制输出四个电机的PWM等效输入或者直接是推力T和力矩τφ、τθ、τψ的合成如果PlantModel.slx的输出端是姿态角和角速度LADRC_GAO.slx的输入端需要匹配。两边的端口宽度不一致时Simulink会在仿真开始时提示端口维度错误。这时可以去Signal Attributes里查看到底是哪里多了个信号。最常见的错误是把[p q r]角速度写成[phi theta psi]角度导致控制器拿到的是角度而不是角速度整个系统直接振荡。我建议在顶层模型加两个Scope或To Workspace模块分别记录控制量和姿态角。这样跑完仿真后用out.logsout就能把数据拖到工作区里继续分析。如果不想在每个通道上加Selector模块可以用向量拼接技巧% 从simout取数绘图 t simout.time; phi simout.signals(1).values(:,1); theta simout.signals(1).values(:,2); psi simout.signals(1).values(:,3); plot(t, phi*180/pi, t, theta*180/pi); grid on;这段代码把姿态角的弧度转成角度并绘图。为什么我总强调要转成角度因为Simulink里默认单位是弧度直接看数据会觉得角度变化很小误以为响应不足其实已经超调了。4. LADRC_GAO.slx内部实现ESO观测器与扰动补偿的仿真逻辑4.1 ESO的方程与Simulink实现要点打开LADRC_GAO.slx你会看到核心是一个ESO子系统。它以被控对象的输出y和控制输入u为输入输出三个估计状态z1是输出估计z2是速度估计z3是扰动估计。在Simulink里ESO一般用积分器搭出来对应前面的微分方程组。为了在仿真里抑制代数环需要把观测器增益β1、β2、β3放到积分器之后、反馈相加点之前。一个典型的ESO实现排列方式是从上到下依次是z1、z2、z3的积分器每个积分器的输入来自前一状态的反馈和误差加权。如果直接在MATLAB Function块里写ESO代码会很短function [z1, z2, z3] eso(y, u, b0, wo) persistent x1 x2 x3; if isempty(x1) x1 0; x2 0; x3 0; % initial state end dt 0.001; % need actual solver step e y - x1; % state update x1 x1 dt*(x2 3*wo*e); x2 x2 dt*(x3 b0*u 3*wo^2*e); x3 x3 dt*(wo^3*e); z1 x1; z2 x2; z3 x3;注意这段代码里硬编码了dt0.001这在固定步长仿真里没问题但如果换成变步长求解器会引入额外误差。实际工程里我推荐在Simulink里用积分器模块让求解器自己控制步长观测器精度会高得多。这也是为什么给你的这个包里LADRC_GAO.slx用图形化积分器而不是MATLAB Function的原因。ESO初值设置很关键。如果z3初始值设成0而实际作用在quadcopter上的重力或其他恒定力不为0那么在仿真最初几毫秒内z3会从0快速爬升。若此时控制器已经输出大油门容易造成初始尖峰。因此我一般把z3的初始值设成悬停时的等效推力除以b0这样扰动估计从仿真开始就贴近实际值起飞阶段不会猛点头。4.2 控制器带宽ωc与观测器带宽ωo的整定原则LADRC的参数只有ωo和ωc但整定并不是随便填两个数就行。下面这张表是我在四旋翼仿真里经常采用的起点按不同响应速度需求划分应用场景ω_c (rad/s)ω_o (rad/s)说明高度外环2~415~25响应慢抗风依赖ESO提供前馈姿态角环10~1560~100需要快速跟随杆量角速度内环20~30100~200噪声放大风险高ω_o和ω_c的比值一般取5~15。比值太小扰动估计跟不上系统会有稳态误差比值太大高频噪声通过ESO进入控制量表现为电机指令颤抖。实际调试时先把ωc调到一个使系统不振荡的值再逐步增大ωo直到扰动响应开始出现噪声尾巴为止然后退回一点。这个“退回一点”就是工程上的安全余量。控制输入b0也是一个隐藏参数。b0是被控对象中控制增益的估计值。对姿态通道b0是角加速度与力矩的比值比如b0≈1/Ixx。如果b0比真实值小ESO会努力把差的这部分也算进扰动里虽然最终可能也能稳定但会消耗更多控制能量。所以在建好的PlantModel.slx里我习惯先用一次开环阶跃响应直接测出角加速度与输入力矩的比值把这个实测值填回LADRC_GAO.slx里的b0效果立竿见影。4.3 仿真发散、抖动与数值积分器的坑很多人在这个项目里遇到的第一个问题是把LADRC_GAO.slx连上PlantModel.slx后仿真开始不到0.1秒就发散屏幕上全是红色错误。原因多半有三个。第一个是采样时间不一致。ESO内部积分器如果用连续时间而控制器里某些模块用了离散控制但离散周期设得太大比如0.1s那么观测器根本无法稳定响应。解决办法是把LADRC_GAO.slx里的所有离散模块统一到一个采样率通常取1ms或更小。第二个是代数环。如果你把ESO的输出z3直接送回控制器并参与ESO自身的输入计算Simulink会检测到代数环。比如有的模型里把控制量u也作为ESO输入而u又依赖z3这就在一个步长里形成了互相依赖。解决办法是在u进ESO之前加一个Memory模块或者Unit Delay打破代数环。代价是会引入一个步长的滞后但对于1ms步长来说完全可接受。第三个是数值求导。有些自己搭的模型里为了得到角加速度会对角速度信号做Derivative。Derivative模块对噪声极度敏感和ESO里大量的高增益反馈叠加后容易在积分时产生跌代振荡。正确做法是从PlantModel.slx的积分器后端直接引出状态而不要用Derivative模块。下面这段代码用脚本方式对LADRC_GAO.slx做参数批量扫描找出临界稳定的ωc值wc_vec 5:1:20; res zeros(size(wc_vec)); for i 1:length(wc_vec) wc wc_vec(i); wo wc * 8; sim(LADRC_GAO.slx); y tout; % actually use logsout % simple stability check: max |phi| 30 deg res(i) max(abs(logsout.get(phi).Values.Data)); end plot(wc_vec, res);这段代码把ωc从5扫到20每个点跑一次仿真记录最大姿态角偏差。如果发现某个ωc下姿态角超过30度说明已经接近失稳边界。用这个曲线选ωc比瞎试快很多。5. 把LADRC调成能扛风突增益调度与验证技巧当你在Simulink里把LADRC跑通了下一步就是验证它是否真的扛得住风。常见做法是在PlantModel.slx里加一个风扰信号比如在水平速度方程上叠加一个随时间变化的加速度干扰模拟突然来一阵侧风。我通常用Signal Builder加一个阶跃加上短暂脉冲的组合这样既能测稳态抗扰又能测瞬态恢复。风扰加到系统里后如果发现姿态角恢复太慢第一反应应该是增大ωo而不是盲目增大ωc。因为ωc决定的是参考跟踪速度对阶跃风扰的抑制主要靠ESO的扰动估计速度。有个验证技巧把ESO的z3信号通过Scope拉出来和真实扰动信号对比。如果两者曲线形状一致只是滞后一小段说明ωo够用如果z3明显跟不上真实扰动加大ωo直到滞后缩短到可接受范围。另一个实用技巧是做“假设扰动不可测”的实验。在仿真中把LADRC的控制输入强制设置为u(u0-z3)/b0然后把z3手动置零相当于关掉扰动补偿看系统会变成什么样子。你会发现系统变成纯PD控制风扰下出现大的稳态偏差。这个对比实验能直观证明ESO的补偿有效也比空口说“ADRC抗扰强”有说服力得多。做完这组对比把两组仿真数据保存成不同的结构体用一段脚本批量绘制抗扰性能指标% 比较有ESO补偿和没有ESO补偿时的最大偏差 with_eso logsout_with.get(phi).Values.Data; without_eso logsout_without.get(phi).Values.Data; max_dev_comp max(abs(with_eso) - abs(phi_ref)); max_dev_uncomp max(abs(without_eso) - abs(phi_ref)); fprintf(Compensated max dev: %.3f deg\n, max_dev_comp*180/pi); fprintf(Uncompensated max dev: %.3f deg\n, max_dev_uncomp*180/pi);最后你可以尝试把ωc和ωo做成随时间缓慢变化的斜坡参数模拟飞行中电池电压下降导致的执行器增益变化。运行同一组仿真脚本观察LADRC在参数漂移下是否仍然保持稳定。如果出现轻微振荡把ESO带宽向后调小10%再跑一次。经过这几轮对抗性测试你对这个quadcopter ADRC仿真工程的理解就从“能跑通”升级为“能把控边界”了。本文还有配套的精品资源点击获取 四旋翼ADRC仿真：LADRC与ESO的Simulink实现与调参
简介面向四旋翼无人机控制研究与开发者这套工程包提供了完整的自抗扰控制ADRC实现方案涵盖无人机动力学建模、扩展状态观测器设计、扰动抑制与稳定控制策略。压缩包共3个文件体积仅113KB包含两个Simulink模型文件和一个MATLAB脚本前者分别对应四旋翼物理模型与线性自抗扰控制器LADRC模型后者用于配置无人机质量、惯量等关键参数便于直接开展仿真调试与参数整定。LADRC部分基于扩展状态观测器与自适应策略能够动态补偿外部干扰为从传统PID到自抗扰控制的进阶理解提供了可运行示例。目前已有180人学习下载适合具备一定控制理论基础的无人机爱好者与相关专业学生可借助该资源快速验证算法效果、掌握Simulink建模方法并作为课程设计或科研实验的起步模板。1. 四旋翼ADRC把“总扰动”当成状态估计出来再消掉调试过quadcopter悬停的人都有体会PID参数在室内无风时好好的拿到室外一阵侧风过来就开始低频晃动换个载重又要从头调。ADRCActive Disturbance Rejection Control之所以在无人机圈子里受欢迎是因为它把模型失配、阵风、机体磨损这些不确定因素统一看成“总扰动”用扩展状态观测器实时估出来并补偿掉。这个包里给的不是教学demo而是一套能直接跑的四旋翼ADRC仿真工程Quadparameters.m负责物理参数PlantModel.slx是被控对象LADRC_GAO.slx是线性自抗扰控制器走的是高志强Gao的带宽参数化路线。适合刚把PID调明白、想往现代控制理论再走一步的飞控工程师。2. ADRC与LADRC_GAO从韩京清到带宽参数化2.1 韩京清提出的ADRC核心构成TD、ESO、NLSEF韩京清先生早年指出PID的“误差取微分”在实际系统中容易放大噪声于是重新设计了控制器结构。第一是跟踪微分器Tracking Differentiator, TD它给参考信号安排一个过渡过程同时提取出近乎无超调的微分信号第二是扩展状态观测器Extended State Observer, ESO这是整个ADRC的发动机它把未建模动态和外部扰动的总和作为“扩张状态”估计出来第三是非线性状态误差反馈NLSEF用非线性组合代替PID的线性加权提高收敛速度。这三块组装起来控制器不再依赖精确模型而是靠观测器实时感知系统在多大程度上偏离了标称模型。在四旋翼上这个思想非常直接。悬停时重力与推力平衡但桨叶老化、电池电压下降、突风都算作扰动。传统PID只能通过积分慢慢抵消稳态误差而ADRC把扰动当作一个虚拟状态在下一个控制周期就补偿掉大部分。这也是为什么ADRC在无人机抗风研究中经常被拿来和PID、LQR做对比。但韩京清原始版本里的非线性函数比如fal(e,α,δ)参数多整定靠试凑。高志强在2003年前后把它简化成线性形式让工程师只需要敲两个带宽数字就能用这就是后面LADRC_GAO.slx的理论根源。2.2 高志强的LADRCESO与控制律线性化带宽参数化LADRC做两点简化第一省掉跟踪微分器参考信号直接进入误差计算第二把ESO和非线性反馈都线性化。以二阶系统为例被控对象写成[ \ddot{y} f(y,\dot{y},w,t) bu ]其中f是总扰动b是控制增益的已知部分。LADRC把f也当作状态构造三阶线性ESO[ \begin{aligned} \dot{z}_1 z_2 \beta_1 (y-z_1) \ \dot{z}_2 z_3 b u \beta_2 (y-z_1) \ \dot{z}_3 \beta_3 (y-z_1) \end{aligned} ]z3就是总扰动f的估计。控制律写成[ u \frac{u_0 - z_3}{b} ]其中u0可以只是PD控制k_p(r-z1) k_d(-\dot{z}_1)。这样扰动被前馈抵消系统被拉回到纯积分的标称结构。高志强把观测器增益设计成带宽形式[ \beta_13\omega_o,\quad \beta_23\omega_o^2,\quad \beta_3\omega_o^3 ]控制器增益同理k_pω_c²k_d2ω_c。这样一来整个控制器只有两个参数ω_o观测器带宽越快则状态估计收敛越快但太高会把测量噪声放大和ω_c控制器带宽决定闭环响应速度。LADRC_GAO.slx这个文件名里的GAO就是高志强的姓模型里用的正是这套带宽参数化。对比一下传统PID和LADRC的调参工作量。PID至少需要调P、I、D三个参数而且对于姿态内环和位置外环两套PID耦合时互相牵制。LADRC每个通道只需要两个带宽而且物理意义明确ω_c针对参考跟踪速度ω_o针对抗扰能力。用这个仿真工程做实验时绝大多数时间是在来回调整这两个带宽而不是面对六七个互相影响的PID系数。2.3 Quadparameters.m参数与控制器参数的关系Quadparameters.m负责把四旋翼的物理参数灌进MATLAB工作区。典型的参数包括机体质量m旋翼力臂l转动惯量Ixx、Iyy、Izz以及螺旋桨的升力系数、扭矩系数等。下面是这个脚本里常见的一段%% Quadcopter physical parameters m 1.2; % mass kg g 9.81; % gravity m/s^2 l 0.25; % arm length m Ixx 0.018; % inertia around X axis kg.m^2 Iyy 0.018; Izz 0.034; Kf 1.2e-5; % motor force coefficient Km 2.0e-7; % motor torque coefficient这里m和I决定被控对象的响应快慢Kf和Km影响控制输入到实际力的换算。在仿真设计中这些参数被Simulink模型里的增益模块或MATLAB Function块引用。用LADRC调参时ω_c的初始值通常取开环穿越频率的2~3倍而ω_o要设计成ω_c的5倍左右如果Quadparameters.m里把Ixx改小转动惯量下降同样的ω_c会让角速度响应变快甚至振荡这时就需要适当降低ω_c。还有一点容易被忽略Quadparameters.m里如果定义了电机响应时间常数T_motor那么LADRC的设计必须把它算进总扰动里。ESO虽然能估计掉未建模动态但电机延迟太大时观测器的补偿会出现相位滞后。所以在仿真里我一般会把电机一阶惯性环节的带宽写进PlantModel.slx然后根据它的时间常数把ω_o限制在电机带宽的1/3以内否则高频段抖得厉害。3. 在Simulink里搭PlantModel.slx动力与测量回路3.1 四旋翼动力学模型位置环与姿态环的耦合四旋翼是一个典型的内外环结构。外环是水平位置状态是[x, y, z, vx, vy, vz]内环是姿态角状态是[φ, θ, ψ, p, q, r]。水平位置的变化由机体倾斜产生的水平推力分量决定[ m\ddot{x} (c_{\psi}s_{\theta}c_{\phi} s_{\psi}s_{\phi}) \cdot T ][ m\ddot{y} (s_{\psi}s_{\theta}c_{\phi} - c_{\psi}s_{\phi}) \cdot T ][ m\ddot{z} c_{\theta}c_{\phi} \cdot T - mg ]姿态角速度方程则跟转动惯量和力矩直接相关[ \dot{p} \frac{I_{yy}-I_{zz}}{I_{xx}} q r \frac{\tau_\phi}{I_{xx}} ]PlantModel.slx通常把这些公式用积分器和乘法器搭出来而不是用一个大的MATLAB Function块。这样做的好处是能直观看到每个积分环节的初始值设置。比如仿真开始时要让四旋翼稳定悬停在一个点上那么姿态角的初始值要和初始油门匹配否则一启动就会先冲出去一段。常见做法是把高度通道的期望值设成1m初始z从0开始控制器内部会通过积分把自己拉起来。位置环和姿态环的耦合在这里体现得很明显。外环控制器给出期望横滚角和俯仰角内环再把角度跟踪到给定值。如果内外环都用LADRC来设计两个ESO会同时工作。外环估计的是风、推力衰减等慢变扰动内环估计的是力矩不平衡、重心偏移等快变扰动。如果两个观测器带宽设置得太接近内外环之间可能出现频率打架具体表现是位置响应缓慢但姿态高频抖动。我在调参时通常保持外环ω_c 1~2 Hz内环ω_c 8~12 Hz差距拉开到5倍以上。3.2 从Quadparameters.m自动加载模型参数到工作区打开Quadparameters.m直接运行它会把参数写入base workspace。需要注意的是Simulink模型里的变量名必须和脚本里完全一致否则模型里引用的变量会显示为undefined。为了保证仿真可复现我习惯在脚本末尾加上一段参数校验%% sanity check assert(Ixx 0 Iyy 0 Izz 0, Inertia must be positive); assert(kf 0 km 0, Force/torque coefficients must be positive); disp(Quadcopter parameters loaded successfully);运行这个脚本后在命令行输入who就能看到所有变量。这一招在调试PlantModel.slx时特别有用。当你双击模型里某个Gain模块看到它的参数是一个变量名比如l但脚本没定义模型就会在仿真启动时报错。提前做校验能少踩很多坑。如果用的是R2019b之后的版本建议在模型初始化回调里直接调用Quadparameters脚本。做法是在Simulink模型窗口中进入Model Properties在InitFcn回调中写run(Quadparameters.m)。这样每次点“运行”按钮都会自动刷新参数不会出现改了脚本但工作区还是旧参数的尴尬情况。3.3 将PlantModel.slx与LADRC_GAO.slx组成闭环几个关键连线这个压缩包里的两个Simulink模型是分离的需要自己组成闭环。一般我会新建一个顶层模型把PlantModel.slx和LADRC_GAO.slx各用子系统引进来。连线时需要明确三组端口参考输入位置和偏航角的期望值通常是一个4维向量xe, ye, ze, psi_e测量反馈PlantModel输出的实际位置、速度和姿态角控制输出四个电机的PWM等效输入或者直接是推力T和力矩τφ、τθ、τψ的合成如果PlantModel.slx的输出端是姿态角和角速度LADRC_GAO.slx的输入端需要匹配。两边的端口宽度不一致时Simulink会在仿真开始时提示端口维度错误。这时可以去Signal Attributes里查看到底是哪里多了个信号。最常见的错误是把[p q r]角速度写成[phi theta psi]角度导致控制器拿到的是角度而不是角速度整个系统直接振荡。我建议在顶层模型加两个Scope或To Workspace模块分别记录控制量和姿态角。这样跑完仿真后用out.logsout就能把数据拖到工作区里继续分析。如果不想在每个通道上加Selector模块可以用向量拼接技巧% 从simout取数绘图 t simout.time; phi simout.signals(1).values(:,1); theta simout.signals(1).values(:,2); psi simout.signals(1).values(:,3); plot(t, phi*180/pi, t, theta*180/pi); grid on;这段代码把姿态角的弧度转成角度并绘图。为什么我总强调要转成角度因为Simulink里默认单位是弧度直接看数据会觉得角度变化很小误以为响应不足其实已经超调了。4. LADRC_GAO.slx内部实现ESO观测器与扰动补偿的仿真逻辑4.1 ESO的方程与Simulink实现要点打开LADRC_GAO.slx你会看到核心是一个ESO子系统。它以被控对象的输出y和控制输入u为输入输出三个估计状态z1是输出估计z2是速度估计z3是扰动估计。在Simulink里ESO一般用积分器搭出来对应前面的微分方程组。为了在仿真里抑制代数环需要把观测器增益β1、β2、β3放到积分器之后、反馈相加点之前。一个典型的ESO实现排列方式是从上到下依次是z1、z2、z3的积分器每个积分器的输入来自前一状态的反馈和误差加权。如果直接在MATLAB Function块里写ESO代码会很短function [z1, z2, z3] eso(y, u, b0, wo) persistent x1 x2 x3; if isempty(x1) x1 0; x2 0; x3 0; % initial state end dt 0.001; % need actual solver step e y - x1; % state update x1 x1 dt*(x2 3*wo*e); x2 x2 dt*(x3 b0*u 3*wo^2*e); x3 x3 dt*(wo^3*e); z1 x1; z2 x2; z3 x3;注意这段代码里硬编码了dt0.001这在固定步长仿真里没问题但如果换成变步长求解器会引入额外误差。实际工程里我推荐在Simulink里用积分器模块让求解器自己控制步长观测器精度会高得多。这也是为什么给你的这个包里LADRC_GAO.slx用图形化积分器而不是MATLAB Function的原因。ESO初值设置很关键。如果z3初始值设成0而实际作用在quadcopter上的重力或其他恒定力不为0那么在仿真最初几毫秒内z3会从0快速爬升。若此时控制器已经输出大油门容易造成初始尖峰。因此我一般把z3的初始值设成悬停时的等效推力除以b0这样扰动估计从仿真开始就贴近实际值起飞阶段不会猛点头。4.2 控制器带宽ωc与观测器带宽ωo的整定原则LADRC的参数只有ωo和ωc但整定并不是随便填两个数就行。下面这张表是我在四旋翼仿真里经常采用的起点按不同响应速度需求划分应用场景ω_c (rad/s)ω_o (rad/s)说明高度外环2~415~25响应慢抗风依赖ESO提供前馈姿态角环10~1560~100需要快速跟随杆量角速度内环20~30100~200噪声放大风险高ω_o和ω_c的比值一般取5~15。比值太小扰动估计跟不上系统会有稳态误差比值太大高频噪声通过ESO进入控制量表现为电机指令颤抖。实际调试时先把ωc调到一个使系统不振荡的值再逐步增大ωo直到扰动响应开始出现噪声尾巴为止然后退回一点。这个“退回一点”就是工程上的安全余量。控制输入b0也是一个隐藏参数。b0是被控对象中控制增益的估计值。对姿态通道b0是角加速度与力矩的比值比如b0≈1/Ixx。如果b0比真实值小ESO会努力把差的这部分也算进扰动里虽然最终可能也能稳定但会消耗更多控制能量。所以在建好的PlantModel.slx里我习惯先用一次开环阶跃响应直接测出角加速度与输入力矩的比值把这个实测值填回LADRC_GAO.slx里的b0效果立竿见影。4.3 仿真发散、抖动与数值积分器的坑很多人在这个项目里遇到的第一个问题是把LADRC_GAO.slx连上PlantModel.slx后仿真开始不到0.1秒就发散屏幕上全是红色错误。原因多半有三个。第一个是采样时间不一致。ESO内部积分器如果用连续时间而控制器里某些模块用了离散控制但离散周期设得太大比如0.1s那么观测器根本无法稳定响应。解决办法是把LADRC_GAO.slx里的所有离散模块统一到一个采样率通常取1ms或更小。第二个是代数环。如果你把ESO的输出z3直接送回控制器并参与ESO自身的输入计算Simulink会检测到代数环。比如有的模型里把控制量u也作为ESO输入而u又依赖z3这就在一个步长里形成了互相依赖。解决办法是在u进ESO之前加一个Memory模块或者Unit Delay打破代数环。代价是会引入一个步长的滞后但对于1ms步长来说完全可接受。第三个是数值求导。有些自己搭的模型里为了得到角加速度会对角速度信号做Derivative。Derivative模块对噪声极度敏感和ESO里大量的高增益反馈叠加后容易在积分时产生跌代振荡。正确做法是从PlantModel.slx的积分器后端直接引出状态而不要用Derivative模块。下面这段代码用脚本方式对LADRC_GAO.slx做参数批量扫描找出临界稳定的ωc值wc_vec 5:1:20; res zeros(size(wc_vec)); for i 1:length(wc_vec) wc wc_vec(i); wo wc * 8; sim(LADRC_GAO.slx); y tout; % actually use logsout % simple stability check: max |phi| 30 deg res(i) max(abs(logsout.get(phi).Values.Data)); end plot(wc_vec, res);这段代码把ωc从5扫到20每个点跑一次仿真记录最大姿态角偏差。如果发现某个ωc下姿态角超过30度说明已经接近失稳边界。用这个曲线选ωc比瞎试快很多。5. 把LADRC调成能扛风突增益调度与验证技巧当你在Simulink里把LADRC跑通了下一步就是验证它是否真的扛得住风。常见做法是在PlantModel.slx里加一个风扰信号比如在水平速度方程上叠加一个随时间变化的加速度干扰模拟突然来一阵侧风。我通常用Signal Builder加一个阶跃加上短暂脉冲的组合这样既能测稳态抗扰又能测瞬态恢复。风扰加到系统里后如果发现姿态角恢复太慢第一反应应该是增大ωo而不是盲目增大ωc。因为ωc决定的是参考跟踪速度对阶跃风扰的抑制主要靠ESO的扰动估计速度。有个验证技巧把ESO的z3信号通过Scope拉出来和真实扰动信号对比。如果两者曲线形状一致只是滞后一小段说明ωo够用如果z3明显跟不上真实扰动加大ωo直到滞后缩短到可接受范围。另一个实用技巧是做“假设扰动不可测”的实验。在仿真中把LADRC的控制输入强制设置为u(u0-z3)/b0然后把z3手动置零相当于关掉扰动补偿看系统会变成什么样子。你会发现系统变成纯PD控制风扰下出现大的稳态偏差。这个对比实验能直观证明ESO的补偿有效也比空口说“ADRC抗扰强”有说服力得多。做完这组对比把两组仿真数据保存成不同的结构体用一段脚本批量绘制抗扰性能指标% 比较有ESO补偿和没有ESO补偿时的最大偏差 with_eso logsout_with.get(phi).Values.Data; without_eso logsout_without.get(phi).Values.Data; max_dev_comp max(abs(with_eso) - abs(phi_ref)); max_dev_uncomp max(abs(without_eso) - abs(phi_ref)); fprintf(Compensated max dev: %.3f deg\n, max_dev_comp*180/pi); fprintf(Uncompensated max dev: %.3f deg\n, max_dev_uncomp*180/pi);最后你可以尝试把ωc和ωo做成随时间缓慢变化的斜坡参数模拟飞行中电池电压下降导致的执行器增益变化。运行同一组仿真脚本观察LADRC在参数漂移下是否仍然保持稳定。如果出现轻微振荡把ESO带宽向后调小10%再跑一次。经过这几轮对抗性测试你对这个quadcopter ADRC仿真工程的理解就从“能跑通”升级为“能把控边界”了。本文还有配套的精品资源点击获取