1. 项目缘起与整体设计思路1.1 为什么选择树莓派Pico做自平衡机器人自平衡机器人这个品类在嵌入式圈子里算是经典练手项目了。我最早接触这类项目是在大学期间当时用的是某款8位单片机光是一个互补滤波就把我折腾了整整一周。后来换到STM32PID调参又调了半个月车体还是抖得像筛糠。直到RP2040这颗芯片出来我才发现原来做自平衡机器人可以这么省心。树莓派Pico搭载的RP2040是双核Cortex-M0主频133MHz关键是它的PIO可编程I/O模块和硬件浮点运算能力对于自平衡这种需要高频姿态解算的场景来说简直是量身定做。我实测下来用Pico跑200Hz的PID控制循环CPU占用率不到15%还有大量余量可以做蓝牙通信、OLED显示这些附加功能。这个项目最核心的价值在于全开源。市面上很多自平衡机器人的教程要么代码不开源要么硬件设计藏着掖着要么就是用了很贵的专用模块。我这个项目从PCB设计、3D打印结构件、到固件代码全部开源总物料成本控制在150元以内适合学生做毕设、也适合爱好者入门嵌入式控制。1.2 系统架构与核心模块选型整个系统我把它拆成四个层次来理解这样调试的时候思路会清晰很多感知层MPU6050六轴传感器负责采集加速度和角速度数据。选它不是因为性能多强而是便宜、资料多、I2C接口简单。注意要买正品模块我踩过坑某宝上几块钱的MPU6050零漂能到0.5度/秒根本没法用。控制层树莓派Pico作为主控负责姿态解算、PID运算、电机控制信号输出。Pico的雙核架构让我可以把姿态解算和电机控制分到两个核心上跑互不干扰。执行层两个带编码器的直流减速电机配合TB6612电机驱动模块。编码器用来测速形成速度闭环这是让机器人真正站稳而不是原地抖的关键。交互层0.96寸OLED显示实时姿态角、PID参数、电池电压蓝牙模块用于无线调参。调参的时候不用反复插拔USB线这个体验提升是巨大的。提示电机选型时注意减速比。我推荐1:30到1:48之间的减速比扭矩够用又不至于响应太慢。减速比太小站不起来太大则响应迟钝。1.3 开源方案的设计取舍做这个项目的时候我给自己定了三条原则第一所有元器件必须能在国内电商平台轻松买到第二代码必须能在Pico官方SDK上直接编译不依赖任何冷门库第三机械结构必须能用普通FDM 3D打印机打出来不需要特殊支撑。基于这三条原则我放弃了用无刷电机的方案驱动复杂、成本高也放弃了用ROS的方案对Pico来说太重了。最终选择了Pico MPU6050 TB6612 带编码器直流电机这个经典组合。这个组合的好处是每个模块都有大量现成资料出了问题容易排查。代码架构上我没有用RTOS而是用了Pico SDK自带的定时器中断来驱动控制循环。这样做的好处是代码结构简单初学者容易看懂。整个固件大概800行C代码分成传感器驱动、姿态解算、PID控制、电机驱动、通信协议五个模块每个模块都有详细注释。2. 核心细节解析与实操要点2.1 姿态解算从原始数据到稳定角度MPU6050输出的原始数据是不能直接用的。加速度计的数据噪声大但长期稳定陀螺仪的数据短期精确但会漂移。把两者融合起来才能得到既响应快又稳定的姿态角。我试过三种融合方案互补滤波、卡尔曼滤波、Mahony滤波。最终选了互补滤波原因很简单——在Pico上跑互补滤波的计算量最小效果却足够好。卡尔曼滤波虽然理论最优但参数调节太玄学不适合开源项目别人拿到你的代码调不出一样的效果就会觉得你藏私。互补滤波的核心公式是这样的// 互补滤波系数通常取0.98左右 #define ALPHA 0.98f // angle: 融合后的角度 // gyro_rate: 陀螺仪角速度度/秒 // accel_angle: 加速度计解算的角度 // dt: 采样周期秒 angle ALPHA * (angle gyro_rate * dt) (1 - ALPHA) * accel_angle;这个公式的物理意义很直观主要相信陀螺仪的积分因为它响应快但用加速度计的角度来慢慢修正陀螺仪的漂移。ALPHA越大越信任陀螺仪响应越快但漂移修正越慢ALPHA越小越信任加速度计角度越稳但响应越迟钝。我实测下来ALPHA取0.98、采样周期5ms200Hz的时候角度输出非常平滑响应延迟在可接受范围内。注意加速度计解算角度时要用atan2函数不要用asin否则在接近90度的时候会出问题。注意MPU6050上电后需要一段时间稳定建议初始化后延时100ms再开始读取数据。另外传感器安装时要尽量靠近车体旋转中心减少振动带来的噪声。2.2 PID控制三个参数到底怎么调PID是自平衡机器人的灵魂。我见过太多人卡在调参这一步最后放弃。这里我把我的调参经验完整分享出来。先调P比例项把I和D都设为0P从0开始慢慢加。加到什么程度加到机器人开始来回摆动但不会倒下。这时候P大概在20到40之间具体取决于电机和车体重量。注意观察摆动的频率如果摆动很慢说明P太小如果摆动很快很剧烈说明P太大了。再调D微分项D的作用是刹车抑制摆动。从0开始加加到机器人摆动明显减小但还没有完全稳定。D通常在P的1到3倍之间。D太大会引入高频抖动这时候你会听到电机发出滋滋的声音说明D过头了。最后调I积分项I的作用是消除稳态误差。如果机器人总是往一个方向偏就需要加I。I从0开始每次加一点点0.01级别加到机器人能稳定在一个位置。I太大会导致低频振荡机器人会慢慢来回晃。调参顺序不能乱。我试过先调D结果机器人直接疯掉。一定要按P→D→I的顺序来。参数作用典型范围调过头的表现P响应倾斜角度20-40高频摆动D抑制摆动30-80电机滋滋响、高频抖动I消除稳态误差0.01-0.1低频来回晃除了角度环我还加了速度环。速度环的输入是编码器测到的轮速输出叠加到角度环的目标角度上。这样机器人不仅能站稳还能抵抗外力推动。速度环的PID参数比角度环小一个数量级P大概在0.5到2之间。2.3 电机驱动与编码器读取TB6612是我用过最顺手的直流电机驱动模块。它比L298N效率高、发热小而且支持PWM频率高达100kHz。我用的是20kHz的PWM频率超出人耳听觉范围电机运行起来很安静。编码器读取我用的是Pico的PIO模块。PIO是RP2040的杀手锏功能可以硬件级地处理编码器脉冲完全不占CPU。我写了一个简单的PIO程序四倍频计数分辨率足够高。// 编码器PIO程序简化版 .program encoder wait 1 pin 0 in pins, 2 wait 0 pin 0 in pins, 2这段PIO代码的意思是等待A相上升沿读取AB两相状态等待A相下降沿再读取AB两相状态。通过判断状态变化顺序就能知道旋转方向。PIO的好处是这些操作完全由硬件完成CPU只需要定期读取计数器就行。电机驱动要注意死区问题。PWM占空比低于某个值大概5%的时候电机是不转的。所以在PID输出到PWM的映射里要加一个死区补偿。我的做法是如果PID输出绝对值小于死区阈值直接输出0否则把输出映射到死区阈值到最大值之间。提示电机正负极不要接反否则PID会变成正反馈机器人会瞬间飞出去。第一次测试时建议把机器人悬空确认电机转向正确后再放到地面上。3. 实操过程与核心环节实现3.1 硬件搭建从零到能跑硬件搭建我分成五步走每一步都有明确的验证标准不要跳步。第一步焊接Pico底板。我用的是自己画的PCB把Pico、MPU6050、TB6612、OLED接口、蓝牙接口都集成在一块板子上。如果你不想画板用杜邦线连接也可以但要注意线要短、要固定好否则振动会导致接触不良。焊接完成后先烧录一个点灯程序确认Pico工作正常。第二步连接MPU6050。MPU6050用I2C接口接Pico的GP4SDA和GP5SCL。注意MPU6050的供电是3.3V不要接5V。连接好后烧录一个I2C扫描程序确认能读到0x68地址。然后读取WHO_AM_I寄存器应该返回0x68。第三步连接TB6612和电机。TB6612的PWMA、AIN1、AIN2接Pico的GP6、GP7、GP8PWMB、BIN1、BIN2接GP9、GP10、GP11。STBY接3.3V。电机接到TB6612的A01、A02和B01、B02。先不装轮子烧录一个电机测试程序确认两个电机都能正反转。第四步安装编码器。编码器的A、B相接Pico的GP12、GP13电机A和GP14、GP15电机B。烧录编码器测试程序手动转动电机轴确认计数正常增加和减少。第五步组装车体。把电机固定到3D打印的底板上装上轮子把PCB和电池固定好。电池我推荐用2节18650锂电池串联电压7.4V容量2000mAh以上能连续跑2小时左右。模块Pico引脚说明MPU6050 SDAGP4I2C数据MPU6050 SCLGP5I2C时钟TB6612 PWMAGP6电机A调速TB6612 AIN1GP7电机A方向TB6612 AIN2GP8电机A方向TB6612 PWMBGP9电机B调速TB6612 BIN1GP10电机B方向TB6612 BIN2GP11电机B方向编码器A A相GP12电机A编码器编码器A B相GP13电机A编码器编码器B A相GP14电机B编码器编码器B B相GP15电机B编码器3.2 固件开发代码结构与关键实现固件我分成五个文件main.c、mpu6050.c、pid.c、motor.c、encoder.c。每个文件职责单一方便调试。main.c是主循环用定时器中断每5ms执行一次控制逻辑。控制逻辑的顺序很重要先读传感器再算姿态再算PID最后输出电机。这个顺序不能乱否则会用旧数据算PID。// 5ms定时器中断回调 bool control_loop(struct repeating_timer *t) { // 1. 读取MPU6050原始数据 mpu6050_read_raw(accel, gyro); // 2. 姿态解算 float angle complementary_filter(accel, gyro, 0.005f); // 3. 读取编码器速度 float speed encoder_get_speed(); // 4. 速度环PID外环 float target_angle speed_pid_calc(speed); // 5. 角度环PID内环 float output angle_pid_calc(angle, target_angle); // 6. 输出到电机 motor_set(output); return true; }pid.c里我实现了一个通用的PID结构体支持积分限幅和输出限幅。积分限幅很重要否则机器人被按住的时候积分项会累积到很大一松手就飞出去。typedef struct { float kp, ki, kd; float integral; float prev_error; float integral_limit; float output_limit; } pid_t; float pid_calc(pid_t *pid, float error) { // 积分项 pid-integral error; // 积分限幅 if (pid-integral pid-integral_limit) pid-integral pid-integral_limit; if (pid-integral -pid-integral_limit) pid-integral -pid-integral_limit; // 微分项 float derivative error - pid-prev_error; pid-prev_error error; // 输出 float output pid-kp * error pid-ki * pid-integral pid-kd * derivative; // 输出限幅 if (output pid-output_limit) output pid-output_limit; if (output -pid-output_limit) output -pid-output_limit; return output; }motor.c里要注意PWM初始化和死区补偿。Pico的PWM频率我设为20kHz分辨率设为1000级。死区补偿的做法是如果PID输出绝对值小于50对应5%占空比直接输出0否则把输出从50到1000映射到0到1000。3.3 蓝牙调参不用反复插拔USB调参的时候如果每次都要重新编译烧录效率太低了。我加了一个HC-05蓝牙模块通过串口和Pico通信。手机上下载一个蓝牙串口APP就能实时修改PID参数。通信协议我设计得很简单每个参数用两个字节表示第一个字节是参数编号第二个字节是参数值。比如0x01表示P参数0x02表示D参数0x03表示I参数。Pico收到数据后更新对应的PID参数并返回确认。// 蓝牙数据处理 void bluetooth_handler(uint8_t *data, uint8_t len) { if (len 2) return; uint8_t param_id data[0]; uint8_t value data[1]; switch (param_id) { case 0x01: angle_pid.kp value; break; case 0x02: angle_pid.kd value; break; case 0x03: angle_pid.ki value / 100.0f; break; default: break; } // 返回确认 uart_putc(param_id); uart_putc(value); }OLED显示我用了u8g2库显示内容有当前角度、目标角度、PID输出、电池电压。调参的时候看着OLED上的实时数据比盲调效率高很多。提示蓝牙模块的供电要注意HC-05工作电流大概40mA直接从Pico的3.3V取电可能会不稳定。建议单独用一个LDO供电或者从电池经AMS1117降压后供电。4. 常见问题与排查技巧实录4.1 机器人站不起来怎么办这是最常见的问题。我总结了一个排查流程按顺序检查基本能定位到问题。先检查电机转向。把机器人拿在手里倾斜车体观察轮子是不是往倾斜方向转。如果转反了把电机线对调或者在代码里把对应电机的输出取反。再检查MPU6050安装方向。MPU6050的X轴应该指向车体前进方向Y轴指向左侧Z轴指向上方。如果安装方向不对角度解算出来的符号是反的PID会变成正反馈。可以在代码里加一个方向校正矩阵或者直接重新安装传感器。然后检查PID参数。如果电机转向和传感器方向都正确但还是站不起来大概率是PID参数不对。把P从0开始慢慢加观察机器人反应。如果P加到很大还是没反应检查电机是不是没接到PWM信号用示波器或者LED测试一下。最后检查电源。电机启动瞬间电流很大如果电池内阻大或者线太细电压会被拉低导致Pico复位。我遇到过这个问题换了粗一点的电源线就好了。建议在电池输出端并一个1000uF的电解电容。现象可能原因解决方法轮子不转电机线接错、PWM没输出检查接线、用LED测试PWM往一个方向飞电机转向反、传感器方向反对调电机线、校正传感器方向高频抖动D太大、P太大减小D和P低频晃动I太大减小I站不稳但方向对P太小增大P一上电就复位电源功率不足换电池、加电容、加粗电源线4.2 角度漂移怎么解决MPU6050的零漂是不可避免的但可以通过校准来减小。我的做法是上电后让机器人静止2秒采集1000个陀螺仪数据取平均值作为零偏。然后在姿态解算时减去这个零偏。// 陀螺仪零偏校准 void gyro_calibrate(void) { float sum_x 0, sum_y 0, sum_z 0; for (int i 0; i 1000; i) { int16_t gx, gy, gz; mpu6050_read_gyro(gx, gy, gz); sum_x gx; sum_y gy; sum_z gz; sleep_ms(2); } gyro_offset_x sum_x / 1000.0f; gyro_offset_y sum_y / 1000.0f; gyro_offset_z sum_z / 1000.0f; }校准的时候机器人一定要静止不能有振动。我一般把机器人放在桌面上等它完全静止后再上电。如果校准后还是有漂移检查一下MPU6050的供电是不是干净电源纹波大会导致零漂增大。另外互补滤波的ALPHA系数也会影响漂移。ALPHA越大漂移修正越慢但响应越快。如果发现角度慢慢往一个方向偏可以适当减小ALPHA让加速度计更多地参与修正。4.3 电机干扰传感器的处理直流电机是很大的干扰源碳刷火花会产生宽频电磁干扰MPU6050很容易受影响。我遇到过电机一转角度就乱跳的情况。解决办法有三个第一在电机两端并一个0.1uF的陶瓷电容吸收高频噪声第二MPU6050的电源加LC滤波用磁珠和电容组成π型滤波第三MPU6050尽量远离电机安装至少保持5cm距离。如果以上都做了还是有干扰可以在软件上做处理在电机启动后的短时间内忽略传感器数据或者对传感器数据做滑动平均滤波。我用的是5点滑动平均效果不错但会引入一点延迟。注意滑动平均滤波的窗口大小要权衡。窗口越大噪声抑制越好但延迟越大。自平衡机器人对延迟很敏感窗口不要超过5个点。4.4 电池电压监测与低电保护锂电池过放会损坏所以必须做低电保护。我用Pico的ADC读取电池电压经过分压电阻当电压低于6.4V单节3.2V时机器人自动停止并报警。分压电阻我用的是10k和3.3k分压比是(103.3)/3.34.03。电池满电8.4V分压后是2.08V在Pico的ADC量程3.3V内。ADC读数换算成实际电压的公式是float battery_voltage adc_read() * 3.3f / 4096.0f * 4.03f;注意ADC参考电压要准确。Pico的3.3V输出实际上可能有偏差最好用万用表量一下实际值然后在代码里用实际值计算。低电保护逻辑很简单每100ms检测一次电压如果低于阈值设置一个标志位主循环检测到标志位后停止电机OLED显示LOW BATTERY。5. 项目扩展与进阶玩法5.1 加装摄像头做视觉巡线Pico本身没有摄像头接口但可以通过SPI接OV5647摄像头模块。不过Pico的算力有限跑图像处理比较吃力。我的建议是加一块ESP32-CAM做视觉处理通过串口把巡线偏差发给PicoPico只负责平衡和运动控制。这样分工的好处是各司其职Pico专注实时控制ESP32专注图像处理。两者通过串口通信协议可以很简单ESP32发一个字节表示偏差-128到127Pico收到后叠加到速度环的目标值上。5.2 用MicroPython快速验证想法虽然我用C开发了正式固件但有时候想快速验证一个控制算法用MicroPython会更方便。Pico官方支持MicroPython烧录固件后直接写Python代码就能跑。from machine import Pin, I2C, PWM import time # 初始化 i2c I2C(0, sdaPin(4), sclPin(5), freq400000) pwm_a PWM(Pin(6)) pwm_a.freq(20000) while True: # 读取角度 angle read_angle(i2c) # 计算PID output pid_calc(angle) # 输出PWM pwm_a.duty_u16(int(abs(output) * 65535 / 1000)) time.sleep_ms(5)MicroPython的缺点是执行速度慢控制频率上不去。我实测MicroPython跑100Hz的控制循环已经是极限了再高就不稳定。所以MicroPython适合验证算法正式产品还是用C。5.3 开源社区协作的经验这个项目我放在GitHub上开源后收到了不少PR和Issue。分享几个开源协作的经验文档比代码重要。我花在写README和接线图上的时间比写代码还多。但这是值得的因为好的文档能减少80%的重复问题。Issue模板要设计好。我设置了三个必填项硬件版本、固件版本、问题描述。这样别人提Issue的时候就会提供足够的信息不用来回问。代码风格要统一。我用的是Google C风格变量命名用蛇形函数命名用蛇形宏定义用全大写。统一的风格让代码更容易阅读和维护。版本管理要规范。我用的是语义化版本号主版本号.次版本号.修订号。每次发布新版本都打Tag写Changelog。这样别人能清楚地知道每个版本改了什么。版本日期主要改动v1.0.02024-01首个正式版支持基本自平衡v1.1.02024-03增加蓝牙调参功能v1.2.02024-05增加速度环抗外力推动v1.3.02024-07优化姿态解算减少漂移6. 物料清单与成本核算6.1 核心物料清单物料型号数量单价元备注主控树莓派Pico125官方版或兼容版均可姿态传感器MPU605018注意买正品电机驱动TB661216比L298N好用直流电机带编码器减速电机225减速比1:30轮子65mm橡胶轮25抓地力好电池18650锂电池2152000mAh以上电池盒2节1865013带开关OLED0.96寸I2C18显示调试信息蓝牙模块HC-05112无线调参PCB定制110可选也可用洞洞板结构件3D打印1套15PLA材料杂项螺丝、杜邦线等-10-合计约1506.2 成本控制技巧如果你预算紧张有几个地方可以省钱PCB可以用洞洞板代替省10块蓝牙模块可以用ESP8266代替省5块OLED可以省略用串口输出调试信息省8块。这样总成本能压到120左右。但有两个地方不建议省钱电机和电池。便宜的电机扭矩不够站不起来便宜的电池内阻大电机一启动就掉电。这两个是核心部件值得多花点钱。提示买电机的时候注意看参数堵转扭矩至少要0.5kg·cm空载转速在200-400rpm之间比较合适。转速太高扭矩不够转速太低响应慢。7. 调试心得与避坑指南7.1 第一次上电的注意事项第一次上电是最容易出问题的环节。我的建议是分步上电先只接Pico确认能烧录程序再接MPU6050确认能读到数据再接TB6612和电机确认电机能转最后接编码器确认计数正常。每一步都确认无误后再进行下一步。千万不要一上来就把所有模块都接好然后通电。如果接线有误可能烧毁模块。我见过有人把MPU6050的VCC接到5V上电就冒烟了。7.2 PID调参的实战技巧调参的时候我习惯用手机录慢动作视频然后逐帧分析机器人的运动。这样能看清楚摆动的频率和幅度比肉眼观察准确得多。另外调参要有耐心。我调这个机器人的PID花了整整两天试了上百组参数。不要指望一次就能调好每次只改一个参数改完测试记录效果。我建了一个Excel表格记录每次的参数和对应的表现这样能快速找到最优参数。还有一个技巧先在仿真里调。我用Python写了一个简单的仿真模型把PID参数在仿真里跑一遍找到大概范围再到实物上微调。这样能节省大量时间。7.3 长期运行的稳定性问题机器人跑久了会发热特别是电机和TB6612。我实测连续跑30分钟后TB6612的温度能到60度左右。虽然还在安全范围内但建议加个小散热片。另外电池电压下降后PID参数需要微调。因为电机转速和电压成正比电压低了同样的PWM占空比转速会下降。我的做法是在代码里做电压补偿根据电池电压调整PWM输出的缩放系数。// 电压补偿 float voltage_scale 7.4f / battery_voltage; output * voltage_scale;这样电池从满电到低电机器人的动态响应基本一致。7.4 结构设计的细节优化3D打印的结构件有几个细节要注意第一电机座要设计加强筋否则电机振动会导致断裂第二PCB固定孔要留公差我一般留0.2mm太紧装不进去太松会晃动第三电池仓要设计卡扣否则机器人摔倒时电池会飞出来。轮子的选择也很重要。我用的是65mm橡胶轮抓地力好但在地毯上会打滑。如果经常在地毯上跑建议换大一点的轮子或者加配重增加摩擦力。8. 写在最后的一些个人体会这个项目从最初的想法到最终开源前后花了大概三个月时间。中间踩过的坑、烧过的模块、调参调到凌晨的经历现在回想起来都是宝贵的经验。我最深的体会是做嵌入式项目硬件和软件同样重要。很多人只关注代码忽略了硬件设计结果调试的时候各种莫名其妙的问题。其实很多问题都是硬件引起的比如电源噪声、接触不良、干扰耦合。把硬件做好软件调试会顺利很多。另外开源不是把代码扔到GitHub就完了。好的开源项目需要好的文档、好的Issue管理、好的版本规划。我花在文档上的时间比写代码还多但这是值得的因为好的文档能让更多人受益。这个项目后续我还会继续维护计划加入的功能有用ESP32做上位机实现手机APP控制、加入陀螺仪温度补偿、优化速度环算法。如果你对这个项目感兴趣欢迎一起交流。