在移动机器人、智能小车以及其他电机控制系统中，直流电机通常需要完成两个基本任务：

  1. 控制电机正转、反转以及停止；
  2. 控制电机实际转速，使其能够稳定跟随目标速度。

如果只使用 PWM 控制电机，可以实现基本的调速，但无法保证实际速度准确。为了获得更稳定的速度控制效果，通常会加入编码器作为速度反馈，再通过 PI/PID 控制器不断修正 PWM，从而构成速度闭环。

一套典型的控制流程如下：

上层运动指令
      ↓
计算电机目标速度
      ↓
读取编码器
      ↓
计算电机实际速度
      ↓
PI计算PWM控制量
      ↓
根据正负号确定电机方向
      ↓
H桥输出PWM
      ↓
电机转动
      ↓
编码器再次反馈
      └─────────────→ 下一控制周期

在典型的机器人控制代码中，通常会分别设计 Drive_Motor()、Get_Velocity_Form_Encoder()、增量式 PI 和 Set_Pwm() 等模块，用来完成运动学、测速、闭环控制和执行器输出。

一、完整示例代码

为了先建立整体认识，下面给出一套简化后的单电机速度闭环示例。

假设：

#define CONTROL_FREQUENCY   100.0f   // 控制频率 100Hz
#define WHEEL_PERIMETER     0.20f    // 车轮周长 0.20m
#define ENCODER_PRECISION   1560.0f  // 车轮转一圈1560个编码器计数

#define PWM_MAX             16799
#define PWM_LIMIT           16700

1. 读取编码器

int Read_Encoder(uint8_t TIMX)
{
    int encoder = 0;

    switch(TIMX)
    {
        case 2:
            encoder = (short)TIM2->CNT;
            TIM2->CNT = 0;
            break;

        case 3:
            encoder = (short)TIM3->CNT;
            TIM3->CNT = 0;
            break;

        case 4:
            encoder = (short)TIM4->CNT;
            TIM4->CNT = 0;
            break;

        case 5:
            encoder = (short)TIM5->CNT;
            TIM5->CNT = 0;
            break;

        default:
            encoder = 0;
            break;
    }

    return encoder;
}

2. 编码器计数转换为实际速度

float Get_Motor_Velocity(void)
{
    int encoder_count;
    float velocity;

    encoder_count = Read_Encoder(2);

    velocity =
        encoder_count
        * CONTROL_FREQUENCY
        * WHEEL_PERIMETER
        / ENCODER_PRECISION;

    return velocity;
}

3. 增量式 PI 速度控制

int Incremental_PI(float encoder, float target)
{
    static float pwm = 0;
    static float last_error = 0;

    float error;
    float kp = 220.0f;
    float ki = 80.0f;

    error = target - encoder;

    pwm += kp * (error - last_error)
         + ki * error;

    if(pwm > PWM_LIMIT)
        pwm = PWM_LIMIT;

    if(pwm < -PWM_LIMIT)
        pwm = -PWM_LIMIT;

    last_error = error;

    return (int)pwm;
}

4. 输出电机 PWM

void Set_Motor_PWM(int motor_pwm)
{
    if(motor_pwm < 0)
    {
        MOTOR_PWM1 = PWM_MAX;
        MOTOR_PWM2 = PWM_MAX + motor_pwm;
    }
    else
    {
        MOTOR_PWM2 = PWM_MAX;
        MOTOR_PWM1 = PWM_MAX - motor_pwm;
    }
}

5. 电机控制主流程

void Motor_Control(void)
{
    float target_velocity = 1.0f;   // 目标速度 1m/s
    float actual_velocity;
    int motor_pwm;

    /* 1. 编码器测速 */
    actual_velocity = Get_Motor_Velocity();

    /* 2. PI速度闭环 */
    motor_pwm = Incremental_PI(
                    actual_velocity,
                    target_velocity
                );

    /* 3. 输出到H桥驱动器 */
    Set_Motor_PWM(motor_pwm);
}

如果控制频率为：

CONTROL_FREQUENCY = 100;

