从零构建自平衡机器人：STM32与ROS融合的完整实践指南

1. 项目概述：从“代步工具”到“自主伙伴”的进化

几年前，当我第一次看到商场里有人骑着平衡车（Segway）穿梭时，我被它那种“人车合一”的灵动感深深吸引。但作为一个硬件和机器人爱好者，我脑子里冒出的第一个念头是：如果它自己能“站”起来，不需要人骑，还能自己到处跑、自己避障、甚至帮我拿个东西，那该多酷？这就是“Self Standing Segway Robot”（自平衡站立式赛格威机器人）项目的起点。它本质上是一个将传统两轮自平衡车的核心控制算法，与移动机器人（尤其是轮式机器人）的自主导航、环境感知能力相结合的产物。

简单来说，它是一台能够像赛格威一样，仅靠两个轮子就保持动态平衡，同时又具备自主“大脑”，可以独立完成移动、转向、甚至执行简单任务的机器人平台。这不仅仅是把平衡车“无人化”，其技术内涵要丰富得多。传统的平衡车，其平衡完全依赖于骑手重心的实时反馈，是一个“人机闭环”系统。而我们要做的，是创造一个能够自我感知姿态、自主决策如何移动以维持平衡，并同时处理外部任务（如路径规划、避障）的“全自主闭环”系统。

这个项目适合谁呢？首先，它非常适合机器人学、控制工程、嵌入式系统方向的学生和工程师，作为一个综合性的实践项目，它能让你一次性接触到姿态传感器（IMU）、PID控制、电机驱动、ROS（机器人操作系统）、SLAM（同步定位与地图构建）等多个核心领域。其次，对于创客和硬件发烧友来说，打造一个能自己“站立”和行走的机器人，其成就感和观赏性远超普通的轮式或履带式小车。最后，从应用场景看，这种高度机动、占地面积小的自平衡机器人，在室内巡检、物流搬运（尤其是狭窄空间）、展示导览甚至家庭服务机器人领域，都有独特的潜力。接下来，我就把自己从零搭建这个项目的完整过程、核心原理、踩过的坑以及一些独家心得，毫无保留地分享给大家。

2. 核心设计思路与方案选型

做一个自平衡机器人，听起来很复杂，但我们可以把它拆解成几个清晰的子系统来看待。整个系统的核心矛盾在于：平衡控制环（高频、毫秒级响应）与任务决策环（低频、秒级响应）如何协同工作而不互相干扰。

2.1 总体架构设计

我的设计采用了经典的“分层控制”架构，这在实际机器人系统中非常普遍，能有效解耦不同实时性要求的任务。

1. 底层执行层（平衡控制层）：这是机器人的“小脑”和“脊髓”。它只关心一件事：无论上身发生什么（比如负载变化），也无论上层发出什么移动指令，都要确保机器人本体不摔倒。这一层需要极高的运行频率（通常100Hz以上），直接读取惯性测量单元（IMU）的数据，计算当前姿态角（俯仰角），并通过PID控制器快速计算出需要给两个轮子电机输出的扭矩。这一层通常运行在实时性强的微控制器（如STM32）上。

2. 中层协调层（运动控制层）：这是机器人的“脑干”。它负责接收来自上层（决策层）的速度、角速度指令（例如：“以0.3米/秒的速度向前走”），并将这些指令“翻译”成底层平衡控制层能理解的“目标姿态”。举个例子，想让机器人前进，中层控制并不是直接命令轮子转，而是告诉底层：“请将机器人的俯仰角向前倾斜X度”。底层为了维持这个“目标倾斜角”，自然会驱动轮子向前转动来“追赶”这个角度，从而产生向前的运动。转向控制也是类似，通过给两个轮子差速来实现。这一层可以放在性能更强的微控制器或与底层共用。

3. 上层决策层（导航与感知层）：这是机器人的“大脑”。它负责高级任务，比如“从A点走到B点”。它需要处理来自激光雷达、摄像头等传感器的数据，构建环境地图（SLAM），规划出一条无碰撞的路径，并将路径分解成一系列的速度指令发送给中层。这一层对实时性要求相对较低，但计算量大，通常运行在嵌入式计算机（如树莓派、Jetson Nano）或PC上，并常采用ROS来管理各个功能模块（节点）。

