自平衡小车：MPU6050 测角 + PID 让两轮车自己立起来

桌上放着一辆两轮小车，两个轮子左右一字排开，底下没有第三个支撑点。松手，它照理该往一边栽下去。可你一通电，它先是微微抖了两下，然后就在原地站住了——你伸手推它一把，它往被推的方向冲出去几厘米，再自己晃回原位，稳稳立着，跟被你戳了一下的不倒翁似的。

这就是自平衡小车，硬件圈里最经典、也最有成就感的一个实战。它没有任何机械支撑，全靠每秒几百次的「测角度—算力度—驱动轮子」循环，把一个本该倒下的系统硬生生维持在直立。这是你第一次把三件单练过的招式真正接成一辆能站住的车： 用 MPU6050 读姿态、用 L2 电机驱动那节让轮子听话、用 PID 把误差换成该给电机多大力。之前它们各是各的，做完这辆车，你手里第一次有了一个「传感器测量→控制器决策→执行器驱动」闭合的实时控制系统。

这一篇不重复推 PID 公式、不重讲 MPU6050 寄存器——那些 robot-pid 和 robot-self-balance 讲透了，本篇默认你读过。我们只干一件事：用可跑的 ESP-IDF 代码把它们拼成一辆车，讲清拼的过程里那些单看一个知识点时看不到的坑。

---

第一步：想清楚要做成什么样，再定选型

动手前先把「成品长什么样」钉死。我们的车只做一件事——站住：

• 直立：松手后在原地站住，轮子来回做几毫米的快速微调，整体不漂走。
• 抗扰：你用手指轻推车身上沿，它先朝被推方向冲几厘米「接住重心」，再晃回直立。
• 左右转向、前进后退留到进阶（串 robot-self-balance 的串级控制），本篇先把「站住」这个地基打牢。

底盘：为什么必须是「两轮无万向轮」

自平衡小车的物理模型是两轮倒立摆——重心在轮轴之上，本该倒。买底盘认准两轮左右排布、没有第三个万向轮的款。带万向轮的是循迹车底盘，三点支撑本身就稳，用不着平衡，买错了项目直接做不成。重心稍高一点（电池板装上层）反而更好调，因为倒得慢、留给控制器反应的时间更长。

IMU：为什么是 MPU6050

测倾角要用惯性测量单元（IMU）。MPU6050 是入门首选：六轴（三轴加速度计 + 三轴陀螺仪）、I2C 接口、几块钱、例程海量。加速度计能直接算倾角但一动就被振动污染，陀螺仪测角速度积分出角度很平滑但会漂——两者互补，正好凑一个又稳又不漂的角度。接线、寄存器、I2C 地址（默认 0x68）细节全在 /sensor/mpu6050/，本篇默认你能读出原始值。

电机驱动：为什么是 TB6612 而不是 L298N

减速电机的电流 GPIO 直接带不动，必须用电机驱动模块做功率级。选 TB6612FNG，别用老掉牙的 L298N。 TB6612 是 MOS 管方案，压降小、发热低，一路能扛 1.2A，双路正好驱两个电机；L298N 是老三极管方案，压降能吃掉一两伏，电池电压本就不高再被它吃掉，电机没劲、车扶不动。每路电机要 3 根信号线：两根方向（IN1/IN2）定正反转、一根 PWM（PWMx）定转速——跟 l2-motor 讲的 H 桥控制一模一样。

电机：减速比与配对

用带减速箱的 N20 或 TT 电机，转速低、扭矩大，适合平衡。两个电机务必买同型号同减速比，否则同样 PWM 下两轮转速不一致，车会一边站一边偏。

---

第二步：接线——MPU6050 的 I2C + 电机驱动，避开 S3 雷区

这辆车要接三组线：MPU6050 走 I2C（两根：SDA/SCL），两个电机各走 3 根到 TB6612，还有一路 PWM 出到 TB6612。ESP32-S3 的 GPIO 看着一排，但有一批碰不得，选脚前先记牢：

避开这些，安全区里挑脚。本项目分配（全在安全区）：

MPU6050（I2C）：
  VCC → 开发板 3V3      GND → GND
  SDA → GPIO8          SCL → GPIO9
  （AD0 悬空或接 GND，I2C 地址 = 0x68）

TB6612 电机驱动：
  VM   → 电池正极（电机电源，独立供电！6~12V 看电机额定）
  VCC  → 开发板 3V3（逻辑电源）
  GND  → 与开发板 GND、电池负极 三者共地
  STBY → 开发板 3V3（拉高使能，接地=待机不转）

  左电机 A：AIN1→GPIO4  AIN2→GPIO5  PWMA→GPIO6
  右电机 B：BIN1→GPIO7  BIN2→GPIO15 PWMB→GPIO16
  AO1/AO2 → 左电机两根线   BO1/BO2 → 右电机两根线

