DIY六足机器人自平衡方案：STM32+MPU6050步态补偿实测

六足为什么需要自平衡

六足机器人比四足稳，但不代表它不需要平衡。当地面不平、重心偏移、或者某条腿踩到障碍物时，机身会倾斜。如果不做补偿，轻则走路姿态难看，重则翻倒。给六足加一套自平衡系统，用IMU实时检测机身姿态，调整每条腿的高度，就能让它在地形起伏时保持水平。

这个项目的方案是STM32F103C8T6做主控，MPU6050六轴IMU做姿态检测，18颗SG90舵机驱动6条腿。核心算法是互补滤波融合加速度计和陀螺仪数据，然后用PID控制器调整步态参数实现自平衡。

MPU6050集成了三轴加速度计和三轴陀螺仪，I2C接口，数据速率最高400kHz。加速度计能给出绝对倾斜角度但噪声大，陀螺仪短期精度高但有漂移，两者互补滤波结合就能得到稳定可靠的姿态估计。

互补滤波的公式很简单：angle = 0.98 * (angle + gyro * dt) + 0.02 * accel_angle。0.98和0.02是两个传感器的权重系数，可以根据实际调。这个滤波在STM32上跑一次大概20微秒，完全跟得上100Hz的控制频率。

六足机器人常用三角步态（tripod gait）：6条腿分成两组交替移动，每组3条腿同时着地支撑。自平衡的关键是，当机身倾斜时，实时调整每组3条腿的"落脚高度"，让支撑面始终和重力方向垂直。

具体做法：MPU6050给出俯仰角（pitch）和横滚角（roll），PID控制器根据目标角度（0度，即水平）和实际角度的偏差，计算出每条腿需要补偿的高度偏移量。然后把这个偏移量叠加到正常的步态轨迹上。比如机身前倾了5度，前腿就降低一些、后腿升高一些，把重心拉回来。

PID的三个参数对平衡效果影响很大。P（比例）决定修正力度，太小反应慢、太大振荡。I（积分）消除稳态误差，但对六足来说不需要太大，因为腿的高度调整本身就有延迟。D（微分）抑制振荡，适当加一些能让过渡更平滑。

我的实测参数是Kp=2.5、Ki=0.02、Kd=1.8，控制周期10ms。调参的时候先把Ki设为0，只调Kp让系统能响应倾斜但不振荡，然后加Kd让动作更平滑，最后稍微加一点Ki消除残差。注意每次调参后要在不同地形上测试，不能只在平地上调。

在15度斜坡上测试，未开自平衡时六足走到坡中间就开始侧滑倾斜，开了自平衡后能稳定行走，机身倾斜角控制在3度以内。在有一块砖头高度的不平地面上，补偿效果也很明显，不会像之前那样一条腿踩空就整个歪掉。

这个方案的局限性在于SG90舵机的响应速度和精度。如果想要更好的效果，可以换数字舵机或者步进电机。另外当前方案只做了静态平衡补偿，动态行走中的平衡还需要加入ZMP（零力矩点）等更高级的算法。