飞控学习（十）

abc_哈哈
44 阅读5分钟

非常好！将现代PID控制理念融入PX4是一个很有价值的项目。以下是您可以做的具体改进方案：

1. 抗饱和PID（Anti-windup PID）

问题识别

PX4当前PID在积分饱和时表现不佳，特别是当控制量饱和时积分项继续累积。

实现方案

class AntiWindupPID {
private:
    float _kp, _ki, _kd;
    float _integral = 0.0f;
    float _prev_error = 0.0f;
    float _output_limit;
    float _integral_limit;
    bool _saturated = false;
    
public:
    float compute(float error, float dt) {
        // 比例项
        float p_term = _kp * error;
        
        // 积分项（带抗饱和）
        if (!_saturated) {
            _integral += _ki * error * dt;
            _integral = math::constrain(_integral, -_integral_limit, _integral_limit);
        }
        
        // 微分项
        float d_term = _kd * (error - _prev_error) / dt;
        
        float output = p_term + _integral + d_term;
        
        // 检测饱和并处理
        _saturated = (fabsf(output) >= _output_limit);
        if (_saturated) {
            output = math::constrain(output, -_output_limit, _output_limit);
            // 反向计算积分限制（back-calculation）
            _integral = output - p_term - d_term;
        }
        
        _prev_error = error;
        return output;
    }
};

2. 设定值加权PID（Setpoint Weighting）

问题识别

PX4的PID对设定值突变响应过于激进，导致超调过大。

实现方案

class SetpointWeightedPID {
private:
    float _b; // 比例项设定值权重 (0-1)
    float _c; // 微分项设定值权重 (0-1)
    
public:
    float compute(float setpoint, float measurement, float dt) {
        float error = setpoint - measurement;
        
        // 加权设定值用于微分计算
        float weighted_setpoint = _b * setpoint + (1 - _b) * measurement;
        float weighted_derivative = _c * setpoint + (1 - _c) * measurement;
        
        // 改进的PID计算
        float p_term = _kp * (weighted_setpoint - measurement);
        float i_term = _integral + _ki * error * dt;
        float d_term = _kd * (weighted_derivative - _prev_measurement) / dt;
        
        _prev_measurement = measurement;
        return p_term + i_term + d_term;
    }
    
    void setWeightingFactors(float b, float c) {
        _b = math::constrain(b, 0.0f, 1.0f); // b=0: 对设定值变化不敏感
        _c = math::constrain(c, 0.0f, 1.0f); // c=0: 纯测量值微分
    }
};

3. 自适应PID（Adaptive PID）

问题识别

固定PID参数在不同飞行状态下性能不一致。

实现方案

class AdaptivePID {
private:
    struct PIDGains {
        float kp, ki, kd;
    };
    
    PIDGains _current_gains;
    PIDGains _aggressive_gains;   // 快速响应
    PIDGains _conservative_gains; // 稳定优先
    
public:
    void adaptBasedOnFlightCondition(const vehicle_status_s &status, 
                                     const vehicle_local_position_s &local_pos) {
        // 根据飞行状态自适应调整参数
        if (is_aggressive_condition(status, local_pos)) {
            interpolateGains(_aggressive_gains, 1.0f);
        } else if (is_conservative_condition(status, local_pos)) {
            interpolateGains(_conservative_gains, 1.0f);
        } else {
            // 平滑过渡
            float blend_factor = calculate_blend_factor(status, local_pos);
            interpolateGains(_aggressive_gains, blend_factor);
        }
    }
    
private:
    bool is_aggressive_condition(const vehicle_status_s &status, 
                                  const vehicle_local_position_s &local_pos) {
        // 高速、高机动性飞行
        return local_pos.vx > 5.0f || 
               local_pos.vy > 5.0f ||
               status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO;
    }
    
    bool is_conservative_condition(const vehicle_status_s &status,
                                  const vehicle_local_position_s &local_pos) {
        // 低速、精确控制
        return local_pos.vx < 1.0f ||
               local_pos.vy < 1.0f ||
               status.nav_state == vehicle_status_s::NAVIGATION_STATE_PRECISION_LAND;
    }
};

4. 模糊PID（Fuzzy PID）

问题识别

传统PID难以处理非线性、时变系统。

实现方案

class FuzzyPID {
private:
    FuzzyEngine _fuzzy_engine;
    
public:
    float compute(float error, float error_derivative, float dt) {
        // 模糊规则调整PID参数
        auto adjusted_gains = _fuzzy_engine.adjustGains(error, error_derivative);
        
        // 使用调整后的参数计算PID
        return calculatePID(error, dt, adjusted_gains);
    }
    
private:
    struct FuzzyGains adjustGains(float error, float error_derivative) {
        // 模糊化输入
        auto error_fuzzy = fuzzifyError(error);
        auto derivative_fuzzy = fuzzifyDerivative(error_derivative);
        
        // 模糊推理
        auto kp_adjustment = inferKpAdjustment(error_fuzzy, derivative_fuzzy);
        auto ki_adjustment = inferKiAdjustment(error_fuzzy, derivative_fuzzy);
        auto kd_adjustment = inferKdAdjustment(error_fuzzy, derivative_fuzzy);
        
