STM32舵机速度控制进阶：从基础PWM到平滑运动算法-智慧文博士

1. 舵机控制基础：从PWM信号说起

第一次接触舵机控制时，很多人会疑惑：为什么一个小小的PWM信号就能让舵机精准转动到指定角度？这要从舵机的工作原理说起。舵机内部其实是一个闭环控制系统，它通过比较外部输入的PWM信号和内部电位器的反馈电压，驱动电机转动直到两者一致。

标准舵机的控制信号是一个周期为20ms的PWM波，关键就在于高电平的持续时间（脉宽）。以常见的180度舵机为例：

0.5ms脉宽对应0度位置
1.5ms脉宽对应90度中间位置
2.5ms脉宽对应180度位置

这个对应关系在实际项目中非常重要。我曾经做过一个机械臂项目，就因为把270度舵机当成180度舵机来配置，导致机械臂动作完全错乱。所以使用前一定要确认舵机的角度范围。

在STM32上配置PWM输出时，定时器的ARR（自动重装载值）和PSC（预分频器）是关键参数。假设系统时钟是72MHz，要产生20ms周期的PWM信号，可以这样计算：

预分频设置为72-1，将时钟分频到1MHz
自动重装载值设为20000-1，这样每个PWM周期就是20ms

具体到代码实现，以STM32F103为例，初始化定时器的代码框架如下：