那么 Motor_Control() 应以大约 10 ms 为周期执行。

二、第一步：产生电机目标速度

整个控制系统首先需要确定：

Target

即电机希望达到的目标速度。

对于只有一个电机的简单系统，可以直接给出：

float target_velocity = 1.0f;

表示：

目标速度 = 1m/s

但对于移动机器人，上层通常不会直接指定每个电机转多快，而是指定机器人整体如何运动。

一般使用三个量：

Vx    // 前后线速度
Vy    // 左右线速度
Vz    // 旋转角速度

例如：

Vx = 0.5f;
Vy = 0;
Vz = 0;

表示机器人以：

0.5m/s

向前直行。

三、第二步：运动学逆解得到各轮目标速度

对于多轮机器人，需要将：

Vx、Vy、Vz

转换为：

A轮目标速度
B轮目标速度
C轮目标速度
D轮目标速度

这一过程称为：

运动学逆解。

例如麦克纳姆轮底盘可以采用类似：

void Drive_Motor(float Vx, float Vy, float Vz)
{
    float L = Axle_spacing + Wheel_spacing;

    MOTOR_A.Target = +Vy + Vx - Vz * L;
    MOTOR_B.Target = -Vy + Vx - Vz * L;
    MOTOR_C.Target = +Vy + Vx + Vz * L;
    MOTOR_D.Target = -Vy + Vx + Vz * L;
}

其中：

Vx：前后运动
Vy：横向运动
Vz：旋转运动

每个车轮的最终目标速度，本质上是这些运动分量按照机械结构进行叠加后的结果。

例如只要求前进：

Vx = 1.0f;
Vy = 0;
Vz = 0;

则：

A = 1
B = 1
C = 1
D = 1

四个轮子保持同样的目标速度。

而只要求横移：

Vx = 0;
Vy = 1.0f;
Vz = 0;

则可能得到：

A = +1
B = -1
C = +1
D = -1

利用麦克纳姆轮特殊的滚轮结构产生横向运动。

实际机器人代码中，Drive_Motor(float Vx, float Vy, float Vz) 就承担了这一层运动合成工作。

四、第三步：编码器采集电机实际运动

得到目标速度之后，还需要知道：

电机实际转得有多快

因此需要使用编码器。

带编码器的直流电机通常会提供 A、B 两相信号。

STM32 的定时器可以配置为 Encoder Mode，由硬件自动完成脉冲计数。

例如：

TIM2->CNT

会随着电机转动不断变化。

一个方向可能表现为：

0 → 1 → 2 → 3 → 4

反方向则表现为：

0 → -1 → -2 → -3

因此程序周期性读取：

encoder = (short)TIM2->CNT;

就可以得到该时间段内的编码器计数。

示例硬件的电机接口中同时引出了 MOTORA1/MOTORA2、MOTORB1/MOTORB2 等编码器信号，可以分别连接四路电机编码器。

五、第四步：周期读取编码器增量

示例函数：

int Read_Encoder(uint8_t TIMX)
{
    int encoder;

    encoder = (short)TIM2->CNT;

    TIM2->CNT = 0;

    return encoder;
}

这里采用的是：

固定周期读取 + 每次读取后清零。

例如控制周期为 10 ms。

第一个周期：

CNT = 80

读取：

80

然后清零。

第二个周期重新计数：

CNT = 82

再读取：

82

因此程序得到的是：

每个控制周期内新增了多少个编码器计数

而不是从开机以来的累计位置。

使用：

(short)TIM2->CNT

则可以利用有符号数表示正反两个旋转方向。

六、第五步：编码器计数转换为实际速度

编码器当前得到的是：

一个周期内的计数

而速度控制器通常需要：

m/s

因此需要进行单位转换。

示例代码：

velocity =
    encoder_count
    * CONTROL_FREQUENCY
    * WHEEL_PERIMETER
    / ENCODER_PRECISION;

对应公式：

其中：

ΔN：一个控制周期的编码器计数
f ：控制频率
C ：车轮周长
N ：车轮旋转一圈对应的编码器总计数

