1. 项目缘起从“立不住”到“站得稳”的挑战两轮自平衡小车这个听起来就充满极客范儿的项目几乎是每个嵌入式、控制工程爱好者都想挑战的“毕业设计”。我第一次接触它是在实验室里看到学长做的一个原型它颤颤巍巍地立在桌上稍微一碰就东倒西歪甚至直接“躺平”。当时我就想这玩意儿真的能像赛格威那样稳定吗后来自己动手从买零件、焊板子、调代码到最终小车能稳稳地跟着我的手移动整个过程踩的坑、掉的头发现在回想起来都是宝贵的经验。今天我就把自己从零搭建一个两轮自平衡小车的完整过程、核心原理和那些“教科书上不会写”的调试心得掰开揉碎了分享给你。这个项目的核心说白了就是让一个只有两个轮子的机器人像倒立的摆锤一样自己保持直立平衡。这背后是卡尔曼滤波和PID控制两大经典算法的精妙配合。卡尔曼滤波负责从“嘈杂”的传感器数据中估算出小车最真实的姿态角度和角速度PID控制器则根据这个估算值快速、精准地计算出电机应该输出多大的力来“扶住”小车。整个过程是一个动态、实时的闭环。无论你是电子专业的学生想做个亮眼的毕设还是嵌入式工程师想深入理解传感器融合与控制理论这个项目都是一个绝佳的实践平台。它能让你把书本上的矩阵运算、微分方程变成眼前一个活生生、能跑能跳的智能体。2. 系统总览硬件骨架与软件灵魂的构建在动手写第一行代码之前我们必须把整个系统的框架搭清楚。一辆自平衡小车可以看作一个由感知、决策、执行三部分构成的有机体。2.1 硬件选型为什么是它们硬件是项目的物理基础选型直接决定了系统的性能上限和调试难度。主控芯片大脑我强烈推荐使用STM32F4系列比如STM32F407或F405。原因有三第一它拥有Cortex-M4内核带硬件浮点运算单元FPU。我们后续的卡尔曼滤波涉及大量浮点矩阵运算有FPU比没有FPU的速度能快几十倍这是保证控制周期稳定的关键。第二它外设丰富有多个定时器可以方便地生成PWM波驱动电机还有SPI、I2C接口连接传感器。第三社区资源庞大遇到问题容易找到解决方案。当然如果预算有限STM32F1系列如F103C8T6即“蓝色药丸”也能用但需要优化算法避免浮点运算成为瓶颈。姿态传感器小脑这是小车的“平衡感”来源。MPU6050是绝对的首选它集成了三轴加速度计和三轴陀螺仪价格低廉资料齐全。加速度计可以测量重力加速度在各轴上的分量从而解算出姿态角但容易受小车运动加速度的干扰陀螺仪通过测量角速度积分得到角度短期精度高但存在累积误差漂移。两者优缺点互补正是卡尔曼滤波施展拳脚的舞台。电机与驱动四肢电机决定了小车的“力气”和“速度”。我选用的是N20减速直流电机搭配橡胶轮。减速电机扭矩大低速时也能提供足够的纠正力矩。驱动芯片方面TB6612FNG或DRV8833这类双路H桥驱动是主流选择它们比古老的L298N效率高、发热小。电机的编码器至关重要我推荐使用AB相增量式编码器它可以反馈电机的转速和旋转方向为后续的速度环、位置环控制提供反馈。电源心脏系统需要稳定的电源。通常使用一块7.4V或11.1V的锂电池通过降压模块如LM2596为单片机、传感器和驱动芯片提供5V或3.3V电压。务必确保电池的放电倍率C数足够能在电机启动瞬间提供大电流。2.2 软件框架一个清晰的控制循环软件上我们采用前后台系统在定时器中断中实现严格周期性的控制循环。这是整个项目稳定性的生命线。// 伪代码示意主循环框架 int main() { hardware_init(); // 初始化GPIO、定时器、PWM、ADC、串口等 sensor_init(); // 初始化MPU6050I2C filter_init(); // 初始化卡尔曼滤波器参数 pid_init(); // 初始化PID控制器参数 encoder_init(); // 初始化编码器计数定时器编码器模式 // 配置一个定时器每5ms产生一次中断 timer_interrupt_enable(5ms); while(1) { // 后台任务参数调试、状态显示、蓝牙指令接收等 debug_and_communication_task(); } } // 定时器中断服务函数每5ms执行一次 void timer_isr() { read_sensor_data(); // 读取MPU6050的原始加速度和角速度数据 kalman_filter_update(); // 执行卡尔曼滤波得到最优估计的角度、角速度 read_encoder(); // 读取编码器值计算轮子转速 balance_pid_calculate(); // 直立环PID输入是角度输出是电机的基本速度 velocity_pid_calculate();// 速度环PID输入是目标速度与实际速度差微调输出 direction_pid_calculate();// 转向环PID处理遥控转向指令 motor_output(); // 综合三个环的输出生成最终的PWM值驱动电机 }这个5ms的中断周期是需要反复调试的关键参数。太快可能单片机算力跟不上太慢则系统响应迟钝容易失控。STM32F4在168MHz主频下完成一次完整的滤波和控制计算5ms是绰绰有余的。3. 感知核心卡尔曼滤波如何“去伪存真”拿到MPU6050的原始数据后你会发现它们噪声很大直接用来控制小车会抖得像“打摆子”。这时就需要卡尔曼滤波出场了。别被它的数学公式吓到我们可以用一个“天气预报”的模型来理解它。3.1 建立小车的“运动模型”我们把小车倾角的变化看作一个物理过程。假设我们知道上一时刻小车的角度和角速度根据物理定律角速度积分等于角度变化我们可以预测出当前时刻的角度应该是什么样。这个预测基于模型但模型不完美存在“过程噪声”比如地面不平带来的微小扰动。同时我们有两个“观测员”在测量角度一个是加速度计它通过测量重力分量来算角度但容易被小车前进后退的加速度“欺骗”产生高频噪声另一个是陀螺仪它通过积分角速度得到角度非常平滑但会随着时间“跑偏”积分漂移产生低频误差。卡尔曼滤波就像一个聪明的指挥官它既相信自己对运动规律的预测模型也相信观测员的报告测量但都不全信。它会根据两者各自的“可信度”在算法中体现为协方差矩阵计算出一个最优的、融合后的估计值。这个估计值比任何单一来源的数据都更接近真实情况。3.2 一维卡尔曼滤波的简化实现对于自平衡小车我们通常最关心的是车体绕轮轴旋转的俯仰角Pitch。我们可以将问题简化对俯仰角单独实施一个一维卡尔曼滤波。以下是核心步骤的简化代码阐述// 定义卡尔曼滤波器结构体 typedef struct { float angle; // 最优估计角度 float bias; // 陀螺仪零偏估计 float rate; // 最优估计角速度 float P[2][2]; // 误差协方差矩阵 float Q_angle; // 过程噪声角度 float Q_bias; // 过程噪声零偏 float R_measure; // 测量噪声加速度计 } Kalman_t; // 卡尔曼滤波更新函数 float kalman_update(Kalman_t *k, float new_angle, float new_rate, float dt) { // 1. 预测阶段基于陀螺仪数据更新状态预测 k-rate new_rate - k-bias; // 用当前角速度减去估计的零偏 k-angle dt * k-rate; // 角度 旧角度 角速度*时间 // 更新误差协方差矩阵P这里简化了矩阵运算体现思想 // P P Q其中Q是过程噪声表示预测的不确定性在增加 k-P[0][0] dt * (dt*k-P[1][1] - k-P[0][1] - k-P[1][0] k-Q_angle); k-P[0][1] - dt * k-P[1][1]; k-P[1][0] - dt * k-P[1][1]; k-P[1][1] k-Q_bias * dt; // 2. 更新阶段融合加速度计测量值 float y new_angle - k-angle; // 测量残差 float S k-P[0][0] k-R_measure; // 残差协方差 float K[2]; // 卡尔曼增益 K[0] k-P[0][0] / S; K[1] k-P[1][0] / S; // 用增益修正预测值 k-angle K[0] * y; k-bias K[1] * y; // 更新估计误差协方差 float P00_temp k-P[0][0]; float P01_temp k-P[0][1]; k-P[0][0] - K[0] * P00_temp; k-P[0][1] - K[0] * P01_temp; k-P[1][0] - K[1] * P00_temp; k-P[1][1] - K[1] * P01_temp; return k-angle; // 返回最优估计角度 }注意上述代码是高度简化的原理性展示。实际中Q_angle、Q_bias、R_measure这三个噪声参数需要根据实际系统调试。一个实用的技巧是先将小车静止放置采集几百组MPU6050的原始数据用MATLAB或Python计算加速度计角度和陀螺仪积分角度的标准差用这个量级作为调试的初始参考值。3.3 互补滤波一个更轻量的替代方案如果你的主控芯片资源非常紧张或者想先快速让小车站起来互补滤波是一个极佳的选择。它的思想极其直观利用高通滤波器滤掉陀螺仪的低频漂移利用低通滤波器滤掉加速度计的高频噪声然后将两者按一个固定系数融合。// 一阶互补滤波 float complementary_filter(float acc_angle, float gyro_rate, float dt) { static float angle 0; float K 0.98; // 融合系数通常取0.98左右表示更相信陀螺仪一些 // 角度 K * (旧角度 陀螺仪角速度*dt) (1-K) * 加速度计角度 angle K * (angle gyro_rate * dt) (1 - K) * acc_angle; return angle; }互补滤波效果不错调参简单主要调一个K值但理论上的最优性不如卡尔曼滤波。我的建议是初学者可以从互补滤波入手快速验证硬件和控制逻辑追求性能和控制品质时再深入调试卡尔曼滤波。4. 控制大脑PID三环串级控制详解得到了稳定可靠的角度信息接下来就要思考如何控制电机来维持平衡了。单一的比例控制P会让小车在平衡点附近来回振荡加入微分控制D可以抑制振荡提供阻尼积分控制I则可以消除静态误差。对于自平衡小车我们通常采用串级PID控制这是整个项目中最需要耐心调试的部分。4.1 直立环内环快速响应的“肌肉反射”直立环是平衡的基础它直接响应角度变化必须足够快。它的输入是期望角度通常设为0度即完全直立与卡尔曼滤波估计出的实际角度之差输出是一个基础的控制量可以理解为电机的目标速度。// 直立环PD控制器 float balance_pid_calculate(float current_angle, float current_gyro) { float desired_angle 0.0; // 目标角度直立 float angle_error desired_angle - current_angle; // PD控制输出 Kp * 角度误差 Kd * 角速度 // 注意这里直接使用陀螺仪测量的角速度作为微分项比用角度的差分更抗噪声 float output BALANCE_KP * angle_error BALANCE_KD * current_gyro; return output; }Kp比例系数决定了小车“扶正”自己的力度。Kp太小小车反应慢一推就倒Kp太大小车会剧烈抖动甚至在平衡点高频振荡。调试时先将Kd设为0慢慢增大Kp直到小车能对倾斜做出明显反应但尚未开始剧烈抖动为止。Kd微分系数相当于一个“阻尼器”。它感知角度变化的趋势角速度在小车快速向一边倒下时提供一个反向的抑制力。它能有效平滑Kp引起的振荡。调试时在Kp的基础上慢慢加入Kd你会看到小车的抖动逐渐消失变得“沉稳”起来。实操心得直立环的调试必须在用手扶住小车的情况下进行通过串口实时输出角度和输出值观察当你手动给小车一个倾斜时输出值是否快速、正确地向反方向变化。确保电机转向正确角度为正时电机应使小车向前运动以恢复平衡。4.2 速度环外环隐形的“平衡大师”如果只有直立环你会发现一个有趣的现象小车虽然能站住但会慢慢地朝一个方向移动直到倒下。这是因为任何微小的偏差或不对称都会在直立环的作用下被放大成持续的运动。速度环就是为了解决这个问题而生的。速度环的输入是目标速度通常为0即希望小车静止与编码器反馈的实际速度之差。它的输出会作为一个修正量叠加到直立环的输出上。它的作用是当小车无意中开始移动时速度环会感知到并轻微地调整小车的目标角度让它“主动向前或向后倒一点”从而产生一个反向的加速度来抵消移动。// 速度环PI控制器 float velocity_pid_calculate(float desired_velocity) { static float integral 0; float current_velocity get_velocity_from_encoder(); // 从编码器计算当前速度 float velocity_error desired_velocity - current_velocity; integral velocity_error; // 积分项累加 // 积分限幅防止积分饱和 if(integral VELOCITY_INTEGRAL_LIMIT) integral VELOCITY_INTEGRAL_LIMIT; if(integral -VELOCITY_INTEGRAL_LIMIT) integral -VELOCITY_INTEGRAL_LIMIT; // PI控制输出 Kp * 速度误差 Ki * 速度误差积分 float output VELOCITY_KP * velocity_error VELOCITY_KI * integral; return output; }调试技巧速度环的调试必须在直立环已经基本调好的基础上进行。先将速度环的Kp和Ki设为0让小车仅用直立环站立。然后用手轻轻推小车它会抵抗一下然后开始移动。这时逐渐加入一个很小的速度环Kp你会发现小车抵抗移动的能力变强了甚至能在你推动后慢慢回到原位。Ki参数用于消除静差但要非常小心Ki太大会引起低频振荡。4.3 转向环独立的“方向盘”转向环相对独立它处理来自遥控器如蓝牙、无线模块的转向指令。输入是目标转向角速度与当前陀螺仪Z轴角速度的差值输出直接叠加到左右电机的差速上。float direction_pid_calculate(float desired_turn_rate) { float current_turn_rate get_gyro_z(); // 获取Z轴角速度 float turn_error desired_turn_rate - current_turn_rate; // 通常一个简单的P控制就够了 float output DIRECTION_KP * turn_error; return output; }最终电机的控制量是三个环输出的综合左电机PWM 直立环输出 速度环输出 - 转向环输出右电机PWM 直立环输出 速度环输出 转向环输出5. 调试血泪史从“跳舞”到“稳如老狗”的必经之路理论是美好的调试是残酷的。下面是我在调试过程中遇到的最典型的几个问题及解决方案。5.1 小车剧烈振荡甚至“跳舞”这是最常见的问题。可能的原因和排查步骤检查极性这是首要怀疑对象确保直立环的极性正确。角度为正小车前倾时电机输出应为正小车向前运动。你可以在代码里手动设置一个固定的正角度观察小车是否向前走。如果不走或者向后走将电机线序对调或者在代码里给输出乘以-1。降低Kp增加Kd如果极性正确但小车在平衡点高频抖动说明直立环的Kp太大或Kd太小。先大幅降低Kp观察反应。然后逐步增加Kd增加系统的阻尼。检查控制周期确保你的控制中断周期是稳定的。在中断服务函数里放一个IO口电平翻转用示波器测量方波频率看是否是预设的5ms。周期不稳定会导致控制效果时好时坏。传感器数据是否稳定将卡尔曼滤波输出的角度通过串口打印出来观察小车静止时角度数据是否平稳。如果原始数据噪声就很大需要检查MPU6050的安装是否牢固或者尝试降低其数字低通滤波器DLPF的带宽。5.2 小车朝一个方向缓慢移动直至倒下这是速度环未起作用或参数不合适的典型表现。确认速度环已启用且极性正确速度环的输出应该能抑制移动。你可以手动给速度环一个很小的固定输出观察小车是加速还是减速。如果加速说明速度环输出极性反了。调整速度环参数速度环的Kp需要非常小通常只有直立环Kp的几十分之一甚至百分之一。从0.001这个量级开始尝试。Ki更要谨慎从0开始如果需要以0.0001的步进微调。检查编码器确保编码器计数方向正确并且能正确反映电机的真实转速。可以用手转动轮子通过串口查看速度值是否线性变化。5.3 参数整定的“懒人”步骤对于PID调试一个比较稳妥的步骤是单独调直立环将速度环和转向环输出置零。手持小车倾斜它通过串口观察输出值变化是否合理。先调Kp让小车有“劲”再调Kd消除抖动。目标是手持时小车能温和而坚定地抵抗你的倾斜。加入速度环让小车在空旷地面自由站立。先给一个极小的速度环Kp如0.001。观察小车是否还能站住并且抵抗轻微推动的能力是否增强。然后微调Kp和Ki目标是轻推后小车能移动一小段距离然后稳稳停住或者缓慢回到原点。最后调转向环转向环独立一般P控制即可让小车能响应遥控指令平滑转向。5.4 硬件上的坑电源干扰电机启动瞬间会产生很大的电流尖峰可能导致单片机复位或传感器数据异常。务必在电机电源端并联大容量如470uF以上的电解电容并在单片机电源入口处加磁珠和去耦电容。机械结构重心越高惯性越大平衡难度越高。尽量将电池等重物放在车体下方降低重心。车轴与轮子要垂直机械结构要对称、紧固任何松动都会引入无法滤除的振动噪声。死区电压电机有启动电压阈值低于这个电压电机不转。如果PID计算出的PWM值很小可能不足以驱动电机导致控制失灵。可以在电机输出函数里设置一个死区补偿。6. 功能进阶让小车从“站稳”到“跑起来”当小车能稳定直立后你就可以在此基础上添加更多有趣的功能了。6.1 蓝牙遥控与APP控制通过HC-05或HC-08蓝牙模块连接单片机串口。在手机端可以用串口调试APP或者自己用MIT App Inventor、Android Studio编写一个简单的控制APP发送前进、后退、转向等指令。单片机解析指令将其转化为速度环的目标速度或转向环的目标角速度。6.2 循迹与避障在车底加装红外或灰度传感器阵列可以实现循迹功能。传感器检测到黑线时通过一个独立的循迹PID算法计算出偏差将这个偏差作为转向环的附加输入或者直接叠加到电机差速上。加装超声波模块如HC-SR04或TOF激光测距传感器可以实现避障或定距跟随。获取前方距离后可以设计一个简单的逻辑当距离小于阈值时速度环的目标速度设为负值后退。6.3 姿态解算与更多传感器融合MPU6050还可以提供三轴加速度和角速度通过更复杂的算法如四元数、Mahony滤波、DMP库可以解算出完整的3D姿态俯仰、横滚、偏航。这为小车实现更复杂的动作比如原地旋转、特定角度倾斜等提供了可能。你还可以融合电子罗盘如HMC5883L来获得绝对的方向信息。调试自平衡小车的过程是一个不断与物理世界和数学模型对话的过程。每一次参数的微调都让你对“反馈”、“稳定”、“动态响应”这些概念有更深的理解。当你的小车第一次摆脱你的手独自稳稳立在桌面时那种成就感是无与伦比的。它不再是一堆零件和代码而是一个被你赋予了“生命”的智能体。这个项目所涉及的传感器融合、实时控制、嵌入式编程思想将会在你后续的机器人、自动驾驶等相关项目中持续发挥作用。 从零搭建两轮自平衡小车：卡尔曼滤波与PID串级控制实战
1. 项目缘起从“立不住”到“站得稳”的挑战两轮自平衡小车这个听起来就充满极客范儿的项目几乎是每个嵌入式、控制工程爱好者都想挑战的“毕业设计”。我第一次接触它是在实验室里看到学长做的一个原型它颤颤巍巍地立在桌上稍微一碰就东倒西歪甚至直接“躺平”。当时我就想这玩意儿真的能像赛格威那样稳定吗后来自己动手从买零件、焊板子、调代码到最终小车能稳稳地跟着我的手移动整个过程踩的坑、掉的头发现在回想起来都是宝贵的经验。今天我就把自己从零搭建一个两轮自平衡小车的完整过程、核心原理和那些“教科书上不会写”的调试心得掰开揉碎了分享给你。这个项目的核心说白了就是让一个只有两个轮子的机器人像倒立的摆锤一样自己保持直立平衡。这背后是卡尔曼滤波和PID控制两大经典算法的精妙配合。卡尔曼滤波负责从“嘈杂”的传感器数据中估算出小车最真实的姿态角度和角速度PID控制器则根据这个估算值快速、精准地计算出电机应该输出多大的力来“扶住”小车。整个过程是一个动态、实时的闭环。无论你是电子专业的学生想做个亮眼的毕设还是嵌入式工程师想深入理解传感器融合与控制理论这个项目都是一个绝佳的实践平台。它能让你把书本上的矩阵运算、微分方程变成眼前一个活生生、能跑能跳的智能体。2. 系统总览硬件骨架与软件灵魂的构建在动手写第一行代码之前我们必须把整个系统的框架搭清楚。一辆自平衡小车可以看作一个由感知、决策、执行三部分构成的有机体。2.1 硬件选型为什么是它们硬件是项目的物理基础选型直接决定了系统的性能上限和调试难度。主控芯片大脑我强烈推荐使用STM32F4系列比如STM32F407或F405。原因有三第一它拥有Cortex-M4内核带硬件浮点运算单元FPU。我们后续的卡尔曼滤波涉及大量浮点矩阵运算有FPU比没有FPU的速度能快几十倍这是保证控制周期稳定的关键。第二它外设丰富有多个定时器可以方便地生成PWM波驱动电机还有SPI、I2C接口连接传感器。第三社区资源庞大遇到问题容易找到解决方案。当然如果预算有限STM32F1系列如F103C8T6即“蓝色药丸”也能用但需要优化算法避免浮点运算成为瓶颈。姿态传感器小脑这是小车的“平衡感”来源。MPU6050是绝对的首选它集成了三轴加速度计和三轴陀螺仪价格低廉资料齐全。加速度计可以测量重力加速度在各轴上的分量从而解算出姿态角但容易受小车运动加速度的干扰陀螺仪通过测量角速度积分得到角度短期精度高但存在累积误差漂移。两者优缺点互补正是卡尔曼滤波施展拳脚的舞台。电机与驱动四肢电机决定了小车的“力气”和“速度”。我选用的是N20减速直流电机搭配橡胶轮。减速电机扭矩大低速时也能提供足够的纠正力矩。驱动芯片方面TB6612FNG或DRV8833这类双路H桥驱动是主流选择它们比古老的L298N效率高、发热小。电机的编码器至关重要我推荐使用AB相增量式编码器它可以反馈电机的转速和旋转方向为后续的速度环、位置环控制提供反馈。电源心脏系统需要稳定的电源。通常使用一块7.4V或11.1V的锂电池通过降压模块如LM2596为单片机、传感器和驱动芯片提供5V或3.3V电压。务必确保电池的放电倍率C数足够能在电机启动瞬间提供大电流。2.2 软件框架一个清晰的控制循环软件上我们采用前后台系统在定时器中断中实现严格周期性的控制循环。这是整个项目稳定性的生命线。// 伪代码示意主循环框架 int main() { hardware_init(); // 初始化GPIO、定时器、PWM、ADC、串口等 sensor_init(); // 初始化MPU6050I2C filter_init(); // 初始化卡尔曼滤波器参数 pid_init(); // 初始化PID控制器参数 encoder_init(); // 初始化编码器计数定时器编码器模式 // 配置一个定时器每5ms产生一次中断 timer_interrupt_enable(5ms); while(1) { // 后台任务参数调试、状态显示、蓝牙指令接收等 debug_and_communication_task(); } } // 定时器中断服务函数每5ms执行一次 void timer_isr() { read_sensor_data(); // 读取MPU6050的原始加速度和角速度数据 kalman_filter_update(); // 执行卡尔曼滤波得到最优估计的角度、角速度 read_encoder(); // 读取编码器值计算轮子转速 balance_pid_calculate(); // 直立环PID输入是角度输出是电机的基本速度 velocity_pid_calculate();// 速度环PID输入是目标速度与实际速度差微调输出 direction_pid_calculate();// 转向环PID处理遥控转向指令 motor_output(); // 综合三个环的输出生成最终的PWM值驱动电机 }这个5ms的中断周期是需要反复调试的关键参数。太快可能单片机算力跟不上太慢则系统响应迟钝容易失控。STM32F4在168MHz主频下完成一次完整的滤波和控制计算5ms是绰绰有余的。3. 感知核心卡尔曼滤波如何“去伪存真”拿到MPU6050的原始数据后你会发现它们噪声很大直接用来控制小车会抖得像“打摆子”。这时就需要卡尔曼滤波出场了。别被它的数学公式吓到我们可以用一个“天气预报”的模型来理解它。3.1 建立小车的“运动模型”我们把小车倾角的变化看作一个物理过程。假设我们知道上一时刻小车的角度和角速度根据物理定律角速度积分等于角度变化我们可以预测出当前时刻的角度应该是什么样。这个预测基于模型但模型不完美存在“过程噪声”比如地面不平带来的微小扰动。同时我们有两个“观测员”在测量角度一个是加速度计它通过测量重力分量来算角度但容易被小车前进后退的加速度“欺骗”产生高频噪声另一个是陀螺仪它通过积分角速度得到角度非常平滑但会随着时间“跑偏”积分漂移产生低频误差。卡尔曼滤波就像一个聪明的指挥官它既相信自己对运动规律的预测模型也相信观测员的报告测量但都不全信。它会根据两者各自的“可信度”在算法中体现为协方差矩阵计算出一个最优的、融合后的估计值。这个估计值比任何单一来源的数据都更接近真实情况。3.2 一维卡尔曼滤波的简化实现对于自平衡小车我们通常最关心的是车体绕轮轴旋转的俯仰角Pitch。我们可以将问题简化对俯仰角单独实施一个一维卡尔曼滤波。以下是核心步骤的简化代码阐述// 定义卡尔曼滤波器结构体 typedef struct { float angle; // 最优估计角度 float bias; // 陀螺仪零偏估计 float rate; // 最优估计角速度 float P[2][2]; // 误差协方差矩阵 float Q_angle; // 过程噪声角度 float Q_bias; // 过程噪声零偏 float R_measure; // 测量噪声加速度计 } Kalman_t; // 卡尔曼滤波更新函数 float kalman_update(Kalman_t *k, float new_angle, float new_rate, float dt) { // 1. 预测阶段基于陀螺仪数据更新状态预测 k-rate new_rate - k-bias; // 用当前角速度减去估计的零偏 k-angle dt * k-rate; // 角度 旧角度 角速度*时间 // 更新误差协方差矩阵P这里简化了矩阵运算体现思想 // P P Q其中Q是过程噪声表示预测的不确定性在增加 k-P[0][0] dt * (dt*k-P[1][1] - k-P[0][1] - k-P[1][0] k-Q_angle); k-P[0][1] - dt * k-P[1][1]; k-P[1][0] - dt * k-P[1][1]; k-P[1][1] k-Q_bias * dt; // 2. 更新阶段融合加速度计测量值 float y new_angle - k-angle; // 测量残差 float S k-P[0][0] k-R_measure; // 残差协方差 float K[2]; // 卡尔曼增益 K[0] k-P[0][0] / S; K[1] k-P[1][0] / S; // 用增益修正预测值 k-angle K[0] * y; k-bias K[1] * y; // 更新估计误差协方差 float P00_temp k-P[0][0]; float P01_temp k-P[0][1]; k-P[0][0] - K[0] * P00_temp; k-P[0][1] - K[0] * P01_temp; k-P[1][0] - K[1] * P00_temp; k-P[1][1] - K[1] * P01_temp; return k-angle; // 返回最优估计角度 }注意上述代码是高度简化的原理性展示。实际中Q_angle、Q_bias、R_measure这三个噪声参数需要根据实际系统调试。一个实用的技巧是先将小车静止放置采集几百组MPU6050的原始数据用MATLAB或Python计算加速度计角度和陀螺仪积分角度的标准差用这个量级作为调试的初始参考值。3.3 互补滤波一个更轻量的替代方案如果你的主控芯片资源非常紧张或者想先快速让小车站起来互补滤波是一个极佳的选择。它的思想极其直观利用高通滤波器滤掉陀螺仪的低频漂移利用低通滤波器滤掉加速度计的高频噪声然后将两者按一个固定系数融合。// 一阶互补滤波 float complementary_filter(float acc_angle, float gyro_rate, float dt) { static float angle 0; float K 0.98; // 融合系数通常取0.98左右表示更相信陀螺仪一些 // 角度 K * (旧角度 陀螺仪角速度*dt) (1-K) * 加速度计角度 angle K * (angle gyro_rate * dt) (1 - K) * acc_angle; return angle; }互补滤波效果不错调参简单主要调一个K值但理论上的最优性不如卡尔曼滤波。我的建议是初学者可以从互补滤波入手快速验证硬件和控制逻辑追求性能和控制品质时再深入调试卡尔曼滤波。4. 控制大脑PID三环串级控制详解得到了稳定可靠的角度信息接下来就要思考如何控制电机来维持平衡了。单一的比例控制P会让小车在平衡点附近来回振荡加入微分控制D可以抑制振荡提供阻尼积分控制I则可以消除静态误差。对于自平衡小车我们通常采用串级PID控制这是整个项目中最需要耐心调试的部分。4.1 直立环内环快速响应的“肌肉反射”直立环是平衡的基础它直接响应角度变化必须足够快。它的输入是期望角度通常设为0度即完全直立与卡尔曼滤波估计出的实际角度之差输出是一个基础的控制量可以理解为电机的目标速度。// 直立环PD控制器 float balance_pid_calculate(float current_angle, float current_gyro) { float desired_angle 0.0; // 目标角度直立 float angle_error desired_angle - current_angle; // PD控制输出 Kp * 角度误差 Kd * 角速度 // 注意这里直接使用陀螺仪测量的角速度作为微分项比用角度的差分更抗噪声 float output BALANCE_KP * angle_error BALANCE_KD * current_gyro; return output; }Kp比例系数决定了小车“扶正”自己的力度。Kp太小小车反应慢一推就倒Kp太大小车会剧烈抖动甚至在平衡点高频振荡。调试时先将Kd设为0慢慢增大Kp直到小车能对倾斜做出明显反应但尚未开始剧烈抖动为止。Kd微分系数相当于一个“阻尼器”。它感知角度变化的趋势角速度在小车快速向一边倒下时提供一个反向的抑制力。它能有效平滑Kp引起的振荡。调试时在Kp的基础上慢慢加入Kd你会看到小车的抖动逐渐消失变得“沉稳”起来。实操心得直立环的调试必须在用手扶住小车的情况下进行通过串口实时输出角度和输出值观察当你手动给小车一个倾斜时输出值是否快速、正确地向反方向变化。确保电机转向正确角度为正时电机应使小车向前运动以恢复平衡。4.2 速度环外环隐形的“平衡大师”如果只有直立环你会发现一个有趣的现象小车虽然能站住但会慢慢地朝一个方向移动直到倒下。这是因为任何微小的偏差或不对称都会在直立环的作用下被放大成持续的运动。速度环就是为了解决这个问题而生的。速度环的输入是目标速度通常为0即希望小车静止与编码器反馈的实际速度之差。它的输出会作为一个修正量叠加到直立环的输出上。它的作用是当小车无意中开始移动时速度环会感知到并轻微地调整小车的目标角度让它“主动向前或向后倒一点”从而产生一个反向的加速度来抵消移动。// 速度环PI控制器 float velocity_pid_calculate(float desired_velocity) { static float integral 0; float current_velocity get_velocity_from_encoder(); // 从编码器计算当前速度 float velocity_error desired_velocity - current_velocity; integral velocity_error; // 积分项累加 // 积分限幅防止积分饱和 if(integral VELOCITY_INTEGRAL_LIMIT) integral VELOCITY_INTEGRAL_LIMIT; if(integral -VELOCITY_INTEGRAL_LIMIT) integral -VELOCITY_INTEGRAL_LIMIT; // PI控制输出 Kp * 速度误差 Ki * 速度误差积分 float output VELOCITY_KP * velocity_error VELOCITY_KI * integral; return output; }调试技巧速度环的调试必须在直立环已经基本调好的基础上进行。先将速度环的Kp和Ki设为0让小车仅用直立环站立。然后用手轻轻推小车它会抵抗一下然后开始移动。这时逐渐加入一个很小的速度环Kp你会发现小车抵抗移动的能力变强了甚至能在你推动后慢慢回到原位。Ki参数用于消除静差但要非常小心Ki太大会引起低频振荡。4.3 转向环独立的“方向盘”转向环相对独立它处理来自遥控器如蓝牙、无线模块的转向指令。输入是目标转向角速度与当前陀螺仪Z轴角速度的差值输出直接叠加到左右电机的差速上。float direction_pid_calculate(float desired_turn_rate) { float current_turn_rate get_gyro_z(); // 获取Z轴角速度 float turn_error desired_turn_rate - current_turn_rate; // 通常一个简单的P控制就够了 float output DIRECTION_KP * turn_error; return output; }最终电机的控制量是三个环输出的综合左电机PWM 直立环输出 速度环输出 - 转向环输出右电机PWM 直立环输出 速度环输出 转向环输出5. 调试血泪史从“跳舞”到“稳如老狗”的必经之路理论是美好的调试是残酷的。下面是我在调试过程中遇到的最典型的几个问题及解决方案。5.1 小车剧烈振荡甚至“跳舞”这是最常见的问题。可能的原因和排查步骤检查极性这是首要怀疑对象确保直立环的极性正确。角度为正小车前倾时电机输出应为正小车向前运动。你可以在代码里手动设置一个固定的正角度观察小车是否向前走。如果不走或者向后走将电机线序对调或者在代码里给输出乘以-1。降低Kp增加Kd如果极性正确但小车在平衡点高频抖动说明直立环的Kp太大或Kd太小。先大幅降低Kp观察反应。然后逐步增加Kd增加系统的阻尼。检查控制周期确保你的控制中断周期是稳定的。在中断服务函数里放一个IO口电平翻转用示波器测量方波频率看是否是预设的5ms。周期不稳定会导致控制效果时好时坏。传感器数据是否稳定将卡尔曼滤波输出的角度通过串口打印出来观察小车静止时角度数据是否平稳。如果原始数据噪声就很大需要检查MPU6050的安装是否牢固或者尝试降低其数字低通滤波器DLPF的带宽。5.2 小车朝一个方向缓慢移动直至倒下这是速度环未起作用或参数不合适的典型表现。确认速度环已启用且极性正确速度环的输出应该能抑制移动。你可以手动给速度环一个很小的固定输出观察小车是加速还是减速。如果加速说明速度环输出极性反了。调整速度环参数速度环的Kp需要非常小通常只有直立环Kp的几十分之一甚至百分之一。从0.001这个量级开始尝试。Ki更要谨慎从0开始如果需要以0.0001的步进微调。检查编码器确保编码器计数方向正确并且能正确反映电机的真实转速。可以用手转动轮子通过串口查看速度值是否线性变化。5.3 参数整定的“懒人”步骤对于PID调试一个比较稳妥的步骤是单独调直立环将速度环和转向环输出置零。手持小车倾斜它通过串口观察输出值变化是否合理。先调Kp让小车有“劲”再调Kd消除抖动。目标是手持时小车能温和而坚定地抵抗你的倾斜。加入速度环让小车在空旷地面自由站立。先给一个极小的速度环Kp如0.001。观察小车是否还能站住并且抵抗轻微推动的能力是否增强。然后微调Kp和Ki目标是轻推后小车能移动一小段距离然后稳稳停住或者缓慢回到原点。最后调转向环转向环独立一般P控制即可让小车能响应遥控指令平滑转向。5.4 硬件上的坑电源干扰电机启动瞬间会产生很大的电流尖峰可能导致单片机复位或传感器数据异常。务必在电机电源端并联大容量如470uF以上的电解电容并在单片机电源入口处加磁珠和去耦电容。机械结构重心越高惯性越大平衡难度越高。尽量将电池等重物放在车体下方降低重心。车轴与轮子要垂直机械结构要对称、紧固任何松动都会引入无法滤除的振动噪声。死区电压电机有启动电压阈值低于这个电压电机不转。如果PID计算出的PWM值很小可能不足以驱动电机导致控制失灵。可以在电机输出函数里设置一个死区补偿。6. 功能进阶让小车从“站稳”到“跑起来”当小车能稳定直立后你就可以在此基础上添加更多有趣的功能了。6.1 蓝牙遥控与APP控制通过HC-05或HC-08蓝牙模块连接单片机串口。在手机端可以用串口调试APP或者自己用MIT App Inventor、Android Studio编写一个简单的控制APP发送前进、后退、转向等指令。单片机解析指令将其转化为速度环的目标速度或转向环的目标角速度。6.2 循迹与避障在车底加装红外或灰度传感器阵列可以实现循迹功能。传感器检测到黑线时通过一个独立的循迹PID算法计算出偏差将这个偏差作为转向环的附加输入或者直接叠加到电机差速上。加装超声波模块如HC-SR04或TOF激光测距传感器可以实现避障或定距跟随。获取前方距离后可以设计一个简单的逻辑当距离小于阈值时速度环的目标速度设为负值后退。6.3 姿态解算与更多传感器融合MPU6050还可以提供三轴加速度和角速度通过更复杂的算法如四元数、Mahony滤波、DMP库可以解算出完整的3D姿态俯仰、横滚、偏航。这为小车实现更复杂的动作比如原地旋转、特定角度倾斜等提供了可能。你还可以融合电子罗盘如HMC5883L来获得绝对的方向信息。调试自平衡小车的过程是一个不断与物理世界和数学模型对话的过程。每一次参数的微调都让你对“反馈”、“稳定”、“动态响应”这些概念有更深的理解。当你的小车第一次摆脱你的手独自稳稳立在桌面时那种成就感是无与伦比的。它不再是一堆零件和代码而是一个被你赋予了“生命”的智能体。这个项目所涉及的传感器融合、实时控制、嵌入式编程思想将会在你后续的机器人、自动驾驶等相关项目中持续发挥作用。