树莓派Pico自平衡机器人：从MPU6050姿态解算到串级PID调参实战

1. 项目缘起与整体设计思路

1.1 为什么选择树莓派Pico做自平衡机器人

自平衡机器人这个品类在创客圈里算是经典中的经典，从早期的Arduino两轮平衡车到后来的STM32方案，几乎每一代玩嵌入式的朋友都会拿它练手。但真正让我决定用树莓派Pico重新做一版的，是三个很现实的理由。

第一是成本。Pico这块板子本身价格极低，核心是RP2040双核Cortex-M0+，主频133MHz，264KB SRAM，2MB Flash，对于自平衡这种需要实时姿态解算和电机控制的任务来说，性能完全够用。相比动辄几十上百的STM32开发板，Pico的性价比优势非常明显，尤其适合学生做毕设或者批量做教学套件。

第二是生态。Pico支持MicroPython和C/C++ SDK两套开发方式，MicroPython上手极快，几行代码就能读取MPU6050的原始数据；而C SDK又能让你深入到寄存器级别做优化。这种"上下通吃"的特性，让同一个硬件平台既能给小白练手，也能给老手做深度开发。

第三是开源。整个项目的硬件设计、固件代码、上位机调参工具全部开源，这意味着任何人拿到这份资料都能复现，也能在此基础上改。我见过太多"只发视频不发代码"的项目，那种东西对学习毫无帮助。这个项目从第一天就定了规矩：所有东西必须能跑通、能复现、能改。

提示：如果你之前只玩过Arduino，转到Pico最大的感受是"自由"。Arduino把很多东西封装好了，Pico则要求你理解底层，比如PWM的时钟分频、I2C的波特率计算，这些在Arduino里是隐藏的，在Pico里需要你自己算。这不是坏事，是学习的机会。

1.2 双轮自平衡的核心原理拆解

自平衡机器人的本质是一个倒立摆系统。你可以把它想象成一根筷子立在手掌上，手掌要不断移动来保持筷子不倒。机器人也一样，它通过轮子的前后移动来维持车身的直立。

这里面的核心逻辑是：当车身向前倾斜时，轮子要向前加速；当车身向后倾斜时，轮子要向后加速。听起来简单，但难点在于"什么时候加速、加速多少"。

整个控制链路是这样的：MPU6050读取加速度和角速度原始数据，经过互补滤波或卡尔曼滤波融合出俯仰角，然后PID控制器根据目标角度（通常是0度，即直立）和当前角度的偏差计算出PWM占空比，最后驱动电机。同时，编码器反馈轮速，形成速度环，防止机器人越跑越快。

为什么需要速度环？因为只有角度环的话，机器人虽然能保持不倒，但会一直往一个方向跑。速度环的作用是让轮子的平均速度趋近于零，这样机器人就能原地保持平衡。

1.3 硬件选型与整体架构

整个项目的硬件清单如下：

选TB6612而不是L298N，是因为TB6612的导通电阻更小，发热低，效率高，而且支持3.3V逻辑电平直接驱动，不需要电平转换。N20电机带编码器是为了做速度闭环，霍尔编码器输出AB相脉冲，Pico的PIO可以轻松解码。

整体架构上，Pico负责所有实时任务：I2C读取MPU6050、PIO读取编码器、PWM输出驱动电机、PID运算。没有上位机参与实时控制，所有闭环都在Pico内部完成。上位机只用于调参和数据显示，通过串口通信。

2. 核心细节解析与实操要点

2.1 MPU6050数据读取与姿态解算

MPU6050通过I2C接口与Pico通信，默认地址0x68。初始化时需要配置几个关键寄存器：电源管理寄存器1（0x6B）要写0x00唤醒设备，配置寄存器（0x1A）设置数字低通滤波器，陀螺仪配置寄存器（0x1B）设置量程，加速度计配置寄存器（0x1C）设置量程。

我一般把陀螺仪量程设为±500°/s，加速度计量程设为±4g。为什么？因为自平衡机器人倾斜角度不会超过±30°，角速度也不会超过300°/s，这个量程既有足够的精度，又不会轻易饱和。

读取原始数据后，需要做姿态解算。最简单的是互补滤波：

angle = 0.98 * (angle + gyro_y * dt) + 0.02 * accel_angle