注意：很多初学者会试图用一个处理器（比如树莓派）同时做平衡PID控制和图像SLAM，这极易导致系统崩溃。因为Linux系统并非实时操作系统，PID控制循环一旦被高优先级的系统任务打断，延迟几毫秒就足以让机器人摔倒。所以硬件分离或实时内核补丁是必须考虑的设计原则。

2.2 关键硬件选型解析

硬件是项目的骨架，选型直接决定了项目的难度上限和性能下限。

1. 主控芯片（底层）：我选择了STM32F4系列（具体是F407）。原因有三：首先，它拥有Cortex-M4内核带FPU（浮点运算单元），做姿态解算和浮点PID运算速度足够快；其次，它有丰富的定时器和通信接口（如CAN, UART, I2C, SPI），方便连接各类传感器和驱动电机；最后，其生态完善，开发工具（如STM32CubeIDE）和资料众多。对于更极致的性能，可以考虑STM32H7系列。

2. 姿态传感器（IMU）：这是平衡的“眼睛”。我强烈推荐使用MPU6050（或MPU6500）作为入门，它集成了三轴陀螺仪和三轴加速度计，性价比极高。但要注意，原始数据噪声大且存在温漂。为了获得稳定的姿态角，必须进行传感器融合。我采用了经典的互补滤波算法入门，后来升级为卡尔曼滤波。对于要求更高的项目，可以直接选用集成算法、能直接输出稳定姿态角的IMU模块，如BMI160或ICM-20948，它们能减轻主控的计算压力。

3. 电机与驱动器：这是机器人的“双腿”。平衡机器人对电机的要求很高：需要快速响应、高扭矩、良好的速度控制特性。

   • 电机类型：无刷直流电机（BLDC）或大扭矩直流减速电机是主流选择。BLDC效率高、扭矩大，但驱动电路复杂；直流减速电机驱动简单，但需要选择带编码器的型号，用于测速和闭环控制。
   • 我的选择：我使用了两个带霍尔编码器的直流减速电机（减速比约30:1，额定电压12V）。编码器分辨率（每转脉冲数，PPR）越高，速度测量越精确，对平衡越有利。
   • 驱动器：必须选择支持双路PWM控制和编码器反馈的驱动器。我使用了TB6612FNG双路电机驱动模块入门，但它电流较小（1.2A）。后期为了驱动更重的机身，换成了基于MOS管的分立H桥驱动电路，配合STM32的定时器产生PWM和捕获编码器信号。

4. 上层决策平台：我选择了树莓派4B。它性能足够运行ROS Noetic和基础的SLAM算法（如gmapping），接口丰富（USB、CSI等），方便连接激光雷达（如RPLIDAR A1）和摄像头。电源管理上，需要单独为树莓派提供稳定的5V电源，并与电机驱动电源隔离，防止电机启动时的电压浪涌导致树莓派重启。

5. 结构件与电源：

   • 车体结构：可以使用铝型材、亚克力板或3D打印件来制作。核心是保证IMU安装位置尽量靠近车体的旋转中心，并紧固无振动。两个轮子的轴距需要仔细考量：轴距太短，平衡更灵敏但容易振荡；轴距太长，稳定性好但转向笨拙。我的第一版轴距为25cm，是一个不错的折中点。
   • 电源：平衡机器人瞬间电流可能很大。我使用了一节12V 锂电池组（容量约2000mAh）为电机驱动供电，并通过一个DC-DC降压模块降至5V为树莓派和STM32供电。务必确保电池的放电倍率（C数）足够，否则在机器人加速或爬坡时可能导致电压骤降而失控。

3. 核心算法解析与姿态控制实现

这是项目的灵魂所在，也是调试过程中最“磨人”的部分。我们将深入探讨如何让机器人“站”起来。

3.1 姿态感知：从原始数据到稳定角度

IMU直接读出的陀螺仪（角速度）和加速度计数据是不能直接用的。陀螺仪积分可以得到角度，但存在累积误差（漂移）；加速度计在静态时可以通过重力分量解算倾角，但在运动时会被线性加速度严重干扰。

解决方案：传感器融合。我实现了两种算法进行对比：

