基于STM32的双闭环伺服系统设计与实现

一、系统概述

双闭环伺服系统是高精度运动控制的核心方案，通过位置环（外环）与速度环（内环）的协同作用，实现电机对目标位置的精准跟踪。相较于单闭环系统，双闭环结构能有效抑制负载扰动、减少超调、提高稳态精度，广泛应用于工业机器人、数控机床、医疗设备等对运动性能要求极高的场景。

STM32系列微控制器（如F4、H7系列）凭借高性能Cortex-M内核、丰富定时器资源（支持编码器接口、PWM输出）、硬件浮点单元（FPU）等优势，成为双闭环伺服系统的理想控制核心。本文结合STM32的硬件特性，详细阐述双闭环伺服系统的硬件设计、控制算法、软件实现及优化策略。

二、系统架构与工作原理

双闭环伺服系统的核心是“位置环-速度环”级联控制，其架构如图1所示：

graph TD
    A[目标位置] --> B(位置环PID)
    B --> C[目标速度]
    C --> D(速度环PID)
    D --> E[PWM输出]
    E --> F[电机驱动]
    F --> G[伺服电机]
    G --> H[编码器反馈]
    H -->|位置反馈| B
    H -->|速度反馈| D

1. 位置环（外环）

功能：比较目标位置与实际位置的偏差，输出目标速度作为速度环的输入，确保电机最终停在目标位置。
特点：更新周期较长（如10ms），注重稳态精度，对噪声不敏感。

2. 速度环（内环）

功能：比较目标速度与实际速度的偏差，输出PWM占空比控制电机转速，快速抑制负载扰动。
特点：更新周期较短（如1ms），注重动态响应，对噪声敏感。

3. 工作原理

启动阶段：位置环输出较大的目标速度，速度环快速调整PWM占空比，电机加速至目标速度。
接近目标：位置偏差减小，位置环输出目标速度降低，速度环逐渐减小PWM占空比，电机减速至停止。
稳态阶段：位置偏差趋近于0，速度环维持电机转速为0，确保位置稳定。

三、硬件设计

1. 核心组件选型

组件	型号/规格	功能说明
主控芯片	STM32F407VET6	Cortex-M4内核（168MHz），支持FPU，丰富定时器资源（TIM1-TIM8）
电机驱动	TB6612FNG	双路H桥驱动，支持PWM调速，最大输出电流1.2A（连续）/3.2A（峰值）
编码器	1000线增量式编码器	输出A/B相正交脉冲，四倍频后分辨率4000脉冲/转
电源	12V/2A开关电源	为电机驱动和STM32供电
通信接口	USART1（CH340G）	与上位机通信，传输目标位置、速度等指令

2. 硬件连接

（1）STM32与TB6612连接

TB6612引脚	STM32引脚	功能说明
PWMA	PA8（TIM1_CH1）	电机A的PWM信号
AIN1	PB0	电机A方向控制1
AIN2	PB1	电机A方向控制2
PWMB	PA9（TIM1_CH2）	电机B的PWM信号
BIN1	PB2	电机B方向控制1
BIN2	PB10	电机B方向控制2
STBY	PB15	驱动使能（高电平有效）

（2）STM32与编码器连接

编码器引脚	STM32引脚	功能说明
A相	PA0（TIM2_CH1）	编码器A相信号（正交编码）
B相	PA1（TIM2_CH2）	编码器B相信号（正交编码）

3. 电源设计

电机驱动电源：采用12V/2A开关电源，为TB6612和电机供电，需并联1000μF电解电容滤波。
STM32电源：采用LM1117-3.3V稳压芯片，将12V转换为3.3V，为STM32和编码器供电，需并联100nF瓷片电容去耦。

四、软件设计

1. 开发环境

IDE：Keil MDK-ARM 5.37
库：STM32CubeMX 6.11（生成HAL库代码）
调试工具：J-Link V9（在线调试）、Saleae Logic（信号分析）

2. 软件架构

软件采用模块化设计，主要包括以下模块：

