STM32 实战：驯服失控的 M3508 电机 - PID 位置环频率的“坑”与“药”

STM32 实战：驯服失控的 M3508 电机 - PID 位置环频率的"坑"与"药"

前言

大家好！今天分享一个我在使用 STM32G474 控制大疆 M3508 电机 + C620 电调进行纯位置闭环控制时遇到的"惊魂一刻"以及最终的解决方案。如果你也在玩转这些硬件，或者对 PID 控制感兴趣，希望这篇博客能给你带来一些启发。

项目背景:

主控: STM32G474VET6
电机与电调: 大疆 M3508 + C620 (CAN 总线通信)
开发环境: CubeMX + CLion
控制目标: 实现一个纯粹的位置 PID 闭环，让电机精确、快速、稳定地转动到指定的圈数（支持小数）。位置环的输出直接作为电流值通过 CAN 发送给电调。
上位机: VOFA+ 用于调参和波形观测。

"疯转"的电机：问题初现

代码初步完成后，通过 VOFA+ 发送较小的目标圈数（比如 0.5 圈，1 圈）时，电机响应正常，能够比较稳定地到达目标位置。然而，当我尝试发送一个较大的目标圈数（比如 10 圈，20 圈）时，电机突然像"脱缰野马"一样开始疯狂高速旋转，完全失控！

问题见下图：这显然不是我们想要的结果。冷静下来，开始分析问题。

寻找"病因"：低频惹的祸？

检查 PID 参数设置、CAN 通信协议、电机反馈数据处理逻辑，似乎都没有明显的低级错误。电机在小目标下表现尚可，说明基本的控制链路是通的。问题出在大目标、高转速的情况下。

我的目光落在了 PID 控制的执行频率上。最初，我将 TIM2 定时器设置为 100Hz，也就是每 10ms 执行一次 PID 计算和 CAN 指令发送。

c 复制代码

// Core/Src/main.c - MX_TIM2_Init (修改前 - 100Hz)
static void MX_TIM2_Init(void)
{
  // ... 其他配置 ...
  // (170MHz / (16999 + 1)) / (99 + 1) = 100 Hz
  htim2.Init.Prescaler = 16999; // 17000 分频
  htim2.Init.Period = 99;      // 计数 100 次
  // ... 其他配置 ...
}

// Core/Src/main.c - HAL_TIM_PeriodElapsedCallback (修改前 - 10ms 任务)
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  // ...
  if (htim->Instance == TIM2) { // 每 10ms 进入一次
    // ... LED 闪烁逻辑 ...

    // 电机控制任务 - 100Hz
    MotorControl_Task();

    // VOFA+数据发送 - 每 100ms 发送一次 (10 * 10ms)
    if (++vofa_send_counter >= 10) {
      vofa_send_counter = 0;
      VOFA_SendData();
    }
  }
  // ...
}

问题猜想:

目标值很大 -> 初始误差很大 -> PID 输出（电流）接近饱和 -> 电机高速旋转。
M3508 编码器分辨率: M3508 电机的编码器一圈有 8192 个脉冲 1。
低控制频率 (100Hz / 10ms): 在这 10ms 的间隔内，高速旋转的电机可能已经转过了超过半圈甚至好几圈。

好的，你说得对，那部分解释确实可以更清晰地阐述变量之间的关系以及它们如何影响 PID 计算，特别是对于新手来说。

我们来重新优化一下"问题猜想"中关于编码器处理和 PID 计算异常的部分，力求更通俗易懂：

编码器读数与位置计算的问题：
- 变量解释:
  - current_encoder: 电机编码器当前时刻的原始读数 (0-8191)。它只表示电机在一圈内的位置。
  - last_encoder: 上一次（比如 10ms 前）读取的编码器原始读数。
  - current_position: 我们程序计算出的电机累计转动圈数 （可以是小数，例如 10.5 圈）。这个值是基于 current_encoder 和 last_encoder 的变化，并累加上经过的整圈数得到的。
- 低频下的问题:
  - 翻转判断可能出错: 电机高速旋转时，在 10ms 内可能转了不止半圈。如果程序仅通过比较 current_encoder 和 last_encoder 的差值（比如 current_encoder - last_encoder）来判断是否过零（即转完一圈），就可能出错。例如，电机正转了 0.7 圈，编码器读数从 1000 变到 1000 + 8192*0.7 ≈ 6734；但如果反转了 0.3 圈，读数会变成 1000 - 8192*0.3 ≈ -1458，实际读数是 8192 - 1458 = 6734。简单的差值计算无法区分这两种情况，导致计算出的累计圈数 current_position 错误。
  - 位置变化量巨大: 即使圈数累加逻辑正确，由于两次计算间隔时间长 (10ms)，current_position 在这期间的变化量（我们称之为 delta_position）也会非常大。
PID 计算异常 (特别是 D 项):
- 变量解释:
  - error: 当前的目标圈数 (target_position) 与当前计算出的实际圈数 (current_position) 之间的差距。 error = target_position - current_position。这是 PID 控制要消除的目标。
  - last_error: 上一次（10ms 前）计算得到的误差值。
  - derivative: 误差的变化率 ，简单理解就是误差变化的速度有多快。在离散控制中，通常近似为 (error - last_error) / 控制周期。在你的 PID_Calculate 函数中，为了简化计算并让 Kd 包含时间因素，直接用了 derivative = error - last_error。
