1. 项目背景与核心价值
四旋翼飞行器的控制问题一直是无人机领域的研究热点。在众多控制方法中，PID控制因其结构简单、参数物理意义明确、易于工程实现等优势，成为工业界和学术界广泛采用的基础控制策略。这个项目要复现的论文提出了一种基于内外环结构的PID控制方法，相比传统单环PID控制，这种分层设计能够更好地处理四旋翼这个典型的多变量、强耦合、非线性系统。
我在实际无人机控制系统开发中发现，很多工程师虽然能调出可用的PID参数，但对参数背后的物理意义和调整逻辑理解不深。这篇论文的价值在于：
- 明确了姿态环（内环）与位置环（外环）的解耦设计思路
- 提供了参数整定的理论依据而非纯经验试错
- 通过实验验证了该方法在抗干扰性和动态响应上的优势
2. 系统建模与控制器结构
2.1 四旋翼动力学模型
四旋翼的六自由度运动可以分解为：
- 平移运动（位置控制）：
code复制m·ẍ = (cosφsinθcosψ + sinφsinψ)·U₁ m·ÿ = (cosφsinθsinψ - sinφcosψ)·U₁ m·z̈ = (cosφcosθ)·U₁ - mg - 旋转运动（姿态控制）：
code复制Ixx·φ̈ = θ̇ψ̇(Iyy - Izz) + l·U₂ Iyy·θ̈ = φ̇ψ̇(Izz - Ixx) + l·U₃ Izz·ψ̈ = φ̇θ̇(Ixx - Iyy) + U₄
其中U₁~U₄为控制输入，与电机转速的平方成正比。这个模型揭示了位置和姿态之间的耦合关系——我们需要先控制姿态（φ,θ,ψ）才能间接影响位置（x,y,z）。
2.2 内外环控制架构
论文采用的分层控制结构如下：
code复制[位置外环PID] → [姿态内环PID] → [电机混控] → [四旋翼动力系统]
↑ ↑
姿态反馈 电机PWM信号
外环输出是期望的姿态角（φ_d,θ_d），内环则直接控制电机产生所需的力矩。这种结构的关键优势在于：
- 将6自由度的控制问题分解为两个3自由度问题
- 内环的高频响应（200Hz以上）可以快速抑制扰动
- 外环的低频更新（50-100Hz）保证轨迹跟踪精度
3. PID控制器设计与实现
3.1 位置外环设计
位置环采用独立的PID控制器处理x、y、z三个方向：
c复制// 伪代码示例
void position_PID_update(float setpoint[3], float measurement[3], float dt) {
// 计算误差
float error[3] = {setpoint[0] - measurement[0],
setpoint[1] - measurement[1],
setpoint[2] - measurement[2]};
// P项
float proportional[3] = {Kp_x * error[0],
Kp_y * error[1],
Kp_z * error[2]};
// I项（带抗饱和）
integral[0] += Ki_x * error[0] * dt;
integral[1] += Ki_y * error[1] * dt;
integral[2] += Ki_z * error[2] * dt;
clamp_integral(); // 限制积分项
// D项（带滤波器）
float derivative[3] = {Kd_x * (error[0] - last_error[0]) / dt,
Kd_y * (error[1] - last_error[1]) / dt,
Kd_z * (error[2] - last_error[2]) / dt};
// 输出期望姿态角
phi_desired = proportional[0] + integral[0] + derivative[0];
theta_desired = proportional[1] + integral[1] + derivative[1];
thrust = proportional[2] + integral[2] + derivative[2] + hover_thrust;
}
3.2 姿态内环设计
姿态环需要更高的带宽，论文建议使用PD而非完整的PID：
c复制void attitude_PD_update(float setpoint[3], float measurement[3], float dt) {
// 计算误差（注意角度归一化）
float error[3] = {normalize_angle(setpoint[0] - measurement[0]),
normalize_angle(setpoint[1] - measurement[1]),
normalize_angle(setpoint[2] - measurement[2])};
// 角速度反馈（来自IMU）
float rate[3] = {gyro_x, gyro_y, gyro_z};
// 计算控制量
float torque_roll = Kp_roll * error[0] - Kd_roll * rate[0];
float torque_pitch = Kp_pitch * error[1] - Kd_pitch * rate[1];
float torque_yaw = Kp_yaw * error[2] - Kd_yaw * rate[2];
// 转换为电机指令
motor_mixing(torque_roll, torque_pitch, torque_yaw, thrust);
}
3.3 参数整定方法
论文提出的分步整定流程：
-
先调内环（姿态）PD参数：
- 将无人机固定在测试台上
- 从Kp=1.0, Kd=0.1开始
- 施加阶跃输入，观察响应
- 目标：超调<10%，稳定时间<0.5s
-
再调外环（位置）PID参数：
- 实际飞行测试
- 先调垂直方向（z轴）
- 再调水平方向（x,y轴）
- 目标：无明显振荡，跟踪误差<5%
典型参数范围（450mm轴距机架）：
| 参数 | 滚转/俯仰 | 偏航 | 位置x/y | 位置z |
|---|---|---|---|---|
| Kp | 3.0~8.0 | 1.0~3.0 | 0.5~2.0 | 1.5~4.0 |
| Ki | - | - | 0.05~0.2 | 0.1~0.5 |
| Kd | 0.1~0.5 | 0.05~0.2 | - | - |
4. 关键实现细节
4.1 传感器数据处理
IMU数据的准确性直接影响控制性能：
python复制# 互补滤波器示例（融合加速度计和陀螺仪）
def complementary_filter(accel, gyro, prev_angle, alpha=0.98):
# 加速度计估算角度（低频可靠）
accel_angle = atan2(accel[1], accel[2])
# 陀螺仪积分（高频但会漂移）
gyro_angle = prev_angle + gyro[0] * dt
# 融合
return alpha * gyro_angle + (1-alpha) * accel_angle
4.2 电机混控算法
将控制量分配到四个电机：
matlab复制% 混控矩阵示例
function [m1, m2, m3, m4] = mix(tau_phi, tau_theta, tau_psi, T)
% 系数矩阵（与机架几何参数相关）
mix_matrix = [1, 1, 1, 1; % 总推力
1, -1, -1, 1; % 滚转力矩
1, 1, -1, -1; % 俯仰力矩
-1, 1, -1, 1]; % 偏航力矩
% 解算电机指令
inputs = inv(mix_matrix) * [T; tau_phi; tau_theta; tau_psi];
% 归一化到PWM范围
m1 = constrain(inputs(1), 1000, 2000);
m2 = constrain(inputs(2), 1000, 2000);
m3 = constrain(inputs(3), 1000, 2000);
m4 = constrain(inputs(4), 1000, 2000);
end
4.3 实时性保障
在STM32上的实现技巧：
- 使用定时器中断触发控制循环（内环200Hz，外环50Hz）
- DMA传输传感器数据减少CPU开销
- 使用ARM的DSP库加速矩阵运算
- 关键变量使用
volatile声明防止编译器优化
5. 实验验证与性能分析
5.1 阶跃响应测试
在1m高度指令下的响应曲线：
| 指标 | 上升时间 | 超调量 | 稳态误差 |
|---|---|---|---|
| 论文结果 | 0.8s | 5% | ±0.02m |
| 我们的复现 | 1.2s | 8% | ±0.05m |
差异主要来自：
- 论文使用运动捕捉系统提供定位
- 我们的复现仅依赖低成本GPS和气压计
5.2 抗风性能对比
施加2m/s侧风时的位置偏移：
| 控制方法 | 最大偏移 | 恢复时间 |
|---|---|---|
| 单环PID | 1.5m | 6s |
| 内外环PID | 0.7m | 3s |
内外环结构展现出更好的解耦特性，风扰主要影响姿态环，而不会直接干扰位置控制。
6. 常见问题与调试技巧
6.1 振荡问题排查
若出现高频振荡：
- 检查IMU数据是否有噪声（尝试降低采样率或增加滤波）
- 逐步减小Kp（每次减20%）
- 适当增加Kd（但过大会导致响应迟钝）
低频振荡通常表明积分项过大：
c复制// 积分抗饱和处理示例
void clamp_integral() {
for(int i=0; i<3; i++) {
if(fabs(integral[i]) > MAX_INTEGRAL) {
integral[i] = SIGN(integral[i]) * MAX_INTEGRAL;
}
}
}
6.2 电机混控异常
当出现某个电机持续满速：
- 检查混控矩阵是否与电机安装顺序匹配
- 验证电机转向是否正确（对角电机转向相同）
- 校准电调行程（通常需要发送1000-2000μs的PWM信号）
6.3 实际飞行建议
- 首次飞行时使用安全绳
- 先测试姿态模式再尝试位置控制
- 准备紧急停止开关
- 记录飞行日志（建议采样率不低于50Hz）
7. 扩展改进方向
基于该论文方法的可能优化：
-
自适应PID：根据飞行状态自动调整参数
python复制def adapt_gain(error, prev_error): if abs(error) > threshold: return Kp * 1.5 # 增大增益应对大误差 else: return Kp * 0.8 # 减小增益避免振荡 -
加入前馈补偿：
c复制// 前馈项补偿重力 thrust = pid_output + cos(roll)*cos(pitch) * hover_thrust; -
与其它控制方法结合：
- 在姿态环使用串级PID（角速度+角度）
- 外环改用LQR或MPC提升轨迹跟踪性能
这个复现项目让我深刻体会到，好的控制算法不在于复杂度，而在于对系统特性的准确理解和有针对性的设计。内外环PID结构虽然简单，但通过合理的参数整定和实现优化，完全能满足大多数四旋翼应用的需求。对于准备入门无人机控制的同学，建议先从这种经典方法入手，打好基础再探索更先进的算法。