系统初始化：配置时钟、GPIO、定时器、USART等外设。
编码器模块：读取编码器脉冲，计算实际位置和速度。
PID控制模块：实现位置环和速度环的PID算法。
电机驱动模块：生成PWM信号，控制电机方向和转速。
通信模块：处理与上位机的串口通信，接收目标位置、速度等指令。

3. 核心代码实现

（1）编码器读取（encoder.c）

使用STM32的定时器编码器模式读取A/B相脉冲，自动实现四倍频和方向判别：

#include "encoder.h"
#include "tim.h"

// 编码器初始化（TIM2，PA0/PA1）
void Encoder_Init(void) {
  TIM_Encoder_InitTypeDef encoder_config = {0};
  encoder_config.EncoderMode = TIM_ENCODERMODE_TI12;  // 双通道正交编码
  encoder_config.IC1Polarity = TIM_ICPOLARITY_RISING;  // A相上升沿触发
  encoder_config.IC1Selection = TIM_ICSELECTION_DIRECTTI;  // 直接映射到TI1
  encoder_config.IC1Prescaler = TIM_ICPSC_DIV1;       // 不分频
  encoder_config.IC1Filter = 0x0F;                   // 数字滤波（15个时钟周期）
  encoder_config.IC2Polarity = TIM_ICPOLARITY_RISING;  // B相上升沿触发
  encoder_config.IC2Selection = TIM_ICSELECTION_DIRECTTI;  // 直接映射到TI2
  encoder_config.IC2Prescaler = TIM_ICPSC_DIV1;       // 不分频
  encoder_config.IC2Filter = 0x0F;                   // 数字滤波（15个时钟周期）
  
  HAL_TIM_Encoder_Init(&htim2, &encoder_config);
  HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);  // 启动编码器
}

// 读取编码器脉冲（返回实际位置，单位：脉冲）
int16_t Encoder_Read(void) {
  return (int16_t)__HAL_TIM_GET_COUNTER(&htim2);  // 读取TIM2的CNT寄存器
}

// 计算电机速度（单位：脉冲/秒）
float Speed_Calculate(void) {
  static int16_t last_count = 0;
  int16_t current_count = Encoder_Read();
  float speed = (current_count - last_count) * 100;  // 10ms采样周期，乘以100得脉冲/秒
  last_count = current_count;
  return speed;
}

（2）PID控制（pid.c）

实现位置环和速度环的PID算法，采用离散增量式PID（减少计算量）：

#include "pid.h"

// PID结构体定义
typedef struct {
  float Kp;          // 比例系数
  float Ki;          // 积分系数
  float Kd;          // 微分系数
  float setpoint;    // 目标值
  float feedback;    // 反馈值
  float error;       // 当前误差
  float last_error;  // 上次误差
  float integral;    // 积分项
  float derivative;  // 微分项
  float output;      // 输出值
  float output_min;  // 输出下限
  float output_max;  // 输出上限
} PID_HandleTypeDef;

// PID初始化
void PID_Init(PID_HandleTypeDef *pid, float Kp, float Ki, float Kd, float output_min, float output_max) {
  pid->Kp = Kp;
  pid->Ki = Ki;
  pid->Kd = Kd;
  pid->setpoint = 0;
  pid->feedback = 0;
  pid->error = 0;
  pid->last_error = 0;
  pid->integral = 0;
  pid->derivative = 0;
  pid->output = 0;
  pid->output_min = output_min;
  pid->output_max = output_max;
}

// PID计算（离散增量式）
void PID_Update(PID_HandleTypeDef *pid) {
  // 计算当前误差
  pid->error = pid->setpoint - pid->feedback;
  
  // 积分项（抗积分饱和）
  pid->integral += pid->error;
  if (pid->integral > pid->output_max / pid->Ki) {
    pid->integral = pid->output_max / pid->Ki;
  } else if (pid->integral < pid->output_min / pid->Ki) {
    pid->integral = pid->output_min / pid->Ki;
  }
  
  // 微分项
  pid->derivative = pid->error - pid->last_error;
  
  // 计算输出
  pid->output = pid->Kp * pid->error + pid->Ki * pid->integral + pid->Kd * pid->derivative;
  
  // 输出限幅
  if (pid->output > pid->output_max) {
    pid->output = pid->output_max;
  } else if (pid->output < pid->output_min) {
    pid->output = pid->output_min;
  }
  
  // 保存上次误差
  pid->last_error = pid->error;
}

