STM32 平衡车三环 PID互补滤波替代 DMP 蓝牙在线调参与 Flash 掉电保存标签STM32、STM32F103、平衡车、PID、MPU6050、互补滤波、蓝牙、Flash、嵌入式仓库https://github.com/kaka12331/stm32-two-wheel-balance-car这篇不讲串级 PID 是什么「速度环套平衡环套转向环」的原理网上和我自己都写烂了。真正卡住绝大多数人的从来不是理解不了公式而是参数到底怎么调出来烧一次程序改一个数、拔线、再烧、再试——一晚上试不了二十组参数试到后面自己都忘了刚才哪组好一点。这篇讲的是这套工程怎么把调参这件事变得可操作蓝牙发一行字符就改参数、dump随时打印当前值、20 Hz 遥测实时回传角度、满意了发save写进 Flash 掉电不丢。外加一份我自己写在Control.c文件头的 7 步整定流程。只想搞懂三环原理的看这篇就够电赛控制组终极指南串级 PID 姿态解算。下面直接进落地。控制架构编码器速度 ──► 速度环 PI ──► 叠加到平衡目标角 MPU 倾角/角速度 ──► 平衡环 PD ──► 共模 PWM 蓝牙转向 偏航角速度 ──► 转向环 ──► 左右差动 │ ▼ 左右电机 PWM跌倒保护切断控制周期TIM3 10 ms100 Hz整个回路在TIM3_IRQHandler里跑完不依赖主循环调度。TIM_TimeBaseInitStructure.TIM_Prescaler71;// 72MHz/(711) 1MHzTIM_TimeBaseInitStructure.TIM_Period9999;// 1MHz/(99991) 100Hz...NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority0;// 最高优先级抢占优先级给 0理由很直接控制回路不能被别的中断拖太久。10 ms 的周期如果被串口或者别的中断插进来占掉几毫秒微分项立刻就乱了。一、姿态自己写互补滤波不用 DMPMPU6050 的 DMP 库能直接吐欧拉角省事。但它有两个代价占 flash、以及你完全不知道它内部做了什么出问题无从下手。这套工程只读原始寄存器自己算int16_tax,ay,az,gx,gy,gz;MPU6050_GetRaw(ax,ay,az,gx,gy,gz);floataccelAngleatan2f((float)ay,(float)az)*57.29578f;// 弧度转角度floatgyroRate(float)gx/MPU_GYRO_LSB_PER_DPS;// deg/sfloatgyroZRate(float)gz/MPU_GYRO_LSB_PER_DPS;// 偏航角速度给转向环s_angle0.98f*(s_anglegyroRate*CONTROL_DT)0.02f*accelAngle;最后一行就是一阶互补滤波的全部内容一行代码陀螺仪积分s_angle gyroRate * dt短期准、不受震动影响但会漂移加速度计反解atan2(ay, az)长期准、不漂但车一动就被震得乱跳0.98 / 0.02 的意思是短期几乎全信陀螺仪用加速度计以 2% 的权重慢慢把漂移拉回来0.98 是常见起点不是定值。车震得厉害就往上加比如 0.99响应慢就往下减。ay/az和gx的轴不一定对atan2f(ay, az)用的是哪两个轴、gx是不是俯仰角速度完全取决于你的 MPU6050 是怎么焊在车上的。焊法一变轴和符号就得改。这就是下面第 1 步存在的原因——先验证方向再谈调参。方向错了你调一晚上 Kp 也不会站起来。二、蓝牙调参协议所有命令都是一行文本蓝牙串口助手直接发命令作用kp/kd平衡环 P / Dvp/vi速度环 P / Itp/td转向环 P / Dza机械零点Med_Angleen0/en1电机使能开关mon1打开 20 Hz 遥测dump打印当前全部参数save/load/default写 Flash / 读 Flash / 恢复默认解析就是一串前缀匹配没有任何花哨的地方voidControl_ApplyCommandLine(constchar*line){if(StartsWith(line,kp))g_Param.Kp_balanceParseFloat(line3);elseif(StartsWith(line,kd))g_Param.Kd_balanceParseFloat(line3);...elseif(strcmp(line,save)0){Param_Save(g_Param);Bluetooth_SendString(saved\r\n);}elseif(strcmp(line,dump)0){SendDump();}else{Bluetooth_SendString(?\r\n);}}注意这个函数在main()的while(1)里调用不在中断里。原因见下一节。自己写 ParseFloat 和 FormatFixed2工程里没有用atof和%f/* 把浮点数格式化成整数.两位小数避免依赖 printf 的浮点数支持 */staticvoidFormatFixed2(char*buf,floatval){int32_tscaled(int32_t)(val*100.0f(val0?0.5f:-0.5f));int32_tintPartscaled/100;int32_tfracPartscaled%100;if(fracPart0)fracPart-fracPart;sprintf(buf,%ld.%02ld,(long)intPart,(long)fracPart);}Keil 下开printf的浮点支持要额外链接库、还要更大的栈很容易在栈溢出里死得莫名其妙。参数只需要两位小数自己写二十行就绕过去了。ParseFloat同理只支持-123.456这种简单十进制够用。三、中断里绝对不能阻塞发串口这是我认为这个工程里最值得抄的一个设计。遥测要 20 Hz 回传角度最直觉的写法是在 TIM3 中断里直接Bluetooth_SendString()。这会毁掉整个控制回路。串口发一行字符是阻塞的——要等 UART 把每个字节都移出去。115200 波特率下发 40 个字节要 3.5 ms 左右。控制周期总共才 10 ms光发个日志就吃掉三分之一微分项直接失真。解法是一个「信箱」/* 遥测数据信箱TIM3 中断只负责在这里留下快照并置位真正的蓝牙发送 会阻塞等待 UART 发送完成放到 main 循环的 Control_Poll() 里做 避免在中断里阻塞几十毫秒拖慢控制周期 */staticvolatileuint8_ts_telemetryReady0;staticvolatilefloats_telemetryAngle0.0f;staticvolatilefloats_telemetryGyro0.0f;staticvolatileint16_ts_telemetrySpeed0;staticvolatileint32_ts_telemetryTurn0;中断里只是几条赋值加一个s_telemetryReady 1微秒级主循环看到标志位就慢慢发发多久都不影响控制。这条原则可以推广中断里只做「短、快、不等待」的事——采样、计算、写寄存器。任何需要等待的操作串口、Flash 擦写、I²C 长事务都甩给主循环。同样的道理Control_ApplyCommandLine里的Param_Save()要擦写 Flash一次几十毫秒也必须在主循环里做。蓝牙断连看门狗#defineBT_IDLE_TIMEOUT_TICKS50// 500ms100Hz 没收到遥控字节就自动归零if(Bluetooth_IdleTicks0xFFFF)Bluetooth_IdleTicks;if(Bluetooth_IdleTicksBT_IDLE_TIMEOUT_TICKS){Bluetooth_DriveForward0;Bluetooth_DriveTurn0;}手机走远了、蓝牙断了、App 崩了——只要 500 ms 没收到字节前进和转向指令自动归零车原地站住。没有这个蓝牙一断车就带着最后一条「全速前进」冲出去了。四、7 步整定流程这是Hardware/Control.c文件头里的完整流程照着做1. 先不上电机en0把车拿在手上发mon1观察A:角度。前后倾斜车身确认角度数值的变化方向和倾斜方向符合直觉G:角速度的变化趋势和 A 一致。不对就去改accelAngle/gyroRate那两行用到的轴或正负号——取决于 MPU6050 实际怎么焊在车上。2. 把车立起来找到车体大致能自己站稳的倾角发dump看当前 A 值然后za这个值设为机械零点Med_Angle。3.en1只调Kp_balance。从 0 开始给正/负都试一下车身倾斜时轮子要明显「往倒的方向追」——如果是「越倒越快」说明符号反了。然后继续加大直到扶着车能感觉到明显的站立力且不高频抖动。4. 加Kd_balance压制高频抖动让站立更稳。5. 调Kp_velocity/Ki_velocity消除「越站越往前 / 往后蠕动」的现象让车能原地站定。6. 用蓝牙 W/A/S/D 遥控测试调Kp_turn/Kd_turn让转向顺畅、不发飘。7. 都满意后发save写入 Flash以后每次上电自动load恢复。第 3 步那句「往倒的方向追」是整个流程的关键判据。符号错的表现不是站不稳是加速倒下很多人第一次调会以为是 Kp 太大越调越小其实是方向反了。关键参数宏值含义CONTROL_DT0.01f控制周期 10 msFALL_ANGLE_DEG35.0f|angle − Med_Angle| 超过就切断电机TARGET_SPEED_MAX40蓝牙前进/后退目标编码器 tick 量级TARGET_TURN_MAX100蓝牙转向目标BT_IDLE_TIMEOUT_TICKS50500 ms 无遥控字节自动归零VELOCITY_LPF_A0.7f速度误差低通系数旧值权重VELOCITY_INTEGRAL_LIM20000.0f速度环积分限幅MONITOR_PERIOD_TICKS5遥测 20 HzFALL_ANGLE_DEG 35不是随便定的正常站立时倾角在零点附近几度内晃超过 35° 基本没救了这时候电机还在全力挣扎只会把车摔得更狠、烧驱动。VELOCITY_INTEGRAL_LIM 20000是速度环 PI 的积分限幅。速度环积分项是最容易饱和的地方——车被挡住走不动误差持续存在积分一路涨到天上等障碍物一挪开车直接窜出去。这就是抗积分饱和的必要性。硬件注意工程里禁用了 JTAG释放 PA15 / PB3 / PB4只保留 SWD把这三个脚让给按键和软件 I²C。如果你烧完程序发现按键没反应、或者 PB3/PB4 死活拉不动先确认这句有没有执行。这是 STM32F103 的经典坑我单独写过一篇stm32f103c8t6PB3 和 PB4 不能正常使用问题复现步骤Keil MDK 打开Project.uvprojx目标STM32F103C8T6以工程配置为准车轮悬空接蓝牙串口助手先发mon1走上面的第 1 步确认角度方向没问题再落地做第 2 步以后的整定全程手边留着en0随时能切电机工程结构stm32-two-wheel-balance-car/ ├── User/main.c # 多页 OLED、按键 ├── Hardware/ │ ├── Control.c/h # 三环 PID TIM3 中断核心文件头有完整调试步骤 │ ├── MPU6050.* Encoder.* Motor.* │ ├── Bluetooth.* Param.* # 协议与 Flash 参数 │ └── OLED.* Key.* LED.* └── Project.uvprojx相关文章三环原理电赛控制组终极指南串级 PID 姿态解算MPU6050 底层与 DMP 对比万字长文吃透 MPU6050PB3/PB4 踩坑PB3 和 PB4 不能正常使用问题仓库地址https://github.com/kaka12331/stm32-two-wheel-balance-car三环结构与增益量级参考了同类平衡车工程作为起点控制代码、蓝牙协议与 Flash 参数模块为本仓库实现。
STM32 平衡车三环 PID互补滤波替代 DMP 蓝牙在线调参与 Flash 掉电保存标签STM32、STM32F103、平衡车、PID、MPU6050、互补滤波、蓝牙、Flash、嵌入式仓库https://github.com/kaka12331/stm32-two-wheel-balance-car这篇不讲串级 PID 是什么「速度环套平衡环套转向环」的原理网上和我自己都写烂了。真正卡住绝大多数人的从来不是理解不了公式而是参数到底怎么调出来烧一次程序改一个数、拔线、再烧、再试——一晚上试不了二十组参数试到后面自己都忘了刚才哪组好一点。这篇讲的是这套工程怎么把调参这件事变得可操作蓝牙发一行字符就改参数、dump随时打印当前值、20 Hz 遥测实时回传角度、满意了发save写进 Flash 掉电不丢。外加一份我自己写在Control.c文件头的 7 步整定流程。只想搞懂三环原理的看这篇就够电赛控制组终极指南串级 PID 姿态解算。下面直接进落地。控制架构编码器速度 ──► 速度环 PI ──► 叠加到平衡目标角 MPU 倾角/角速度 ──► 平衡环 PD ──► 共模 PWM 蓝牙转向 偏航角速度 ──► 转向环 ──► 左右差动 │ ▼ 左右电机 PWM跌倒保护切断控制周期TIM3 10 ms100 Hz整个回路在TIM3_IRQHandler里跑完不依赖主循环调度。TIM_TimeBaseInitStructure.TIM_Prescaler71;// 72MHz/(711) 1MHzTIM_TimeBaseInitStructure.TIM_Period9999;// 1MHz/(99991) 100Hz...NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority0;// 最高优先级抢占优先级给 0理由很直接控制回路不能被别的中断拖太久。10 ms 的周期如果被串口或者别的中断插进来占掉几毫秒微分项立刻就乱了。一、姿态自己写互补滤波不用 DMPMPU6050 的 DMP 库能直接吐欧拉角省事。但它有两个代价占 flash、以及你完全不知道它内部做了什么出问题无从下手。这套工程只读原始寄存器自己算int16_tax,ay,az,gx,gy,gz;MPU6050_GetRaw(ax,ay,az,gx,gy,gz);floataccelAngleatan2f((float)ay,(float)az)*57.29578f;// 弧度转角度floatgyroRate(float)gx/MPU_GYRO_LSB_PER_DPS;// deg/sfloatgyroZRate(float)gz/MPU_GYRO_LSB_PER_DPS;// 偏航角速度给转向环s_angle0.98f*(s_anglegyroRate*CONTROL_DT)0.02f*accelAngle;最后一行就是一阶互补滤波的全部内容一行代码陀螺仪积分s_angle gyroRate * dt短期准、不受震动影响但会漂移加速度计反解atan2(ay, az)长期准、不漂但车一动就被震得乱跳0.98 / 0.02 的意思是短期几乎全信陀螺仪用加速度计以 2% 的权重慢慢把漂移拉回来0.98 是常见起点不是定值。车震得厉害就往上加比如 0.99响应慢就往下减。ay/az和gx的轴不一定对atan2f(ay, az)用的是哪两个轴、gx是不是俯仰角速度完全取决于你的 MPU6050 是怎么焊在车上的。焊法一变轴和符号就得改。这就是下面第 1 步存在的原因——先验证方向再谈调参。方向错了你调一晚上 Kp 也不会站起来。二、蓝牙调参协议所有命令都是一行文本蓝牙串口助手直接发命令作用kp/kd平衡环 P / Dvp/vi速度环 P / Itp/td转向环 P / Dza机械零点Med_Angleen0/en1电机使能开关mon1打开 20 Hz 遥测dump打印当前全部参数save/load/default写 Flash / 读 Flash / 恢复默认解析就是一串前缀匹配没有任何花哨的地方voidControl_ApplyCommandLine(constchar*line){if(StartsWith(line,kp))g_Param.Kp_balanceParseFloat(line3);elseif(StartsWith(line,kd))g_Param.Kd_balanceParseFloat(line3);...elseif(strcmp(line,save)0){Param_Save(g_Param);Bluetooth_SendString(saved\r\n);}elseif(strcmp(line,dump)0){SendDump();}else{Bluetooth_SendString(?\r\n);}}注意这个函数在main()的while(1)里调用不在中断里。原因见下一节。自己写 ParseFloat 和 FormatFixed2工程里没有用atof和%f/* 把浮点数格式化成整数.两位小数避免依赖 printf 的浮点数支持 */staticvoidFormatFixed2(char*buf,floatval){int32_tscaled(int32_t)(val*100.0f(val0?0.5f:-0.5f));int32_tintPartscaled/100;int32_tfracPartscaled%100;if(fracPart0)fracPart-fracPart;sprintf(buf,%ld.%02ld,(long)intPart,(long)fracPart);}Keil 下开printf的浮点支持要额外链接库、还要更大的栈很容易在栈溢出里死得莫名其妙。参数只需要两位小数自己写二十行就绕过去了。ParseFloat同理只支持-123.456这种简单十进制够用。三、中断里绝对不能阻塞发串口这是我认为这个工程里最值得抄的一个设计。遥测要 20 Hz 回传角度最直觉的写法是在 TIM3 中断里直接Bluetooth_SendString()。这会毁掉整个控制回路。串口发一行字符是阻塞的——要等 UART 把每个字节都移出去。115200 波特率下发 40 个字节要 3.5 ms 左右。控制周期总共才 10 ms光发个日志就吃掉三分之一微分项直接失真。解法是一个「信箱」/* 遥测数据信箱TIM3 中断只负责在这里留下快照并置位真正的蓝牙发送 会阻塞等待 UART 发送完成放到 main 循环的 Control_Poll() 里做 避免在中断里阻塞几十毫秒拖慢控制周期 */staticvolatileuint8_ts_telemetryReady0;staticvolatilefloats_telemetryAngle0.0f;staticvolatilefloats_telemetryGyro0.0f;staticvolatileint16_ts_telemetrySpeed0;staticvolatileint32_ts_telemetryTurn0;中断里只是几条赋值加一个s_telemetryReady 1微秒级主循环看到标志位就慢慢发发多久都不影响控制。这条原则可以推广中断里只做「短、快、不等待」的事——采样、计算、写寄存器。任何需要等待的操作串口、Flash 擦写、I²C 长事务都甩给主循环。同样的道理Control_ApplyCommandLine里的Param_Save()要擦写 Flash一次几十毫秒也必须在主循环里做。蓝牙断连看门狗#defineBT_IDLE_TIMEOUT_TICKS50// 500ms100Hz 没收到遥控字节就自动归零if(Bluetooth_IdleTicks0xFFFF)Bluetooth_IdleTicks;if(Bluetooth_IdleTicksBT_IDLE_TIMEOUT_TICKS){Bluetooth_DriveForward0;Bluetooth_DriveTurn0;}手机走远了、蓝牙断了、App 崩了——只要 500 ms 没收到字节前进和转向指令自动归零车原地站住。没有这个蓝牙一断车就带着最后一条「全速前进」冲出去了。四、7 步整定流程这是Hardware/Control.c文件头里的完整流程照着做1. 先不上电机en0把车拿在手上发mon1观察A:角度。前后倾斜车身确认角度数值的变化方向和倾斜方向符合直觉G:角速度的变化趋势和 A 一致。不对就去改accelAngle/gyroRate那两行用到的轴或正负号——取决于 MPU6050 实际怎么焊在车上。2. 把车立起来找到车体大致能自己站稳的倾角发dump看当前 A 值然后za这个值设为机械零点Med_Angle。3.en1只调Kp_balance。从 0 开始给正/负都试一下车身倾斜时轮子要明显「往倒的方向追」——如果是「越倒越快」说明符号反了。然后继续加大直到扶着车能感觉到明显的站立力且不高频抖动。4. 加Kd_balance压制高频抖动让站立更稳。5. 调Kp_velocity/Ki_velocity消除「越站越往前 / 往后蠕动」的现象让车能原地站定。6. 用蓝牙 W/A/S/D 遥控测试调Kp_turn/Kd_turn让转向顺畅、不发飘。7. 都满意后发save写入 Flash以后每次上电自动load恢复。第 3 步那句「往倒的方向追」是整个流程的关键判据。符号错的表现不是站不稳是加速倒下很多人第一次调会以为是 Kp 太大越调越小其实是方向反了。关键参数宏值含义CONTROL_DT0.01f控制周期 10 msFALL_ANGLE_DEG35.0f|angle − Med_Angle| 超过就切断电机TARGET_SPEED_MAX40蓝牙前进/后退目标编码器 tick 量级TARGET_TURN_MAX100蓝牙转向目标BT_IDLE_TIMEOUT_TICKS50500 ms 无遥控字节自动归零VELOCITY_LPF_A0.7f速度误差低通系数旧值权重VELOCITY_INTEGRAL_LIM20000.0f速度环积分限幅MONITOR_PERIOD_TICKS5遥测 20 HzFALL_ANGLE_DEG 35不是随便定的正常站立时倾角在零点附近几度内晃超过 35° 基本没救了这时候电机还在全力挣扎只会把车摔得更狠、烧驱动。VELOCITY_INTEGRAL_LIM 20000是速度环 PI 的积分限幅。速度环积分项是最容易饱和的地方——车被挡住走不动误差持续存在积分一路涨到天上等障碍物一挪开车直接窜出去。这就是抗积分饱和的必要性。硬件注意工程里禁用了 JTAG释放 PA15 / PB3 / PB4只保留 SWD把这三个脚让给按键和软件 I²C。如果你烧完程序发现按键没反应、或者 PB3/PB4 死活拉不动先确认这句有没有执行。这是 STM32F103 的经典坑我单独写过一篇stm32f103c8t6PB3 和 PB4 不能正常使用问题复现步骤Keil MDK 打开Project.uvprojx目标STM32F103C8T6以工程配置为准车轮悬空接蓝牙串口助手先发mon1走上面的第 1 步确认角度方向没问题再落地做第 2 步以后的整定全程手边留着en0随时能切电机工程结构stm32-two-wheel-balance-car/ ├── User/main.c # 多页 OLED、按键 ├── Hardware/ │ ├── Control.c/h # 三环 PID TIM3 中断核心文件头有完整调试步骤 │ ├── MPU6050.* Encoder.* Motor.* │ ├── Bluetooth.* Param.* # 协议与 Flash 参数 │ └── OLED.* Key.* LED.* └── Project.uvprojx相关文章三环原理电赛控制组终极指南串级 PID 姿态解算MPU6050 底层与 DMP 对比万字长文吃透 MPU6050PB3/PB4 踩坑PB3 和 PB4 不能正常使用问题仓库地址https://github.com/kaka12331/stm32-two-wheel-balance-car三环结构与增益量级参考了同类平衡车工程作为起点控制代码、蓝牙协议与 Flash 参数模块为本仓库实现。