        // 解模糊化
        return defuzzify(kp_adjustment, ki_adjustment, kd_adjustment);
    }
};

5. 基于模型的PID整定（Model-based Tuning）

问题识别

手动调参耗时且难以获得最优性能。

实现方案

class ModelBasedPIDTuner {
public:
    void autoTune(const SystemModel &model, const PerformanceSpec &spec) {
        // 使用Ziegler-Nichols或其他方法自动整定
        PIDGains initial_gains = zieglerNicholsTuning(model);
        
        // 基于模型仿真优化
        PIDGains optimized_gains = optimizeWithSimulation(model, initial_gains, spec);
        
        applyGains(optimized_gains);
    }
    
private:
    PIDGains zieglerNicholsTuning(const SystemModel &model) {
        // 经典Z-N整定法
        float ku = findUltimateGain(model);
        float tu = findOscillationPeriod(model);
        
        return {
            .kp = 0.6f * ku,
            .ki = 1.2f * ku / tu,
            .kd = 0.075f * ku * tu
        };
    }
};

6. 在PX4中的具体集成方案

替换现有控制器

// 在mc_att_control模块中集成现代PID
class ModernAttitudeControl : public ModuleBase<ModernAttitudeControl>
{
private:
    AdaptivePID _roll_pid;
    AdaptivePID _pitch_pid; 
    AdaptivePID _yaw_pid;
    
public:
    bool update() override {
        // 获取当前状态
        vehicle_attitude_s attitude;
        _vehicle_attitude_sub.copy(&attitude);
        
        // 自适应参数调整
        adaptPidParameters();
        
        // 使用现代PID计算控制量
        Vector3f control_output = computeModernPidControl(attitude);
        
        // 发布控制指令
        publish_actuator_controls(control_output);
        
        return true;
    }
    
private:
    void adaptPidParameters() {
        // 根据飞行状态、风速、载重等条件自适应
        _roll_pid.adaptBasedOnCondition(_current_flight_condition);
        _pitch_pid.adaptBasedOnCondition(_current_flight_condition);
        _yaw_pid.adaptBasedOnCondition(_current_flight_condition);
    }
};

7. 性能评估框架

量化对比指标

class PIDPerformanceEvaluator {
public:
    struct PerformanceMetrics {
        float rise_time;      // 上升时间
        float settling_time;  // 调节时间  
        float overshoot;      // 超调量
        float ise;           // 积分平方误差
        float iae;           // 积分绝对误差
        float robustness;    // 鲁棒性指标
    };
    
    PerformanceMetrics compareControllers(const BaseController &old_pid, 
                                         const ModernPID &new_pid) {
        // 在相同测试场景下对比性能
        auto old_performance = evaluateController(old_pid, _test_scenarios);
        auto new_performance = evaluateController(new_pid, _test_scenarios);
        
        return calculateImprovement(old_performance, new_performance);
    }
};

8. 参数配置接口

用户友好的调参界面

// 扩展PX4参数系统
class ModernPIDParameters {
public:
    // 抗饱和参数
    PARAM_DEFINE_FLOAT(MC_PID_AW_ENABLE, 1);      // 抗饱和使能
    PARAM_DEFINE_FLOAT(MC_PID_AW_GAIN, 0.1);     // 抗饱和增益
    
    // 设定值加权参数  
    PARAM_DEFINE_FLOAT(MC_PID_B_FACTOR, 0.3);     // 比例项权重
    PARAM_DEFINE_FLOAT(MC_PID_C_FACTOR, 0.0);     // 微分项权重
    
    // 自适应参数
    PARAM_DEFINE_FLOAT(MC_PID_ADAPT_ENABLE, 1);   // 自适应使能
    PARAM_DEFINE_FLOAT(MC_PID_ADAPT_SPEED, 0.5);  // 自适应速度
};

实施建议

第一阶段：基础改进（1个月）

  1. 实现抗饱和PID​ - 解决积分饱和问题
  2. 设定值加权PID​ - 改善设定值跟踪

第二阶段：智能优化（1个月）

  1. 自适应PID​ - 根据飞行状态调整参数
  2. 自动整定工具​ - 简化调参过程

第三阶段：高级功能（1个月）

  1. 模糊PID​ - 处理非线性情况
  2. 性能评估系统​ - 量化改进效果

测试验证方案

# 对比测试脚本
def test_modern_pid_improvements():
    test_cases = [
        {'scenario': 'step_response', 'metric': 'overshoot', 'improvement': '>30%'},
        {'scenario': 'disturbance_rejection', 'metric': 'settling_time', 'improvement': '>20%'},
        {'scenario': 'setpoint_tracking', 'metric': 'ise', 'improvement': '>25%'}
    ]

这个项目不仅能显著提升PX4的控制性能，还能展示您对现代控制理论的深入理解。建议从抗饱和PID开始，这是改进最大且实现相对简单的部分。

avatar
文章
阅读
粉丝