STM32实现旋转倒立摆PID控制全解析

1. 旋转倒立摆系统概述

旋转倒立摆是控制理论中的经典实验平台，也是电子设计竞赛中的常见题目。这个看似简单的系统实际上包含了丰富的控制原理和工程实践挑战。系统由旋转臂和自由摆杆组成，通过电机驱动旋转臂运动来控制摆杆的姿态。我们的目标是实现六大核心功能：规律摆动、全周旋转、倒立稳定、自主起摆、抗干扰和倒立旋转。

硬件配置方面，我们选用STM32F103C8T6作为主控芯片，这款ARM Cortex-M3内核的MCU具有丰富的外设资源和足够的运算能力。电机选用带AB相编码器的直流减速电机，编码器分辨率为2000线/转，可提供精确的位置反馈。机械结构参数为：旋转臂长度22cm，摆杆长度35cm，这些尺寸参数直接影响系统的动力学特性。

注意：机械结构的加工精度和装配质量对系统性能影响极大。在实际调试中，我们发现哪怕0.5mm的轴系间隙都会导致控制效果明显恶化。

2. 硬件系统搭建与信号处理

2.1 编码器接口配置

编码器信号的准确采集是整个系统的基础。我们使用STM32的TIM3定时器工作在编码器接口模式，配置为TI1和TI2双通道上升沿触发：

c复制void Encoder_Init(void){
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
    TIM_ICInitTypeDef TIM_ICInitStructure;
    
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
    
    TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
    TIM_TimeBaseStructure.TIM_Prescaler = 0;
    TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);

    TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, 
                             TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
    TIM_ICStructInit(&TIM_ICInitStructure);
    TIM_ICInitStructure.TIM_ICFilter = 6; //适当滤波
    TIM_ICInit(TIM3, &TIM_ICInitStructure);
    
    TIM_ClearFlag(TIM3, TIM_FLAG_Update);
    TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
    TIM_Cmd(TIM3, ENABLE);
}

2.2 角度计算与滤波处理

编码器原始数据需要转换为实际角度值，同时要考虑机械零点的校准：

c复制#define ENCODER_RESOLUTION 2000.0f //编码器线数
#define MECHANICAL_RATIO 30.0f //减速比

float Get_FilteredAngle(void){
    static float angle_buf[5] = {0};
    static uint8_t index = 0;
    int16_t cnt = TIM_GetCounter(TIM3);
    
    // 角度计算考虑减速比
    float raw_angle = cnt * 360.0f / (ENCODER_RESOLUTION * MECHANICAL_RATIO);
    
    // 滑动平均滤波
    angle_buf[index] = raw_angle;
    index = (index + 1) % 5;
    float filtered = 0;
    for(uint8_t i=0; i<5; i++){
        filtered += angle_buf[i];
    }
    return filtered / 5.0f;
}

3. PID控制算法实现

3.1 基础PID控制器设计

我们采用位置式PID算法，结构体封装各参数：

c复制typedef struct {
    float kp;       //比例系数
    float ki;       //积分系数
    float kd;       //微分系数
    float max_out;  //输出限幅
    float max_i;    //积分限幅
    float dt;       //采样时间(s)
    float last_err; //上次误差
    float integral; //积分项
} PID_Controller;

void PID_Init(PID_Controller* pid, float p, float i, float d, float dt){
    pid->kp = p;
    pid->ki = i;
    pid->kd = d;
    pid->dt = dt;
    pid->max_out = 1000.0f; //默认限幅值
    pid->max_i = 500.0f;
    pid->last_err = 0;
    pid->integral = 0;
}

float PID_Calculate(PID_Controller* pid, float target, float feedback){
    float err = target - feedback;
    
    //积分项处理
    pid->integral += err * pid->dt;
    if(pid->integral > pid->max_i) pid->integral = pid->max_i;
    if(pid->integral < -pid->max_i) pid->integral = -pid->max_i;
    
    //微分项处理(带不完全微分)
    float derivative = (err - pid->last_err) / pid->dt;
    pid->last_err = err;
    
    //PID输出计算
    float output = pid->kp * err + pid->ki * pid->integral + pid->kd * derivative;
    
    //输出限幅
    if(output > pid->max_out) output = pid->max_out;
    if(output < -pid->max_out) output = -pid->max_out;
    
    return output;
}

3.2 串级PID控制实现

倒立稳定功能需要采用串级PID控制，内环为速度环，外环为角度环：

c复制PID_Controller angle_pid; //角度环
PID_Controller speed_pid; //速度环