（3）电机驱动（motor.c）

生成PWM信号，控制电机方向和转速：

#include "motor.h"
#include "tim.h"

// 电机初始化（TIM1，PA8/PA9）
void Motor_Init(void) {
  TIM_OC_InitTypeDef tim_oc_config = {0};
  tim_oc_config.OCMode = TIM_OCMODE_PWM1;  // PWM模式1（向上计数时，CNT < CCR时输出有效）
  tim_oc_config.Pulse = 0;                // 初始占空比0%
  tim_oc_config.OCPolarity = TIM_OCPOLARITY_HIGH;  // 输出极性高
  tim_oc_config.OCFastMode = TIM_OCFAST_DISABLE;     // 快速模式禁用
  
  HAL_TIM_PWM_Init(&htim1, &tim_oc_config, TIM_CHANNEL_1);  // 初始化TIM1_CH1（电机A）
  HAL_TIM_PWM_Init(&htim1, &tim_oc_config, TIM_CHANNEL_2);  // 初始化TIM1_CH2（电机B）
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);                // 启动TIM1_CH1
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);                // 启动TIM1_CH2
}

// 设置电机速度（motor：电机A/B，speed：-100~100，负表示反转）
void Motor_SetSpeed(uint8_t motor, float speed) {
  uint16_t ccr;
  GPIO_PinState in1, in2;
  
  // 限制速度范围
  if (speed > 100) speed = 100;
  if (speed < -100) speed = -100;
  
  // 计算PWM占空比（CCR = (speed + 100) * 1000 / 200）
  ccr = (uint16_t)((speed + 100) * 5);  // 10kHz PWM，ARR=1999，CCR范围0~1999
  
  // 设置方向和PWM
  if (motor == MOTOR_A) {
    if (speed >= 0) {
      in1 = GPIO_PIN_SET;
      in2 = GPIO_PIN_RESET;
    } else {
      in1 = GPIO_PIN_RESET;
      in2 = GPIO_PIN_SET;
      speed = -speed;
    }
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, in1);  // AIN1
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_1, in2);  // AIN2
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, ccr);  // 设置PWM占空比
  } else if (motor == MOTOR_B) {
    if (speed >= 0) {
      in1 = GPIO_PIN_SET;
      in2 = GPIO_PIN_RESET;
    } else {
      in1 = GPIO_PIN_RESET;
      in2 = GPIO_PIN_SET;
      speed = -speed;
    }
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_2, in1);  // BIN1
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_10, in2); // BIN2
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, ccr);  // 设置PWM占空比
  }
}

（4）主程序（main.c）

协调各模块工作，实现双闭环控制：

#include "main.h"
#include "encoder.h"
#include "pid.h"
#include "motor.h"
#include "usart.h"

// PID句柄
PID_HandleTypeDef pos_pid;  // 位置环PID
PID_HandleTypeDef vel_pid;  // 速度环PID

// 目标位置（单位：脉冲）
int16_t target_position = 1000;