几处关键，别踩：

• VM 和 VCC 是两回事。 VM 是电机的大电流电源，接电池；VCC 是驱动芯片的逻辑电源，接开发板 3V3。新手最常把 VM 也接到板子上，一上力就 brownout 重启（见开头 safety 框）。
• STBY 必须拉高。 忘了接，电机一动不动还以为代码错了——这是 TB6612 头号坑。
• 三者共地是铁律。 开发板、电池、TB6612 的 GND 必须连一起，否则信号没有参考地，电机行为随机。
• MPU6050 的 SDA/SCL 选了 GPIO8/9，都在安全区。有些模块板载上拉电阻，没有的话 SDA/SCL 各串一个 4.7kΩ 上拉到 3V3。

---

第三步：分步把代码写出来

我们不一次甩一大坨，而是分三步长出来，每步都能单独烧进去看效果——出问题时你才知道是哪一步坏的。全部是能跑的 ESP-IDF 代码。

步 1：先读出准确的 pitch 角（互补滤波）

第一步只验证一件事：MPU6050 接对了、能读出一个又稳又不漂的倾角。电机先别接。这段用 i2c_master 新驱动读 MPU6050，再用互补滤波融合出 pitch：

#include "driver/i2c_master.h"
#include "esp_timer.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include <math.h>

static const char *TAG = "balance";

#define I2C_SDA   GPIO_NUM_8
#define I2C_SCL   GPIO_NUM_9
#define MPU_ADDR  0x68               // AD0 接地时是 0x68

static i2c_master_dev_handle_t mpu;

// 写一个寄存器
static void mpu_write(uint8_t reg, uint8_t val) {
    uint8_t buf[2] = { reg, val };
    ESP_ERROR_CHECK(i2c_master_transmit(mpu, buf, 2, 100));
}

// 从 reg 起连读 n 字节
static void mpu_read(uint8_t reg, uint8_t *out, size_t n) {
    ESP_ERROR_CHECK(i2c_master_transmit_receive(mpu, &reg, 1, out, n, 100));
}

static void mpu_init(void) {
    i2c_master_bus_config_t bus_cfg = {
        .i2c_port = I2C_NUM_0,
        .sda_io_num = I2C_SDA,
        .scl_io_num = I2C_SCL,
        .clk_source = I2C_CLK_SRC_DEFAULT,
        .glitch_ignore_cnt = 7,
        .flags.enable_internal_pullup = true,
    };
    i2c_master_bus_handle_t bus;
    ESP_ERROR_CHECK(i2c_new_master_bus(&bus_cfg, &bus));

    i2c_device_config_t dev_cfg = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = MPU_ADDR,
        .scl_speed_hz = 400000,       // 400kHz 快速模式
    };
    ESP_ERROR_CHECK(i2c_master_bus_add_device(bus, &dev_cfg, &mpu));

    mpu_write(0x6B, 0x00);            // PWR_MGMT_1：解除休眠
    vTaskDelay(pdMS_TO_TICKS(50));
    ESP_LOGI(TAG, "MPU6050 已唤醒");
}

// 读一次原始加速度 + 角速度（转成物理量：g 和 度/秒）
static void mpu_read_motion(float *ay, float *az, float *gx) {
    uint8_t d[14];
    mpu_read(0x3B, d, 14);            // ACCEL_XOUT_H 起 14 字节
    int16_t ax_r = (d[0]<<8)|d[1];  (void)ax_r;
    int16_t ay_r = (d[2]<<8)|d[3];
    int16_t az_r = (d[4]<<8)|d[5];
    // d[6..7] 是温度，跳过
    int16_t gx_r = (d[8]<<8)|d[9];
    *ay = ay_r / 16384.0f;           // ±2g 量程灵敏度
    *az = az_r / 16384.0f;
    *gx = gx_r / 131.0f;             // ±250°/s 量程灵敏度
}

void app_main(void) {
    mpu_init();
    float pitch = 0.0f;
    int64_t lastT = esp_timer_get_time();

    while (1) {
        float ay, az, gx;
        mpu_read_motion(&ay, &az, &gx);

        int64_t now = esp_timer_get_time();
        float dt = (now - lastT) / 1e6f;   // 秒
        lastT = now;

        // 加速度计直接算角度；陀螺仪积分。互补滤波融合：
        float accAngle = atan2f(ay, az) * 57.2958f;         // 弧度转度
        pitch = 0.98f * (pitch + gx * dt) + 0.02f * accAngle;

        ESP_LOGI(TAG, "pitch = %.2f", pitch);
        vTaskDelay(pdMS_TO_TICKS(5));       // 约 200Hz
    }
}

