动态反演与ESO融合的无人机鲁棒自适应姿态控制器复现解析

最近我把这套基于动态反演和扩展状态观测器ESO的无人机鲁棒反馈线性化自适应姿态控制器从头到尾复现了一遍。拿到这个项目标题的时候我的第一反应是又是一个把几个控制名词叠在一起的“缝合怪”。但等我把公式推完、把Simulink模型跑通之后我承认这套设计是有一套内在逻辑的并不是表面看起来那样堆砌。它能解决什么问题传统反馈线性化控制强依赖精确模型模型一偏性能就明显下滑ESO专门负责在线估计总扰动并做前馈补偿自适应模块进一步处理控制通道增益漂移这类“老顽固”。三者叠在一起的姿态控制器对参数不确定和外部风扰都有不错的容忍度。如果你正在做飞控算法或者想把一篇论文里的方案真正跑通看曲线这篇复现笔记应该能帮你省下不少折腾时间。1. 方案选型动态反演、ESO和自适应各解决什么问题1.1 动态反演把非线性拉直成一阶积分器动态反演Dynamic Inversion本质上是一种“基于模型做反向计算”的控制思想。先写出系统微分方程再把控制输入当成未知数解出来让系统对外呈现出一个期望的简单动态。放在无人机姿态控制里这个思路特别好用。四旋翼姿态动力学的非线性主要来自两个地方角速度耦合项 ω × (Jω)以及欧拉角运动学变换里的三角函数。如果把这些项当作已知信息去参与控制量计算配合一个相对准确的转动惯量矩阵 J就能把“角加速度实际是多少”和“我施加了多大力矩”这条关系强行拉平。控制逻辑一下子清爽你想要多大的角加速度控制器就算出对应力矩中间的非线性全部被对消掉被控对象看起来像几个积分器串联。但动态反演的软肋也很明显它对模型的信任有点过头。模型参数与真实参数不一致控制器里算出来的耦合项和实际不匹配外界还有风场扰动完全没有模型任何一项都会让非线性对消得不彻底。这一部分残留误差会直接作用在系统上这也是单纯用动态反演做无人机控制器总会出现“仿真很好实际一飞就抖”的原因。所以要引入ESO这类在线观测手段来收拾残局。1.2 ESO把一个额外状态装进观测器专吞扰动ESO全称Extended State Observer扩展状态观测器。它比普通状态观测器多了一维状态专门用来估计系统的“总扰动”。这里“总扰动”是个打包概念外部风扰力矩、未建模动态、参数偏差、甚至传感器偏置全都可以塞进去。为什么这种打包有意义因为从控制器视角看它能知道的信息只有两种我给了多少输入输出发生了什么变化。任何不在“我预期模型”里的响应偏差都可以视为总扰动的贡献。ESO利用控制输入和测量输出实时反推总扰动然后把估计值送到控制前馈里抵消掉。这是典型的“边测边补”思路有点像我开车时感觉方向盘偏了一点就下意识修正方向——不是事先找到所有原因而是直接补偿那个让我跑偏的总效果。在这个项目里ESO放在内环角速度回路中。用它的原因很直接动态反演残留误差、外部阵风力矩、参数偏差最后都会以“角速度响应偏离预期”这种形式体现出来。ESO守在这个位置刚好能把这一堆说不清道不明的东西一网打尽。1.3 自适应在这里不是摆设很多人觉得有了动态反演加ESO自适应就是锦上添花。实际复现之后我的看法变了自适应解决的是一个很具体且容易被忽略的问题——ESO前馈通道的控制增益失配。以四旋翼模型为例滚转角速度通道的理论模型是 Ix·ṗ τx ...控制器里用到的是标称惯量 Ix0。如果真实惯量和标称值偏差较大观测器里用的 b0 1/Ix0 与实际通道增益对不上ESO估计出的扰动就会带着系统性偏差补偿效果自然打折。自适应律在线修正这个增益把修正后的数值同时回传给控制器和ESO整条回路才算真正闭合。理论上当然可以做更复杂的在线惯量辨识但工程上复杂度高不少收益未必明显。我的建议是先把“ESO补偿加通道增益自适应”这个组合在仿真里跑稳再考虑要不要上更重的辨识算法。2. 核心原理推导从姿态方程到控制律2.1 无人机姿态模型刚体动力学与欧拉角运动学这一步最值得花时间很多复现失败都是因为模型符号写反了。我采用刚体假设忽略旋翼摆振和机体弹性形变四旋翼姿态动力学在机体坐标系下写成[ J\dot{\omega} \omega \times (J\omega) \tau d_{ext} ]其中 ω [p, q, r]^T 是机体角速度J diag(Ix, Iy, Iz) 是转动惯量矩阵τ 是控制力矩d_ext 是外部扰动力矩。这个式子本身不复杂角速度叉乘自己产生的陀螺力矩项是姿态动力学中最主要的非线性来源之一。姿态运动学用欧拉角表示滚转 φ、俯仰 θ、偏航 ψ[ \begin{bmatrix} \dot{\phi} \ \dot{\theta} \ \dot{\psi} \end{bmatrix} \begin{bmatrix} 1 \sin\phi\tan\theta \cos\phi\tan\theta \ 0 \cos\phi -\sin\phi \ 0 \sin\phi/\cos\theta \cos\phi/\cos\theta \end{bmatrix} \begin{bmatrix} p \ q \ r \end{bmatrix} ]这个矩阵在俯仰角接近 ±90°时会奇异所以仿真机动范围要控制住否则数值会直接爆炸。我在Simulink里加了一个角度限幅模块把俯仰角限制在 ±60°以内算是一个保险措施。还有一个容易踩的坑欧拉角速率是惯性系下的量而角速度和力矩都是机体系下的量中间要通过这个矩阵变换。新手搭模型经常在不经意间把矩阵转置写反。验证方法其实很简单给一个纯滚转速率 p理论上应该只产生 φ̇如果输出里出现明显的 θ̇ 或 ψ̇那矩阵就写错了。2.2 动态反演控制律的设计流程控制律按内外环拆分这是无人机姿态控制的事实标准。外环把姿态误差变成期望角速度内环把角速度误差变成力矩。外环控制器先取最简单的P结构[ \omega_{des} K_{att} \cdot e_{att} ]其中 e_att 是姿态角误差向量K_att 是对角增益矩阵通常取 2.0 到 3.01/s量级。如果要做轨迹跟踪可以把期望角速度前馈加进来阶跃指令场景用纯P已经够了。内环是动态反演的主场。先把动力学方程改写成便于对消的形式[ \dot{\omega} -J^{-1}(\omega \times J\omega) J^{-1}\tau d_{total} ]d_total 是等效到角加速度域的总扰动。设内环期望角加速度为[ \dot{\omega}{des} K_v (\omega{des} - \omega) ]动态反演直接解出控制力矩[ \tau J(\dot{\omega}{des} - \hat{d}{total}) \omega \times (J\omega) ]第一项让闭环呈现出近似一阶积分器特性第二项把已知非线性项对消第三项用ESO估计出的扰动做前馈补偿。这个式子就是整个控制器的核心后续所有模块都围绕它展开。K_v 的选择直接决定角速度环的响应速度我习惯按期望闭环带宽 ω_rate 来整定K_v ω_rate。工程上内环比外环快 3 到 5 倍比较合理姿态缓慢跟踪时才不容易振荡。2.3 线性ESO的参数配置带宽设定与观测方程把内环某个角速度通道独立出来以滚转角速度 p 通道为例[ \dot{p} f_p(\omega) b_0 \tau_x d_p ]f_p 是已知模型项在这个通道里是 (-\frac{I_z - I_y}{I_x}qr)b0 1/Ix 是标称通道增益d_p 是集总扰动。线性ESO构造如下[ \begin{aligned} \dot{z}_1 z_2 f_p b_0 \tau_x - \beta_1(z_1 - p) \ \dot{z}_2 -\beta_2(z_1 - p) \end{aligned} ]z1 跟踪角速度 pz2 跟踪总扰动 d_p。如果取观测器带宽为 ωo增益配置为[ \beta_1 2\omega_o, \quad \beta_2 \omega_o^2 ]这样观测误差的动态特征方程是 s² 2ωo s ωo² 0两个极点都配置在 ωo 处。滚转、俯仰、偏航三个角速度通道各配一个相同结构的ESO即可。带宽怎么取太低了扰动估计慢补偿滞后明显太高了测量噪声会被放大。我实测下来的经验值在仿真步长 1ms、内环带宽 6 rad/s 的前提下ωo 取 30 rad/s 就很稳大约 4 到 6 倍内环带宽。如果传感器噪声明显可以先给角速度测量加一阶低通滤波再把 ωo 降到 20 rad/s 附近。2.4 自适应通道增益辨识如何不破坏ESO的估计前面说ESO里的 b0 要尽量准那怎么在线修这里用最直观的梯度自适应律。还是拿滚转通道举例定义角速度跟踪误差 e p_des - p。把待估计的通道增益写成标称值的倍数[ b k \cdot b_0 ]其中 k 是无量纲倍数初始设为 1 表示完全信任标称值。自适应律更新这个倍数工程上做成归一化的形式[ \dot{\hat{k}} \gamma \cdot e \cdot \frac{\tau_x}{\tau_{nom}} \cdot \text{Proj} ]γ 是自适应增益τ_nom 是控制力矩的参考标尺用于把数值拉到同一量级。这个律的直观含义是如果角速度误差和控制输入方向持续同号说明增益估计偏了持续修正。为了防止漂移到不合理范围投影算子或直接限幅必不可少[ \hat{k} \text{sat}_{[0.5,\ 2.0]}(\hat{k}) ]允许通道增益在标称值一半到两倍之间浮动。为什么用限幅自适应律在持续激励不足时很容易发散纯仿真里尤其常见——系统只在某一个小角度范围机动k̂ 会因为数值原因越跑越远。加了限幅即使激励不足它也只会缓慢走到边界而不是爆炸。这里必须强调一下分工ESO负责估计“当前时刻的总扰动”自适应负责修正“通道增益模型”。两者叠加动态反演对消的那一步才能始终保持在比较可信的状态。3. Simulink与m脚本实战从零搭建可复现工程3.1 项目文件结构谁负责什么拿到项目文件后别急着双击打开 .slx 模型先把脚本和模型分类理清楚。我复现后的常用工程结构是params_init.m定义所有参数控制器增益ESO带宽仿真时长状态初值quad_attitude_model.slxSimulink模型包含被控对象、控制器、ESO、自适应模块run_sim.m调用sim()函数跑仿真plot_results.m绘制姿态响应、角速度、控制力矩、扰动估计曲线m脚本和Simulink的分工很明确脚本管参数和数据处理模型管信号流。这样以后改参数、跑批量仿真、出对比图都会顺很多。我会额外留一个compare_sweep.m跑不同ESO带宽的批量仿真这对理解参数影响特别有帮助。3.2 被控对象子系统六自由度姿态模型搭建被控对象是前面推导的六状态刚体姿态模型。在Simulink里既可以用积分器、增益、乘法器手搭也可以直接在MATLAB Function里写微分方程右端函数。我强烈推荐后者调试效率和数值一致性都更好。核心代码就几行function dstate plantModel(state, tau, d_ext) phi state(1); theta state(2); psi state(3); p state(4); q state(5); r state(6); Jx 0.0050; Jy 0.0050; Jz 0.0090; J diag([Jx, Jy, Jz]); omega [p; q; r]; omega_dot -J \ cross(omega, J*omega) J \ (tau d_ext); 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)/cos(theta) r*cos(phi)/cos(theta); dstate [phi_dot; theta_dot; psi_dot; omega_dot]; end注意这里的参数值只是便于演示正式工程建议通过MATLAB Function的附加参数从工作区传入。d_ext 是外部扰动力矩向量仿真里我习惯在第5秒用一个阶跃信号注入。要验证ESO的性能这个干扰一定要加不然全程无扰你根本看不出扰动补偿的贡献在哪里。3.3 控制器子系统内外环怎么衔接控制器子系统接收三路信号期望姿态角、实际姿态角、实际角速度输出控制力矩向量。外环先算期望角速度omega_des K_att * (ref_att - att);滚转角误差和俯仰角误差直接做差偏航角误差要 wrap 到 [-pi, pi]防止 10 度和 350 度之间出现“假大误差”。这个细节特别坑偏航大角度机动时最容易暴露。内环把期望角速度与实际角速度做差乘上角速度环增益omega_dot_des K_rate * (omega_des - omega);建议给 omega_dot_des 加限幅比如限制在 ±10 rad/s²否则大阶跃指令会瞬间产生过大的力矩尖峰数值上容易触发欧拉角奇异甚至直接导致仿真发散。最后是核心控制律加ESO扰动补偿和动态反演项tau J * (omega_dot_des - d_hat) cross(omega, J * omega);用信号线搭建的话这几个模块对应的是增益 K_att、增益 K_rate、矩阵乘法和cross模块、以及来自ESO的 d_hat 信号。我的搭建习惯是从外到内逐级推进先只接被控对象和动态反演跑通以后再接ESO最后接自适应。每加一级就跑一次仿真千万不要一下子画完整张图再排错。3.4 ESO与自适应模块MATLAB Function实现细节ESO我建议写成离散形式放MATLAB Function模块里而不是用连续积分器搭。离散形式更好控制时序也能避免Simulink里常见的代数环问题。一个滚转通道的ESO函数如下function [z1, z2] esoRoll(z1_prev, z2_prev, p_meas, tau_x, q_meas, r_meas, dt, omega_o, Ix, Iy, Iz) beta1 2 * omega_o; beta2 omega_o^2; f_known -(Iz - Iy)/Ix * q_meas * r_meas; b0 1 / Ix; e z1_prev - p_meas; z1 z1_prev dt * (z2_prev f_known b0*tau_x - beta1*e); z2 z2_prev dt * (-beta2*e); end三个通道结构完全一样区别只在 f_known 的具体形式。由于角速度三通道天然耦合f_known 要用到实际角速度 q 和 r这在Simulink里会形成一个反馈关系但只进观测器不会形成纯代数环。自适应模块用一个持续变量记录增益倍数 k_hat% 简化归一化自适应律tau_nom 是参考力矩标尺比如 0.05 N·m function k_hat_next adaptGain(e_w, tau_x, k_hat_prev, gamma, tau_nom, dt) update gamma * e_w * (tau_x / tau_nom) * dt; rough k_hat_prev update; k_hat_next min(max(rough, 0.5), 2.0); ende_w 的单位是 rad/stau_x 是力矩tau_nom 是参考标尺。要做归一化的原因很简单如果不先统一量纲gamma 很难选出一个对整个仿真都适用的值。我通常会先用 gamma 0.1 试跑看 k_hat 收敛速度太慢就加大出现抖动就减小。3.5 m脚本参数初始化、仿真调用与后处理绘图参数初始化脚本把所有变量写到工作区方便日后批量跑。核心参数见下表参数符号数值备注滚转/俯仰惯量Ix, Iy0.005 kg·m²小型四旋翼量级偏航惯量Iz0.009 kg·m²比滚转俯仰偏大姿态外环增益K_att2.5姿态角到角速度角速度内环增益K_rate6.0角速度环带宽ESO带宽omega_o30内环带宽的5倍自适应增益gamma0.1归一化后试跑调整仿真步长Ts1e-3 s固定步长仿真时长T_end30 s便于观察扰动后恢复扰动注入时间t_dist5 s阶跃扰动力矩初始化之后run_sim.m 里一行调用就够了out sim(quad_attitude_model.slx); logsout out.logsout;我会在Simulink里开启信号记录把姿态角、角速度、控制力矩、ESO的 z2 全部记录到 logsout然后统一在 plot_results.m 里处理。验证图一般包含四张子图姿态角阶跃响应、角速度响应、控制力矩曲线、ESO扰动估计与真实扰动的对比。这套后处理脚本做成模板之后换参数、出图、写报告都特别快。4. 调试实录复现过程中最容易踩的坑4.1 Simulink代数环与步长引起的发散这套控制环路里最典型的代数环路径是控制器输出 τ → 被控对象 → 角速度测量 → ESO → d_hat → 控制器。如果ESO的离散时序不对或者没有用上一时刻的状态做更新Simulink会直接报代数环错误或者给出一个看起来能跑但姿态纹丝不动的结果。解决办法很明确ESO模块里使用 z1_prev、z2_prev 这样的离散状态更新而不是在当前时刻解算跨越多个模块的隐式方程。固定步长建议 1ms。ESO带宽 30 rad/s 时一个振荡周期大约 0.21 秒1ms 的采样间隔已经留出了足够的余量。4.2 ESO初始值、噪声放大与带宽取舍我对ESO初值的印象特别深。如果 z1(0)0、z2(0)0但实际角速度初值是 0.5 rad/s仿真前 0.5 秒会有一段明显的观测暂态。控制器会把这段观测误差当成扰动去补偿产生一个不真实的力矩尖峰。处理办法很简单初始化脚本里把 z1 的初值设成与实际角速度一致z2 设成 0。噪声放大是另一个高频问题。ESO本质上是带微分作用的观测器带宽一旦高于系统等效采样率的五分之一测量噪声会直接漏进扰动估计然后被当成“真扰动”补偿掉。我在模型里给角速度测量加了一个截止频率 50 rad/s 的一阶滤波器再接进ESO。代价是滤波器会引入相位滞后所以滤波截止频率不能取得比ESO带宽低太多否则补偿相位就乱了。4.3 自适应律发散投影算子与死区处理自适应律在仿真里最容易出问题的两种情形持续激励不足时 k_hat 慢慢漂移以及误差在零点附近抖动时 k_hat 来回震荡。除了限幅死区也是一个有效手段if abs(e_w) 0.01 update 0; else update gamma * e_w * (tau_x / tau_nom) * dt; end这个死区范围 0.01 rad/s能让仿真稳定性显著提升。实际飞控里这个死区必不可少因为传感器噪声会让角速度误差永远非零不加死区增益倍数会一直乱跳。这里再提醒一点限幅范围 [0.5, 2.0] 不是拍脑袋选的。如果真实惯量与标称值偏差超过这个范围说明模型标称值本身就不合格应该回头重新校准模型而不是指望自适应把所有问题都扛下来。4.4 如何验证复现成功几条可靠的判断曲线复现控制器最怕跑出和预期不符的曲线却说不清楚是参数有出入还是实现有bug。我整理了几条判断标准姿态阶跃响应在 2 秒内收敛稳态误差小于 0.5°超调量小于 10%角速度偏差在机动结束后平滑归零没有高频抖振控制力矩在扰动注入前后有明显对应变化恢复时间在 2 秒以内ESO 的 z2 输出在无扰阶段接近 0有扰阶段能跟踪真实扰动量级曲线平滑k_hat 收敛到真实通道增益附近而不是一直压在限幅边界上如果前两条满足但第四条完全对不上说明ESO很可能只观测到了测量噪声要么信号接线错了要么带宽设置太高。如果 k_hat 一直压在边界说明真实惯量离标称值太远或者自适应增益太小需要回到 m 脚本重新整定参数。4.5 一点个人体会这次复现给我最大的感受是这类听起来很高级的组合控制器本质上是在解决一个很朴素的矛盾——模型永远不精确但控制律又必须依赖模型作为基准。动态反演把模型里可信的部分用到极致ESO把模型里不可信的部分在线估计出来自适应负责让模型部分不至于越漂越偏。三者配合才能在“依赖模型”和“容忍不确定性”之间取得一个工程上能用的平衡。如果你准备从头搭一遍我给的建议是不要把所有模块一次性建完再调。先只搭被控对象和动态反演用真实惯量跑通然后故意把惯量参数设偏 20%观察性能恶化再接入ESO和自适应对比偏前、偏中、偏后三条曲线。这个逐级添加的过程比任何公式推导都能帮你建立对这套控制器直观的理解。实际飞控调试本质上也是这样一个逐级叠加补偿的过程。