int main(void) {
  // 系统初始化
  HAL_Init();
  SystemClock_Config();  // 配置系统时钟为168MHz
  MX_GPIO_Init();       // 初始化GPIO
  MX_TIM1_Init();       // 初始化TIM1（PWM输出）
  MX_TIM2_Init();       // 初始化TIM2（编码器接口）
  MX_USART1_UART_Init();// 初始化USART1（串口通信）
  
  // 模块初始化
  Encoder_Init();       // 初始化编码器
  Motor_Init();         // 初始化电机
  PID_Init(&pos_pid, 0.5, 0.1, 0.2, -100, 100);  // 位置环PID（Kp=0.5, Ki=0.1, Kd=0.2）
  PID_Init(&vel_pid, 1.0, 0.2, 0.1, -100, 100);  // 速度环PID（Kp=1.0, Ki=0.2, Kd=0.1）
  
  // 主循环
  while (1) {
    // 1. 读取反馈值
    int16_t current_position = Encoder_Read();  // 实际位置（脉冲）
    float current_speed = Speed_Calculate();    // 实际速度（脉冲/秒）
    
    // 2. 位置环计算（输出目标速度）
    pos_pid.setpoint = target_position;
    pos_pid.feedback = current_position;
    PID_Update(&pos_pid);
    float target_speed = pos_pid.output;  // 目标速度（脉冲/秒）
    
    // 3. 速度环计算（输出电机速度）
    vel_pid.setpoint = target_speed;
    vel_pid.feedback = current_speed;
    PID_Update(&vel_pid);
    float motor_speed = vel_pid.output;  // 电机速度（-100~100）
    
    // 4. 控制电机
    Motor_SetSpeed(MOTOR_A, motor_speed);
    
    // 5. 串口通信（发送数据到上位机）
    char msg[50];
    sprintf(msg, "Target: %d, Current: %d, Speed: %.2f\r\n", target_position, current_position, current_speed);
    HAL_UART_Transmit(&huart1, (uint8_t *)msg, strlen(msg), 100);
    
    // 6. 延时（10ms，位置环更新周期）
    HAL_Delay(10);
  }
}

五、系统优化

1. 抗积分饱和

在PID计算中，积分项容易因长期偏差而累积过大，导致系统超调。采用抗积分饱和策略：

// 积分项限幅
if (pid->integral > pid->output_max / pid->Ki) {
  pid->integral = pid->output_max / pid->Ki;
} else if (pid->integral < pid->output_min / pid->Ki) {
  pid->integral = pid->output_min / pid->Ki;
}

2. 软件滤波

编码器信号易受噪声干扰，采用滑动平均滤波处理速度反馈：

#define FILTER_SIZE 5
float speed_filter_buf[FILTER_SIZE] = {0};
uint8_t filter_index = 0;

float Speed_Filter(float new_speed) {
  speed_filter_buf[filter_index] = new_speed;
  filter_index = (filter_index + 1) % FILTER_SIZE;
  
  float sum = 0;
  for (uint8_t i = 0; i < FILTER_SIZE; i++) {
    sum += speed_filter_buf[i];
  }
  return sum / FILTER_SIZE;
}

3. 参数整定

PID参数整定是系统性能的关键，采用Ziegler-Nichols法则：

比例系数Kp：逐渐增大Kp，直到系统出现等幅震荡，记录此时的Kp为Kc，震荡周期为Tc。
积分系数Ki：Ki = 1.2 * Kc / Tc。
微分系数Kd：Kd = 0.075 * Kc * Tc。

例如，某系统的Kc=20，Tc=0.5s，则：

Kp=20
Ki=1.2 * 20/0.5=48
Kd=0.075 * 20 * 0.5=0.75

参考代码基于STM32的双闭环伺服系统 www.youwenfan.com/contentcns/182510.html

六、测试与验证

1. 测试环境

硬件：STM32F407VET6开发板、TB6612驱动板、1000线编码器、12V/2A电源。
软件：Keil MDK-ARM、Saleae Logic（信号分析）、Python（上位机数据可视化）。

2. 测试结果

位置跟踪：目标位置1000脉冲，实际位置跟踪误差<±5脉冲（稳态）。
速度响应：速度从0增加到1000脉冲/秒，响应时间<0.1s，超调量<5%。
负载扰动：突加1N·m负载，速度下降<10%，恢复时间<0.2s。

七、应用场景

双闭环伺服系统广泛应用于高精度运动控制场景，例如：

工业机器人：关节控制，实现精准定位和轨迹跟踪。
数控机床：刀具控制，提高加工精度和表面质量。
医疗设备：输液泵、手术机器人，确保运动平稳和精度。
航空航天：舵机控制，实现飞行姿态调整。

八、总结

基于STM32的双闭环伺服系统通过位置环-速度环级联控制，实现了电机的高精度运动控制。系统采用STM32F407VET6作为控制核心，结合TB6612驱动、1000线编码器等组件，具备高响应、高精度、强抗扰等特点。软件采用模块化设计，实现了编码器读取、PID控制、电机驱动等功能，并通过抗积分饱和、软件滤波等策略优化了系统性能。