1. 四旋翼控制系统的核心挑战
四旋翼飞行器的轨迹跟踪控制本质上是一个多变量、强耦合的非线性控制问题。飞行器在空中需要同时控制六个自由度（x/y/z位置和滚转/俯仰/偏航姿态），而控制输入只有四个电机的转速。这种欠驱动特性使得控制算法设计极具挑战性。
关键难点：姿态动力学与位置控制之间存在强耦合。例如当需要向前飞行时，必须先产生俯仰角，这个过程中又会引起高度变化。这种耦合关系在代码中体现在
vx_dot和vy_dot的计算公式里。
2. 动力学模型深度解析
2.1 坐标系定义
- 机体坐标系(Body Frame)：原点在重心，X轴向前，Y轴向左，Z轴向上
- 惯性坐标系(Inertial Frame)：固定地面参考系，通常采用东北天(ENU)约定
2.2 姿态表示
欧拉角(φ,θ,ψ)存在万向节锁问题，实际工程中常配合四元数使用。代码中的姿态动力学方程：
python复制phi_dot = p + q*sin(phi)*tan(theta) + r*cos(phi)*tan(theta) # 滚转角速度
theta_dot = q*cos(phi) - r*sin(phi) # 俯仰角速度
psi_dot = q*sin(phi)/cos(theta) + r*cos(phi)/cos(theta) # 偏航角速度
当θ接近±90°时会出现奇异点，这是欧拉角的固有缺陷。
2.3 电机推力分配
四个电机的总推力F和力矩τ计算：
code复制F = k_f*(ω1² + ω2² + ω3² + ω4²)
τ_φ = L*k_f*(-ω2² + ω4²) # 滚转力矩
τ_θ = L*k_f*(-ω1² + ω3²) # 俯仰力矩
τ_ψ = k_m*(ω1² - ω2² + ω3² - ω4²) # 偏航力矩
其中L是电机到重心的距离，k_f和k_m分别是推力系数和力矩系数。
3. PID控制器实现细节
3.1 串级控制结构
典型的三层控制架构：
- 外环：位置PID控制器（输出期望姿态角）
- 中环：姿态PID控制器（输出角速度）
- 内环：角速度PID控制器（输出电机指令）
3.2 参数整定方法
- 先调内环再调外环
- 先设Ki=Kd=0，增大Kp直到出现小幅振荡
- 加入Kd抑制振荡
- 最后加入Ki消除静差
实测技巧：角速度环的采样频率建议≥500Hz，位置环可以低至50-100Hz。我常用Ziegler-Nichols法进行初步整定后，再通过试飞微调。
3.3 积分抗饱和处理
改进的PID实现：
python复制def control(self, setpoint, current_value, dt):
error = setpoint - current_value
# 积分项带抗饱和
if abs(self.integral) < self.integral_limit:
self.integral += error * dt
# 微分项采用测量值微分
derivative = (current_value - self.prev_value) / dt
output = self.kp*error + self.ki*self.integral - self.kd*derivative
self.prev_value = current_value
return np.clip(output, -self.output_limit, self.output_limit)
4. ADRC核心技术剖析
4.1 扩张状态观测器(ESO)原理
将系统总扰动扩展为新的状态变量z₃，通过以下方程估计：
code复制ẋ₁ = x₂ + β₁(y - x₁)
ẋ₂ = x₃ + β₂(y - x₁) + b*u
ẋ₃ = β₃(y - x₁)
其中β为观测器增益，b为控制增益。通过适当配置β可以实现对扰动的快速跟踪。
4.2 带宽参数化法
将ESO极点配置在-ω₀处：
code复制β₁ = 3ω₀, β₂ = 3ω₀², β₃ = ω₀³
ω₀越大，扰动估计越快但噪声敏感度越高。实际工程中通常取ω₀为系统带宽的3-5倍。
4.3 非线性反馈控制律
改进的控制律设计：
python复制def control(self, setpoint, current_value, dt):
e = setpoint - self.z1
v = self.kp*e - self.kd*self.z2 # 非线性状态反馈
u = (v - self.z3) / self.b # 扰动补偿
return u
其中b需要近似已知，当b不准确时会影响控制性能。
5. 对比实验与性能分析
5.1 抗扰动测试
在相同阶跃响应测试中：
- PID：存在约15%的超调，恢复时间1.2s
- ADRC：超调<5%，恢复时间0.8s
当施加持续风扰时，PID的稳态误差达0.3m，ADRC可保持在0.05m内
5.2 参数敏感性
- PID：质量变化±20%时性能明显下降
- ADRC：质量变化±50%仍能保持稳定控制
5.3 计算开销对比
在STM32F4平台(168MHz)上：
- PID：约5μs/次
- ADRC：约25μs/次
ADRC的实时性仍能满足大多数应用需求
6. 工程实现关键问题
6.1 传感器噪声处理
- 角速度测量需要低通滤波，截止频率建议取50-100Hz
- 位置估计建议采用卡尔曼滤波融合IMU和GPS数据
6.2 执行器饱和
电机存在最大最小转速限制，控制分配时需考虑：
python复制def allocate_control(u1, u2, u3, u4):
# 归一化处理
base = (u1 + u2 + u3 + u4) / 4
delta = np.array([u1,u2,u3,u4]) - base
scale = min(1, (max_rpm - base)/max(delta))
return base + scale * delta
6.3 故障处理策略
- 电机失效检测：监测电流和转速反馈
- 容错控制：重新分配剩余电机推力维持基本姿态
7. 进阶发展方向
7.1 自适应参数调整
python复制class AdaptivePID:
def update_gains(self, error):
# 根据误差动态调整参数
if abs(error) > threshold:
self.kp *= 1.2
self.kd *= 0.8
7.2 混合控制架构
组合PID和ADRC优势：
- 内环用ADRC处理快速动态和扰动
- 外环用PID保证稳态精度
7.3 强化学习优化
用DDPG等算法在线优化控制器参数：
python复制agent.learn(observations, actions, rewards)
在实际飞行测试中，我发现ADRC对电池电压下降导致的动力衰减具有更好的适应性。一个实用技巧是在ESO中加入电池电压观测，可以进一步提升抗扰动性能。对于需要快速原型开发的项目，建议先用PID实现基本功能，再逐步替换为ADRC优化性能。