ROS2系列教程：控制硬件实战（电机驱动，原理）

发布时间：2026-10-14 00:50:45

🕒 阅读时间：18 分钟📝 字数：3915👀 阅读量：Loading...

本文是 ROS2 系列教程的第 29 篇

本文是 ROS2 系列教程的第 29 篇：控制硬件实战（电机驱动，原理）。第 28 篇我们理解了 ros2_control 的架构，但“关节命令”到“电机转动”之间还有最后一步：硬件驱动。本篇深入电机驱动的原理：直流电机的 H 桥驱动与 PWM 调速、编码器测速原理、PID 闭环控制，并给出完整的代码实现，最后把它们接到 ros2_control 的硬件接口上，让机器人真正“动起来”。

一、直流电机驱动原理

1.1 直流电机怎么转

直流电机（DC Motor）通电就会转，转速与电压成正比：电压越高转越快，反向电压反转。看似简单，但直接控制电机有三个问题：

电流方向：改变方向需要换向（H 桥）。
调速精度：电压不能随意连续变化，需要用 PWM。
电流保护：电机启动电流很大（堵转电流可达额定电流数倍），直接接 GPIO 会烧毁单片机引脚。

所以电机不能直接接 MCU 引脚，必须经过电机驱动模块。

1.2 H 桥原理

H 桥（H-Bridge）是控制电机方向的标准电路，名字来自它的形状像字母 H：4 个开关（三极管/MOSFET）控制电机的两个接线端。

1
    VCC
2
   /    \
3
  S1     S2
4
   |      |
5
  M+----M-（电机）
6
   |      |
7
  S3     S4
8
   \    /
9
    GND

S1+S4 导通：电流从左到右，电机正转。
S2+S3 导通：电流从右到左，电机反转。
全部断开：电机自由停止（滑行）。
S1+S2 同时导通：刹车（短路电机两端）。

常用的 H 桥驱动芯片：L298N（经典，压降大）、TB6612FNG（效率高，机器人常用）、DRV8833（小型化）。TB6612 是目前两轮小车的主流选择。

1.3 PWM 调速原理

PWM（脉冲宽度调制）通过改变方波的占空比控制平均电压，从而控制转速：

1
占空比 100%：▉▉▉▉▉▉▉▉▉▉  →  全速
2
占空比  50%：▉▉▉▉▉▉▉▉░░░░  →  半速（平均电压一半）
3
占空比  20%：▉▉▉▉░░░░░░░░  →  低速

频率通常 10-20kHz（听不见的频段），STM32 的定时器 PWM 输出或 ESP32 的 LEDC 都支持。电机实际感受到的是平均电压：V_avg = Vcc × 占空比。

1.4 完整控制组合

电机控制 = H 桥管方向（2 个引脚：IN1/IN2）+ PWM 管速度（1 个引脚：PWM）：

1
MCU GPIO_IN1 ──┐
2
MCU GPIO_IN2 ──┼── TB6612 ── 电机
3
MCU PWM_PIN  ──┘

IN1	IN2	PWM	电机行为
0	0	×	停止（滑行）
1	0	占空比	正转，速度=占空比
0	1	占空比	反转，速度=占空比
1	1	×	刹车

二、编码器测速原理

2.1 为什么需要编码器

电机转多快、轮子转了几圈，单片机需要知道——否则就是“开环控制”：命令 50% 占空比，实际可能因负载、电池电压而偏快或偏慢。编码器（Encoder） 就是给电机装“眼睛”，测量真实转速，实现闭环控制。

2.2 增量式编码器原理

机器人最常用的是霍尔编码器（增量式）：电机轴上带磁环（多个磁极），霍尔传感器检测磁场变化，每转过一个磁极输出一个脉冲。常见编码器：13PPR（每转 13 脉冲）、2 相输出（A/B 相）。

1
磁环：N-S-N-S-N-S...（每转 13 对磁极）
2
A 相：▂▅▂▅▂▅▂▅▂▅▂▅▂▅   （方波，13 脉冲/转）
3
B 相：▅▂▅▂▅▂▅▂▅▂▅▂▅▂   （与 A 相差 90°）

A/B 两相的作用：

