1. 四旋翼无人机姿态控制基础
四旋翼无人机的姿态控制本质上是对三个欧拉角（滚转、俯仰、偏航）的精确调控。就像杂技演员走钢丝时需要不断调整身体平衡一样，无人机在空中飞行时也需要实时调整姿态来保持稳定。我们先从最基础的物理模型开始拆解。
1.1 欧拉角与旋转动力学
当无人机在空中飞行时，它的姿态可以用三个角度完整描述：
- 滚转角（φ）：左右倾斜的角度，控制无人机左右移动
- 俯仰角（θ）：前后倾斜的角度，控制无人机前后移动
- 偏航角（ψ）：绕垂直轴旋转的角度，控制无人机转向
这三个角度的变化率与机体角速度（p,q,r）之间的关系可以用以下矩阵表示：
matlab复制E = [1, sin(phi)*tan(theta), cos(phi)*tan(theta);
0, cos(phi), -sin(phi);
0, sin(phi)/cos(theta), cos(phi)/cos(theta)];
omega_body = E \ [p; q; r]; % 机体角速度转欧拉角速率
这个转换矩阵有个有趣的特点：当俯仰角θ接近±90°时，tan(θ)会趋向无穷大，这就是著名的"万向节锁"问题。在实际飞行控制中，我们通常假设无人机不会出现如此大的姿态角变化。
1.2 力矩方程与转动惯量
无人机的姿态变化遵循刚体旋转动力学方程：
code复制M = I * α + ω × (I * ω)
其中：
- M是作用在机体上的总力矩
- I是转动惯量矩阵
- α是角加速度
- ω是角速度
- ×表示叉积运算
对于对称设计的四旋翼，转动惯量矩阵通常可以简化为对角矩阵：
matlab复制I = diag([Ixx, Iyy, Izz]); % 滚转、俯仰、偏航轴的转动惯量
在实际工程中，准确测量或估算这些转动惯量参数至关重要。我曾经遇到过一个案例：团队将转动惯量单位误设为kg·m²（实际应为kg·m²/s²），导致控制器输出完全失常，无人机像喝醉酒一样乱转。
2. ADRC控制原理深度解析
自抗扰控制器(ADRC)之所以在无人机控制中表现出色，关键在于它能够实时估计并补偿系统内外的各种扰动。这就像一个有经验的司机，不仅能根据路况调整方向盘，还能预判可能出现的侧风影响。
2.1 ADRC的三重奏
一个完整的ADRC控制器由三部分组成：
- 跟踪微分器(TD)：安排过渡过程
- 扩张状态观测器(ESO)：估计系统状态和总扰动
- 非线性状态误差反馈(NLSEF)：生成控制量
2.1.1 跟踪微分器实现
matlab复制function [x1, x2] = TD(v, h, T)
persistent v1 v2
if isempty(v1)
v1 = 0; v2 = 0;
end
fh = fhan(v1 - v, v2, 100, h);
v1 = v1 + T * v2;
v2 = v2 + T * fh;
x1 = v1;
x2 = v2;
end
function y = fhan(x1, x2, r, h)
d = r*h^2;
a0 = h*x2;
y = x1 + a0;
a1 = sqrt(d*(d + 8*abs(y)));
a2 = a0 + sign(y)*(a1 - d)/2;
sy = (sign(y + d) - sign(y - d))/2;
y = -r*(a2/d - sign(y))*sy - r*sign(y);
end
TD中的非线性函数fhan是其精髓所在，相比线性微分器，它能提供更快的跟踪速度而无超调。参数r决定了跟踪速度，相当于"油门"大小；h则是滤波因子，可以抑制测量噪声。
调试经验：当发现指令跟踪有超调时，不要立即减小r值，先检查TD的输出是否已经出现振荡。如果TD输出平滑但有超调，才是真正需要调整r的时候。
2.1.2 扩张状态观测器设计
ESO是ADRC的核心，它能将系统内部动态和外部扰动统一估计并补偿：
matlab复制function z = ESO(y, u, beta, delta, T)
persistent z1 z2 z3
if isempty(z1)
z1 = 0; z2 = 0; z3 = 0;
end
e = z1 - y;
fe = fal(e, 0.5, delta);
z1 = z1 + T*(z2 - beta(1)*e);
z2 = z2 + T*(z3 - beta(2)*fe + 0.8*u); % 0.8是控制增益估计值
z3 = z3 + T*(-beta(3)*fe);
z = [z1; z2; z3];
end
function y = fal(e, alpha, delta)
if abs(e) > delta
y = abs(e)^alpha * sign(e);
else
y = e / (delta^(1 - alpha));
end
end
beta参数的选择遵循"带宽"概念，通常取beta=[ω₀, 3ω₀², 5ω₀³]，其中ω₀是观测器带宽。在实际调试中，我习惯先用这个公式计算初始值，然后通过以下步骤微调：
- 先设置β₁，使状态估计能跟上真实值
- 再调整β₂，改善微分信号的平滑度
- 最后调节β₃，优化扰动估计的响应速度
避坑指南：如果z3输出出现高频抖动，说明观测器带宽过高，超过了系统实际动态范围的3-5倍，或者接近了采样频率的1/3（奈奎斯特极限）。
3. 姿态控制器的MATLAB实现
现在我们将各个模块组合起来，实现完整的姿态控制系统。这个过程就像组装一台精密钟表，每个齿轮都必须严丝合缝。
3.1 控制回路结构
每个姿态通道（滚转、俯仰、偏航）都有独立的ADRC控制器，结构如下：
matlab复制for k = 1:N
% 滚转通道
[phi_ref, ~] = TD(phi_cmd(k), 0.01, Ts);
z_phi = ESO(phi_actual, u_phi, [80, 200, 500], 0.01, Ts);
e1 = phi_ref - z_phi(1);
u0_phi = 150*fal(e1, 0.75, 0.05) - 10*z_phi(2);
u_phi = u0_phi - z_phi(3)/0.8;
% 俯仰通道
[theta_ref, ~] = TD(theta_cmd(k), 0.01, Ts);
z_theta = ESO(theta_actual, u_theta, [80, 200, 500], 0.01, Ts);
e1 = theta_ref - z_theta(1);
u0_theta = 150*fal(e1, 0.75, 0.05) - 10*z_theta(2);
u_theta = u0_theta - z_theta(3)/0.8;
% 偏航通道
[psi_ref, ~] = TD(psi_cmd(k), 0.01, Ts);
z_psi = ESO(psi_actual, u_psi, [50, 150, 300], 0.01, Ts);
e1 = psi_ref - z_psi(1);
u0_psi = 100*fal(e1, 0.75, 0.05) - 8*z_psi(2);
u_psi = u0_psi - z_psi(3)/0.6;
end
3.2 参数调试方法论
ADRC参数调试是一门艺术，也是科学。根据我的工程实践，总结出以下调试步骤：
-
初始化TD参数：
- r值设为期望上升时间的倒数（如希望0.3秒达到指令值，则r≈3）
- h取采样周期的1-2倍
-
ESO参数整定：
- 初始带宽ω₀设为系统自然频率的3-5倍
- 按照β=[ω₀, 3ω₀², 5ω₀³]计算初始值
- 观察z1跟踪性能，微调β₁
- 观察z2平滑度，调整β₂
- 最后优化z3的扰动估计，调节β₃
-
非线性反馈调节：
- 先设置较大的增益保证基本响应
- 然后逐步降低增益至临界振荡点
- 最后取临界值的60-70%作为稳定增益
实测技巧：调试时可以故意加入20%的阶跃扰动，观察ESO的估计速度和补偿效果。良好的ADRC控制器应该在0.1秒内准确估计并补偿这种扰动。
4. 仿真分析与性能优化
完成控制器实现后，我们需要通过仿真验证其性能。这就像飞机首飞前的风洞试验，能提前发现潜在问题。
4.1 典型响应指标
一个调校良好的ADRC姿态控制器应该具备以下特性：
- 上升时间：0.2-0.5秒（取决于无人机尺寸）
- 超调量：<5%
- 稳态误差：<1°
- 抗扰动恢复时间：<0.2秒
4.2 耦合效应处理
虽然我们为三个通道设计了独立控制器，但实际上它们之间存在耦合效应。例如滚转运动会产生偏航力矩（反之亦然），这种现象在航空领域称为"惯性耦合"。
在MATLAB仿真中，可以通过以下方式模拟这种耦合：
matlab复制% 在动力学模型中添加耦合项
phi_ddot = (Mx + 0.1*psi_dot*theta_dot*(Izz-Iyy) - Kp*phi_dot)/Ixx;
theta_ddot = (My + 0.1*phi_dot*psi_dot*(Ixx-Izz) - Kp*theta_dot)/Iyy;
psi_ddot = (Mz + 0.1*phi_dot*theta_dot*(Iyy-Ixx) - Kp*psi_dot)/Izz;
ADRC的优势在于，ESO能够将这些耦合效应视为"总扰动"的一部分进行估计和补偿。不过在实际调试中，我发现如果耦合效应特别强（如大型无人机），还是需要在控制器设计时显式考虑解耦策略。
4.3 实时性优化技巧
当在嵌入式平台上实现时，ADRC的计算效率至关重要。以下是我总结的几点优化经验：
- 函数近似：将fal函数用分段线性或查表法实现
- 定点运算：将浮点运算转换为定点运算，特别适合没有FPU的微控制器
- 采样率匹配：TD和ESO的更新率可以低于主控制回路（如主回路1kHz，观测器500Hz）
- 参数量化：将所有参数缩放为整数范围，减少存储空间
matlab复制% 定点运算示例（Q15格式）
function y = fal_fixed(e, alpha, delta)
e_int = int16(e * 32768);
delta_int = int16(delta * 32768);
if abs(e_int) > delta_int
y = (e_int^alpha) * sign(e_int) / (32768^(alpha-1));
else
y = e_int / (delta_int^(1 - alpha)) * (32768^(1 - alpha));
end
y = double(y) / 32768;
end
5. 工程实践中的挑战与解决方案
在实际项目中应用ADRC控制器时，会遇到许多教科书上没讲过的问题。这部分分享我的实战经验，希望能帮你少走弯路。
5.1 传感器噪声处理
ADRC虽然对噪声有一定鲁棒性，但过大的噪声仍会影响ESO的估计精度。我常用的处理方法是：
- 硬件滤波：在传感器信号进入ADC前，添加一阶RC低通滤波（截止频率设为信号带宽的2-3倍）
- 软件滤波：在ESO前加入移动平均滤波（窗口大小3-5个采样点）
- 参数调整：适当增大TD的h值和ESO的δ值，增强鲁棒性
特别注意：滤波会引入相位滞后，因此滤波器的截止频率不能太低，否则会影响控制性能。我通常先用MATLAB仿真确定最大可接受的滞后时间。
5.2 执行器饱和应对
无人机电机和电调都有物理限幅，当控制器输出超过这些限幅时，会出现饱和现象。这会导致ESO的估计出现偏差，进而引发积分饱和问题。
解决方案：
- 抗饱和补偿：在ESO中加入饱和补偿项
matlab复制u_sat = min(max(u, -umax), umax); % 实际执行器输出 delta_u = u - u_sat; % 饱和差值 z3 = z3 + T*(-beta(3)*fe + 0.5*delta_u); % 补偿项 - 指令限幅：对参考指令进行速率和幅值限制
- 增益调度：在大误差区域自动降低控制器增益
5.3 参数自适应策略
固定参数的ADRC在不同飞行状态下可能表现不佳。我开发了几种自适应策略：
-
基于误差的自适应：
matlab复制% 根据跟踪误差动态调整ESO带宽 current_error = abs(y - z1); if current_error > 5 % 度 beta = [150, 450, 1500]; % 高带宽 else beta = [80, 200, 500]; % 正常带宽 end -
飞行状态识别：
- 通过IMU数据识别当前是悬停、加速还是机动状态
- 为每种状态预置不同的参数组
- 实现平滑切换
-
在线学习：
使用递归最小二乘法(RLS)在线更新模型参数，并相应调整ADRC参数
这些策略在复杂环境下的无人机（如强风条件、载重变化）中特别有效。我曾经参与的一个农业无人机项目，通过引入自适应ADRC，在喷洒农药（质量逐渐减少）过程中保持了稳定的飞行性能。