- 低频下的问题:
  - P (比例) 项: Kp * error。当目标圈数很大，或者 current_position 计算错误时，error 会很大，P 项会持续输出较大的控制量（电流），让电机加速。
  - D (微分) 项: Kd * derivative，即 Kd * (error - last_error)。这是问题的关键！
    
    想象一下，10ms 前误差 last_error 是 5 圈，现在由于电机高速转动且位置计算可能不准，误差 error 变成了 -2 圈（甚至可能因为圈数计算错误变成一个奇怪的值）。
    
    那么 error - last_error 就等于 -7 圈，这是一个非常大的误差变化量。
    
    这个巨大的变化量乘以 Kd 系数后，会得到一个绝对值极大的 D 项输出。这个输出要么是一个极大的正电流，要么是一个极大的负电流（取决于误差变化方向）。
    
    这个剧烈的、可能是错误方向的"猛推"或"猛拉"，很容易超出电调能接受的电流范围 (int16_t, -16384~16384)，或者让电机瞬间反向或加速失控。本质上，低频导致 D 项对位置的微小变化或噪声反应过度，放大了不稳定性。
  - I (积分) 项: Ki * integral。如果长时间存在较大的 error，积分项 integral 会不断累加，最终达到饱和值，失去调节能力，也可能导致输出持续饱和。

结论： 100Hz 的控制频率对于这种需要快速响应的直接位置环来说太低了。控制器无法足够快地"看到"电机的实际位置变化并作出调整，尤其是在高速运动时，误差计算（特别是误差变化率 D 项）很容易因为采样间隔太长而变得"疯狂"，导致整个系统不稳定。

对症下药：提高控制频率！

解决方案就是提高 PID 控制的频率。我们将目标频率设定为 1000Hz (1ms)，这在 STM32G474 上是完全可行的。

修改 MX_TIM2_Init 函数:

根据系统时钟 (170MHz) 和目标频率 (1kHz) 重新计算 Prescaler 和 Period。

javascript 复制代码

// Core/Src/main.c - MX_TIM2_Init (修改后 - 1000Hz)
static void MX_TIM2_Init(void)
{
  // ... 其他配置 ...
  // (170MHz / (169 + 1)) / (999 + 1) = 1000 Hz
  htim2.Init.Prescaler = 169;  // <--- 修改分频值
  htim2.Init.Period = 999;     // <--- 修改重装载值
  // ... 其他配置 ...
}

修改 HAL_TIM_PeriodElapsedCallback 函数:

中断频率提高了 10 倍，需要相应调整依赖计数的任务（LED 闪烁、VOFA 发送）的计数值。

scss 复制代码

// Core/Src/main.c - HAL_TIM_PeriodElapsedCallback (修改后 - 1ms 任务)
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  // ...
  if (htim->Instance == TIM2) { // 每 1ms 进入一次
    static uint32_t led_counter = 0;
    static uint32_t system_counter = 0;

    system_counter++; // 每 1ms 增加 1

    // LED1 快闪 - 每 500ms 闪烁一次 (500 * 1ms)
    if (++led_counter >= 500) { // <--- 计数值 * 10
      led_counter = 0;
      LED1_TOGGLE();
    }

    // LED2 慢闪 - 每 2 秒闪烁一次 (2000 * 1ms)
    if (system_counter % 2000 == 0) { // <--- 计数值 * 10
      LED2_TOGGLE();
    }

    // 电机控制任务 - 现在以 1000Hz 运行！
    MotorControl_Task();

    // VOFA+数据发送 - 每 100ms 发送一次 (100 * 1ms)
    if (++vofa_send_counter >= 100) { // <--- 计数值 * 10
      vofa_send_counter = 0;
      VOFA_SendData();
    }
  }
  // ...
}

药到病除：电机恢复平静

修改代码，重新编译烧录后，再次尝试发送较大的目标圈数。这次电机表现得非常"听话"，能够平稳、快速地加速、减速，并准确地停在目标位置附近！VOFA+ 上的波形也显示 current_position 能够很好地跟随 target_position，输出电流 output_current 变化平滑，不再出现之前的剧烈震荡和饱和。

效果见下图：

总结与思考

这次调试经历告诉我们：

控制频率至关重要: 对于响应速度要求较高的系统（如电机位置控制），过低的控制频率是稳定性的"杀手"。提高频率能让控制器更及时地获取反馈、进行计算和调整输出。
理解硬件限制: 了解编码器的分辨率、电调的通信协议和数据范围，有助于分析问题和设置合理的参数（如 PID 输出限幅）。
系统性分析: 当遇到问题时，不要只盯着 PID 参数本身，要从整个控制流程（采样、计算、执行、通信）和时间尺度（控制周期）去分析可能的原因。

当然，将频率从 100Hz 提高到 1000Hz 后，之前的 PID 参数可能不再最优，需要根据实际效果重新进行整定 ，特别是 Kd 参数对噪声会更敏感，可能需要减小或者配合滤波使用。

希望这次的"踩坑"与"填坑"记录能对你有所帮助！Happy Coding！