计数：统计 A 相脉冲数，乘以每转脉冲数即可得转角。
辨向：根据 A/B 相相位差判断正反转（A 领先 B 为正转，反之反转）。
倍频：4 倍频技术（检测 A/B 的上升沿和下降沿）把分辨率提高 4 倍。

2.3 测速公式

1
轮子角速度 ω = 脉冲计数 / (每转脉冲数 × 采样周期)
2

3
示例：每转 13 脉冲 × 4 倍频 = 52 计数/转
4
100ms 采样周期内计数 26：
5
ω = 26 / (52 × 0.1) = 5 转/秒 = 5 × 2π = 31.4 rad/s

若已知轮子半径 r，线速度 v = ω × r——这正是 diff_drive_controller 需要的 wheel/velocity 状态。

2.4 外部中断计数（STM32 示例）

编码器脉冲用外部中断计数（不能用轮询，否则丢脉冲）：

1
// 编码器计数（STM32 HAL 库，PA0/PA1 接 A/B 相）
2
volatile int32_t encoder_count = 0;
3

4
// 定时器/外部中断回调：A 相上升沿触发
5
void EXTI0_IRQHandler(void) {
6
    if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_0)) {
7
        __HAL_GPIO_EXTI_CLEAR_IT(GPIO_PIN_0);
8
        // 根据 B 相电平判断方向
9
        if (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_1)) {
10
            encoder_count++;   // 正转
11
        } else {
12
            encoder_count--;   // 反转
13
        }
14
    }
15
}

更专业的做法是用 STM32 定时器的编码器模式（硬件自动计数，不占用 CPU），这里只展示原理。

三、示例 1：TB6612 电机驱动（STM32 HAL）

3.1 硬件接线

以两轮差速小车为例，TB6612 驱动两路电机：

1
STM32          TB6612          电机
2
PA6(PWM)   →  PWMA           → M1A ┐
3
PA7(PWM)   →  PWMB              M1B ┘ 左电机
4
PA4(IN1)   →  AIN1
5
PA5(IN2)   →  AIN2
6
PB0(IN3)   →  BIN1
7
PB1(IN4)   →  BIN2
8
PA8        →  STBY（必须拉高，否则芯片不工作）

3.2 驱动代码

1
// motor_driver.c：TB6612 电机驱动（STM32 HAL）
2
#include "main.h"
3

4
// 电机引脚宏定义（根据实际接线修改）
5
#define MOTOR_L_IN1  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, ...)
6
#define MOTOR_L_IN2  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, ...)
7
#define MOTOR_R_IN3  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, ...)
8
#define MOTOR_R_IN4  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_1, ...)
9

10
// 速度范围 -100 ~ +100（负值反转）
11
void motor_left_set_speed(int speed) {
12
    speed = (speed > 100) ? 100 : (speed < -100) ? -100 : speed;
13
    if (speed > 0) {
14
        MOTOR_L_IN1(1); MOTOR_L_IN2(0);      // 正转
15
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, (uint32_t)(speed * 8.0));
16
    } else if (speed < 0) {
17
        MOTOR_L_IN1(0); MOTOR_L_IN2(1);      // 反转
18
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, (uint32_t)((-speed) * 8.0));
19
    } else {
20
        MOTOR_L_IN1(0); MOTOR_L_IN2(0);      // 停止
21
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 0);
22
    }
23
}
24

25
void motor_right_set_speed(int speed) {
26
    speed = (speed > 100) ? 100 : (speed < -100) ? -100 : speed;
27
    if (speed > 0) {
28
        MOTOR_R_IN3(1); MOTOR_R_IN4(0);
29
        __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, (uint32_t)(speed * 8.0));
30
    } else if (speed < 0) {
31
        MOTOR_R_IN3(0); MOTOR_R_IN4(1);
32
        __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, (uint32_t)((-speed) * 8.0));
33
    } else {
34
        MOTOR_R_IN3(0); MOTOR_R_IN4(0);
35
        __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, 0);
36
    }
37
}
38

39
// 初始化：PWM 定时器 + 方向引脚
40
void motor_init(void) {
41
    // 使能 STBY（TB6612 芯片使能）
42
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_SET);
43
    // 启动 PWM 通道
44
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
45
    HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
46
    motor_left_set_speed(0);
47
    motor_right_set_speed(0);
48
}