假设：

一个周期得到：78 count
控制频率：100Hz
车轮一圈：1560 count
车轮周长：0.2m

首先计算一秒内的计数：

78 × 100
= 7800 count/s

计算每秒旋转圈数：

7800 / 1560
= 5 r/s

车轮每圈移动：

0.2m

所以：

5 × 0.2
= 1m/s

最终即可得到：

MOTOR_A.Encoder = 1.0f;

这个值就是速度闭环中的实际速度反馈。

七、第六步：比较目标速度和实际速度

现在控制器已经有了两个数据：

MOTOR_A.Target

表示：

目标速度

以及：

MOTOR_A.Encoder

表示：

实际速度

因此可以计算速度误差：

error = target - encoder;

例如：

目标速度：1.0m/s
实际速度：0.7m/s

得到：

error = 0.3

此时实际速度低于目标速度，需要提高电机驱动力。

反之：

目标速度：1.0m/s
实际速度：1.2m/s

则：

error = -0.2

此时需要降低电机驱动力。

这就是闭环控制最基本的数据关系：

目标值
  ↓
与反馈值比较
  ↓
得到误差
  ↓
根据误差调整执行器

八、第七步：增量式 PI 计算 PWM

示例代码：

int Incremental_PI(float encoder, float target)
{
    static float pwm = 0;
    static float last_error = 0;

    float error;

    error = target - encoder;

    pwm += kp * (error - last_error)
         + ki * error;

    last_error = error;

    return (int)pwm;
}

增量式 PI 可以写成：

然后：

在电机控制中：

u(k) = PWM

因此它的工作方式是：

读取当前误差
      ↓
根据当前误差和上次误差
计算本周期需要增加/减少多少PWM
      ↓
在原PWM基础上累加

例如当前：

PWM = 5000

实际速度低于目标速度，PI 计算：

ΔPWM = +300

则：

新PWM = 5300

下一周期继续测速。

如果速度已经更接近目标值：

ΔPWM = +80

新的 PWM：

5380

经过不断迭代，电机速度逐渐逼近目标值。

九、第八步：PWM 输出限幅

PI 控制器计算得到的 PWM 必须限制在硬件允许范围内。

例如：

if(pwm > 16700)
    pwm = 16700;

if(pwm < -16700)
    pwm = -16700;

最终：

-16700 ≤ PWM ≤ 16700

这里可以利用 PWM 的：

正负号 → 表示电机转动方向

绝对值 → 表示控制输出大小

例如：

+5000

表示一个方向，中等驱动力。

-5000

表示相反方向，同样大小的驱动力。

十、第九步：将有符号 PWM 转换成 H 桥控制信号

STM32 本身不能直接驱动直流电机，所以通常需要 H 桥驱动芯片。

典型连接关系：

这样的双输入、双输出结构。示例原理图中四路电机分别由四颗 AT8236 驱动，并由 MCU 输出 DRIVE_A1/DRIVE_A2 等控制信号。

于是可以使用：

void Set_Motor_PWM(int motor_pwm)
{
    if(motor_pwm < 0)
    {
        MOTOR_PWM1 = PWM_MAX;
        MOTOR_PWM2 = PWM_MAX + motor_pwm;
    }
    else
    {
        MOTOR_PWM2 = PWM_MAX;
        MOTOR_PWM1 = PWM_MAX - motor_pwm;
    }
}

正方向控制

假设：

motor_pwm = 5000;

那么

PWM1 = 16799 - 5000
     = 11799

PWM2 = 16799

反方向控制

假设：

motor_pwm = -5000;

那么：

PWM1 = 16799

PWM2 = 16799 - 5000
     = 11799

可以看到，两个方向下：

PWM1 / PWM2

的作用关系发生交换。

这样就可以改变电机两端的有效电压方向，从而实现正反转。

十一、第十步：形成完整速度闭环

至此，一个完整控制周期已经形成：

① 上层给出目标速度
        ↓
② 获取编码器计数
        ↓
③ 编码器计数转换成实际速度
        ↓
