1. 四旋翼无人机控制基础
四旋翼无人机作为典型的欠驱动系统，其动力学特性复杂且具有强耦合性。我在2015年第一次接触四旋翼控制时，就被它独特的运动机理所吸引——仅通过四个电机的转速差就能实现六自由度的空间运动。这种特性使得控制算法设计既充满挑战又极具研究价值。
1.1 四旋翼动力学模型
四旋翼的运动控制本质上是对欧拉-拉格朗日方程的应用。通过建立机体坐标系和惯性坐标系之间的转换关系，我们可以得到如下非线性动力学方程：
matlab复制% 平移运动方程
m * ddot_x = (cosψ sinθ cosφ + sinψ sinφ) * U1
m * ddot_y = (sinψ sinθ cosφ - cosψ sinφ) * U1
m * ddot_z = (cosθ cosφ) * U1 - m*g
% 旋转运动方程
Ix * ddot_φ = θ_dot * ψ_dot*(Iy - Iz) + l*U2
Iy * ddot_θ = φ_dot * ψ_dot*(Iz - Ix) + l*U3
Iz * ddot_ψ = φ_dot * θ_dot*(Ix - Iy) + U4
其中U1-U4为控制输入，分别对应总升力和三个轴向力矩。这个模型揭示了四旋翼控制的核心难点：姿态角（φ,θ,ψ）与位置（x,y,z）之间存在非线性耦合关系。
1.2 PD控制器原理
比例-微分(PD)控制器因其结构简单、易于实现的特点，成为无人机控制的入门首选。其控制律可表示为：
code复制u(t) = Kp * e(t) + Kd * de(t)/dt
在四旋翼控制中，我们需要设计六个独立的PD控制器——三个用于姿态控制（滚转、俯仰、偏航），三个用于位置控制（x,y,z方向）。实际调试中发现，姿态环的响应速度应该比位置环快5-10倍，这样才能保证系统的稳定性。
2. Matlab实现方案设计
2.1 仿真环境搭建
我推荐使用Matlab的Simulink环境进行建模，其可视化编程界面特别适合控制系统的快速原型开发。核心模块包括：
- 四旋翼动力学模块：根据前述方程建立S函数
- 传感器仿真模块：添加高斯白噪声模拟IMU测量误差
- 控制器模块：实现PD控制算法
- 可视化模块：使用Aerospace Blockset的Animation功能
重要提示：在Simulink中建模时，务必使用固定步长求解器（如ode4），变步长求解器可能导致控制效果异常。
2.2 PD控制器实现代码
以下是姿态环PD控制的Matlab函数实现：
matlab复制function [U2, U3, U4] = attitude_PD_control(phi_des, theta_des, psi_des, ...
phi, theta, psi, ...
p, q, r, ...
Kp_phi, Kd_phi, ...
Kp_theta, Kd_theta, ...
Kp_psi, Kd_psi)
% 计算姿态误差
e_phi = phi_des - phi;
e_theta = theta_des - theta;
e_psi = psi_des - psi;
% 角速度误差（假设期望角速度为0）
e_p = 0 - p;
e_q = 0 - q;
e_r = 0 - r;
% PD控制律
U2 = Kp_phi * e_phi + Kd_phi * e_p;
U3 = Kp_theta * e_theta + Kd_theta * e_q;
U4 = Kp_psi * e_psi + Kd_psi * e_r;
end
位置控制器的实现类似，但需要注意将期望位置转换为姿态角指令时需要进行坐标变换：
matlab复制% 将位置误差转换为期望俯仰/滚转角度
theta_des = (x_des - x) * Kp_x + (x_dot_des - x_dot) * Kd_x;
phi_des = -(y_des - y) * Kp_y + (y_dot_des - y_dot) * Kd_y;
3. 参数整定与调试技巧
3.1 参数初始化方法
经过多次项目实践，我总结出PD参数初始值的经验公式：
code复制Kp ≈ 0.6 * J * ωn^2
Kd ≈ 0.6 * J * 2ζωn
其中：
- J为对应轴的转动惯量
- ωn为期望带宽（姿态环通常取10-20rad/s）
- ζ为阻尼比（通常取0.7-1.0）
例如对于重量1.5kg，x轴转动惯量0.03kg·m²的四旋翼，若取ωn=15rad/s，ζ=0.8，则：
matlab复制Kp_phi = 0.6 * 0.03 * (15)^2 ≈ 4.05
Kd_phi = 0.6 * 0.03 * 2*0.8*15 ≈ 0.432
3.2 调试实战经验
- 先内环后外环：先调姿态环直到响应快速无超调，再调位置环
- 激励测试法：给阶跃信号观察响应，调整参数使超调<5%
- 抗扰测试：在仿真中施加脉冲干扰，验证恢复性能
- 频率扫描法：通过扫频测试找出系统谐振点
调试过程中常见的现象与对策：
| 现象 | 可能原因 | 解决方案 |
|---|---|---|
| 持续振荡 | Kp过大或Kd过小 | 增大Kd，减小Kp |
| 响应迟缓 | Kp过小 | 逐步增大Kp |
| 稳态误差 | 缺乏积分项 | 考虑切换为PID或增加前馈 |
4. 性能优化与扩展
4.1 抗饱和处理
实际系统中电机推力有限，需要增加抗饱和措施：
matlab复制% 推力限幅
U1 = min(max(U1, 0), U1_max);
% 力矩分配限幅
[U2, U3, U4] = limit_moments(U2, U3, U4, U2_max, U3_max, U4_max);
4.2 模型不确定性补偿
针对模型参数不准确的问题，可以增加鲁棒项：
matlab复制U2 = U2_nominal + ρ * sign(s);
其中s为滑模面，ρ为鲁棒增益。
4.3 进阶控制策略
当PD控制无法满足性能要求时，可以考虑：
- 串级PID：增加速度环提高抗扰性
- LQR控制：基于状态空间的优化控制
- ADRC：自抗扰控制处理未知扰动
- MPC：模型预测控制处理约束
我在最近的一个农业植保无人机项目中，就采用了PD+前馈的复合控制策略，将喷洒作业时的轨迹跟踪误差控制在±5cm以内。
5. 完整仿真案例
以下是一个实现高度控制的完整Simulink模型要点：
- 输入模块：Step信号生成期望高度
- 控制器：PD控制算法子系统
- 无人机模型：六自由度非线性模型
- 观测模块：显示高度曲线和电机指令
关键参数设置示例：
matlab复制% 高度控制参数
Kp_z = 15;
Kd_z = 8;
% 姿态控制参数
Kp_theta = 4.5;
Kd_theta = 0.5;
仿真结果显示，系统能在2秒内稳定到目标高度，超调量约3%，满足大多数应用需求。实测中发现，增加一个速度前馈项可以显著改善对斜坡指令的跟踪性能。