1. 互补滤波：这是一个简单高效的入门算法。其核心思想是：利用高通滤波器滤除陀螺仪的低频漂移误差，利用低通滤波器滤除加速度计的高频运动噪声，然后将两者按一个权重系数α融合。

   • 公式：angle = α * (angle + gyro * dt) + (1 - α) * acc_angle
   • 代码片段（简化）：

     // dt为采样周期，Gyro_z为Z轴角速度，Accel_angle为加速度计解算的俯仰角 float ComplementaryFilter(float Gyro_z, float Accel_angle, float dt) { static float angle = 0.0f; const float alpha = 0.98f; // 权重系数，需调试 // 陀螺仪积分（高通特性） angle += Gyro_z * dt; // 与加速度计融合（低通特性） angle = alpha * angle + (1 - alpha) * Accel_angle; return angle; }

   • 调试心得：系数α是调参关键。α越接近1，系统越信任陀螺仪，响应快但漂移大；α越小，越信任加速度计，静态稳但动态响应差。我通常从0.96开始调试。

2. 卡尔曼滤波：这是一种最优估计算法，性能远超互补滤波，但理论复杂。它通过预测和更新两个步骤，结合系统模型和测量值，给出状态（角度和角速度）的最优估计。在STM32上实现一维卡尔曼滤波（只估计俯仰角）是完全可行的。

   • 优势：能更有效地抑制噪声，在动态情况下也能给出更准确的角度估计。
   • 实操建议：网上有大量开源的C语言一维卡尔曼滤波代码。你不需要完全理解其数学推导，但需要理解几个关键参数：过程噪声协方差（Q）和测量噪声协方差（R）。Q代表了你对系统模型（即陀螺仪）的信任程度，Q越大，滤波器越相信测量值（加速度计）；R代表了你对测量值的信任程度，R越大，滤波器越相信预测值（陀螺仪）。调试过程就是调整Q和R的过程。

踩坑记录：最初我将IMU模块用螺丝直接固定在亚克力板上，电机一转，机器人就剧烈振荡。后来发现是电机振动传递到了IMU，导致加速度计数据中混入了大量噪声。解决方案：用海绵双面胶或硅胶垫将IMU模块“软性”粘贴在车体中心位置，能极大改善振动隔离。这是硬件上必须做好的“减震”措施。

3.2 平衡之魂：PID控制器的整定

得到稳定的俯仰角（current_angle）后，我们需要通过PID控制器来驱动电机，使这个角度稳定在我们设定的target_angle（通常为0度，即直立状态）。

PID控制器公式：Output = Kp * error + Ki * ∫error dt + Kd * d(error)/dt其中，error = target_angle - current_angle。

• 比例项（P）：与当前误差成正比。它提供了基本的恢复力。Kp太小，机器人无力回正，会缓慢倒下；Kp太大，机器人会产生剧烈振荡，在平衡点来回摆动。
• 积分项（I）：累积历史误差，用于消除静态误差（如重心不绝对居中导致的微小倾斜）。Ki太大会引起系统超调和振荡。
• 微分项（D）：与误差变化率成正比，具有“阻尼”作用，能预测误差变化趋势，抑制振荡。Kd是抑制振荡的关键，但太大也会导致系统响应迟钝。

我的调试步骤与口诀：

1. 先P后D，最后I：这是黄金法则。首先将Ki和Kd设为0。
2. 调大P值：慢慢增加Kp，直到机器人能对倾斜做出反应，但会在平衡点附近持续振荡（有点像“发抖”）。记下这个使系统开始振荡的Kp值。
3. 加入D值：逐渐增加Kd，你会发现振荡被逐渐抑制。目标是找到一个Kd值，让机器人能快速稳定在平衡点，没有或只有一两次轻微过冲。D项能有效抑制P项引起的振荡。
4. 微调I项：如果机器人稳定后，存在一个固定的倾斜角度（静态误差），再引入一个很小的Ki值来消除它。对于平衡机器人，I项通常很小，有时甚至可以设为0。
5. “扶起来”测试：用手将机器人推至一个倾斜角度，然后松开，观察它能否快速、平稳地回到直立状态，且不来回摇摆。

核心技巧：微分项d(error)/dt在离散系统中等于(本次error - 上次error) / 采样周期dt。这里有一个关键细节：直接对误差error进行微分，会引入很大的噪声（因为角度测量本身有噪声）。更专业的做法是对测量值（current_angle）进行微分来近似角速度，或者直接使用陀螺仪输出的角速度作为微分项（D项）的输入。这被称为“微分先行”或“使用测量微分”，能显著提高系统稳定性。我的最终方案是：Output = Kp * angle_error + Ki * angle_error_integral + Kd * gyro_rate。