烧进去 idf.py build flash monitor。你应该看到：把车手扶到正直立时，串口打印的 pitch 接近某个固定值（不一定是 0，取决于 MPU6050 装的角度）；你慢慢前后倾车身，pitch 平滑地跟着变、方向对得上，松手静止时读数不乱漂。记下直立时 pitch 的真实值，这就是下一步的零点 targetAngle。

为什么互补滤波是 0.98 × 陀螺 + 0.02 × 加速度？陀螺仪积分项占 98% 保证平滑，加速度计项占 2% 每帧悄悄把漂移往真值拉回来。这一行就够把车立住，别一上来折腾卡尔曼滤波——对入门小车，互补滤波性价比无人能及。原理详见 robot-self-balance 的传感器融合一节。

步 2：让电机能按「带符号的力」正反转调速

角度读准了，接下来把电机接进来，先不平衡、只验证一件事：给一个带正负号的数，电机能对应正转/反转、大小对应转速。 这一步把 TB6612 的方向脚 + LEDC PWM 封成一个 drive_motors(float out)：

#include "driver/gpio.h"
#include "driver/ledc.h"

// 左电机
#define AIN1 GPIO_NUM_4
#define AIN2 GPIO_NUM_5
#define PWMA GPIO_NUM_6
// 右电机
#define BIN1 GPIO_NUM_7
#define BIN2 GPIO_NUM_15
#define PWMB GPIO_NUM_16
#define STBY GPIO_NUM_17               // 使能脚，拉高

#define LEDC_MODE   LEDC_LOW_SPEED_MODE  // S3 只有低速这组
#define LEDC_TIMER  LEDC_TIMER_0
#define CH_A        LEDC_CHANNEL_0
#define CH_B        LEDC_CHANNEL_1
#define PWM_MAX     255                  // 8 位分辨率

static void motor_init(void) {
    // 方向脚 + STBY 设为普通输出
    gpio_config_t io = {
        .pin_bit_mask = (1ULL<<AIN1)|(1ULL<<AIN2)|(1ULL<<BIN1)|(1ULL<<BIN2)|(1ULL<<STBY),
        .mode = GPIO_MODE_OUTPUT,
    };
    gpio_config(&io);
    gpio_set_level(STBY, 1);            // 使能 TB6612，忘了这句电机不转

    // 两路 PWM
    ledc_timer_config_t t = {
        .speed_mode = LEDC_MODE, .duty_resolution = LEDC_TIMER_8_BIT,
        .timer_num = LEDC_TIMER, .freq_hz = 20000,   // 20kHz，超声频段听不到啸叫
        .clk_cfg = LEDC_AUTO_CLK,
    };
    ESP_ERROR_CHECK(ledc_timer_config(&t));
    ledc_channel_config_t ca = { .gpio_num=PWMA, .speed_mode=LEDC_MODE, .channel=CH_A,
        .timer_sel=LEDC_TIMER, .duty=0, .hpoint=0 };
    ledc_channel_config_t cb = { .gpio_num=PWMB, .speed_mode=LEDC_MODE, .channel=CH_B,
        .timer_sel=LEDC_TIMER, .duty=0, .hpoint=0 };
    ESP_ERROR_CHECK(ledc_channel_config(&ca));
    ESP_ERROR_CHECK(ledc_channel_config(&cb));
}

// 两个电机同向驱动。out 正=前、负=后，大小=力度(0~255)
static void drive_motors(float out) {
    if (out > PWM_MAX)  out = PWM_MAX;
    if (out < -PWM_MAX) out = -PWM_MAX;
    int pwm = (int)fabsf(out);
    int fwd = (out >= 0) ? 1 : 0;

    // 左：IN1/IN2 一高一低定方向
    gpio_set_level(AIN1, fwd);  gpio_set_level(AIN2, !fwd);
    ledc_set_duty(LEDC_MODE, CH_A, pwm);
    ledc_update_duty(LEDC_MODE, CH_A);   // set 完必须 update 才生效
    // 右
    gpio_set_level(BIN1, fwd);  gpio_set_level(BIN2, !fwd);
    ledc_set_duty(LEDC_MODE, CH_B, pwm);
    ledc_update_duty(LEDC_MODE, CH_B);
}