void Cascade_PID_Init(void){
    //角度环参数
    PID_Init(&angle_pid, 8.5f, 0.01f, 2.5f, 0.002f); //2ms采样周期
    angle_pid.max_out = 300.0f; //角度环输出限幅
    
    //速度环参数
    PID_Init(&speed_pid, 0.3f, 0.0f, 0.05f, 0.002f);
    speed_pid.max_out = 1000.0f; //PWM满幅输出
}

float Inverted_Control(float target_angle, float current_angle){
    static float last_angle = 0;
    float angle_err = target_angle - current_angle;
    
    //角度环计算
    float speed_target = PID_Calculate(&angle_pid, target_angle, current_angle);
    
    //速度环计算
    float current_speed = (current_angle - last_angle) / angle_pid.dt;
    last_angle = current_angle;
    
    return PID_Calculate(&speed_pid, speed_target, current_speed);
}

4. 功能实现详解

4.1 功能一：规律摆动控制

实现摆杆±60°的规律摆动，采用位置式PID控制：

c复制#define SWING_AMPLITUDE 60.0f //摆动幅度
#define SWING_PERIOD 2000     //摆动周期(ms)

void Swing_Control(void){
    static uint32_t last_tick = 0;
    uint32_t current_tick = HAL_GetTick();
    float dt = (current_tick - last_tick) / 1000.0f;
    last_tick = current_tick;
    
    //生成目标轨迹(正弦波)
    float target_angle = SWING_AMPLITUDE * 
                        sin(2 * PI * current_tick / SWING_PERIOD);
    
    //获取当前角度
    float current_angle = Get_FilteredAngle();
    
    //PID控制
    float output = PID_Calculate(&swing_pid, target_angle, current_angle);
    
    //电机驱动
    PWM_SetDuty((int16_t)output);
    
    //刹车处理(过零点附近降低输出)
    if(fabs(target_angle) < 10.0f){
        PWM_SetDuty(output * 0.3f); //降低输出
        swing_pid.integral *= 0.5f; //削弱积分项
    }
}

调试技巧：摆动控制容易出现的问题是过冲和抖动。建议调试步骤：

1. 先将KD设为0，逐步增大KP直到系统开始振荡
2. 然后加入适量的KD阻尼振荡
3. 最后加入很小的KI消除静差
4. 在过零点附近加入刹车逻辑可显著改善稳定性

4.2 功能四：自主起摆控制

自主起摆采用能量控制法，核心思想是通过给电机施加周期性激励，逐步增大摆杆的摆动幅度：

c复制typedef enum {
    STARTUP,
    SWING_UP,
    BALANCE
} SwingState;

SwingState swing_state = STARTUP;
uint32_t swing_start_time = 0;

void Swing_Up_Control(void){
    float current_angle = Get_FilteredAngle();
    float current_speed = Get_AngleSpeed(); //通过角度差分得到
    
    switch(swing_state){
    case STARTUP:
        //初始阶段给一个脉冲激励
        PWM_SetDuty(900); 
        if(fabs(current_angle) > 5.0f){ //检测到初始运动
            swing_state = SWING_UP;
            swing_start_time = HAL_GetTick();
        }
        break;
        
    case SWING_UP:
        //能量控制阶段
        if(current_angle * current_speed > 0){
            PWM_SetDuty(800); //同向加速
        }else{
            PWM_SetDuty(-600); //反向制动
        }
        
        //判断是否达到倒立条件
        if(fabs(current_angle) > 30.0f && fabs(current_speed) < 50.0f){
            swing_state = BALANCE;
            angle_pid.integral = 0; //重置积分项
        }
        
        //超时保护
        if(HAL_GetTick() - swing_start_time > 5000){
            swing_state = STARTUP;
        }
        break;
        
    case BALANCE:
        //切换到倒立控制
        float output = Inverted_Control(0.0f, current_angle);
        PWM_SetDuty((int16_t)output);
        break;
    }
}

4.3 功能三：倒立稳定控制

倒立稳定是系统最核心也是最难的部分，我们采用串级PID结合动态参数调整：

c复制#define BALANCE_ANGLE 0.0f //目标平衡角度
#define MAX_ANGLE_ERR 15.0f //最大允许角度偏差