这个公式的意思是：陀螺仪积分得到角度，但会漂移；加速度计计算角度，但噪声大。互补滤波用陀螺仪的短期精度和加速度计的长期稳定性，加权融合。0.98和0.02这两个系数是经验值，实际调试时可以根据情况微调。

注意：MPU6050的陀螺仪零偏需要校准。上电后保持机器人静止，采集几百个样本求平均值，作为零偏补偿。不做这一步，机器人会一直往一个方向漂。

2.2 PID控制器的参数整定

PID是自平衡的核心。我用的是串级PID：外环是角度环，内环是速度环。

角度环的输入是目标角度（0度）和当前角度的偏差，输出是目标速度。速度环的输入是目标速度和编码器反馈的实际速度，输出是PWM占空比。

参数整定的顺序是：先调角度环的P和D，再调速度环的P和I。

角度环的P决定响应速度，P太小机器人站不起来，P太大机器人会高频抖动。D决定阻尼，D太小机器人会来回晃，D太大机器人会反应迟钝。我的经验值是P在15-25之间，D在0.5-1.5之间，具体看电机和车体重量。

速度环的P决定速度跟踪的快慢，I消除稳态误差。速度环的参数一般比角度环小一个数量级，P在0.5-2之间，I在0.01-0.1之间。

// 角度环 angle_error = target_angle - current_angle; angle_output = angle_kp * angle_error + angle_kd * (angle_error - last_angle_error); last_angle_error = angle_error; // 速度环 speed_error = angle_output - current_speed; speed_output = speed_kp * speed_error + speed_ki * speed_integral; speed_integral += speed_error;

提示：调参时先把速度环的I设为0，只调P，让机器人能基本保持不倒但会缓慢移动。然后再加I，让机器人能停住。最后微调角度环的D，让机器人更稳定。

2.3 电机驱动与编码器读取

TB6612的驱动逻辑很简单：每个电机有两个方向引脚（AIN1/AIN2）和一个PWM引脚（PWMA）。AIN1和AIN2决定转向，PWMA决定速度。

Pico的PWM频率我设为20kHz，这个频率高于人耳听觉范围，电机不会发出啸叫声。PWM分辨率为16位，但实际有效位数取决于时钟分频。RP2040的PWM时钟是125MHz，分频后得到20kHz，分辨率约为12位，足够精细。

编码器读取用PIO（可编程I/O）实现。PIO是RP2040的独门绝技，可以硬件级解码正交编码器信号，不占用CPU资源。我写了一个简单的PIO程序，检测AB相的上升沿和下降沿，输出计数和方向。

# PIO编码器读取示例 @rp2.asm_pio() def encoder(): wrap_target() wait(1, pin, 0) # 等待A相上升沿 jmp(pin, "forward") # 如果B相为高，正转 jmp("backward") # 否则反转 label("forward") jmp(x_dec, "forward") # 计数减一 label("backward") jmp(x_inc, "backward") # 计数加一 wrap()

注意：编码器的计数方向要和电机转向对应。如果发现速度环正反馈（机器人越跑越快），把编码器计数取反即可。

3. 实操过程与核心环节实现

3.1 硬件组装与接线

组装顺序很重要，我建议先搭电路再装车体，这样调试方便。

第一步，把Pico插到面包板上，连接MPU6050。MPU6050的VCC接3.3V，GND接GND，SCL接Pico的GP1，SDA接GP0。注意MPU6050是3.3V器件，不要接5V。

第二步，连接TB6612。TB6612的VCC接5V（逻辑电源），VM接7.4V（电机电源），GND共地。AIN1/AIN2接GP2/GP3，BIN1/BIN2接GP4/GP5，PWMA接GP6，PWMB接GP7。STBY接5V使能。

第三步，连接编码器。左编码器A相接GP8，B相接GP9；右编码器A相接GP10，B相接GP11。

第四步，连接电源。2节18650串联得到7.4V，一路直接给TB6612的VM，另一路经过LM2596降压到5V给Pico和TB6612的逻辑电源。

提示：电机电源和逻辑电源一定要分开走线，电机启动瞬间电流很大，会拉低电压导致Pico复位。我在VM和GND之间并了一个1000uF的电解电容，效果很明显。

3.2 固件烧录与基础测试

Pico的固件烧录很简单：按住BOOTSEL键，插USB，电脑会出现一个U盘，把UF2文件拖进去就行。

我建议先烧MicroPython固件，用Thonny IDE写几个测试脚本，确认每个模块都能工作。

