1. 项目背景与核心思路
四旋翼无人机作为典型的欠驱动系统，其姿态控制与轨迹跟踪一直是飞行控制领域的核心挑战。传统PID控制在面对非线性、强耦合的无人机动力学特性时往往表现不佳，而增量式PID算法通过引入差分运算，能有效抑制积分饱和问题，更适合实时性要求高的飞行控制场景。
这个项目最吸引我的地方在于将连续域PID控制器进行离散化处理，并与无人机动力学模型紧密结合。在实际飞行测试中，我们经常遇到离散控制器与连续系统匹配度不足导致的振荡问题。通过完整的建模-离散化-代码实现链条，可以系统性地解决这个工程痛点。
2. 四旋翼动力学建模要点
2.1 坐标系定义与转换
建立机体坐标系(B系)与地面坐标系(E系)的转换关系是建模基础。采用Z-Y-X欧拉角旋转顺序时，旋转矩阵R可表示为：
matlab复制R = [cosθ*cosψ, sinφ*sinθ*cosψ-cosφ*sinψ, cosφ*sinθ*cosψ+sinφ*sinψ;
cosθ*sinψ, sinφ*sinθ*sinψ+cosφ*cosψ, cosφ*sinθ*sinψ-sinφ*cosψ;
-sinθ, sinφ*cosθ, cosφ*cosθ];
其中φ/θ/ψ分别为滚转/俯仰/偏航角。这个转换关系直接影响后续力矩计算的准确性。
2.2 刚体动力学方程
考虑四旋翼的6自由度运动，牛顿-欧拉方程可表示为：
code复制平移运动：
m*dv/dt = R*F - m*g*[0;0;1] - kf*v
旋转运动：
I*dω/dt + ω×(I*ω) = τ - km*ω
其中kf/km为空气阻力系数，这个简化模型已能反映主要动力学特性。在实际建模时，需要特别注意惯性矩阵I的测量误差——我们曾因电机质量估算偏差导致实际飞行出现耦合振荡。
3. 增量PID控制器设计
3.1 标准PID的离散化处理
采用后向差分法对连续PID进行离散化：
code复制u(k) = Kp*e(k) + Ki*T*Σe(j) + Kd*(e(k)-e(k-1))/T
其中T为采样周期。这个形式存在积分项累积问题，在无人机快速机动时容易导致饱和。
3.2 增量式改进方案
将输出改为控制量增量：
code复制Δu(k) = Kp*(e(k)-e(k-1)) + Ki*T*e(k) + Kd*(e(k)-2e(k-1)+e(k-2))/T
这种形式具有三大优势：
- 无积分项累积，抗饱和能力强
- 执行器变化平滑，减少机械损耗
- 易于实现手动/自动切换
实际调试中发现：当T>0.02s时，增量式会引入明显相位滞后，建议采样周期控制在10ms以内
4. MATLAB实现关键代码解析
4.1 控制器核心代码
matlab复制function [u, errors] = incPID(err, Kp, Ki, Kd, T, prev_err, prev2_err, prev_u)
delta_u = Kp*(err - prev_err) + Ki*T*err + ...
Kd*(err - 2*prev_err + prev2_err)/T;
u = prev_u + delta_u;
errors = [err, prev_err]; % 更新误差记录
end
这段代码的亮点在于：
- 显式处理了三个历史误差值
- 输出包含当前控制量和更新后的误差队列
- 避免使用全局变量，适合嵌入式移植
4.2 轨迹生成模块
采用三次样条插值生成平滑轨迹：
matlab复制t = linspace(0,tf,N);
waypoints = [0 1 2 3; 0 2 1 0]; % 示例航路点
spline_obj = spline([0 tf], [0 waypoints 0]);
desired_pos = ppval(spline_obj, t);
这种方法的加速度连续特性显著优于梯形速度规划，实测可减少约40%的跟踪超调。
5. 仿真与参数整定技巧
5.1 分层调参策略
-
先调内环（姿态控制）：
- 从纯P控制开始，增大Kp至出现轻微振荡
- 加入微分项抑制振荡，Kd≈Kp*T/5
- 最后加入小量积分补偿稳态误差
-
再调外环（位置控制）：
- 保持内环参数不变
- 按同样流程调整外环PID
- 典型比例关系：Kp_out ≈ Kp_in/10
5.2 频域验证方法
通过bode图分析稳定性裕度：
matlab复制sys = tf([Kd Kp Ki],[T 1 0]); % 近似PID传递函数
margin(sys)
建议相位裕度保持在45°-60°之间，这是我们团队经过上百次飞行测试得出的经验值。
6. 实际飞行中的问题排查
6.1 典型故障现象与对策
| 现象 | 可能原因 | 解决方案 |
|---|---|---|
| 高度缓慢漂移 | 积分项累积或气压计干扰 | 增加积分限幅或改用气压差分滤波 |
| 快速机动时发散 | 采样周期过长 | 降低T至<5ms或增加Kd |
| 悬停时高频抖动 | 微分增益过大或电机死区 | 减小Kd或校准电机PWM死区 |
6.2 传感器融合建议
单纯依赖PID控制难以应对复杂环境，推荐两种增强方案：
- 互补滤波：融合IMU与视觉数据
matlab复制angle = 0.98*(angle + gyro*dt) + 0.02*accel_angle; - 简易卡尔曼滤波：适用于处理GPS漂移
matlab复制K = P/(P+R); x = x + K*(z - x); P = (1-K)*P;
7. 工程实现中的经验总结
- 电机响应非线性补偿：
实测发现大多数消费级无刷电机在PWM<20%时存在死区。我们采用分段补偿：
matlab复制if cmd < 0.2
pwm = 0.05 + 0.75*cmd;
else
pwm = 0.2 + 0.8*(cmd-0.2);
end
- 电池电压衰减补偿：
飞行过程中电压下降会导致电机增益变化，建议动态调整：
matlab复制Kp_actual = Kp_nominal * (V_rated/V_actual);
- 抗风扰策略：
在增量PID基础上添加前馈补偿：
matlab复制delta_u += Kff * (ref_pos - 2*prev_pos + prev2_pos)/T^2;
这个项目给我最深的体会是：理论上的优美算法必须经过充分的工程化处理才能真正可靠飞行。我们开源的全部MATLAB代码都包含详细的异常处理和保护逻辑，这些在学术论文中很少提及但却至关重要。