从建模到实战：基于串级ADRC的四旋翼无人机轨迹跟踪控制

1. 四旋翼无人机建模基础四旋翼无人机作为典型的欠驱动系统其动力学特性一直是控制领域的研究热点。我第一次接触四旋翼建模时发现很多论文都直接从欧拉方程开始推导这对新手来说确实不太友好。其实理解四旋翼的运动原理可以从更直观的物理现象入手。想象你手里拿着一个十字形的竹蜻蜓当四个旋翼转速相同时会产生向上的升力当对角旋翼转速出现差异时就会产生滚转或俯仰力矩前后旋翼与左右旋翼的转速差则控制偏航运动。这种直观理解可以帮助我们快速把握建模的关键要素。在实际建模中我们通常需要建立两个坐标系机体坐标系固定在无人机上和地面惯性坐标系。通过坐标变换可以得到六自由度的运动方程。这里我习惯用牛顿-欧拉法建立动力学模型主要包括平移运动方程描述无人机位置变化旋转运动方程描述姿态角变化电机动力学将PWM信号转换为实际推力% 简化的四旋翼动力学模型示例 function dx quadcopter_dynamics(t, x, u) % 状态变量: [x y z vx vy vz phi theta psi p q r] % 控制输入: [U1 U2 U3 U4] % 物理参数 m 1.2; % 质量(kg) g 9.81; % 重力加速度 l 0.25; % 轴距(m) I diag([0.023, 0.023, 0.046]); % 惯性矩阵 % 解算状态变量 phi x(7); theta x(8); psi x(9); p x(10); q x(11); r x(12); % 旋转矩阵 R [cos(theta)*cos(psi) sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi) cos(phi)*sin(theta)*cos(psi)sin(phi)*sin(psi); cos(theta)*sin(psi) sin(phi)*sin(theta)*sin(psi)cos(phi)*cos(psi) cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi); -sin(theta) sin(phi)*cos(theta) cos(phi)*cos(theta)]; % 平移动力学 dx(1:3) x(4:6); dx(4:6) [0;0;-g] R*[0;0;u(1)]/m; % 旋转动力学 omega [p;q;r]; tau [l*(u(2)-u(4)); l*(u(3)-u(1)); (u(1)-u(2)u(3)-u(4))]; dx(10:12) I\(tau - cross(omega, I*omega)); % 欧拉角微分 dx(7:9) [1 sin(phi)*tan(theta) cos(phi)*tan(theta); 0 cos(phi) -sin(phi); 0 sin(phi)/cos(theta) cos(phi)/cos(theta)] * omega; end这个模型虽然简化了一些因素如空气阻力、电机动态等但已经包含了四旋翼的核心动力学特性。在实际项目中我通常会先用这个基础模型验证控制算法然后再逐步添加更复杂的因素。2. ADRC控制原理详解自抗扰控制(ADRC)最吸引我的地方在于它处理不确定性的能力。记得第一次在实验室测试时传统PID控制器遇到突风扰动就会剧烈震荡而ADRC却能稳稳保持姿态。这种抗干扰的特性正是通过扩张状态观测器(ESO)实现的。2.1 ADRC的核心组件ADRC控制器主要由三部分组成跟踪微分器(TD)解决快速性与超调的矛盾。我把它比作一个聪明的导航员不仅能告诉你现在的位置还能预测未来的运动趋势。扩张状态观测器(ESO)这是ADRC的灵魂所在。它把系统内部动态和外部扰动都当作总扰动来估计就像给无人机装了个第六感。在实际调试中我发现ESO的带宽选择特别关键——太高会放大噪声太低则响应迟缓。非线性状态误差反馈(NLSEF)不同于PID的线性组合这里采用非线性函数处理误差。我常用的fal函数组合可以根据误差大小自动调整增益小误差时高增益保证精度大误差时低增益避免饱和。2.2 ESO的工程实现技巧在四旋翼项目中ESO的实现有几个实用技巧带宽参数化将观测器参数表示为带宽的函数简化调试过程。对于二阶系统beta01 3*omega_o; beta02 3*omega_o^2; beta03 omega_o^3;其中omega_o是观测器带宽根据系统动态选择通常取控制系统带宽的3~5倍。抗噪声处理实际系统中ESO容易放大测量噪声。我的经验是在ESO前加一阶低通滤波适当降低观测器带宽使用离散化ESO时采用双线性变换法% 离散化ESO实现示例 function [z1_k1, z2_k1, z3_k1] discrete_ESO(z1, z2, z3, u, y, h, beta) e z1 - y; z1_k1 z1 h*(z2 - beta(1)*e); z2_k1 z2 h*(z3 - beta(2)*e b0*u); z3_k1 z3 h*(-beta(3)*e); end这个实现采用了前向欧拉离散化在实际飞行控制中我更喜欢用零阶保持法稳定性更好。3. 串级ADRC控制器设计串级控制就像给无人机装上了双重保险——外环负责大方向内环确保执行精准。我在多个四旋翼项目中发现这种结构特别适合处理无人机这种快慢动态耦合的系统。3.1 内外环分工协作外环位置控制控制周期50-100Hz主要应对轨迹跟踪、抗风扰输出姿态角指令内环姿态控制控制周期200-500Hz主要应对电机动态、陀螺效应输出电机PWM信号这种分工带来了明显优势当无人机遇到侧风时外环ADRC通过ESO估计出风扰调整位置指令内环ADRC则快速响应无需知道扰动具体来源。我在实测中发现这种结构比单级ADRC的跟踪误差降低了40%以上。3.2 参数整定实战经验调参是ADRC应用中最耗时的环节。经过多次试错我总结出一套三步法先内后外先调好内环姿态控制再调外环位置控制。内环带宽通常设为外环的5倍左右。先线性后非线性先用线性ESO和线性反馈确定基础参数再引入非线性函数优化性能。先仿真后实机在Gazebo或MATLAB中完成初步验证再用小步长增量法进行实机调试。这里分享一个典型的位置环ADRC参数配置表参数物理意义X轴取值Y轴取值Z轴取值调整原则omega_c控制器带宽2.52.53.0响应速度需求omega_o观测器带宽1515203~5倍omega_cb0补偿因子0.80.81.2近似系统增益倒数delta线性区间宽度0.10.10.15测量噪声水平alpha1非线性因子0.50.50.6误差较大时的增益衰减率这些参数在8字轨迹跟踪中表现良好但实际应用中还需要根据具体机型调整。记得有一次给一架轴距更大的无人机调参b0值需要减小30%才能获得理想效果。4. 轨迹跟踪实战与结果分析在实验室里我最喜欢用8字轨迹来测试控制器的性能。这个轨迹包含了直线、圆弧、正反曲率变化能全面检验控制器的跟踪能力和抗扰性。4.1 仿真环境搭建我通常采用MATLAB/Simulink搭建仿真平台主要模块包括轨迹生成器ADRC控制器组无人机动力学模型扰动注入模块可视化工具一个实用的技巧是在仿真中加入电机动态特性% 电机模型简化实现 function omega motor_dynamics(u, omega_prev, Ts) % u: 输入PWM信号(0-1) % omega_prev: 上一时刻转速 % Ts: 采样时间 tau_m 0.02; % 电机时间常数 omega_max 1000; % 最大转速(rad/s) omega omega_prev Ts/tau_m*(omega_max*u - omega_prev); end这个二阶模型比常简单的一阶模型更能反映实际系统的动态特性。4.2 典型测试场景理想环境测试无风扰模型参数准确测试目的验证基本跟踪性能抗扰测试加入脉冲风扰模拟突风持续侧风5m/s测试目的验证ESO的扰动估计能力鲁棒性测试故意错配模型参数如质量20%电机效率下降30%测试目的验证参数适应性在最近一次测试中串级ADRC在15m/s的突风干扰下位置跟踪误差仍能保持在0.2m以内而传统PID控制已经出现了1.5m的偏差并伴随持续振荡。4.3 实测数据解读这是典型的X轴位置跟踪曲线对比时间(s) 期望位置(m) ADRC实际位置(m) PID实际位置(m) 0.0 0.00 0.00 0.00 1.0 1.50 1.48 1.35 2.0 3.00 2.97 2.60 3.0 3.00 3.01 3.15 4.0 1.50 1.52 1.70可以看到ADRC在转折点处的超调明显小于PID而且在稳态段几乎没有静态误差。更关键的是当我在3.5秒注入一个脉冲扰动时ADRC在0.3秒内就恢复了稳定跟踪而PID则出现了持续振荡。5. 工程实现中的坑与经验在实际部署ADRC控制器时我踩过不少坑这里分享几个典型案例5.1 离散化带来的问题第一次将连续域设计的ADRC移植到飞控时出现了奇怪的振荡现象。后来发现是离散化方法不当导致的错误做法直接替换微分方程为差分方程正确做法采用双线性变换或零阶保持法改进后的离散ESO实现function [z1_new, z2_new, z3_new] bilinear_ESO(z1, z2, z3, u, y, h, beta) % 中间变量 e z1 - y; dz1 z2 - beta(1)*e; dz2 z3 - beta(2)*e b0*u; dz3 -beta(3)*e; % 双线性变换 z1_new z1 h*dz1; z2_new z2 h*dz2; z3_new z3 h*dz3; % 第二次校正 e_new z1_new - y; z1_new z1 h/2*(dz1 (z2_new - beta(1)*e_new)); z2_new z2 h/2*(dz2 (z3_new - beta(2)*e_new b0*u)); z3_new z3 h/2*(dz3 (-beta(3)*e_new)); end5.2 计算量优化在STM32F4飞控上运行时发现ADRC的计算耗时是PID的3倍。通过以下优化将计算量降低了60%将fal函数改为分段线性近似使用q格式定点数运算预先计算并存储常用参数组合优化后的fal函数实现// 优化后的定点数fal函数 int32_t fal_fixed(int32_t e, int32_t alpha, int32_t delta) { int32_t abs_e abs(e); if(abs_e delta) { return (e * alpha) 8; // Q24格式运算 } else { int32_t sign e 0 ? 1 : -1; return (sign * fp_pow(delta, alpha-1) * e) 16; } }5.3 实际飞行建议对于准备在实际无人机上部署ADRC的开发者我的建议是安全第一先在系留测试台上验证基本功能参数渐变飞行测试时用小步长逐步调整参数数据记录完整记录每次飞行的状态和控制量故障预案设置紧急停止开关和控制器切换逻辑记得有一次户外测试时因为没设置足够的控制器超时保护导致无人机在无线电干扰下失控。现在我的标准做法是设置100ms的看门狗定时器准备备用PID控制器实时监测ESO的扰动估计值