2026/9/20 5:25:21
FEATURED · 精选文章 
1. 为什么我最终选了Pico来做这台双轮自平衡机器人双轮自平衡机器人这个项目在创客圈里算是经典中的经典。你随便搜一下用Arduino的、用STM32的、用树莓派的方案一抓一大把。我前前后后也做过三四个版本从最早拿MPU6050加Arduino Uno搭的抖抖车到后来用STM32F103加编码器电机的稳如老狗版本踩过的坑基本能写一本小册子。这次之所以重新开一版核心原因就一个我想找一个既能跑实时控制、又足够便宜、还完全开源的平台把整个方案的门槛压到最低让刚入门的朋友也能照着复现出来。树莓派Pico正好卡在这个点上。它用的是RP2040芯片双核Cortex-M0主频133MHz264KB的SRAM关键是价格——官方定价4美元国内拿货也就二十块出头。你别看它便宜RP2040有个别的MCU很少见的东西可编程IOPIO。这个特性对做机器人的人来说太香了后面讲编码器读取的时候我会详细说。再加上Pico的生态现在非常成熟MicroPython和C/C SDK两条路都走得通社区里现成的库一大堆完全符合全开源这个定位。那为什么不用树莓派4B或者树莓派5来做主控这个问题我被问过很多次。答案很简单Linux系统的实时性不够。树莓派跑的是完整操作系统任务调度、中断响应都有不可控的延迟你让它去做1kHz的PID闭环控制抖动会大到电机发疯。树莓派适合做上层决策、视觉处理、网络通信这些事但底层电机控制必须交给MCU。所以合理的架构是树莓派管脑子Pico管小脑和脊髓这台自平衡机器人我选择纯Pico方案是因为平衡控制本身就是个纯实时任务不需要上层算力把Pico单独拿出来做反而更干净、更便宜、更容易复现。这台机器人最终实现的效果是上电后能自己站稳用手推它会有阻尼感地回正能通过蓝牙或者串口接收前进、后退、转向指令续航大概两三个小时。整机物料成本控制在两百块以内所有代码和PCB文件我都会开源出去。下面我把整个项目的设计思路、硬件选型、控制算法、调试过程完整地拆一遍你照着做基本能少走八成弯路。2. 硬件选型每一个零件背后的取舍逻辑2.1 主控与电机驱动的搭配主控就是Pico这个没悬念。但电机驱动这块值得说道说道。市面上常见的方案有L298N、TB6612、DRV8833这几种。L298N便宜大碗但它的导通压降太大了一颗芯片上要吃掉将近2V的电压而且发热严重效率低得感人。我最早那版用L298N电池7.4V进去电机端只剩5V出头扭矩直接打七折。TB6612FNG是我这次选的方案。它是MOSFET工艺的H桥导通电阻只有0.5欧姆左右压降小、发热低单通道持续电流1.2A峰值3.2A驱动这种小型的减速直流电机绰绰有余。而且它逻辑电平兼容3.3V可以直接和Pico的GPIO对接不需要额外的电平转换。接线也简单每个电机三根控制线AIN1、AIN2、PWMA两路电机一共六根加上Pico的GPIO足够用。电机我用的是带霍尔编码器的直流减速电机减速比1:30空载转速大概330RPM额定电压6V。为什么一定要带编码器因为纯靠MPU6050的角度做平衡机器人只能站住但没法走——你给它一个前进指令它倾斜之后电机转了多少、转速多少系统完全不知道只能靠角度反馈去猜结果就是走起来一顿一顿的速度根本控不住。有了编码器你就能做速度环让机器人真正实现倾斜-加速-匀速的闭环走起来才顺滑。2.2 姿态传感器MPU6050还是ICM20602姿态传感器是整个平衡车的内耳。MPU6050是经典款六轴三轴加速度三轴陀螺仪I2C接口价格便宜资料多到烂大街。但它有个老毛病陀螺仪的零漂比较明显而且内部DMP数字运动处理器的配置比较麻烦很多人直接用原始数据自己做融合。我这次用的是ICM20602算是MPU6050的升级替代。它的陀螺仪噪声更低零漂更小SPI接口速率可以拉到很高采样率轻松上1kHz。价格比MPU6050贵不了几块钱但姿态解算出来的角度明显更干净。如果你手头只有MPU6050也完全能做只是滤波参数要调得更保守一些。这里有个细节要提醒传感器一定要刚性固定在车体上不能有任何松动。我见过有人用双面胶随便一贴结果机器人一震动传感器跟着晃角度数据全是毛刺怎么调PID都站不稳。正确做法是用M2螺丝拧在3D打印的支架上或者至少用热熔胶打满固定死。2.3 电源方案为什么我用两节18650而不是锂电池组电源这块我纠结了很久。常见的方案有三种单节锂电池升压、两节18650串联、专用航模锂电池组。最后我选了两节18650串联标称7.4V经过降压给Pico供5V电机直接吃7.4VTB6612支持到15V没问题。为什么不选航模电池因为航模电池虽然放电能力强但需要专门的平衡充电器对新手不友好而且过放容易鼓包甚至出危险。18650就省心多了随便一个充电器就能充两节串联的电压刚好合适容量也够我用的是2600mAh的实测续航两个多小时。降压我用的是MP1584模块输入7.4V输出5V给Pico供电。注意Pico的VSYS引脚输入范围是1.8V到5.5V5V正好。但有个坑电机启动瞬间会有电压跌落如果降压模块响应慢Pico可能会复位。解决办法是在Pico的电源输入端并一个470uF的电解电容把瞬态电流兜住。这个电容我强烈建议加上我第一版没加机器人一加速就重启查了半天才找到原因。2.4 车体结构与重心分布车体我用3D打印的材料PLA壁厚2mm。结构很简单一个底板放电池和驱动板一个立板放Pico和传感器两个电机固定在底板两侧轮子用65mm的橡胶轮。重心分布是自平衡机器人的命门。重心必须高于轮轴而且越集中越好。如果重心太低机器人会变得迟钝需要更大的倾斜角才能产生足够的回复力矩如果重心太高又太散惯性大响应慢。我的经验是把电池最重的部件放在轮轴正上方偏后一点的位置让整体重心落在轮轴上方大约5到8厘米处。这个高度可以通过调整电池的安装位置来微调装好之后用手推一下感受回正的力度太软就往上挪太硬就往下挪。3. 控制算法从能站到站得稳的三环嵌套3.1 姿态解算互补滤波为什么够用姿态解算的目标是得到一个准确、低延迟的倾角。加速度计能测出绝对角度但对震动极其敏感陀螺仪积分出来的角度短期很准但会随时间漂移。所以要把两者融合。常见的融合算法有卡尔曼滤波、Mahony滤波、互补滤波。卡尔曼效果最好但计算量大、调参复杂Mahony适合有磁力计的场景互补滤波最简单对这台机器人来说完全够用。它的核心思想就一句话高通滤波取陀螺仪的短期变化低通滤波取加速度计的长期基准两者加权相加。公式大概是这样// 互补滤波核心逻辑 float dt 0.001f; // 1kHz采样 float alpha 0.98f; // 陀螺仪权重 // 陀螺仪积分得到角度变化 angle gyro_y * dt; // 加速度计算出绝对角度 float accel_angle atan2(accel_x, accel_z) * 57.2958f; // 互补融合 angle alpha * angle (1 - alpha) * accel_angle;alpha这个系数是关键。取0.98意味着98%信任陀螺仪、2%信任加速度计。这个值越大角度越平滑但漂移修正越慢越小则响应快但容易被震动干扰。我实测下来0.96到0.99之间都可以具体看你的车体震动情况。如果机器人静止时角度缓慢漂移说明alpha太大了调小一点如果机器人对震动特别敏感、角度乱跳说明alpha太小了调大一点。注意加速度计算角度时一定要先做低通滤波否则电机震动会让角度数据全是噪声。我一般用一个简单的一阶低通截止频率设在20Hz左右。3.2 三环PID的嵌套关系自平衡机器人的控制本质上是三个环嵌套直立环角度环是内环速度环是外环转向环独立。直立环的任务是让车体保持垂直。输入是目标角度通常是0度或者根据速度环输出微调反馈是当前角度输出是电机的PWM值。这个环的响应要快一般跑在1kHz。速度环的任务是控制机器人前进后退的速度。输入是目标速度来自遥控指令反馈是编码器测出的实际速度输出是目标角度的偏移量。注意速度环的输出不是直接给电机而是去欺骗直立环——想让机器人前进就让直立环的目标角度往前偏一点机器人为了追这个角度自然就往前倾、往前走了。这个环跑得慢一些一般100Hz就够了。转向环最简单输入是目标转向速度反馈是左右轮速差输出是左右电机的差速PWM。这个环和直立环叠加在一起输出到电机。三个环的代码结构大概是这样// 1kHz定时器中断里执行 void control_loop() { // 读取传感器 float angle get_filtered_angle(); float gyro get_gyro_y(); // 速度环每10次执行一次即100Hz if (tick % 10 0) { speed_target get_remote_speed(); speed_actual (encoder_left encoder_right) / 2.0f; speed_output pid_speed.calc(speed_target, speed_actual); } // 直立环 float angle_target speed_output; // 速度环输出作为角度目标 float angle_output pid_angle.calc(angle_target, angle); // 转向环 float turn_output pid_turn.calc(turn_target, gyro_z); // 混合输出 int pwm_left angle_output turn_output; int pwm_right angle_output - turn_output; set_motor(pwm_left, pwm_right); }3.3 PID参数整定的实操顺序调PID是新手最容易翻车的地方。我的建议是严格按顺序来一次只调一个环。第一步先调直立环的P。把速度环和转向环全部关掉只留直立环。P从小往大加加到机器人能勉强站住但还在小幅高频抖动为止。这时候的P大概在30到60之间取决于你的电机和车体。然后加DD的作用是抑制抖动加上去之后机器人会明显变稳。D一般取P的0.5到1.5倍。I先不加直立环的I容易引入低频振荡除非你的机器人有固定的角度偏差否则不建议加。第二步调速度环。直立环调好之后机器人能站住了但用手推它会慢慢跑偏。这时候加速度环的P让机器人被推动后能自己回到原位。速度环的P一般很小0.5到2之间就够了太大了会和直立环打架导致机器人前后剧烈晃动。速度环的I可以加一点用来消除稳态误差但也要小0.1到0.5之间。第三步调转向环。这个最简单P给个5到15D给个0到5基本就能转得很顺。转向环不需要I。整个调参过程我建议在机器人下面垫一块海绵或者用手扶着防止摔坏。每调一个参数都要反复推拉测试记录下机器人的反应。我一般会准备一张表格把每次的参数和现象记下来这样能快速定位到最优区间。调试阶段参数典型范围现象判断直立环PKp_angle30-60太小站不住太大高频抖动直立环DKd_angle15-90太小抖动太大反应迟钝速度环PKp_speed0.5-2太小回不到原位太大前后晃速度环IKi_speed0.1-0.5消除稳态误差太大会振荡转向环PKp_turn5-15太小转向无力太大转向过冲4. 编码器读取PIO才是RP2040的正确打开方式4.1 为什么不用外部中断读编码器霍尔编码器输出的是两路正交方波A相和B相通过判断两相的相位差来确定转向通过计数脉冲来确定转速。常规做法是用GPIO外部中断A相上升沿触发读B相电平判断方向然后计数。这个方法在低速下没问题但高速时问题就来了。我的电机空载330RPM减速前是10000RPM左右编码器线数是13线四倍频之后每转52个脉冲算下来脉冲频率大概8.6kHz。这个频率用中断处理CPU有大概10%的时间在响应中断还能接受。但如果电机转速再高一点或者你同时要处理其他中断就会丢脉冲速度测量直接失真。更麻烦的是抖动。机械编码器在低速时会有抖动A相在阈值附近反复跳变中断会被触发很多次计数直接乱掉。软件消抖又增加了CPU负担。4.2 PIO状态机的实现思路RP2040的PIO是个硬件状态机它可以独立于CPU运行自己读GPIO、自己计数、自己判断方向完全不占用CPU资源。这玩意儿说白了就是给你一块可编程的硬件逻辑你用几条汇编指令描述它的行为它就按这个行为一直跑下去。读正交编码器的PIO程序大概长这样简化版# MicroPython中使用PIO读编码器 import rp2 from machine import Pin rp2.asm_pio() def quadrature_encoder(): # 等待A相变化 wait(1, pin, 0) # 读B相判断方向 in_(pins, 2) # 更新计数 jmp(x_dec, decrement) jmp(increment) label(decrement) # ... 计数逻辑 label(increment) # ... 计数逻辑实际用的时候Pico的SDK里已经提供了现成的quadrature_encoderPIO程序你直接调用就行。它会自动维护一个计数器你随时去读这个计数器的值就是当前的脉冲数。CPU完全不用管中断也不用开速度再快也不会丢脉冲。我实测下来用PIO读编码器即使电机全速运转计数也一个不丢速度测量非常干净。这是RP2040相比其他MCU的一个巨大优势也是我选Pico做这个项目的核心原因之一。4.3 速度计算与滤波拿到脉冲数之后速度计算就简单了单位时间内的脉冲增量除以每转脉冲数再乘以轮子周长就是线速度。// 每10ms计算一次速度 float calc_speed() { int32_t current_count quadrature_encoder_get_count(); int32_t delta current_count - last_count; last_count current_count; // 每转52个脉冲轮子周长204mm float speed delta / 52.0f * 204.0f / 0.01f; // mm/s return speed; }但原始速度数据会有噪声尤其是低速时。我一般会加一个滑动平均滤波取最近5次的平均值。这样速度曲线会平滑很多速度环的PID也更好调。提示编码器的计数方向要和电机转向对应上。如果发现速度正负号反了把A相和B相在代码里对调一下就行不用改硬件接线。5. 调试过程中踩过的那些坑5.1 机器人一加速就重启这个问题我前面提过但值得展开说。现象是机器人静止时一切正常一旦电机加速Pico就复位串口打印一堆乱码。一开始我以为是代码问题查了半天没找到。后来用示波器看电源波形发现电机启动瞬间7.4V的电池电压被拉低到5V以下经过降压模块之后Pico的供电直接跌到3V以下触发欠压复位。解决办法有两个一是加电容在Pico的VSYS和GND之间并一个470uF到1000uF的电解电容把瞬态电流兜住二是给电机驱动和Pico分别供电用两路独立的降压。我两个都做了之后再也没有重启过。这个坑的教训是电机是感性负载启动电流是额定电流的好几倍电源设计一定要留足余量电容该加就加别省这几毛钱。5.2 角度数据全是毛刺第二个坑是姿态传感器的数据质量。我一开始把MPU6050用双面胶贴在车体上结果角度数据噪声极大机器人抖得像筛糠。后来换成螺丝固定噪声小了一半但还是有周期性毛刺。排查之后发现两个原因一是电机震动通过车体传到了传感器二是I2C读取频率太低只有100Hz数据更新跟不上控制周期。解决办法是传感器用减震垫我用的是硅胶垫隔离震动同时把I2C速率提到400kHz采样率提到500Hz以上。如果还不行就换SPI接口的传感器比如ICM20602SPI可以跑到10MHz采样率轻松上1kHz。5.3 PID调好了但走起来一顿一顿的这个问题的根源是速度环和直立环的采样率不匹配。我一开始把速度环和直立环都放在1kHz的中断里跑结果速度环的积分项累积太快输出剧烈波动机器人走起来一冲一冲的。正确的做法是让速度环跑得慢一些一般100Hz就够了。因为速度环的响应本来就不需要那么快跑太快反而会和直立环耦合振荡。我把速度环改成每10次中断执行一次之后机器人走起来立刻顺滑了。5.4 转向时机器人往一边倒这个坑比较隐蔽。现象是机器人直行没问题一转向就往一边倒。查了半天发现是左右电机的死区不一致。两个电机虽然型号一样但死区电压有差异同样的PWM值一个转得快一个转得慢导致转向时左右轮速差不匹配机器人产生额外的倾斜。解决办法是在代码里给每个电机单独标定死区补偿值。具体做法是给电机一个从小到大的PWM记录它开始转动的最小值左右电机分别记录然后在输出PWM时加上各自的补偿值。这个标定过程花不了十分钟但效果立竿见影。6. 开源资料与复现建议6.1 代码结构与关键文件说明整个项目的代码我分成了几个模块方便你按需修改main.c主循环和初始化包含定时器中断的配置pid.c / pid.hPID控制器的通用实现三个环共用imu.c / imu.h姿态传感器驱动和互补滤波encoder.c / encoder.hPIO编码器读取和速度计算motor.c / motor.h电机驱动和死区补偿control.c / control.h三环控制逻辑编译环境用的是Pico SDKCMake构建。如果你用MicroPython我也提供了一份Python版本的代码功能一样只是性能会差一些控制频率只能跑到500Hz左右但做基础平衡完全够用。6.2 复现时的物料清单与预算部件型号数量参考价格主控树莓派Pico125元电机驱动TB6612FNG模块18元姿态传感器ICM20602模块115元减速电机1:30带编码器240元电池18650 2600mAh230元降压模块MP158415元轮子65mm橡胶轮215元车体3D打印件1套20元杂项螺丝、导线、电容-20元合计约178元这个预算已经压得很低了如果你手头有一些现成的零件成本还能再降。6.3 给新手的三个建议第一先让机器人站住再考虑走。很多人一上来就想做遥控、做避障、做视觉结果基础平衡都没调好后面全是空中楼阁。我建议你花至少两天时间专门调直立环调到用手推它、它能稳稳回正为止。第二电源一定要做好。我见过太多人代码写得没问题就是电源不行机器人各种诡异行为。电容、稳压、走线这三样一个都不能马虎。电机线和信号线要分开走避免干扰。第三善用串口调试。Pico可以通过USB直接输出调试信息把角度、速度、PWM这些关键变量实时打印出来用串口示波器比如VOFA或者SerialPlot画成曲线调PID的时候一目了然。比盲调效率高十倍。这台机器人我前后做了大概三周其中一半时间花在调试上。但当你看到它第一次自己站稳、第一次听话地往前走的时候那种成就感是真的值。RP2040这颗芯片在嵌入式控制领域被低估了它的PIO和双核架构在很多场景下比STM32还灵活价格却便宜得多。如果你也在找一个适合入门嵌入式控制的项目双轮自平衡机器人加上Pico我觉得是目前性价比最高的组合。
基于树莓派Pico的双轮自平衡机器人：从硬件选型到三环PID控制
1. 为什么我最终选了Pico来做这台双轮自平衡机器人双轮自平衡机器人这个项目在创客圈里算是经典中的经典。你随便搜一下用Arduino的、用STM32的、用树莓派的方案一抓一大把。我前前后后也做过三四个版本从最早拿MPU6050加Arduino Uno搭的抖抖车到后来用STM32F103加编码器电机的稳如老狗版本踩过的坑基本能写一本小册子。这次之所以重新开一版核心原因就一个我想找一个既能跑实时控制、又足够便宜、还完全开源的平台把整个方案的门槛压到最低让刚入门的朋友也能照着复现出来。树莓派Pico正好卡在这个点上。它用的是RP2040芯片双核Cortex-M0主频133MHz264KB的SRAM关键是价格——官方定价4美元国内拿货也就二十块出头。你别看它便宜RP2040有个别的MCU很少见的东西可编程IOPIO。这个特性对做机器人的人来说太香了后面讲编码器读取的时候我会详细说。再加上Pico的生态现在非常成熟MicroPython和C/C SDK两条路都走得通社区里现成的库一大堆完全符合全开源这个定位。那为什么不用树莓派4B或者树莓派5来做主控这个问题我被问过很多次。答案很简单Linux系统的实时性不够。树莓派跑的是完整操作系统任务调度、中断响应都有不可控的延迟你让它去做1kHz的PID闭环控制抖动会大到电机发疯。树莓派适合做上层决策、视觉处理、网络通信这些事但底层电机控制必须交给MCU。所以合理的架构是树莓派管脑子Pico管小脑和脊髓这台自平衡机器人我选择纯Pico方案是因为平衡控制本身就是个纯实时任务不需要上层算力把Pico单独拿出来做反而更干净、更便宜、更容易复现。这台机器人最终实现的效果是上电后能自己站稳用手推它会有阻尼感地回正能通过蓝牙或者串口接收前进、后退、转向指令续航大概两三个小时。整机物料成本控制在两百块以内所有代码和PCB文件我都会开源出去。下面我把整个项目的设计思路、硬件选型、控制算法、调试过程完整地拆一遍你照着做基本能少走八成弯路。2. 硬件选型每一个零件背后的取舍逻辑2.1 主控与电机驱动的搭配主控就是Pico这个没悬念。但电机驱动这块值得说道说道。市面上常见的方案有L298N、TB6612、DRV8833这几种。L298N便宜大碗但它的导通压降太大了一颗芯片上要吃掉将近2V的电压而且发热严重效率低得感人。我最早那版用L298N电池7.4V进去电机端只剩5V出头扭矩直接打七折。TB6612FNG是我这次选的方案。它是MOSFET工艺的H桥导通电阻只有0.5欧姆左右压降小、发热低单通道持续电流1.2A峰值3.2A驱动这种小型的减速直流电机绰绰有余。而且它逻辑电平兼容3.3V可以直接和Pico的GPIO对接不需要额外的电平转换。接线也简单每个电机三根控制线AIN1、AIN2、PWMA两路电机一共六根加上Pico的GPIO足够用。电机我用的是带霍尔编码器的直流减速电机减速比1:30空载转速大概330RPM额定电压6V。为什么一定要带编码器因为纯靠MPU6050的角度做平衡机器人只能站住但没法走——你给它一个前进指令它倾斜之后电机转了多少、转速多少系统完全不知道只能靠角度反馈去猜结果就是走起来一顿一顿的速度根本控不住。有了编码器你就能做速度环让机器人真正实现倾斜-加速-匀速的闭环走起来才顺滑。2.2 姿态传感器MPU6050还是ICM20602姿态传感器是整个平衡车的内耳。MPU6050是经典款六轴三轴加速度三轴陀螺仪I2C接口价格便宜资料多到烂大街。但它有个老毛病陀螺仪的零漂比较明显而且内部DMP数字运动处理器的配置比较麻烦很多人直接用原始数据自己做融合。我这次用的是ICM20602算是MPU6050的升级替代。它的陀螺仪噪声更低零漂更小SPI接口速率可以拉到很高采样率轻松上1kHz。价格比MPU6050贵不了几块钱但姿态解算出来的角度明显更干净。如果你手头只有MPU6050也完全能做只是滤波参数要调得更保守一些。这里有个细节要提醒传感器一定要刚性固定在车体上不能有任何松动。我见过有人用双面胶随便一贴结果机器人一震动传感器跟着晃角度数据全是毛刺怎么调PID都站不稳。正确做法是用M2螺丝拧在3D打印的支架上或者至少用热熔胶打满固定死。2.3 电源方案为什么我用两节18650而不是锂电池组电源这块我纠结了很久。常见的方案有三种单节锂电池升压、两节18650串联、专用航模锂电池组。最后我选了两节18650串联标称7.4V经过降压给Pico供5V电机直接吃7.4VTB6612支持到15V没问题。为什么不选航模电池因为航模电池虽然放电能力强但需要专门的平衡充电器对新手不友好而且过放容易鼓包甚至出危险。18650就省心多了随便一个充电器就能充两节串联的电压刚好合适容量也够我用的是2600mAh的实测续航两个多小时。降压我用的是MP1584模块输入7.4V输出5V给Pico供电。注意Pico的VSYS引脚输入范围是1.8V到5.5V5V正好。但有个坑电机启动瞬间会有电压跌落如果降压模块响应慢Pico可能会复位。解决办法是在Pico的电源输入端并一个470uF的电解电容把瞬态电流兜住。这个电容我强烈建议加上我第一版没加机器人一加速就重启查了半天才找到原因。2.4 车体结构与重心分布车体我用3D打印的材料PLA壁厚2mm。结构很简单一个底板放电池和驱动板一个立板放Pico和传感器两个电机固定在底板两侧轮子用65mm的橡胶轮。重心分布是自平衡机器人的命门。重心必须高于轮轴而且越集中越好。如果重心太低机器人会变得迟钝需要更大的倾斜角才能产生足够的回复力矩如果重心太高又太散惯性大响应慢。我的经验是把电池最重的部件放在轮轴正上方偏后一点的位置让整体重心落在轮轴上方大约5到8厘米处。这个高度可以通过调整电池的安装位置来微调装好之后用手推一下感受回正的力度太软就往上挪太硬就往下挪。3. 控制算法从能站到站得稳的三环嵌套3.1 姿态解算互补滤波为什么够用姿态解算的目标是得到一个准确、低延迟的倾角。加速度计能测出绝对角度但对震动极其敏感陀螺仪积分出来的角度短期很准但会随时间漂移。所以要把两者融合。常见的融合算法有卡尔曼滤波、Mahony滤波、互补滤波。卡尔曼效果最好但计算量大、调参复杂Mahony适合有磁力计的场景互补滤波最简单对这台机器人来说完全够用。它的核心思想就一句话高通滤波取陀螺仪的短期变化低通滤波取加速度计的长期基准两者加权相加。公式大概是这样// 互补滤波核心逻辑 float dt 0.001f; // 1kHz采样 float alpha 0.98f; // 陀螺仪权重 // 陀螺仪积分得到角度变化 angle gyro_y * dt; // 加速度计算出绝对角度 float accel_angle atan2(accel_x, accel_z) * 57.2958f; // 互补融合 angle alpha * angle (1 - alpha) * accel_angle;alpha这个系数是关键。取0.98意味着98%信任陀螺仪、2%信任加速度计。这个值越大角度越平滑但漂移修正越慢越小则响应快但容易被震动干扰。我实测下来0.96到0.99之间都可以具体看你的车体震动情况。如果机器人静止时角度缓慢漂移说明alpha太大了调小一点如果机器人对震动特别敏感、角度乱跳说明alpha太小了调大一点。注意加速度计算角度时一定要先做低通滤波否则电机震动会让角度数据全是噪声。我一般用一个简单的一阶低通截止频率设在20Hz左右。3.2 三环PID的嵌套关系自平衡机器人的控制本质上是三个环嵌套直立环角度环是内环速度环是外环转向环独立。直立环的任务是让车体保持垂直。输入是目标角度通常是0度或者根据速度环输出微调反馈是当前角度输出是电机的PWM值。这个环的响应要快一般跑在1kHz。速度环的任务是控制机器人前进后退的速度。输入是目标速度来自遥控指令反馈是编码器测出的实际速度输出是目标角度的偏移量。注意速度环的输出不是直接给电机而是去欺骗直立环——想让机器人前进就让直立环的目标角度往前偏一点机器人为了追这个角度自然就往前倾、往前走了。这个环跑得慢一些一般100Hz就够了。转向环最简单输入是目标转向速度反馈是左右轮速差输出是左右电机的差速PWM。这个环和直立环叠加在一起输出到电机。三个环的代码结构大概是这样// 1kHz定时器中断里执行 void control_loop() { // 读取传感器 float angle get_filtered_angle(); float gyro get_gyro_y(); // 速度环每10次执行一次即100Hz if (tick % 10 0) { speed_target get_remote_speed(); speed_actual (encoder_left encoder_right) / 2.0f; speed_output pid_speed.calc(speed_target, speed_actual); } // 直立环 float angle_target speed_output; // 速度环输出作为角度目标 float angle_output pid_angle.calc(angle_target, angle); // 转向环 float turn_output pid_turn.calc(turn_target, gyro_z); // 混合输出 int pwm_left angle_output turn_output; int pwm_right angle_output - turn_output; set_motor(pwm_left, pwm_right); }3.3 PID参数整定的实操顺序调PID是新手最容易翻车的地方。我的建议是严格按顺序来一次只调一个环。第一步先调直立环的P。把速度环和转向环全部关掉只留直立环。P从小往大加加到机器人能勉强站住但还在小幅高频抖动为止。这时候的P大概在30到60之间取决于你的电机和车体。然后加DD的作用是抑制抖动加上去之后机器人会明显变稳。D一般取P的0.5到1.5倍。I先不加直立环的I容易引入低频振荡除非你的机器人有固定的角度偏差否则不建议加。第二步调速度环。直立环调好之后机器人能站住了但用手推它会慢慢跑偏。这时候加速度环的P让机器人被推动后能自己回到原位。速度环的P一般很小0.5到2之间就够了太大了会和直立环打架导致机器人前后剧烈晃动。速度环的I可以加一点用来消除稳态误差但也要小0.1到0.5之间。第三步调转向环。这个最简单P给个5到15D给个0到5基本就能转得很顺。转向环不需要I。整个调参过程我建议在机器人下面垫一块海绵或者用手扶着防止摔坏。每调一个参数都要反复推拉测试记录下机器人的反应。我一般会准备一张表格把每次的参数和现象记下来这样能快速定位到最优区间。调试阶段参数典型范围现象判断直立环PKp_angle30-60太小站不住太大高频抖动直立环DKd_angle15-90太小抖动太大反应迟钝速度环PKp_speed0.5-2太小回不到原位太大前后晃速度环IKi_speed0.1-0.5消除稳态误差太大会振荡转向环PKp_turn5-15太小转向无力太大转向过冲4. 编码器读取PIO才是RP2040的正确打开方式4.1 为什么不用外部中断读编码器霍尔编码器输出的是两路正交方波A相和B相通过判断两相的相位差来确定转向通过计数脉冲来确定转速。常规做法是用GPIO外部中断A相上升沿触发读B相电平判断方向然后计数。这个方法在低速下没问题但高速时问题就来了。我的电机空载330RPM减速前是10000RPM左右编码器线数是13线四倍频之后每转52个脉冲算下来脉冲频率大概8.6kHz。这个频率用中断处理CPU有大概10%的时间在响应中断还能接受。但如果电机转速再高一点或者你同时要处理其他中断就会丢脉冲速度测量直接失真。更麻烦的是抖动。机械编码器在低速时会有抖动A相在阈值附近反复跳变中断会被触发很多次计数直接乱掉。软件消抖又增加了CPU负担。4.2 PIO状态机的实现思路RP2040的PIO是个硬件状态机它可以独立于CPU运行自己读GPIO、自己计数、自己判断方向完全不占用CPU资源。这玩意儿说白了就是给你一块可编程的硬件逻辑你用几条汇编指令描述它的行为它就按这个行为一直跑下去。读正交编码器的PIO程序大概长这样简化版# MicroPython中使用PIO读编码器 import rp2 from machine import Pin rp2.asm_pio() def quadrature_encoder(): # 等待A相变化 wait(1, pin, 0) # 读B相判断方向 in_(pins, 2) # 更新计数 jmp(x_dec, decrement) jmp(increment) label(decrement) # ... 计数逻辑 label(increment) # ... 计数逻辑实际用的时候Pico的SDK里已经提供了现成的quadrature_encoderPIO程序你直接调用就行。它会自动维护一个计数器你随时去读这个计数器的值就是当前的脉冲数。CPU完全不用管中断也不用开速度再快也不会丢脉冲。我实测下来用PIO读编码器即使电机全速运转计数也一个不丢速度测量非常干净。这是RP2040相比其他MCU的一个巨大优势也是我选Pico做这个项目的核心原因之一。4.3 速度计算与滤波拿到脉冲数之后速度计算就简单了单位时间内的脉冲增量除以每转脉冲数再乘以轮子周长就是线速度。// 每10ms计算一次速度 float calc_speed() { int32_t current_count quadrature_encoder_get_count(); int32_t delta current_count - last_count; last_count current_count; // 每转52个脉冲轮子周长204mm float speed delta / 52.0f * 204.0f / 0.01f; // mm/s return speed; }但原始速度数据会有噪声尤其是低速时。我一般会加一个滑动平均滤波取最近5次的平均值。这样速度曲线会平滑很多速度环的PID也更好调。提示编码器的计数方向要和电机转向对应上。如果发现速度正负号反了把A相和B相在代码里对调一下就行不用改硬件接线。5. 调试过程中踩过的那些坑5.1 机器人一加速就重启这个问题我前面提过但值得展开说。现象是机器人静止时一切正常一旦电机加速Pico就复位串口打印一堆乱码。一开始我以为是代码问题查了半天没找到。后来用示波器看电源波形发现电机启动瞬间7.4V的电池电压被拉低到5V以下经过降压模块之后Pico的供电直接跌到3V以下触发欠压复位。解决办法有两个一是加电容在Pico的VSYS和GND之间并一个470uF到1000uF的电解电容把瞬态电流兜住二是给电机驱动和Pico分别供电用两路独立的降压。我两个都做了之后再也没有重启过。这个坑的教训是电机是感性负载启动电流是额定电流的好几倍电源设计一定要留足余量电容该加就加别省这几毛钱。5.2 角度数据全是毛刺第二个坑是姿态传感器的数据质量。我一开始把MPU6050用双面胶贴在车体上结果角度数据噪声极大机器人抖得像筛糠。后来换成螺丝固定噪声小了一半但还是有周期性毛刺。排查之后发现两个原因一是电机震动通过车体传到了传感器二是I2C读取频率太低只有100Hz数据更新跟不上控制周期。解决办法是传感器用减震垫我用的是硅胶垫隔离震动同时把I2C速率提到400kHz采样率提到500Hz以上。如果还不行就换SPI接口的传感器比如ICM20602SPI可以跑到10MHz采样率轻松上1kHz。5.3 PID调好了但走起来一顿一顿的这个问题的根源是速度环和直立环的采样率不匹配。我一开始把速度环和直立环都放在1kHz的中断里跑结果速度环的积分项累积太快输出剧烈波动机器人走起来一冲一冲的。正确的做法是让速度环跑得慢一些一般100Hz就够了。因为速度环的响应本来就不需要那么快跑太快反而会和直立环耦合振荡。我把速度环改成每10次中断执行一次之后机器人走起来立刻顺滑了。5.4 转向时机器人往一边倒这个坑比较隐蔽。现象是机器人直行没问题一转向就往一边倒。查了半天发现是左右电机的死区不一致。两个电机虽然型号一样但死区电压有差异同样的PWM值一个转得快一个转得慢导致转向时左右轮速差不匹配机器人产生额外的倾斜。解决办法是在代码里给每个电机单独标定死区补偿值。具体做法是给电机一个从小到大的PWM记录它开始转动的最小值左右电机分别记录然后在输出PWM时加上各自的补偿值。这个标定过程花不了十分钟但效果立竿见影。6. 开源资料与复现建议6.1 代码结构与关键文件说明整个项目的代码我分成了几个模块方便你按需修改main.c主循环和初始化包含定时器中断的配置pid.c / pid.hPID控制器的通用实现三个环共用imu.c / imu.h姿态传感器驱动和互补滤波encoder.c / encoder.hPIO编码器读取和速度计算motor.c / motor.h电机驱动和死区补偿control.c / control.h三环控制逻辑编译环境用的是Pico SDKCMake构建。如果你用MicroPython我也提供了一份Python版本的代码功能一样只是性能会差一些控制频率只能跑到500Hz左右但做基础平衡完全够用。6.2 复现时的物料清单与预算部件型号数量参考价格主控树莓派Pico125元电机驱动TB6612FNG模块18元姿态传感器ICM20602模块115元减速电机1:30带编码器240元电池18650 2600mAh230元降压模块MP158415元轮子65mm橡胶轮215元车体3D打印件1套20元杂项螺丝、导线、电容-20元合计约178元这个预算已经压得很低了如果你手头有一些现成的零件成本还能再降。6.3 给新手的三个建议第一先让机器人站住再考虑走。很多人一上来就想做遥控、做避障、做视觉结果基础平衡都没调好后面全是空中楼阁。我建议你花至少两天时间专门调直立环调到用手推它、它能稳稳回正为止。第二电源一定要做好。我见过太多人代码写得没问题就是电源不行机器人各种诡异行为。电容、稳压、走线这三样一个都不能马虎。电机线和信号线要分开走避免干扰。第三善用串口调试。Pico可以通过USB直接输出调试信息把角度、速度、PWM这些关键变量实时打印出来用串口示波器比如VOFA或者SerialPlot画成曲线调PID的时候一目了然。比盲调效率高十倍。这台机器人我前后做了大概三周其中一半时间花在调试上。但当你看到它第一次自己站稳、第一次听话地往前走的时候那种成就感是真的值。RP2040这颗芯片在嵌入式控制领域被低估了它的PIO和双核架构在很多场景下比STM32还灵活价格却便宜得多。如果你也在找一个适合入门嵌入式控制的项目双轮自平衡机器人加上Pico我觉得是目前性价比最高的组合。 