注意：speed * 8.0 是把 -100~100 映射到定时器计数范围（8 位 PWM 是 0-255，100 × 2.55 ≈ 255，取 8 是近似）。实际工程应按 TIM_Period 精确映射。

3.3 ESP32 Arduino 版本（对比）

1
// ESP32 Arduino：TB6612 驱动（LEDC PWM 通道）
2
#define PWMA 25
3
#define AIN1 26
4
#define AIN2 27
5
#define STBY 14
6

7
void motorInit() {
8
    pinMode(PWMA, OUTPUT);
9
    pinMode(AIN1, OUTPUT);
10
    pinMode(AIN2, OUTPUT);
11
    pinMode(STBY, OUTPUT);
12
    digitalWrite(STBY, HIGH);
13
    ledcSetup(0, 20000, 8);   // 通道 0，20kHz，8 位分辨率
14
    ledcAttachPin(PWMA, 0);
15
}
16

17
void motorSetSpeed(int speed) {  // -100 ~ 100
18
    if (speed > 0) {
19
        digitalWrite(AIN1, HIGH);
20
        digitalWrite(AIN2, LOW);
21
    } else if (speed < 0) {
22
        digitalWrite(AIN1, LOW);
23
        digitalWrite(AIN2, HIGH);
24
    } else {
25
        digitalWrite(AIN1, LOW);
26
        digitalWrite(AIN2, LOW);
27
    }
28
    ledcWrite(0, abs(speed) * 255 / 100);
29
}
30

31
void setup() {
32
    motorInit();
33
}
34

35
void loop() {
36
    motorSetSpeed(50);   // 50% 速度正转
37
    delay(2000);
38
    motorSetSpeed(0);    // 停止
39
    delay(1000);
40
    motorSetSpeed(-50);  // 50% 速度反转
41
    delay(2000);
42
}

四、示例 2：PID 闭环控制（原理+代码）

4.1 为什么需要 PID

开环给 50% 占空比，实际转速可能只有目标的一半（电池电压下降、负载增加）。PID（比例-积分-微分）控制器根据误差实时调整输出，让实际转速快速、准确地跟上目标：

1
误差 e(t) = 目标转速 - 实际转速
2
输出 u(t) = Kp × e(t) + Ki × ∫e(t)dt + Kd × de(t)/dt

P（比例）：误差越大输出越大，快速逼近。但单独用 P 会有稳态误差（到达目标附近后输出不足以维持）。
I（积分）：累加历史误差，消除稳态误差。但过大容易超调振荡。
D（微分）：预测误差变化趋势，抑制超调。但会放大噪声。

4.2 位置式 PID（C 语言实现）

1
// pid.c：位置式 PID 控制器
2
typedef struct {
3
    float kp, ki, kd;
4
    float integral;
5
    float last_error;
6
    float out_min, out_max;   // 输出限幅
7
    float integral_limit;     // 积分限幅（防积分饱和）
8
} PID_t;
9

10
void pid_init(PID_t *pid, float kp, float ki, float kd) {
11
    pid->kp = kp; pid->ki = ki; pid->kd = kd;
12
    pid->integral = 0;
13
    pid->last_error = 0;
14
    pid->out_min = -100; pid->out_max = 100;
15
    pid->integral_limit = 50;
16
}
17

18
float pid_update(PID_t *pid, float target, float actual) {
19
    float error = target - actual;
20
    // 积分项（带限幅，防积分饱和）
21
    pid->integral += error;
22
    if (pid->integral > pid->integral_limit) pid->integral = pid->integral_limit;
23
    if (pid->integral < -pid->integral_limit) pid->integral = -pid->integral_limit;
24
    // 微分项
25
    float derivative = error - pid->last_error;
26
    pid->last_error = error;
27
    // 输出 = P + I + D
28
    float output = pid->kp * error + pid->ki * pid->integral + pid->kd * derivative;
29
    // 输出限幅
30
    if (output > pid->out_max) output = pid->out_max;
31
    if (output < pid->out_min) output = pid->out_min;
32
    return output;
33
}

4.3 速度闭环主循环

把编码器测速和 PID 结合，形成一个 10ms 周期的速度闭环：

1
// speed_control.c：电机速度闭环
2
#include "pid.h"
3
#include "encoder.h"
4
#include "motor_driver.h"
5

