🚁 第4课：PID控制器

class PID:
    def __init__(self, kp, ki, kd, i_limit=None):
        self.kp, self.ki, self.kd = kp, ki, kd
        self.i_limit = i_limit
        self.integral = 0.0
        self.prev_error = None
    
    def update(self, error, dt):
        self.integral += error * dt
        if self.i_limit:
            self.integral = max(-self.i_limit, min(self.i_limit, self.integral))
        derivative = 0 if self.prev_error is None else (error-self.prev_error)/dt
        self.prev_error = error
        return self.kp*error + self.ki*self.integral + self.kd*derivative

抗饱和方法：1.积分限幅 2.条件积分 3.Back-calculation

Ziegler-Nichols方法

1. 只用P，增大Kp到持续振荡→临界增益Ku
2. 测量振荡周期Tu
3. Kp=0.6Ku, Ki=2Kp/Tu, Kd=Kp·Tu/8

手动调参：P → D → I

✅ PID是姿态控制基础算法

✅ 积分限幅和微分滤波是工程关键

✅ 调参顺序P→D→I

PID传递函数: C(s) = Kp + Ki/s + Kd*s

# 频域指标:
# 增益穿越频率 wc: |G(jwc)| = 1
# 相位裕度 PM: 180 + angle(G(jwc))
# 增益裕度 GM: 1/|G(jwpi)|

# 无人机典型指标:
# 相位裕度 > 45度
# 增益裕度 > 6dB
# 角速度环带宽 > 20Hz, 角度环 > 5Hz

# 继电器反馈自动调参(Astrom-Hagglund)
# 1. 用继电器代替PID控制器
# 2. 系统产生极限振荡
# 3. 从振幅和周期计算Ku和Tu
# 4. 按Z-N公式计算PID参数

关键公式回顾

本课涉及的核心公式和算法需要反复练习才能真正掌握。建议通过修改仿真参数、添加扰动等方式深入理解每个参数对系统行为的影响。在实际飞行中，这些参数的选择往往需要在理论分析的基础上结合实验微调。

推荐实验

• 修改关键参数，观察系统响应变化
• 添加不同类型的噪声和扰动
• 对比不同算法的精度和计算效率
• 将仿真结果与理论分析对比验证
• 尝试将多个模块组合成完整系统

🔧 工程实践要点

• 实际部署时注意数值稳定性(归一化/限幅/防除零)
• 实时性要求: 控制环1kHz+，导航环50-100Hz
• 传感器数据时间戳对齐至关重要
• 异常处理: 传感器失效需安全降级策略
• 日志记录: 飞行数据全程记录便于事后分析
• 固件升级: 始终使用稳定版本，升级前备份参数

📊 性能指标参考

class FlightPID:
    # 飞行控制器完整PID实现
    def __init__(self):
        # 角速度环参数
        self.rate = {
            'roll':  PID(kp=0.15, ki=0.2, kd=0.003, i_limit=0.5),
            'pitch': PID(kp=0.15, ki=0.2, kd=0.003, i_limit=0.5),
            'yaw':   PID(kp=0.2,  ki=0.1, kd=0.0,   i_limit=0.3),
        }
        # 角度环参数
        self.angle = {
            'roll':  PID(kp=6.5, ki=0, kd=0, i_limit=0),
            'pitch': PID(kp=6.5, ki=0, kd=0, i_limit=0),
        }
    
    def update(self, target_rates, attitude, gyro, dt):
        # 自稳模式: 角度PID → 角速度目标
        if self.mode == 'stabilized':
            target_rates.roll = self.angle['roll'].update(
                target_angles.roll - attitude.roll, dt)
            target_rates.pitch = self.angle['pitch'].update(
                target_angles.pitch - attitude.pitch, dt)
        
        # 角速度PID → 力矩
        torques = {
            'roll':  self.rate['roll'].update(
                     target_rates.roll - gyro.p, dt),
            'pitch': self.rate['pitch'].update(
                     target_rates.pitch - gyro.q, dt),
            'yaw':   self.rate['yaw'].update(
                     target_rates.yaw - gyro.r, dt),
        }
        return torques

算法复杂度分析

