简介这份基于STM32F407ZGT6与MPU6050的二轮自平衡车控制资料面向电子信息、自动化等专业学生及嵌入式爱好者完整呈现了利用速度环与位置环串级PID实现小车平衡的算法工程。资源共188个文件压缩包约5.33MB以C语言源码49个.c和头文件59个.h为主辅以Keil工程配置文件.uvprojx、.uvoptx、编译链接产物.o、.axf、.hex及调试映射文件.map、.crf等既可直接下载运行也便于对照学习PID调参与姿态解算流程。内容覆盖MPU6050驱动、数据读取与滤波以及串级PID控制器的搭建与整定适合作为课程设计、期末大作业或毕业设计的参考资料。目前已有357人学习使用对于想要快速上手平衡车项目的读者来说是一份结构完整的实战素材。1. 两轮平衡车的控制难点为什么MPU6050串级PID是标配基于STM32F407ZGT6和MPU6050的两轮自平衡车直接上电你会发现它根本站不住要么往前冲要么往后倒。原因很简单两轮平衡车本质是倒立摆重心高于轮轴天然不稳定。要实现“站住”控制器必须以毫秒级周期读取车身倾角并输出反向力矩。传感器方面MPU6050集成了三轴加速度计和陀螺仪是这类项目最常见的选型。控制算法方面只用单环PID往往会在启动或外力扰动时失控因为速度反馈没有参与调节。实际工程普遍采用串级PID外环速度环输出目标倾角内环角度环控制倾角两个环路配合才能兼顾直立与速度稳定。这篇笔记我基于STM32F407ZGT6加上MPU6050和电机编码器拆解整个实现过程适合正在做课程设计或想搞懂串级PID的嵌入式开发者。2. MPU6050姿态解算寄存器配置与DMP读取流程2.1 MPU6050的硬件接口与初始化MPU6050通过I2C接口与STM32F407ZGT6通信SDA和SCL分别接到PB7和PB6I2C1。我习惯用硬件I2C也可以模拟但硬件I2C中断处理更省CPU。初始化分四步打开GPIO和I2C时钟配置引脚为开漏复用设置I2C时钟频率最后写MPU6050的电源管理寄存器。void MPU_Init(void) { GPIO_InitTypeDef GPIO_InitStructure; I2C_InitTypeDef I2C_InitStructure; RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); GPIO_InitStructure.GPIO_Pin GPIO_Pin_6 | GPIO_Pin_7; GPIO_InitStructure.GPIO_Mode GPIO_Mode_AF; GPIO_InitStructure.GPIO_Speed GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_OType GPIO_OType_OD; GPIO_InitStructure.GPIO_PuPd GPIO_PuPd_UP; GPIO_Init(GPIOB, GPIO_InitStructure); I2C_InitStructure.I2C_Mode I2C_Mode_I2C; I2C_InitStructure.I2C_ClockSpeed 400000; // 400kHz 快速模式 I2C_InitStructure.I2C_DutyCycle I2C_DutyCycle_2; I2C_InitStructure.I2C_Ack I2C_Ack_Enable; I2C_InitStructure.I2C_AcknowledgedAddress I2C_AcknowledgedAddress_7bit; I2C_Init(I2C1, I2C_InitStructure); I2C_Cmd(I2C1, ENABLE); // 写寄存器0x6B 电源管理退出休眠 MPU_WriteReg(0x6B, 0x00); // 设置陀螺仪量程为±2000°/s寄存器0x1B MPU_WriteReg(0x1B, 0x18); // 设置加速度计量程为±16g寄存器0x1C MPU_WriteReg(0x1C, 0x18); }这段代码做了两件关键事I2C总线初始化到400kHz然后把MPU6050从休眠模式唤醒。0x6B写入0x00是必须的否则后续读不到数据。0x1B和0x1C分别把陀螺仪和加速度计的量程调到最大这样在平衡车剧烈倾斜时不会溢出。量程大了分辨率会下降但对控制足够。如果希望噪声更小可以把量程调低比如陀螺仪±250°/s此时敏感度是131 LSB/(°/s)对应到后续计算换算系数不同。2.2 加速度计与陀螺仪原始数据的读取与校验数据读取使用I2C连续读MPU6050的加速度计输出寄存器起始地址是0x3B陀螺仪是0x43。一次突发读取14字节包含acc_x, acc_y, acc_z, temp, gyro_x, gyro_y, gyro_z。读取后需要符号扩展成short。uint8_t buf[14]; MPU_ReadRegs(0x3B, buf, 14); int16_t acc_x (buf[0] 8) | buf[1]; int16_t acc_y (buf[2] 8) | buf[3]; int16_t acc_z (buf[4] 8) | buf[5]; int16_t gyro_x (buf[8] 8) | buf[9]; int16_t gyro_y (buf[10] 8) | buf[11]; int16_t gyro_z (buf[12] 8) | buf[13];这里要注意字节序是大端低地址在高位。buf[0]是高字节。有的例程把高字节和低字节搞反导致角度跳动很大。读取后判断加速度矢量的模是否接近1g如果偏差超过一定范围说明传感器可能被摔坏或处于饱和状态控制逻辑应当做保护。常见做法是每帧计算sqrt(acc_x^2 acc_y^2 acc_z^2)正常在16384左右±2g量程下。如果用DMP原始数据的读取就不需要自己写了inv_mpu.c内部会有读取函数但初始化本身还是要做。2.3 使用DMP进行姿态解算与欧拉角输出inv_mpu.c是InvenSense官方DMP驱动库能直接从MPU6050的DMP硬件单元读出四元数再转换成欧拉角。比纯软件解算的优势在于DMP在硬件层面完成了姿态融合且内置了温度补偿CPU占用低。移植时把inv_mpu.c加进工程注意它依赖I2C_WriteRead和delay_ms等底层接口需要自己实现。不同版本的inv_mpu.c函数命名可能有区别比如初始化函数可能是inv_mpu_init或inv_mpu_initialize以你拿到的源码为准。初始化DMP的流程如下void MPU_DMP_Init(void) { MPU_Init(); // 基础I2C和传感器配置 inv_mpu_init(); // MPU内部状态机初始化 inv_mpu_set_gyro_fsr(2000); // 和寄存器配置一致 inv_mpu_set_accel_fsr(2); // DMP要求加速度计范围固定为±2g这里必须写2 inv_mpu_set_compass_sample_rate(10); // 无磁力计跳过也可 inv_mpu_init_dmp(); // 加载DMP固件 inv_enable_quaternion(); // 使能四元数输出 inv_enable_dmp(); // 启动DMP }这里有个容易忽略的坑DMP固件只支持±2g的加速度计量程所以之前在MPU_Init里把量程调到±16g在用DMP前必须改回±2g。代码里写inv_mpu_set_accel_fsr(2)不是没用的。我遇到过因为量程不匹配导致DMP输出四元数全是0的情况。配置完成后DMP会以内部默认100Hz产生中断也可以在主循环里面通过轮询fifo数据获取结果。读取四元数再转换欧拉角的代码long q[4]; inv_get_quaternion(q); // 获取四元数 Q float q0 q[0] / 16384.0f; float q1 q[1] / 16384.0f; float q2 q[2] / 16384.0f; float q3 q[3] / 16384.0f; float pitch asin(-2.0f * (q1*q3 - q0*q2)) * 57.29578f; float roll atan2(2.0f * (q2*q3 q0*q1), q0*q0 - q1*q1 - q2*q2 q3*q3) * 57.29578f;四元数到欧拉角的公式有十几种上面这个对应Z-Y-X顺序。平衡车一般只需要pitch俯仰角或者roll横滚角取决于车子前进方向在哪个轴。如果传感器安装方向不同可能需要交换坐标轴。例如把MPU6050的正X轴对准车头那么平衡控制使用的是pitch角对准车侧就用roll角。我习惯在代码里根据安装方向做一个轴映射而不是改公式。2.4 姿态角的滤波与数据融合边界DMP的优势在于出厂前已经调好滤波器系数使用中不需要额外加低通滤波。但要注意DMP的数据频率是固定的不能随意改。如果内环控制频率是5ms200Hz而DMP输出100Hz那你需要在两个DMP数据之间做插值或者干脆把控制频率降为100Hz。另一种选择是禁用DMP直接用互补滤波自己写Mahony算法。我的经验是对于课程设计级别的平衡车用DMP足够省心如果偏研究可以自己实现互补滤波来对比效果。互补滤波的公式很直白angle 0.98 * (angle gyro_y * dt) 0.02 * acc_angle;这里0.98和0.02的系数决定了陀螺仪和加速度计的信任权重。权重越高响应越快但漂移大权重低则平滑但滞后。DMP相当于这一公式的硬件最优实现。3. 串级PID的结构速度环与位置环的整定方法3.1 为什么两轮平衡车必须用串级PID单环PID控制直立时只对角度误差作比例微分控制输出PWM直接驱动电机。你会发现车子能短暂站住但一旦甚至人为推一下车身会往一个方向加速倒下去因为控制器没有感知车速。串级PID的外环以车速为反馈输出一个目标倾角内环再控制实际倾角跟踪这个目标。这样物理意义清晰速度慢了车身前倾加速速度快了车身向后倾斜减速。高速时车辆不会跑飞低速时也不会来回震荡。内环的采样频率一般建议在100Hz-200Hz外环在20Hz-50Hz两个环路的时间间隔差别越大系统越稳定但响应变慢。3.2 内环位置环的PD控制模型内环控制对象是车身倾角角度本身是位置的积分所以叫位置环。它只需要PD两个环节因为一旦角度偏差为零车身直立不需要持续输出。P项提供回复力D项提供阻尼。在数字实现中微分项直接用陀螺仪角速度代替这样噪声更小比做差分干净得多。float angle_ctrl(float target_angle, float current_angle, float gyro_rate) { float err target_angle - current_angle; float output Kp_angle * err - Kd_angle * gyro_rate; // 限幅 if (output 400) output 400; if (output -400) output -400; return output; }注意微分项的符号陀螺仪角速度方向与角度误差方向相反时需要取负。具体取决于你定义的倾角正方向。Kp_angle的单位是PWM/度Kd_angle的单位是PWM/(度/秒)。一般先设Kd为0慢慢增大Kp直到车身出现低频抖动再增大Kd消除抖动。这里限幅400是因为PWM定时器满幅值是1000内环输出作为速度环的补偿量不能占满整个PWM范围否则电机饱和后调节失效。我见过的课程设计里限幅往往设得过大导致车子一振荡就满速。3.3 外环速度环的PI控制与防积分饱和速度环输入是目标速度与当前速度的偏差输出是目标倾角。平衡车的目标速度通常为零即原地站住。速度反馈来自左右电机编码器的平均值或者两个轮子速度的平均。速度环必须带积分项否则因为车身为了抵消各种阻力会一直有一个残余倾角导致车速缓慢漂移。PI控制器的公式float speed_ctrl(float target_speed, float current_speed) { float err target_speed - current_speed; speed_error_sum err * dt_speed; float output Kp_speed * err Ki_speed * speed_error_sum; if (output max_angle) output max_angle; // 目标倾角限幅 if (output -max_angle) output -max_angle; return output; }速度环输出是目标倾角限幅要很严格通常最大5度10度。如果限幅过大车身倾斜太猛容易倒。积分项要加windup保护常见做法是当输出截断时清零或钳位误差积分。另一种用的是增量式PID只输出倾角的增量让目标倾角慢慢变化。增量式的好处是即使积分环节出问题输出不会突变。平衡车这个场景速度环相当于“缓慢移动的目标角度发生器”增量式更适合。在后面第5章我会专门讲增量式怎么调。3.4 串级整定顺序与经验参数整定顺序一定是先内环后外环。因为内环是快速环外环是慢速环。内环不先稳定外环一闭合就会发散。先单独调角度环用手扶住车身给一个目标角度比如0度观察是否能够抵抗手推动作。调好角度环后再闭合速度环。下面是常见参数区间实际需要根据机械重心位置作调整环路比例系数积分/微分系数采样周期角度环P100-50005ms角度环D20-8005ms速度环P10-50020ms速度环I0.5-5020ms这个表格里角度环P的单位是“PWM值/度”D的单位是“PWM值/(度/秒)”。如果使用增量式PID参数含义不同但整定顺序一样。一个实用的方法先用串口把内环输出值打印出来当车身静止时输出应该在0附近波动如果输出偏置很大说明角度计算有零点误差需要在代码中减去静止时的偏移量。4. 基于STM32F407ZGT6的实现从传感器数据到PWM输出4.1 工程文件结构与关键模块拿到完整的工程源码先看文件结构。Template.uvguix.Administrator是Keil MDK的工程文件双击打开即可。Template.axf是编译好的镜像没太大用处。keilkilll.bat是清理脚本用来删除编译中间文件。stm32f4xx_rcc.c、stm32f4xx_adc.c、stm32f4xx_can.c等属于STM32F4标准外设库注意这些文件不能随意更换版本ADC、RTC、CAN模块虽然不一定都用但在标准库中它们有互相依赖关系。比如stm32f4xx_rcc.c是时钟配置基础所有外设都要调用。inv_mpu.c是MPU6050的DMP驱动库它依赖I2C和GPIO驱动。核心调用关系是这样的main.c初始化时钟、定时器、I2C然后进入主循环定时器中断每隔5ms调用一次Angle_Control每隔20ms调用一次Speed_Controlinv_mpu.c在后台通过DMP中断或者查询方式更新姿态角。电机控制用TIM1输出两路PWM方向用GPIO控制。如果你的工程里没有motor.c文件可能在main.c里面直接操作寄存器了因为ST标准外设库对高级定时器的代码比较繁琐。4.2 控制定时器中断配置控制周期必须精确我用TIM6作为基本定时器产生100Hz的中断用于速度环。TIM7产生200Hz中断用于角度环。注意两个中断的优先级要设计好角度环优先级高于速度环因为内环是快速保护环。void TIM6_Config(u16 period) { TIM_TimeBaseInitTypeDef TIM_Init; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); TIM_Init.TIM_Prescaler 8400 - 1; // 84MHz/8400 10kHz TIM_Init.TIM_Period period - 1; // 10kHz/(period) 为中断频率 TIM_Init.TIM_CounterMode TIM_CounterMode_Up; TIM_TimeBaseInit(TIM6, TIM_Init); NVIC_InitTypeDef NVIC_Init; NVIC_Init.NVIC_IRQChannel TIM6_DAC_IRQn; NVIC_Init.NVIC_IRQChannelPreemptionPriority 1; NVIC_Init.NVIC_IRQChannelSubPriority 1; NVIC_Init.NVIC_IRQChannelCmd ENABLE; NVIC_Init(NVIC_Init); TIM_ITConfig(TIM6, TIM_IT_Update, ENABLE); TIM_Cmd(TIM6, ENABLE); }上述代码中预分频8400-1是因为APB1定时器时钟为84MHz分频后得到10kHz的计数频率。如果要200Hz中断period就设为50100Hz就是100。这里有个新手容易踩的坑STM32F4的标准外设库中APB1预分频系数如果不是1定时器时钟实际是APB1的2倍。如果你的工程改了系统时钟必须重新确认定时器输入时钟。我建议在while(1)里翻转一个GPIO用示波器量中断周期确认无误再开始调PID。4.3 角度环与速度环的串联执行逻辑在定时器中断中角度环和速度环一起执行。速度环输出限幅后的角度目标作为角度环的输入。两个环之间用一个全局变量angle_target传递。volatile float speed_output 0; volatile float angle_measured 0; volatile float gyro_y 0; // TIM7中断 5ms 角度环 void TIM7_IRQHandler(void) { if (TIM_GetITStatus(TIM7, TIM_IT_Update)) { TIM_ClearITPendingBit(TIM7, TIM_IT_Update); angle_measured get_dmp_pitch(); gyro_y get_dmp_gyro(); float pwm_angle angle_ctrl(speed_output, angle_measured, gyro_y); motor_set(pwm_angle); } } // TIM6中断 20ms 速度环 void TIM6_DAC_IRQHandler(void) { if (TIM_GetITStatus(TIM6, TIM_IT_Update)) { TIM_ClearITPendingBit(TIM6, TIM_IT_Update); float speed (motor_left_speed motor_right_speed) / 2.0f; speed_output speed_ctrl(0.0f, speed); // 目标速度为0 } }两个中断都执行完之后速度环更新speed_output它不是目标倾角的稳态值而是目标倾角的瞬时值。在调和周期不同的情况下这种交替执行会导致速度环在两次更新之间保持旧值对控制效果影响很小。需要注意speed_output的值域要约束在±5度之间而angle_ctrl内部还有对PWM的限幅这样双重限幅能避免异常情况。4.4 PWM输出与电机方向控制PWM输出使用TIM1的CCR1和CCR2通道配置成PWM1模式输出极性为高。电机驱动板一般为H桥需要两个PWM或者一个PWM加一个方向引脚。这里用方向引脚加PWM方式void motor_set(int pwm) { if (pwm 0) { GPIO_SetBits(GPIOE, GPIO_Pin_4); // 方向正转 GPIO_ResetBits(GPIOE, GPIO_Pin_5); TIM_SetCompare1(TIM1, pwm); } else { GPIO_ResetBits(GPIOE, GPIO_Pin_4); GPIO_SetBits(GPIOE, GPIO_Pin_5); TIM_SetCompare1(TIM1, -pwm); } }这里pwm是带符号数正负代表方向。注意死区保护在切换方向的瞬间如果上下桥臂同时导通会烧驱动芯片。有的H桥内部有死区有的没有软件上加一个小延时比如切换方向前将PWM设为0延时1ms。我这个例程里实际上没有加延时因为用的驱动板自带输出使能引脚控制。如果你自己搭H桥加一句delay_ms(1)成本很低。5. 调参与验证增量式速度环的调试技巧5.1 增量式速度环的工程实现外环速度环如果使用位置式PI在反复扶正车身的过程中容易积分饱和表现为松开手后车子突然冲出去。增量式PID只计算目标倾角的增量天然没有绝对误差积累更适合平衡车。核心实现如下float speed_loop_incremental(float target_speed, float current_speed) { float err target_speed - current_speed; static float last_err 0; float delta Kp_speed * (err - last_err) Ki_speed * err; last_err err; static float target_angle 0; target_angle delta; if (target_angle 8.0f) target_angle 8.0f; if (target_angle -8.0f) target_angle -8.0f; return target_angle; }这里的Kp_speed是比例增量系数Ki_speed相当于积分增量系数数值都比位置式小一个数量级。我一般从Kp_speed1.0f、Ki_speed0.01f起步每次增加20%观察车身是否来回“找速度”。5.2 用串口波形量化调参调参不能靠肉眼感觉至少要能看到角度、速度、PWM三个变量。我在代码里放了一个调试printfprintf(ang:%.2f spd:%.2f pwm:%d\r\n, angle_measured, current_speed, motor_pwm);波特率用921600每10ms输出一次。用串口绘图工具打开后很容易发现角度环未收敛时波形是一根等幅正弦波速度环参数过大时速度波形会持续积分变成三角形波。典型的正确波形是静止时角度在±0.5度内白噪声状波动速度在±0.05m/s内波动PWM在一个小范围内随机抖动。5.3 验证平衡车性能的土办法调好之后把车放在瓷砖地面上双手松开记录它不倒下且不跑出直径0.5米圆圈的持续时间。超过20秒说明串级PID基本合格。接着用手轻推一下车壳观察它应当在一两步内恢复直立过程不能有两秒以上的低频摇摆。如果要测速度环的动态响应可以把目标速度设置为0.3m/s用手机慢动作拍摄看车子从倾斜到匀速前进是否有明显超调。最后把MPU6050的采样频率提高一倍同样的参数如果车身出现抖动说明控制频率已经接近传感器极限这时优先考虑减小角度环的Kd而不是提高采样率。本文还有配套的精品资源点击获取