6
PID_t pid_left, pid_right;
7
volatile float target_speed = 0.0;   // 目标速度（rad/s），外部设置
8

9
// 定时器中断：每 10ms 执行一次速度闭环
10
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
11
    if (htim->Instance == TIM6) {   // 10ms 定时器
12
        // 1. 读取编码器计算实际速度（每转脉冲数 × 4 倍频）
13
        float actual_left = encoder_get_speed_rads(ENC_LEFT);   // rad/s
14
        float actual_right = encoder_get_speed_rads(ENC_RIGHT);
15

16
        // 2. PID 计算 PWM 输出（-100 ~ 100）
17
        int out_left  = (int)pid_update(&pid_left, target_speed, actual_left);
18
        int out_right = (int)pid_update(&pid_right, target_speed, actual_right);
19

20
        // 3. 写入电机
21
        motor_left_set_speed(out_left);
22
        motor_right_set_speed(out_right);
23
    }
24
}

4.4 调参经验（重要）

现象	处理
响应慢、误差大	增大 Kp
震荡、电机嗡嗡响	减小 Kp，增大 Kd
稳态误差（始终差一点）	增大 Ki
超调大、来回冲	减小 Ki，增大 Kd
积分饱和（长时间回不来）	加积分限幅（如上代码）

常用起步参数：Kp=1.0, Ki=0.1, Kd=0.0，再按现象微调。真实工程多使用增量式 PID（输出增量而非绝对值）或速度环+电流环双环，原理相同。

五、示例 3：把电机接到 ros2_control 硬件接口

第 28 篇的仿真硬件（mock_components）换成真实电机，只需实现一个自定义 Hardware Interface。它继承 hardware_interface::SystemInterface，核心实现 read()（读编码器）和 write()（写 PWM）。

5.1 自定义硬件接口（C++）

1
// real_motor_hw.cpp：真实电机硬件接口（串口/MicroROS 连接下位机）
2
#include <hardware_interface/system_interface.hpp>
3
#include <hardware_interface/handle.hpp>
4
#include <rclcpp/rclcpp.hpp>
5

6
namespace my_robot_hw {
7

8
class RealMotorHW : public hardware_interface::SystemInterface {
9
public:
10
  // 声明关节及其接口
11
  hardware_interface::CallbackReturn on_init(
12
      const hardware_interface::HardwareInfo &info) override {
13
    // 基类解析 URDF 的 <ros2_control> 配置
14
    if (SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) {
15
      return hardware_interface::CallbackReturn::ERROR;
16
    }
17
    // 打开串口（伪代码，实际用 serial 库）
18
    // serial_.open(info_.hardware_parameters["port"], 115200);
19
    RCLCPP_INFO(rclcpp::get_logger("RealMotorHW"), "hardware init ok");
20
    return hardware_interface::CallbackReturn::SUCCESS;
21
  }
22

23
  // 导出状态接口（编码器读数）
24
  std::vector<hardware_interface::StateInterface> export_state_interfaces() override {
25
    std::vector<hardware_interface::StateInterface> state_interfaces;
26
    for (const auto &joint : info_.joints) {
27
      state_interfaces.emplace_back(joint.name, "position", &position_[joint.name]);
28
      state_interfaces.emplace_back(joint.name, "velocity", &velocity_[joint.name]);
29
    }
30
    return state_interfaces;
31
  }
32

33
  // 导出命令接口（PWM/速度命令）
34
  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override {
35
    std::vector<hardware_interface::CommandInterface> command_interfaces;
36
    for (const auto &joint : info_.joints) {
37
      command_interfaces.emplace_back(joint.name, "velocity", &velocity_cmd_[joint.name]);
38
    }
39
    return command_interfaces;
40
  }
41

42
  // 读状态：从串口获取编码器数据
43
  hardware_interface::return_type read(
44
      const rclcpp::Time &, const rclcpp::Duration &) override {
45
    // 伪代码：解析下位机回传的编码器帧
46
    // frame = serial_read_line();
47
    // position_[joint] = parse_position(frame);
48
    // velocity_[joint] = parse_velocity(frame);
49
    return hardware_interface::return_type::OK;
50
  }
51

52
  // 写命令：把速度命令发给下位机
53
  hardware_interface::return_type write(
54
      const rclcpp::Time &, const rclcpp::Duration &) override {
55
    // 伪代码：拼接命令帧发送
56
    // char buf[64];
57
    // snprintf(buf, 64, "CMD:VEL:%.3f", velocity_cmd_["wheel_left_joint"]);
58
    // serial_write(buf);
59
    return hardware_interface::return_type::OK;
60
  }
61

62
private:
63
  std::map<std::string, double> position_;
64
  std::map<std::string, double> velocity_;
65
  std::map<std::string, double> velocity_cmd_;
66
};
67

68
}  // namespace my_robot_hw
69