void Balance_Control(void){
    float current_angle = Get_FilteredAngle();
    static float last_output = 0;
    
    //角度安全检测
    if(fabs(current_angle) > 70.0f){
        Motor_Brake(); //超出安全范围立即刹车
        return;
    }
    
    //动态参数调整
    if(fabs(current_angle - BALANCE_ANGLE) > MAX_ANGLE_ERR){
        //大偏差时增强控制
        angle_pid.kp = 12.0f;
        angle_pid.kd = 3.5f;
    }else{
        //小偏差时恢复常规参数
        angle_pid.kp = 8.5f;
        angle_pid.kd = 2.5f;
    }
    
    //串级PID控制
    float output = Inverted_Control(BALANCE_ANGLE, current_angle);
    
    //输出平滑处理
    output = 0.7f * output + 0.3f * last_output;
    last_output = output;
    
    PWM_SetDuty((int16_t)output);
    
    //抗积分饱和处理
    if((output >= speed_pid.max_out && angle_pid.integral > 0) || 
       (output <= -speed_pid.max_out && angle_pid.integral < 0)){
        angle_pid.integral *= 0.95f;
    }
}

5. 系统状态机设计

为管理六个功能模式，我们设计了一个状态机系统：

c复制typedef enum {
    MODE_SWING,
    MODE_ROTATE,
    MODE_BALANCE,
    MODE_SWING_UP,
    MODE_DISTURB,
    MODE_ROTATE_BALANCE
} SystemMode;

SystemMode current_mode = MODE_SWING;

void System_StateMachine(void){
    static uint32_t mode_start_time = 0;
    float current_angle = Get_FilteredAngle();
    
    switch(current_mode){
    case MODE_SWING:
        Swing_Control();
        if(Button_Pressed(1)){ //模式切换按键
            current_mode = MODE_ROTATE;
            mode_start_time = HAL_GetTick();
        }
        break;
        
    case MODE_ROTATE:
        //旋转臂控制代码
        if(HAL_GetTick() - mode_start_time > 5000){ //5秒后自动切换
            current_mode = MODE_BALANCE;
        }
        break;
        
    case MODE_BALANCE:
        if(current_angle > 160.0f){ //检测到手动抬起
            Balance_Control();
        }
        break;
        
    //其他模式处理...
    }
}

6. 抗干扰设计与参数整定

6.1 抗干扰策略实现

c复制#define DISTURB_THRESHOLD 10.0f //干扰检测阈值
#define RECOVERY_TIME 500       //恢复时间(ms)

void Disturbance_Handler(void){
    static float last_angle = 0;
    static uint32_t disturb_time = 0;
    float current_angle = Get_FilteredAngle();
    float angle_rate = (current_angle - last_angle) / 0.002f; //2ms采样周期
    
    //突变检测
    if(fabs(angle_rate) > 300.0f && fabs(current_angle) < 30.0f){
        disturb_time = HAL_GetTick();
        angle_pid.kp *= 1.8f; //临时提高比例增益
        angle_pid.kd *= 1.5f; //提高微分作用
    }
    
    //恢复处理
    if(HAL_GetTick() - disturb_time > RECOVERY_TIME){
        angle_pid.kp = 8.5f; //恢复默认参数
        angle_pid.kd = 2.5f;
    }
    
    last_angle = current_angle;
}

6.2 PID参数整定经验

1. 角度环参数整定：

   • 先设KI=0, KD=0，逐渐增大KP直到系统开始等幅振荡
   • 记录此时的KP_crit和振荡周期T_crit
   • 根据Ziegler-Nichols法则：
     • KP = 0.6 * KP_crit
     • KI = 2 * KP / T_crit
     • KD = KP * T_crit / 8

2. 速度环参数整定：

   • 先关闭角度环，手动摆动摆杆
   • 调整速度环参数使电机产生适当的阻尼力
   • 典型值约为角度环参数的1/10~1/5

3. 现场微调技巧：

   • 白天与晚上参数可能不同（电网电压变化）
   • 温度变化会影响电机特性
   • 电池供电时注意电压下降的影响

7. 常见问题与解决方案

8. 机械结构调整建议

1. 轴系间隙处理：

   • 使用弹性联轴器吸收安装误差
   • 添加轴向预紧消除窜动
   • 关键部位使用角接触轴承

2. 配重调整：

   • 摆杆重心位置影响系统动力学
   • 可通过配重块调整转动惯量

3. 线材管理：

   • 电机导线应固定避免摆动干扰
   • 使用导电滑环实现无限制旋转

经过两周的密集调试，我们最终实现的性能指标：

• 起摆时间：<3秒
• 稳态误差：<0.5°
• 抗干扰恢复时间：<1秒（500g扰动）
• 旋转控制精度：±2°

调试过程中最大的收获是认识到理论计算只能提供初始参数，真正的优化必须基于实际系统的响应。每个倒立摆系统都有其独特的"性格"，需要耐心地与之"对话"。