④ 目标速度 - 实际速度
        ↓
⑤ 增量式PI
        ↓
⑥ 得到PWM
        ↓
⑦ PWM限幅
        ↓
⑧ H桥正反转处理
        ↓
⑨ 电机运行
        ↓
⑩ 编码器重新采集
        ↓
进入下一个控制周期

假设：

目标速度 = 1m/s

第一次采样：

实际速度 = 0.5m/s

PI 增加 PWM。

第二次：

实际速度 = 0.7m/s

继续调整。

第三次：

实际速度 = 0.9m/s

继续小幅调整。

最后逐渐稳定在：

实际速度 ≈ 1m/s

因此，闭环控制不是一次计算完成，而是周期性不断执行：

测量 → 比较 → 调整 → 再测量

十二、多电机机器人中的控制结构

四轮机器人可以在单电机闭环的基础上进行扩展。

首先通过运动学得到：

MOTOR_A.Target
MOTOR_B.Target
MOTOR_C.Target
MOTOR_D.Target

然后分别读取四个编码器：

MOTOR_A.Encoder
MOTOR_B.Encoder
MOTOR_C.Encoder
MOTOR_D.Encoder

每个电机独立运行一个 PI：

MOTOR_A.Motor_Pwm =
    Incremental_PI_A(
        MOTOR_A.Encoder,
        MOTOR_A.Target
    );

MOTOR_B.Motor_Pwm =
    Incremental_PI_B(
        MOTOR_B.Encoder,
        MOTOR_B.Target
    );

MOTOR_C.Motor_Pwm =
    Incremental_PI_C(
        MOTOR_C.Encoder,
        MOTOR_C.Target
    );

MOTOR_D.Motor_Pwm =
    Incremental_PI_D(
        MOTOR_D.Encoder,
        MOTOR_D.Target
    );

最终：

             机器人Vx/Vy/Vz
                    ↓
                运动学逆解
                    ↓

       ┌────────┬────────┬────────┐
       ↓        ↓        ↓        ↓
    A目标     B目标     C目标     D目标
       ↓        ↓        ↓        ↓
      PI       PI       PI       PI
       ↓        ↓        ↓        ↓
     PWM A    PWM B    PWM C    PWM D
       ↓        ↓        ↓        ↓
     电机A     电机B     电机C     电机D
       ↑        ↑        ↑        ↑
    编码器A   编码器B   编码器C   编码器D

因此四轮机器人并没有改变单电机闭环的基本原理，只是在上层多了一层运动学分配，同时拥有四套独立的速度闭环。

实际控制接口中也通常会分别声明四路增量式 PI、速度解算以及四电机 PWM 输出接口。

十三、完整控制程序结构

将前面的模块整合后，一个典型程序可以写成：

void Motor_Control_Task(void)
{
    /* 1. 获取机器人目标运动速度 */
    Get_Command(&Vx, &Vy, &Vz);

    /* 2. 运动学逆解 */
    Drive_Motor(Vx, Vy, Vz);

    /* 3. 编码器测速 */
    Get_Velocity_Form_Encoder();

    /* 4. 四轮速度PI */
    MOTOR_A.Motor_Pwm =
        Incremental_PI_A(
            MOTOR_A.Encoder,
            MOTOR_A.Target
        );

    MOTOR_B.Motor_Pwm =
        Incremental_PI_B(
            MOTOR_B.Encoder,
            MOTOR_B.Target
        );

    MOTOR_C.Motor_Pwm =
        Incremental_PI_C(
            MOTOR_C.Encoder,
            MOTOR_C.Target
        );

    MOTOR_D.Motor_Pwm =
        Incremental_PI_D(
            MOTOR_D.Encoder,
            MOTOR_D.Target
        );

    /* 5. PWM限幅 */
    Limit_Pwm(16700);

    /* 6. 输出到电机驱动器 */
    Set_Pwm(
        MOTOR_A.Motor_Pwm,
        MOTOR_B.Motor_Pwm,
        MOTOR_C.Motor_Pwm,
        MOTOR_D.Motor_Pwm
    );
}