70
#include "pluginlib/class_list_macros.hpp"
71
PLUGINLIB_EXPORT_CLASS(my_robot_hw::RealMotorHW, hardware_interface::SystemInterface)

5.2 插件声明与配置

1
<library path="real_motor_hw">
2
  <class name="my_robot_hw/RealMotorHW"
3
         type="my_robot_hw::RealMotorHW"
4
         base_class_type="hardware_interface::SystemInterface"/>
5
</library>

URDF 中替换仿真插件：

1
<ros2_control name="RealMotor" type="system">
2
  <hardware>
3
    <plugin>my_robot_hw/RealMotorHW</plugin>
4
    <param name="port">/dev/ttyUSB0</param>
5
    <param name="baud">115200</param>
6
  </hardware>
7
  <joint name="wheel_left_joint">
8
    <command_interface name="velocity"/>
9
    <state_interface name="position"/>
10
    <state_interface name="velocity"/>
11
  </joint>
12
  <joint name="wheel_right_joint">
13
    <command_interface name="velocity"/>
14
    <state_interface name="position"/>
15
    <state_interface name="velocity"/>
16
  </joint>
17
</ros2_control>

换硬件只改一处：把 <plugin> 从 mock_components/GenericSystem 换成 my_robot_hw/RealMotorHW，控制器和上层导航代码一行都不用改。这就是第 28 篇说的“硬件抽象”的兑现。

六、下位机端串口协议实现（原理）

硬件接口通过串口与下位机通信，需要一个简单的帧协议。推荐“帧头 + 数据 + 校验”格式：

1
帧格式：0xAA 0x55 | CMD(1B) | DATA_LEN(1B) | DATA(nB) | CHECKSUM(1B)
2

3
下位机接收（STM32 串口中断 + 状态机解析）：
4
0xAA 0x55 | 0x01(设速度) | 0x08 | 左轮速度(4B float) | 右轮速度(4B float) | 校验和

校验和 = 数据区所有字节异或，接收端重算比对，防止干扰帧误执行。这是“上位机大脑 + 下位机肌肉”的标准通信范式。

七、常见问题与排障

现象	原因	解决
电机不转	STBY 未拉高 / 电源不足 / PWM 未启动	检查 STBY、外接电源、HAL_TIM_PWM_Start
电机只会一个方向转	IN1/IN2 逻辑反	交换 IN1/IN2 接线或代码
电机嗡嗡响但不转	占空比太低 / 频率不合适	提高 PWM 频率到 10-20kHz，占空比加死区
编码器读数不动	编码器电源未接 / 引脚中断未配置	检查编码器 VCC、EXTI 配置
PID 一直震荡	Kp 过大 / Kd 过小	减小 Kp 增大 Kd，先 P 后 I 再 D 顺序调参
硬件接口 read 报错	串口未打开 / 波特率不匹配	检查串口权限 `sudo chmod 666 /dev/ttyUSB0` 与波特率

八、小结与下一篇预告

本篇核心：

电机控制 = H 桥（方向）+ PWM（速度），TB6612 是主流驱动方案。
编码器（A/B 相）是电机的“眼睛”：计数 + 辨向 + 4 倍频，实现速度反馈。
PID 闭环把开环“盲控”变成“精准跟踪”，P 快速逼近、I 消除误差、D 抑制超调。
ros2_control 硬件接口 = 读编码器（read）+ 写 PWM（write），换硬件只换 plugin。

至此电机驱动全链路打通。下一篇（第 30 篇）进入 控制硬件实战（舵机与传感器，原理）：讲舵机（PWM 控制、SG90/串行总线舵机）、常用传感器（IMU、超声波、红外、激光雷达）的 ROS2 接入原理，并完成硬件控制阶段（26-30）的收官。敬请期待。