写个临时的 app_main 单测它（此时务必把车架空、轮子悬空）：

void app_main(void) {
    motor_init();
    while (1) {
        drive_motors(150);   vTaskDelay(pdMS_TO_TICKS(800));  // 正转
        drive_motors(-150);  vTaskDelay(pdMS_TO_TICKS(800));  // 反转
        drive_motors(0);     vTaskDelay(pdMS_TO_TICKS(400));  // 停
    }
}

你应该看到：架空的两个轮子同向正转 0.8 秒、反转 0.8 秒、停一下，循环。重点确认两个轮子转向一致（都往车头方向、或都往车尾方向）——若有一个反了，把那个电机的两根线对调，或代码里把它的 fwd/!fwd 互换。这一步把「电机方向 + 转速」这个最容易接反的地方隔离验证掉，等接进 PID 就不用再怀疑硬件。

步 3：接上 PID 闭环，让车自己站住

最后一步，把前两步拼起来：每一轮循环，读 pitch → 算它离直立差多少 → PID 换算出该给电机多大力 → drive_motors 驱动。 车往哪边倒，轮子就往哪边加速，用车身反冲接住头顶重心。

// ==== PID 参数：这是你唯一要反复调的三个数 ====
static float Kp = 22.0f;   // 先从这个量级试，下面讲怎么调
static float Ki = 0.0f;    // 入门先不开 I，极易积分饱和窜车
static float Kd = 0.8f;

static float targetAngle = 0.0f;   // 直立零点：填步 1 记下的真实 pitch

void app_main(void) {
    mpu_init();
    motor_init();

    float pitch = 0.0f;
    int64_t lastT = esp_timer_get_time();

    while (1) {
        // 1) 读姿态 + 互补滤波（同步 1）
        float ay, az, gx;
        mpu_read_motion(&ay, &az, &gx);
        int64_t now = esp_timer_get_time();
        float dt = (now - lastT) / 1e6f;
        lastT = now;
        float accAngle = atan2f(ay, az) * 57.2958f;
        pitch = 0.98f * (pitch + gx * dt) + 0.02f * accAngle;

        // 2) 角度环 PID
        float error = targetAngle - pitch;
        // D 项直接用陀螺仪角速度 -gx（本身就是干净的角速度，省一次会放大噪声的求导）
        float output = Kp * error + Kd * (-gx);

        // 3) 倒得太狠就放弃（比如被拿起来），松力保护，防止狂转
        if (fabsf(error) > 45.0f) {
            drive_motors(0);
        } else {
            drive_motors(output);
        }

        vTaskDelay(pdMS_TO_TICKS(5));   // 约 200Hz 控制周期
    }
}

你应该看到（架空调试通过、参数大致对之后）：把车轻轻放地上松手，它在原地站住，轮子来回做几毫米快速微调；你用手指轻推车身上沿，它朝被推方向冲几厘米再晃回直立。

到这里核心全齐了。回头看你没写多少新东西：MPU6050 读取是 /sensor/mpu6050/ 的、电机驱动是 l2-motor 的、PID 是 robot-pid 的——你干的是把它们用一个 200Hz 控制循环组织起来。这个「组织」，就是 project 比 guide 多出来的那层功夫。

**几个直接给你结论的设计选择：**D 项用陀螺仪 -gx 而非误差差分——陀螺仪本身就干净地测角速度，省掉一次放大噪声的求导，这是自平衡的标准做法；I 项入门设 0——积分在小车上极易饱和，一饱和就突然全速窜出去，先把 PD 调好能站住再说；|error|>45° 的松力保护——车被拿起或彻底倒地时别让电机疯转，既保护硬件也保护你的手。

---

第四步：调试——对不上就查这张表

分步烧的好处是哪一步出问题范围已经缩小。调参的正确顺序是单变量、分阶段，一步都别跳：

1. 先让车读准角度（步 1 已做）：串口看 pitch，手扶直立时稳定、前后倾时平滑跟随，把直立真实值填进 targetAngle。这一步不过关后面全白搭。
2. 单独调 Kp：Ki=0、Kd=0，从小往大加。太小车软绵绵倒下去、扶不动；太大车高频剧烈抖动（过冲）。加到「开始明显抖、但还能勉强维持」那个值附近，这是临界 P。
3. 加 Kd 压抖：保持 Kp，Kd 从 0 慢慢加。加对了，刚才的抖动肉眼可见地平息，车从「哆嗦」变「沉稳站着」。太大反而引入新的高频噪声抖动，往回收一点。
4. 微调零点：车总往固定一边缓慢漂，多半是 targetAngle 没校准准，微调它。还有稳态偏差再加极小的 Ki（0.05 起）。