整个程序的数据流可以概括为：

运动指令
   ↓
目标轮速
   ↓
实际轮速
   ↓
速度误差
   ↓
PI
   ↓
PWM
   ↓
H桥
   ↓
电机
   ↓
编码器反馈

十四、实际开发中的注意事项

前面的内容描述的是控制主流程。在真实项目中，还需要特别关注下面几个问题。

1. 控制周期必须稳定

如果：

CONTROL_FREQUENCY = 100;

就意味着编码器速度计算假设控制周期为：

10ms

如果程序实际上有时 10ms、有时 20ms 才执行一次，速度计算就会产生误差。

在 RTOS 中通常可以使用固定周期任务保证执行频率。

2. 编码器一圈计数需要正确计算

ENCODER_PRECISION 通常不是简单的编码器线数。

可能需要考虑：

编码器线数
×
A/B相倍频
×
减速比

例如：

13 × 4 × 30 = 1560 count/rev

如果该参数错误，最终得到的速度会整体成比例偏差。

3. 左右电机可能需要进行极性修正

由于电机通常是镜像安装，同样的机械前进方向可能出现：

左轮编码器：+100
右轮编码器：-100

程序可以通过：

encoder_right = -Read_Encoder(...);

统一软件中的正方向。

PWM 输出也可能存在类似的方向修正。

4. PWM 与 H 桥逻辑必须结合硬件确认

不同 H 桥支持：

  • IN1/IN2；
  • PH/EN；
  • PWM/DIR；

等不同接口方式。

PWM 高有效还是低有效也可能不同。

因此 Set_Motor_PWM() 的具体实现必须根据：

驱动芯片真值表
+
STM32 PWM极性

共同确定。

5. PI 参数需要根据实际系统整定

KP 和 KI 不是通用参数。

它们会受到：

电机型号
减速比
车轮直径
编码器精度
控制频率
电源电压
机械负载

等因素影响。

实际项目通常需要通过实验逐步调整。

6. PWM 必须进行限幅

控制器在大误差情况下可能输出非常大的值。

因此应在写入硬件之前进行：

PWM上限
PWM下限

保护，防止超出定时器或电机驱动器允许范围。

7. 读取编码器后立即清零并不是唯一方案

本文示例采用：

读取 CNT
↓
CNT = 0

这种方式实现周期测速。

另外一种常见方案是：

本次累计CNT
-
上次累计CNT
=
本周期编码器增量

两种方法都可以使用，需要结合计数器位宽、控制周期以及并发访问方式选择。

总结

一个完整的 STM32 直流电机速度闭环，可以按照下面的顺序理解：

第一层：运动目标
Vx / Vy / Vz
        ↓
第二层：运动学
得到各轮 Target
        ↓
第三层：编码器测速
得到各轮 Encoder
        ↓
第四层：PI速度控制
Target - Encoder
        ↓
得到 Motor_Pwm
        ↓
第五层：H桥执行
PWM大小 + 正反转
        ↓
第六层：电机运动
        ↓
编码器重新反馈

其中，运动学负责决定“每个轮子应该多快”，编码器负责测量“每个轮子实际多快”，PI 控制器负责根据两者的差异计算控制量，H 桥与 PWM 则负责真正驱动电机。

这几部分组合起来，就构成了移动机器人中最常见的一套直流电机速度闭环控制系统。

# stm32 # 嵌入式硬件 # 单片机
Logo
DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

cover

成功率不到 70%，Kinova双臂做遮挡抓取究竟差在哪?

avatar DAMO开发者矩阵

鱼香ros2（一）在ubuntu中安装ros2

ros包含大量搭建机器人所需要的软件和工具，是目前应用最广发的机器人操作系统，未来搭载ros的机器人将不断出现在生产和生活中。

avatar DAMO开发者矩阵
cover

Franka 赋能模型应用新研究，Zero-WAM：基于人类视频上下文的世界-动作模型与零样本任务泛化

avatar DAMO开发者矩阵