在实际飞控系统中，算法必须在有限的计算资源下实时运行。以下为本课核心算法的复杂度分析：

实时系统设计原则

无人机飞控是典型的实时嵌入式系统，必须满足严格的时序约束：

• 硬实时：姿态控制环必须在1ms内完成(1kHz)，否则可能失控
• 软实时：EKF更新应在5ms内完成(200Hz)，偶尔超时可接受
• 非实时：路径规划可在后台线程运行，100ms-1s响应均可

# 典型飞控任务优先级(高→低)
# 1. IMU采样+姿态估计 (1kHz, 优先级最高)
# 2. 角速度PID控制     (1kHz)
# 3. 姿态PID控制       (500Hz)
# 4. EKF状态更新       (200Hz)
# 5. 位置/速度控制      (100Hz)
# 6. 路径规划/避障      (10-50Hz)
# 7. 通信/日志          (1-10Hz)

# RTOS调度: 优先级抢占 + 时间片轮转

代码质量与测试

飞控软件是安全关键系统，代码质量要求极高：

• 编码规范：MISRA C/C++，禁止动态内存分配
• 静态分析：Coverity/Cppcheck扫描所有警告
• 单元测试：每个算法模块100%覆盖率
• SIL测试：软件在环仿真验证功能
• HIL测试：硬件在环仿真验证时序
• 飞行测试：从悬停开始逐步扩大飞行包线

# 单元测试示例
def test_pid_output():
    pid = PID(kp=1.0, ki=0, kd=0)
    assert pid.update(1.0, 0.01) == 1.0  # P=1*1.0

def test_pid_integral_limit():
    pid = PID(kp=0, ki=1.0, kd=0, i_limit=5.0)
    for _ in range(1000):
        pid.update(1.0, 0.01)  # 大量积分
    assert abs(pid.integral) <= 5.0  # 不超过限幅

def test_kalman_convergence():
    kf = KalmanFilter(...)
    for _ in range(100):
        kf.predict()
        kf.update(measurement)
    assert kf.P[0,0] < initial_P  # 协方差下降

开源飞控架构对比

安全关键系统设计

无人机系统属于安全关键(Safety-Critical)系统，设计时必须遵循以下原则：

• Fail-safe设计：任何单点故障不应导致灾难性后果
• 冗余设计：关键传感器(IMU/GPS)至少双冗余
• 看门狗：硬件看门狗检测软件挂起，超时复位
• 状态机：明确的飞行状态转换，禁止非法状态
• 地理围栏：硬编码的飞行区域限制
• 最大倾斜角限制：防止翻机失控
• 电池保护：低压自动返航/降落
• 信号丢失保护：遥控信号丢失5s自动RTH

典型应用场景

本课所学技术在以下场景中直接应用：

• 电力巡检：自主飞行沿输电线路巡检，自动识别绝缘子缺陷、连接点过热、树障距离不足等问题。每天可完成30-50基铁塔的巡检任务，效率是人工的10倍以上。
• 应急救援：灾区快速测绘，10分钟内生成1km2正射影像，为救援决策提供实时地图。红外热成像搜索受困人员，夜间也能有效搜救。
• 精准农业：多光谱相机获取作物NDVI指数，识别病虫害区域，指导变量施药。相比全田喷洒节约30-50%农药。
• 建筑监测：定期无人机巡检生成三维模型，对比分析建筑变形、沉降、裂缝发展。精度可达毫米级。
• 物流配送：山区/海岛等交通不便区域的医疗物资、快递配送。无人机30分钟内可覆盖20km半径范围。

系统集成经验

将本课模块集成到完整系统时的注意事项：

• 传感器数据流：IMU 1kHz -> 姿态估计 500Hz -> 控制 400Hz -> 电机PWM 400Hz
• 时间戳同步：所有传感器数据必须使用同一时间基准(通常为飞控启动后的微秒计数)
• 数据延迟补偿：GPS延迟约100ms，需在EKF中用IMU前向传播补偿
• 传感器切换：GPS可用时用GPS，室内切换到光流/VIO，需平滑过渡
• 故障检测：EKF创新序列(innovation)监测传感器异常，过大则降低该传感器权重
• 参数存储：所有PID/滤波器参数持久化到Flash，上电自动恢复