3.3 运动控制：如何让它“走”起来

让机器人静止平衡只是第一步，我们还需要它能受控地移动。如前所述，我们通过改变“目标平衡点”来实现运动。

• 前进/后退：想让机器人前进，我们不是直接让轮子转，而是将target_angle设置为一个微小的前倾角度（例如+2度）。PID控制器为了“追”上这个目标角度，就会驱动轮子向前转动，从而产生向前的加速度。这个target_angle的偏移量，可以由上层根据期望速度通过一个比例关系映射过来，形成另一个速度环PID（外环），而平衡PID是角度环（内环）。
• 转向：转向通过差速实现。在计算最终输出给左右轮的电机的PWM值时，在平衡控制量的基础上进行加减。例如：PWM_left = balance_output + turn_output；PWM_right = balance_output - turn_output。turn_output可以由上层根据期望的转向角速度给出。

代码结构框架示意：

void ControlLoop() { // 1. 读取传感器 ReadIMU(&gyro, &accel); ReadEncoders(&left_speed, &right_speed); // 2. 传感器融合，获取当前俯仰角 pitch pitch = KalmanFilter(gyro.z, accel.y, accel.z); // 假设pitch由Y、Z轴加速度计计算 // 3. 计算平衡控制量（角度环PID） angle_error = target_pitch - pitch; // target_pitch 通常为0，运动时由速度环修改 balance_output = PID_Angle(angle_error, gyro.z); // PID_Angle内部使用了陀螺仪角速度作为D项 // 4. 计算转向控制量 turn_output = PID_Turn(target_turn_rate - current_turn_rate); // 转向环PID // 5. 合成最终输出 left_motor_output = balance_output + turn_output; right_motor_output = balance_output - turn_output; // 6. 限幅并输出PWM LimitOutput(&left_motor_output, &right_motor_output); SetMotorPWM(left_motor_output, right_motor_output); }

4. 系统集成与上层应用开发

当底层机器人能够稳定受控地平衡和移动后，我们就可以为其赋予“智能”，让它从一台机器升级为一个机器人。

4.1 通信协议设计：上下层对话的桥梁

STM32（底层）和树莓派（上层）之间需要可靠、高效的通信。我选择了串口通信（UART），因为它简单、可靠，且双方都原生支持。

• 数据帧设计：为了抗干扰和解析方便，需要自定义一个简单的数据帧协议。我使用了如下结构：

  [帧头 0xAA] [帧头 0x55] [数据长度] [命令字] [数据区] [校验和] [帧尾 0x0D] [帧尾 0x0A]

  • 命令字：定义不同类型的指令，如0x01代表上层发送的速度指令，0x02代表上层查询机器人状态。
  • 数据区：对于速度指令，可以包含[前进速度 float][转向角速度 float]；对于状态查询回复，可以包含[电池电压 float][当前俯仰角 float][左轮速度 int][右轮速度 int]。
  • 校验和：通常对数据长度、命令字、数据区所有字节进行累加和，用于检错。

• 通信流程：树莓派以固定频率（如20Hz）通过串口向STM32发送包含目标速度和角速度的指令包。STM32接收到后解析，并更新target_pitch和target_turn_rate。同时，STM32可以定时向上层回传状态信息，用于监控。

4.2 ROS集成：赋予机器人“大脑”

在树莓派上安装ROS后，我们可以将机器人封装成一个ROS节点。

1. 创建驱动节点：编写一个C++/Python节点（例如segway_driver_node），这个节点负责：

   • 打开并配置连接到STM32的串口（/dev/ttyUSB0）。
   • 订阅ROS中的速度指令话题（通常是/cmd_vel，类型为geometry_msgs/Twist）。这是ROS中标准的移动基座控制接口。
   • 将/cmd_vel中的线速度linear.x和角速度angular.z按照一定比例关系，映射成前进的目标俯仰角偏移量和转向差速值，并通过串口协议发送给STM32。
   • 从串口读取STM32上传的状态数据，并发布到ROS话题（如/battery_voltage,/imu/data等）。

2. 配置导航栈：现在，你的自平衡机器人已经成为一个标准的ROS移动机器人了。你可以：

   • 使用robot_state_publisher和URDF模型描述机器人。
   • 连接一个激光雷达（如RPLIDAR），运行gmapping或cartographer进行SLAM建图。
   • 使用amcl进行定位。
   • 使用move_base进行全局和局部路径规划。move_base最终会生成/cmd_vel指令，正好被我们的驱动节点接收，从而控制机器人自主导航。

4.3 实际搭建与调试经验实录

1. 上电顺序与安全：务必先确保机器人被物理支撑（或用绳子吊起），轮子离地，再进行上电和程序下载调试。第一次调试平衡时，最好用两个木块在两侧轻轻夹住机器人，防止它失控猛冲出去。

2. PID参数调试现场：这是我调试第一台样机时的参数记录：

   • 初始振荡期：Kp=20, Ki=0, Kd=0。机器人对倾斜有反应，但一松手就向一边加速倒下。
   • 找到振荡点：逐渐增加Kp到35，机器人开始能在倒下前来回摆动几次。
   • 加入阻尼：开始增加Kd。当Kp=35, Kd=0.8时，机器人已经可以短暂（1-2秒）保持平衡，但仍有低频晃动。
   • 精细调整：微调至Kp=40, Ki=0.1, Kd=1.2。此时，机器人能够稳定直立超过10秒，用手轻推后能快速恢复。
   • 运动调试：引入速度环，将期望速度映射为目标角度偏移量，比例系数Kv=0.05。调试转向差速比例系数Kturn=0.3。

3. 电源噪声排查：在加入所有外设（树莓派、雷达）后，机器人平衡变得不稳定。用示波器检查给STM32供电的5V线路，发现电机启停时存在高达200mV的毛刺。解决方案：在电机驱动电源入口和单片机电源入口处，并联了大容量（1000uF）电解电容和小容量（0.1uF）陶瓷电容进行滤波，问题得到解决。

5. 常见问题与进阶优化方向

即使按照步骤操作，你也可能会遇到以下问题。这里是我的排查清单和后续升级思路。

5.1 故障排查速查表

5.2 项目进阶与优化思路

当你成功实现基础功能后，可以考虑以下方向让机器人变得更强大、更智能：

1. 状态估计升级：将卡尔曼滤波从一维（仅俯仰角）扩展到四元数或互补滤波的全姿态解算，这样可以得到更准确的横滚角、偏航角信息，为更复杂的运动（如原地旋转、抗侧向扰动）打下基础。

2. 控制算法升级：

   • 串级PID：实现更精确的速度控制。外环是速度PID，其输出作为内环（角度PID）的目标角度偏移量。这样你可以直接命令机器人“以0.5米/秒的速度前进”，而不是“向前倾斜2度”。
   • 模糊PID：传统的PID参数是固定的，但在机器人负载变化、速度变化时，最优参数可能不同。模糊PID可以根据误差和误差变化率动态调整Kp， Ki， Kd，适应能力更强。
   • 线性二次型调节器（LQR）：这是一种基于状态空间模型的最优控制方法，理论上能提供比PID更好的性能，但需要建立准确的系统数学模型。

3. 功能扩展：

   • 远程监控与操控：在树莓派上搭建一个Web服务器（如使用Flask），通过网页实时显示机器人摄像头画面、传感器数据，并发送控制指令。
   • 机械臂集成：在机器人顶部加装一个轻量级的舵机机械臂。这带来了新的挑战——机械臂运动会改变机器人的重心，需要平衡控制器具备一定的抗扰动能力，或者引入基于IMU数据的动态重心补偿算法。
   • 语音交互：接入离线的语音识别模块（如科大讯飞）或在线的语音助手，实现语音控制移动。

这个项目从构思到实现，是一个不断遇到问题、分析问题、解决问题的过程。最让我有成就感的时刻，不是它第一次颤颤巍巍地站起来，而是当我通过手机发送一个目标点，看着它自己规划路径、平稳避障、精准到达的那一刻。它从一个需要我时刻“搀扶”的学步车，真正成长为了一个能独立完成任务的自主伙伴。如果你也准备开始，我的建议是：不要畏惧数学和控制理论，从最简单的互补滤波和PID开始，亲手调试每一个参数，观察机器人的每一次反应。当你理解了每一个误差、每一次振荡背后的物理意义时，你收获的将不仅仅是一个会动的玩具，而是对动态系统控制的深刻直觉。这份直觉，对于任何从事自动化、 robotics 相关领域的工程师来说，都是无比宝贵的财富。