测试MPU6050：

from machine import I2C, Pin i2c = I2C(0, scl=Pin(1), sda=Pin(0), freq=400000) print(i2c.scan()) # 应该输出[104]，即0x68

测试电机：

from machine import Pin, PWM pwm = PWM(Pin(6)) pwm.freq(20000) pwm.duty_u16(30000) # 约50%占空比

测试编码器：

from machine import Pin import time enc_a = Pin(8, Pin.IN, Pin.PULL_UP) enc_b = Pin(9, Pin.IN, Pin.PULL_UP) count = 0 def on_a(pin): global count if enc_b.value(): count += 1 else: count -= 1 enc_a.irq(trigger=Pin.IRQ_RISING, handler=on_a) time.sleep(1) print(count)

每个模块单独测试通过后，再整合到一起。

3.3 控制循环的实时性优化

自平衡对实时性要求很高，控制周期必须稳定。我用的控制周期是5ms，即200Hz。

在MicroPython里，用time.ticks_ms()做定时，但MicroPython的GC（垃圾回收）会导致周期抖动。解决办法是：控制循环里不分配新对象，所有变量预分配；关闭GC或者手动GC。

import gc gc.disable() # 控制循环期间关闭GC

如果对实时性要求更高，建议用C SDK重写控制循环。C SDK没有GC，中断响应也更快。我的最终版本是用C写的，控制周期稳定在5ms，抖动小于0.1ms。

注意：I2C读取MPU6050需要约1ms，如果控制周期是5ms，留给PID运算的时间只有4ms。RP2040的133MHz主频做浮点PID运算绰绰有余，但要注意不要用double，用float就够了。

3.4 上位机调参工具的使用

调参是自平衡项目最耗时的环节。我写了一个简单的Python上位机，通过串口接收Pico发送的角度、速度、PWM数据，实时绘制曲线。

上位机用pyserial读串口，用matplotlib绘图。Pico每5ms发送一帧数据，格式是angle,speed,pwm\n。上位机解析后更新曲线。

调参时，先看角度曲线。如果角度曲线是等幅振荡，说明P太大或D太小；如果角度曲线是发散振荡，说明P太大且D太小；如果角度曲线是缓慢收敛，说明参数基本合适。

速度曲线用来观察速度环的效果。如果速度曲线在零附近小幅波动，说明速度环工作正常；如果速度曲线持续偏向一边，说明速度环的I不够。

提示：调参时把机器人用支架架起来，轮子悬空，这样即使参数不对也不会摔坏。等参数基本合适了再放到地上跑。

4. 常见问题与排查技巧实录

4.1 机器人站不起来或立即倒下

这是最常见的问题，原因通常有三个。

第一，角度方向反了。检查MPU6050的安装方向，确保机器人前倾时角度为正。如果反了，把角度取反即可。

第二，电机转向反了。检查TB6612的接线，确保机器人前倾时轮子向前转。如果反了，把电机的两根线对调，或者把PWM取反。

第三，PID参数太保守。如果P太小，机器人会软绵绵地倒下。先把P调到20左右，D调到1左右，再微调。

4.2 机器人高频抖动

高频抖动通常是D太大或P太大。D的作用是阻尼，但D太大会放大陀螺仪的噪声，导致抖动。解决办法是降低D，或者在陀螺仪数据上加低通滤波。

我在MPU6050的配置寄存器里设置了DLPF（数字低通滤波器），带宽设为20Hz。这样能滤掉大部分高频噪声，同时不影响姿态解算的实时性。

4.3 机器人缓慢漂移

缓慢漂移通常是陀螺仪零偏没校准，或者速度环的I太小。先重新校准陀螺仪零偏，如果还有漂移，增大速度环的I。

速度环的I的作用是消除稳态误差。如果I太小，机器人会一直往一个方向缓慢移动；如果I太大，机器人会低频振荡。

4.4 常见问题速查表

提示：排查问题时，先用上位机看曲线，不要盲目改参数。曲线能告诉你问题出在角度环还是速度环，能省很多时间。

4.5 独家避坑经验

第一个坑：MPU6050的I2C上拉电阻。很多模块自带4.7k上拉，但Pico的内部上拉只有几十k，如果模块没带上拉，I2C通信会不稳定。解决办法是在SCL和SDA上各并一个4.7k电阻到3.3V。