真出岔子照这张表查：

现象	最可能的原因	怎么办
一给电机上力板子就重启	电机从开发板取电导致 brownout	电机必须走电池→TB6612 的 VM，只与板子共 GND（见开头 safety）
电机完全不转	STBY 没拉高 / VM 没接电池 / 三者没共地	确认 STBY=1、VM 接电池正、板子和电池 GND 连一起
一个轮子转向反了、车原地打转	那个电机两根线接反	对调该电机两根线，或代码里互换它的 fwd/!fwd
直接倒地、电机像没力	Kp 太小，或电机/电源功率不够、L298N 压降吃电压	加大 Kp；换 TB6612、电压拉到电机额定
站着但高频剧烈抖动	Kp 太大过冲，或 Kd 太小	减小 Kp、加大 Kd
总往固定一边缓慢窜	targetAngle 零点没校准准	串口看直立真实 pitch，回填零点
pitch 读数缓慢漂移	互补滤波系数不对 / 陀螺仪零偏	加大加速度计权重（0.98→0.97）；开机静止时采零偏均值再补偿
一动就突然全速窜出去	I 项积分饱和	先把 Ki 设 0；要用 I 必须给积分项加限幅（见 robot-pid）
方向完全相反、越扶越倒	电机接线或 pitch 符号反了	把 output 加负号，或对调左右电机方向
板子刷不进 / 一直重启	信号脚踩了 strapping（0/3/45/46）或 flash 区（26-37）	对照第二步雷区表换脚
串口没日志 / 一连就断	用了 GPIO19/20（USB 脚）当信号脚	换脚，这两根是 USB D-/D+

---

第五步：从「能站住」做到「能遥控着走」

车能站住只是起点。「站住」和「一辆好玩的车」之间还差几步，正好通向后面的阶梯，先给你指条路。

加前进后退与转向

现在的车只会原地站着。要让它走，思路是串级控制：外层加速度环（目标速度→算出一个很小的目标倾角），内层还是本篇的角度环。想前进就给一个小前倾目标角，车为了「接住」前倾重心会持续往前滚——倒立摆走路本质就是「可控地一直往前倒」；左右转向则在两轮 PWM 上叠加差速。这套串级做法 robot-self-balance 的变体讲得更细，那也是更深挖 PID/自平衡的下一站（属 卷 R R4 控制专题 的进阶方向）。

加蓝牙遥控

ESP32-S3 自带蓝牙：手机发指令改 targetAngle（控前进后退）和左右差速（控转向）。平衡环完全不动，遥控只在上面叠加偏置，互不干扰——这也是「基础控制稳了，功能往上叠」的典型思路，用 NimBLE 起一个自定义服务即可。

做成真正的成品

面包板上一团线只算原型。想变成能天天玩、甚至送人的东西，要走产品化：电机与逻辑电源做好隔离滤波、电路挪到洞洞板或画块小 PCB、配外壳把 MPU6050 固定牢（IMU 松动会让角度乱跳）、固件量产烧录。这些是 L5 阶梯 的活儿，手感稳了再来啃。

---

小结 · 你做出了什么、下一步去哪

• 你做出了一辆没有第三支撑、松手不倒、被推能自己晃回直立的两轮自平衡小车，从选型、独立供电接线、分步写可跑的 ESP-IDF 代码到调参救场，走完了一件成品的全流程。
• 你第一次把 MPU6050 读姿态（/sensor/mpu6050/）、TB6612 + LEDC 驱动电机（l2-motor）、PID 闭环（robot-pid）三件单练的事，用一个 200Hz 控制循环接成了一条完整链路——这个「组织零件」的功夫，就是 project 比 guide 多出来的那一层。
• 你学到了几个关键工程习惯：功率负载必须独立供电（brownout 的根源）、互补滤波融合两个各有缺陷的传感器、调参必须单变量分阶段、给失控状态加松力保护。

下一步：想让车不光站住还能遥控着跑，去啃 robot-self-balance 的串级控制，那是本篇的自然延伸；想系统补 PID 的调参理论，回看 robot-pid；想在这辆车上再叠视觉、语音、AI 决策，进阶成会看会听的 AI 机器人，从 机器人导览 起步看整条路线。回看全部实战项目见项目总览，更多机器人主题见 /robot/，传感器专题见 /sensor/。这辆车是你所有硬件项目里最值得留着的一个——它把「测量—决策—执行」这条控制链，第一次在你手里闭合成了实物。