void PWM_Init(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; // 开启时钟和GPIO配置 RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); // 定时器基础配置 TIM_TimeBaseStructure.TIM_Period = 20000-1; // ARR值 TIM_TimeBaseStructure.TIM_Prescaler = 72-1; // PSC值 TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); // PWM输出配置 TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_Pulse = 1500; // 初始占空比(1.5ms) TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; TIM_OC1Init(TIM2, &TIM_OCInitStructure); TIM_Cmd(TIM2, ENABLE); }

实际项目中，我建议把角度转换为脉宽的函数单独封装，这样代码更清晰：

uint16_t AngleToPulse(uint16_t angle, uint16_t max_angle) { // 限制角度范围 if(angle > max_angle) angle = max_angle; // 线性转换公式 return (uint16_t)(angle * (2000.0/max_angle) + 500); }

2. 基础速度控制：定时分步法

直接让舵机从一个角度跳到另一个角度，不仅会产生机械冲击，还会缩短舵机寿命。我第一次做云台项目时就遇到过这个问题——舵机在快速转动时发出刺耳的噪音，一个月后就开始出现齿轮磨损。

解决这个问题的基本方法是定时分步法，也就是把大角度变化分解为多个小步，每步之间加入延时。比如要让舵机从0度转到180度，可以分成20步，每步转9度，间隔50ms：

void SmoothMove(uint16_t start_angle, uint16_t end_angle, uint16_t steps) { float increment = (float)(end_angle - start_angle)/steps; float current_angle = start_angle; for(int i=0; i<steps; i++) { current_angle += increment; uint16_t pulse = AngleToPulse((uint16_t)current_angle, 180); TIM_SetCompare1(TIM2, pulse); Delay_ms(50); // 控制速度的关键延时 } }

这种方法虽然简单，但有三个需要注意的地方：

延时时间不能太短，否则舵机可能来不及响应
步数不宜过多，否则会显得运动迟缓
实际项目中最好用定时器中断来实现延时，避免阻塞主程序

我曾经用这个方法做了一个自动窗帘控制器，发现步数设为30-50、间隔20-50ms时，运动效果最自然。具体参数需要根据实际舵机型号调整。

3. 均匀插值算法实现平滑运动

定时分步法虽然简单，但运动过程还是能看出明显的分段感。为了获得更平滑的运动效果，可以使用均匀插值算法。这个算法的核心思想是：在给定的时间内，均匀地改变PWM的占空比。

假设我们要在T时间内从角度A移动到角度B，可以将这段时间分为N个间隔，每个间隔Δt=T/N。在每个时间点tn=nΔt，计算当前目标角度：

θ(tn) = A + (B-A) * (n/N)

在STM32上实现时，可以用定时器中断来触发角度更新。下面是一个简化版的实现框架：

// 全局变量 uint16_t current_pulse = 1500; // 当前脉宽 uint16_t target_pulse = 2000; // 目标脉宽 uint16_t step_count = 0; uint16_t total_steps = 50; // 总步数 float pulse_increment; // 每步增量 void TIMx_IRQHandler(void) { if(TIM_GetITStatus(TIMx, TIM_IT_Update) != RESET) { if(step_count < total_steps) { current_pulse += pulse_increment; TIM_SetCompare1(TIM2, (uint16_t)current_pulse); step_count++; } else { // 运动完成，可以禁用定时器 TIM_Cmd(TIMx, DISABLE); } TIM_ClearITPendingBit(TIMx, TIM_IT_Update); } } void StartSmoothMove(uint16_t start, uint16_t end, uint16_t time_ms) { current_pulse = start; target_pulse = end; step_count = 0; total_steps = time_ms / 2; // 假设定时器中断周期为2ms pulse_increment = (float)(end - start)/total_steps; // 配置定时器中断周期为2ms TIM_SetAutoreload(TIMx, 72000/5 - 1); // 72MHz/5=14.4MHz, 14.4MHz/7200=2ms TIM_Cmd(TIMx, ENABLE); }

在实际的机械臂项目中，我发现这种算法虽然平滑，但在运动开始和结束时还是会有轻微抖动。这是因为舵机在运动起点和终点需要瞬间改变速度，产生加速度突变。为了解决这个问题，我们需要更高级的算法。

4. 三次插值算法实现加减速控制

要让舵机运动真正丝滑，需要控制它的加速度变化。三次插值算法（也叫S曲线算法）通过在运动开始和结束时降低加速度，中间段保持匀速，实现平滑的加减速过程。

这个算法的数学表达式为：

θ(t) = θ₀ + 3(θ₁-θ₀)(t/T)² - 2(θ₁-θ₀)(t/T)³

其中：

θ₀是起始角度
θ₁是目标角度
T是总运动时间
t是当前时间

这个公式的妙处在于它的导数（速度）是平滑变化的：

t=0时速度为0
t=T时速度也为0
中间过程速度先加速后减速

在STM32上的实现需要一些数学计算：

#include <math.h> // 三次插值函数 float CubicInterpolation(float start, float end, float t, float T) { float ratio = t/T; return start + 3*(end-start)*ratio*ratio - 2*(end-start)*ratio*ratio*ratio; } // 在定时器中断中调用 void TIMx_IRQHandler(void) { static uint32_t elapsed_time = 0; if(TIM_GetITStatus(TIMx, TIM_IT_Update) != RESET) { if(elapsed_time <= total_time) { float angle = CubicInterpolation(start_angle, end_angle, elapsed_time, total_time); uint16_t pulse = AngleToPulse((uint16_t)angle, 270); TIM_SetCompare1(TIM2, pulse); elapsed_time += 2; // 假设中断周期为2ms } else { TIM_Cmd(TIMx, DISABLE); } TIM_ClearITPendingBit(TIMx, TIM_IT_Update); } }

我在一个摄影云台项目中使用了这个算法，效果非常惊艳。云台转动时几乎没有任何抖动，拍摄的视频画面极其平稳。相比简单的均匀插值，三次插值的实现虽然复杂一些，但对运动质量的提升非常明显。

5. 多舵机协同控制技巧

当需要控制多个舵机协同工作时（比如机械臂项目），情况会变得复杂。每个舵机的运动时间和目标角度可能不同，需要更高级的管理策略。

一个实用的方法是创建舵机任务队列。为每个舵机维护一个运动指令队列，在主循环中统一处理：

typedef struct { uint16_t target_pulse; uint16_t move_time; uint8_t servo_id; } ServoCommand; #define MAX_COMMANDS 10 ServoCommand command_queue[MAX_COMMANDS]; uint8_t queue_head = 0; uint8_t queue_tail = 0; void AddCommand(uint8_t id, uint16_t pulse, uint16_t time) { command_queue[queue_tail].servo_id = id; command_queue[queue_tail].target_pulse = pulse; command_queue[queue_tail].move_time = time; queue_tail = (queue_tail + 1) % MAX_COMMANDS; } void ProcessCommands(void) { if(queue_head != queue_tail) { ServoCommand cmd = command_queue[queue_head]; StartSmoothMove(GetCurrentPulse(cmd.servo_id), cmd.target_pulse, cmd.move_time); queue_head = (queue_head + 1) % MAX_COMMANDS; } }

在实际的六足机器人项目中，我采用了这种架构配合三次插值算法，成功实现了12个舵机的协调运动。关键是要合理规划每个舵机的运动时序，避免所有舵机同时运动导致电流过大。

6. 性能优化与实际问题解决

在真实项目中，舵机控制会遇到各种实际问题。这里分享几个我踩过的坑和解决方案：

问题1：舵机抖动症状：即使没有发送指令，舵机也会轻微抖动解决方案：

检查电源是否足够（每个舵机最好单独供电）
确保PWM信号稳定（用示波器检查）
在程序中添加死区，避免频繁微调

问题2：运动不流畅症状：舵机运动时有卡顿感解决方案：

增加插值步数
降低运动速度
检查机械结构是否有阻力

问题3：多舵机同时运动时系统复位症状：多个舵机同时运动时MCU重启解决方案：

增加电源容量（舵机启动电流很大）
错开舵机运动时间
在电源端添加大容量电容（我通常用470-1000μF）

一个实用的电源优化方案是为舵机单独供电，并通过MOS管控制通断：

// 舵机电源控制 void ServoPowerEnable(uint8_t on) { GPIO_WriteBit(GPIOA, GPIO_Pin_4, on ? Bit_SET : Bit_RESET); } // 初始化 void InitServoPower(void) { GPIO_InitTypeDef GPIO_InitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOA, &GPIO_InitStructure); ServoPowerEnable(0); // 初始关闭 }

7. 进阶话题：PID控制在舵机系统中的应用

对于高精度应用，可以考虑在舵机控制中加入PID算法。虽然舵机本身是闭环系统，但外部的PID控制可以更好地处理负载变化和机械误差。

一个简单的比例控制实现：

float Kp = 0.5; // 比例系数 uint16_t target_angle = 90; uint16_t current_angle = 0; void PID_Control(void) { // 获取当前角度（通过编码器或其他传感器） float error = target_angle - current_angle; float adjustment = Kp * error; // 限制调整范围 if(adjustment > 10) adjustment = 10; if(adjustment < -10) adjustment = -10; uint16_t new_angle = current_angle + (uint16_t)adjustment; uint16_t pulse = AngleToPulse(new_angle, 180); TIM_SetCompare1(TIM2, pulse); }

在3D打印机的热床调平系统中，我使用了类似的算法配合测距传感器，成功将调平精度控制在0.1mm以内。关键是要根据实际响应调整PID参数，避免振荡。