第二个坑：电机的PWM频率。如果PWM频率太低（比如1kHz），电机会发出刺耳的啸叫声，而且低速时转矩脉动大。20kHz是甜点频率，高于听觉范围，转矩也平滑。

第三个坑：电池电压。18650电池满电是4.2V，两节串联是8.4V，但放电到3.0V时只有6.0V。如果电机驱动没有稳压，电压下降会导致PWM有效值下降，机器人性能变差。我在代码里加了电压补偿，根据电池电压调整PWM上限。

第四个坑：车体重心。重心越高，机器人越难平衡。我把电池放在车体底部，MPU6050放在重心正上方，这样姿态解算最准确。

第五个坑：轮子打滑。65mm橡胶轮在光滑地板上会打滑，导致编码器计数和实际位移不符。解决办法是在轮子上缠一圈电工胶带，增加摩擦力。

5. 开源资料与二次开发建议

5.1 开源文件结构说明

整个项目的开源资料分为四个目录：

hardware/目录下是3D打印的STL文件和电路原理图。STL文件包括车体底板、电机支架、电池仓、MPU6050支架。原理图用KiCad绘制，包含Pico、MPU6050、TB6612、编码器接口、电源部分。

firmware/目录下是Pico的固件代码。分为MicroPython版本和C SDK版本。MicroPython版本适合快速验证，C SDK版本适合最终产品。

software/目录下是上位机调参工具。用Python编写，依赖pyserial和matplotlib。

docs/目录下是组装说明和调参指南。组装说明有详细的图文步骤，调参指南有参数整定的流程图和示例曲线。

5.2 二次开发方向

这个项目的基础框架可以扩展出很多玩法。

第一个方向是加摄像头。Pico没有摄像头接口，但可以加一个ESP32-CAM模块，通过串口把图像数据传给Pico，做简单的视觉巡线或目标跟踪。

第二个方向是加OLED屏幕。用I2C接口的0.96寸OLED，实时显示角度、速度、PID参数，方便现场调试。

第三个方向是加蓝牙模块。用HC-05或HC-06，通过串口与手机通信，用手机APP调参和显示数据。

第四个方向是加ROS。Pico通过串口与树莓派4B通信，树莓派4B跑ROS2，做SLAM和导航。这个方向适合做毕设或者科研项目。

提示：二次开发时，建议先保持基础功能稳定，再加新功能。我见过太多人一上来就加一堆模块，结果基础平衡都没调好，最后不了了之。

5.3 代码贡献与社区协作

这个项目采用MIT许可证，意味着你可以自由使用、修改、分发，甚至商用。但如果你改了代码，建议把改进提交回来，让更多人受益。

提交代码时，注意几点：代码风格要统一，变量命名要清晰，关键部分要有注释。如果是新功能，先在issue里讨论，避免做了半天发现方向不对。

我在实际维护这个项目的过程中发现，最有价值的贡献往往不是代码，而是文档。很多人愿意写代码，但不愿意写文档。如果你能补充一份详细的组装教程或者调参案例，对社区的帮助比写几百行代码还大。

5.4 从Pico到RP2040的移植

如果你想把项目移植到其他RP2040开发板上，比如Pico W或者第三方的RP2040板子，需要注意几点。

Pico W的无线功能会占用一些GPIO，但I2C、PWM、PIO的引脚是兼容的。移植时只需要改引脚定义。

第三方RP2040板子的Flash大小可能不同，Pico是2MB，有些板子是4MB或8MB。如果代码里用了文件系统，需要调整分区表。

Pico W的WiFi功能可以用来做无线调参，通过TCP socket发送数据，比串口方便。但WiFi会引入额外的功耗和干扰，如果对实时性要求高，建议还是用串口。

我个人在实际操作中的体会是，自平衡机器人这个项目，硬件是骨架，PID是灵魂，调参是修行。同样的硬件，不同的人调出来的效果天差地别。我见过有人用几十块钱的电机做出稳稳当当的平衡车，也见过有人用几百块钱的电机做出摇摇晃晃的"醉汉"。差别不在硬件，在对控制原理的理解和对参数的耐心。

最后再分享一个小技巧：调参时把机器人的轮子悬空，用手轻轻拨动轮子，感受电机的响应。如果电机响应迅速且平滑，说明参数基本合适；如果电机响应迟钝或者抖动，说明参数还需要调。这个"手感"比看曲线更直观，练多了就能快速判断参数好坏。