一、前言上一篇文章我们搭好了串级PID的骨架让平衡车能站稳——角度环保持在0°车身不倒了。但站稳只是第一步。一辆实用的平衡车需要能听话地前进后退你推摇杆往前它就按你指定的速度往前走推摇杆往后它就倒车。这就需要一个速度环——以线速度m/s为目标的闭环控制系统。本文在串级PID的基础上加入速度环作为最外层形成四层串级控制层级名称周期输入输出第0层新速度环5ms目标车速(m/s)目标倾角(rad)第1层角度环5ms目标倾角目标角速度(rad/s)第2层角速度环5ms目标角速度目标角加速度(rad/s²)—逆解算5ms角加速度车轮加速度(m/s²)—积分5ms加速度车轮转速(rad/s)第3层电机速度环1ms目标转速PWM本文对应课程第8.1~8.4节绘制系统结构框图 → 速度环悖论 → 闭环传递函数与参数整定 → 编写代码。硬件平台STM32F103C8T6 MPU6500 TB6612 霍尔编码器直流减速电机22线/1:20.4减速比。二、系统结构框图划分单片机世界与物理世界2.1 为什么要画这条分界线在写速度环代码之前我们需要先想清楚一个问题哪些量在MCU内部计算哪些量属于外部物理世界这条分界线看似简单但其实很关键——它直接决定了反馈回路该怎么接单片机内部MCU │ 物理世界Physical World ───────────────────────────────────┼────────────────────────────────── 速度PID、角度PID、角速度PID │ 电机、车轮、车身 逆解算、积分 │ 重力 设定值管理 │ MPU6050测量倾角、角速度 │ 编码器测量转速2.2 完整的四层系统框图速度设定值(m/s) │ ↓ ┌─────────────── 第0层速度环5ms───────┐ │ v_ref →[]→ [PID_v] → atan(v/g) → θ_ref │ │ ↑ Kp10, Ki1 │ │ │ 实际车速 x_dot (分解后) │ └───────────────┬──────────────────────────┘ ↓ θ_ref ┌─────────────── 第1层角度环5ms───────┐ │ θ_ref →[]→ [PID_θ] → ω_ref │ │ ↑ Kp4.0 │ │ │ pitch (MPU6050) │ └───────────────┬──────────────────────────┘ ↓ ω_ref ┌─────────────── 第2层角速度环5ms─────┐ │ ω_ref →[]→ [PID_ω] → α_ref │ │ ↑ Kp10, Ki10 │ │ │ gx (MPU6050) │ └───────────────┬──────────────────────────┘ ↓ α_ref ┌──────── 逆解算α_ref → x_ref ────────┐ │ x (g·sinθ - α_ref·L) / cosθ │ └───────────────┬──────────────────────────┘ ↓ x_ref ┌──────── 积分x → ω_wheel ────────────┐ │ ω_wheel x/rw · dt │ └───────────────┬──────────────────────────┘ ↓ ω_wheel ┌─────────────── 第3层电机速度环1ms──┐ │ ω_wheel →[]→ [PI_L/R] → PWM │ │ ↑ Kp0.5, Ki7 │ │ │ 编码器T法测速 │ └──────────────────────────────────────────┘注意从速度环的视角看θ倾角属于物理世界——它是车身姿态的物理量速度环不直接控制它而是通过改变角度环的设定值来间接影响车速。2.3 为什么角度环的设定值不再固定为0在上一篇文章中角度环的目标永远是θ_ref 0°直立。加入速度环后θ_ref变成了动态值想前进 →θ_ref略大于 0车身前倾重力产生向前的水平分量想后退 →θ_ref略小于 0车身后仰想停下 →θ_ref 0°车身直立不产生水平加速度这就是平衡车听话的物理本质通过微调倾角来改变重力带来的水平加速度从而控制车速。三、速度环悖论3.1 悖论描述课程第8.2节提出了一个非常精彩的问题——如果我直接用编码器测量的车轮转速做反馈能控制速度吗目标车速 → [PID] → PWM → 电机 → 编码器测量 → 反馈看起来没问题但跑起来会这样车速比设定值慢 → PID要求加速 → 减小θ车身前倾 → 获得向前的水平加速度为了前倾车轮需要往后撤一小段 → 编码器读数显示车轮在减速甚至反转PID看到车速更慢了 → 进一步加大输出 → θ进一步前倾 → 车轮进一步后撤恶性循环 → 车身过度前倾 → 倒下悖论的核心编码器测量的是车轮相对于车身的转动而不是车身相对于地面的移动。当车身调整姿态时车轮会转但车身不一定在动。3.2 编码器测量值的两个分量编码器的实测值 ω_encoder 实际包含了两个物理来源ω_encoder ω_移动 ω_姿态调整 ↑ ↑ 平衡车在地面 车身倾角变化导致 上真实移动 车轮被迫转动分量来源物理含义ω_移动车身在地面上实际平移我们要控制的目标量ω_姿态调整车身倾角变化迫使车轮转动平衡控制的副产物不是真实移动3.3 解决方案分解反馈值既然编码器值被污染了我们就要把它洗干净——在输入速度PID之前先减去姿态调整产生的转动分量// ω_姿态调整 -角速度 × (摆长 轮半径) / 轮半径floatomega_2-omega*(lprw)/rw;// ω_移动 编码器实测 - ω_姿态调整floatomega_1velocity_omega-omega_2;// 实际车速 ω_移动 × 轮半径floatx_dotomega_1*rw;在单片机内部减去欧米噶2只得到平衡车在地上真实移动的距离物理推导车身以角速度 ω 前倾时车轮中心相对于地面接触点产生 (L r)·ω 的线速度L为摆长r为轮半径。除以轮半径 r 就得到车轮的等效角速度。负号是因为前倾时车轮相对于车身向后转。这个分解术是整个速度环的理论核心。如果不做这一步速度PID收到的反馈是被姿态控制污染的数据参数永远调不通。四、逆解算从速度到角度4.1 物理直觉速度环PID输出的是一个加速度需求m/s²“我想让车速增加多少”。但平衡车不是靠轮子直接加速的——它靠在重力的分量来加速车速慢 → 需要加速 → PID输出正加速度 → 车身往前倾 → 重力产生向前分量 → 实际加速4.2 公式推导对平衡车做受力分析车身倾角为 θ 时重力在水平方向的分量为F_h m·g·tan(θ)水平加速度a g·tan(θ)所以要实现加速度velocity_output需要的倾角为θ_ref atan(velocity_output / g)代码中就是这一行floattheta_refqatan(velocity_output/g);为什么是atan而不是简单的线性关系因为平衡车可能有大角度倾斜比如急加减速tan的线性近似 sinθ≈θ 在大角度时误差太大了。atan是精确的物理关系。4.3 限幅的意义速度环输出限幅设为 ±0.5gPID_LimitConfig(pid_velocity,0.5f*g,-0.5f*g);对应最大倾角atan(0.5) ≈ 26.6°。这个值不是随便定的太大会导致车身过度倾斜传感器可能失效太小会限制加速能力0.5g 对应约 5 m/s² 的加速度对平衡车来说已经相当激进4.4完善系统框图在单片机内部引入逆解算五、闭环传递函数与参数整定5.1框图化简对Gtheta(s)化简详细化简过程参考ppt8.35.2 系统传递函数课程第8.3节用MATLAB分析了速度环的闭环特性被控对象倒立摆 角度环 角速度环已在上篇文章中验证稳定 速度环在此基础上串联一个PI控制器MATLAB验证代码来自课程PPT8.3 参数整定stf(s);% 定义拉普拉斯算子Gpid4;% 角度环等效增益上一章已验证% 角度→角速度传递函数Gtheta_dot1/(s*(s/14.81)*(s/20.41));% 闭环传递函数Phifeedback(Gpid*Gtheta_dot,1);% 分析结果dcgain(Phi)% 增益 1zero(Phi)% 没有零点pole(Phi)% 极点-5.06±4.75i, -25.15.3 参数整定结果引入pi控制器参数值依据Kp10.0速度偏差的直接响应较大的Kp让车速跟踪更果断Ki1.0消除稳态误差——静止时车速必须精确归零Kd0速度反馈本身就是微分量的产物再加微分噪声太大为什么 Ki 这么小速度环的Ki1.0比角速度环的Ki10.0小了10倍。因为角速度环需要快速消除角速度的稳态误差车身偏了必须立刻纠正速度环的时间尺度更慢——车速变化要经过倾角→角速度→加速度→积分才能响应这是一个三阶延迟链。Ki太大会让速度环过度修正车身来回摆动5.4 极点分析闭环系统的主导极点是-5.06 ± 4.75i共轭复极点阻尼比约 0.73和-25.1实极点。共轭极点说明系统在响应阶跃输入时会有轻微振荡但快速衰减——这是速度环的典型行为车速会在目标值附近悠一下再稳定。六、代码实现6.1app_control.h— 新增复位接口#ifndef__APP_CONTRL_H__#define__APP_CONTRL_H__#includestm32f10x.hvoidApp_Control_Init(void);voidApp_Control_Proc(void);voidApp_Control_Reset(void);// 新增控制系统复位#endif6.2app_control.c— 四层串级PID完整实现#includeapp_control.h#includepid.h#includetask.h#includeapp_mpu6050.h#includeqmath.h#includedelay.h#includeapp_motor.h#includeapp_encoder.hstaticPID_TypeDef pid_velocity;//速度PID控制器,用于计算速度参考值staticPID_TypeDef pid_theta;//角度PID控制器staticPID_TypeDef pid_omega;//角速度PID控制器staticfloatomega_ref0.0f;staticconstfloatg9.8f;// 重力加速度 (m/s²)staticfloatlp0.062f;// 摆长 (m)staticconstfloatrw0.032f;// 车轮半径 (m)staticuint64_tlast_time0;voidApp_Control_Init(void){// 角度环纯P, Kp4.0PID_Init(pid_theta,4.0f,0.0f,0.0f,0.0f);// 角速度环PI, Kp10.0, Ki10.0PID_Init(pid_omega,10.0f,10.0f,0.0f,0.0f);// 速度环PI, Kp10.0, Ki1.0PID_Init(pid_velocity,10.0f,1.0f,0.0f,0.0f);// 输出限幅PID_LimitConfig(pid_theta,12.57f,-12.57f);// 角速度 ±2转/秒PID_LimitConfig(pid_omega,125.7f,-125.7f);// 角加速度限幅PID_LimitConfig(pid_velocity,0.5f*g,-0.5f*g);// 加速度 ±0.5g}voidApp_Control_Proc(void){PERIODIC(5);uint64_tnow_timeGetUs();floatdt(now_time-last_time)*1.0e-6f;// ═══ 第0层速度环 ═══// #-1. 设置速度目标值PID_ChangeSetpoint(pid_velocity,0.0f);// #-2. 读取传感器floatthetaApp_MPU6050_GetPitch()/360.0f*2.0f*3.1415926f;floatomegaApp_MPU6050_GetGx()/360.0f*2.0f*3.1415926f;floatvelocity_omega(App_Encoder_GetSpeed_R()App_Encoder_GetSpeed_L())/2.0f;// #-3. 分解编码器反馈值核心算法// ω_2 姿态角调整产生的车轮转动非真实移动// ω_1 ω_encoder - ω_2 真实移动产生的车轮转动floatomega_2-omega*(lprw)/rw;floatomega_1velocity_omega-omega_2;floatx_dotomega_1*rw;// 实际车速 (m/s)// #-4. 速度PID计算floatvelocity_outputPID_Calc(pid_velocity,x_dot);// #-5. 逆解算加速度 → 倾角floattheta_refqatan(velocity_output/g);// ═══ 第1层角度环 ═══PID_ChangeSetpoint(pid_theta,theta_ref);floattheta_output_refPID_Calc(pid_theta,theta);// ═══ 第2层角速度环 ═══PID_ChangeSetpoint(pid_omega,theta_output_ref);floatomega_output_refPID_Calc(pid_omega,omega);// ═══ 逆解算 积分 ═══floatx_dot_dot_ref(g*qsin(theta)-omega_output_ref*lp)/qcos(theta);if(dt0.0f){omega_ref1.0f/rw*x_dot_dot_ref*dt;}// ═══ 第3层电机速度环由 App_Motor_Proc 独立运行═══App_Motor_SetOmega(omega_ref,omega_ref);last_timenow_time;}// 控制系统复位所有PID归零积分清零voidApp_Control_Reset(void){PID_Reset(pid_theta);PID_Reset(pid_omega);PID_Reset(pid_velocity);omega_ref0.0f;last_time0;}6.3 代码关键路径解析理解这段代码的核心是数据流向我给你逐行拆解第1步PID_ChangeSetpoint(pid_velocity, 0.0f) → 告诉速度环目标车速0替换为遥控器指令就能前进后退 第2步float velocity_omega (App_Encoder_GetSpeed_R() ...) / 2.0f → 读左右编码器测量的车轮转速rad/s取平均作为反馈值 第3步float omega_2 -omega * (lp rw) / rw → 计算姿态调整产生的虚假速度 → 前倾omega0时车轮往后转所以有个负号 第4步float x_dot (velocity_omega - omega_2) * rw → 编码器测量值减去虚假分量 真实车速 第5步float velocity_output PID_Calc(pid_velocity, x_dot) → 速度PID输出需要的加速度m/s² 第6步float theta_ref qatan(velocity_output / g) → 要获得这个加速度车身需要倾斜 atan(a/g) 的角度 → 这成为了角度环的新指令 第7步PID_ChangeSetpoint(pid_theta, theta_ref) → 不再是固定的0°而是速度环动态计算的倾斜角度 → 从这一步往后就是上篇文章的串级PID逻辑了6.4 反馈值分解后再做什么编码器GetSpeed得到的是车轮的瞬时角速度但如第三节所述这个值里混合了真实移动和姿态调整两个分量。关键就在后面两行floatomega_2-omega*(lprw)/rw;// 姿态调整分量floatomega_1velocity_omega-omega_2;// 真实移动分量floatx_dotomega_1*rw;// 实际车速 (m/s)拆干净之后才送入速度PID——这是整个速度环不失控的保证。七、主程序集成7.1 不变的主循环main.c不需要改——速度环已经集成在App_Control_Proc()内部while(1){App_Bat_Proc();// 电池电压100msApp_Motor_Proc();// 电机速度环1msApp_Button_Proc();// 按键轮询App_Control_Proc();// 串级PID 速度环5ms}7.2 后续集成遥控器未来接入蓝牙遥控时只需要把速度设定值从0.0f改成遥控器指令即可// 当前PID_ChangeSetpoint(pid_velocity,0.0f);// 将来PID_ChangeSetpoint(pid_velocity,remote_speed_cmd);速度环的PI参数已经调好遥控器发来的速度指令会被平滑追踪。八、经验总结速度环悖论是理解平衡车控制的关键门槛。编码器测量的不只是车速——它还包含了车身姿态调整时车轮被迫转动的分量。不分解这个量速度环永远调不通。反馈值分解是数学不是黑魔法。ω_2 -ω × (Lr) / r这个公式直接来自运动学约束车轮与地面的纯滚动条件。读懂了这个公式你就真正理解了倒立摆的物理本质。速度环在单片机世界和物理世界之间架了一座桥物理世界的编码器读数被洗干净后送入MCUMCU的速度PID输出被逆解算成倾角指令后送回物理世界两个世界通过传感器和执行器形成完整闭环atan(v/g) 是速度环的画龙点睛之笔。它不是可有可无的近似而是物理学要求的精确变换——平衡车只靠重力加速所以加速度和倾角的关系是精确的a g·tan(θ)。四层串级各有分工速度环决定以多快速度移动秒级响应角度环决定倾斜多少来产生加速度百毫秒级角速度环决定回正的力度十毫秒级电机速度环决定轮子怎么转毫秒级每层只处理自己能快速响应的物理量层与层之间通过物理方程精确连接App_Control_Reset()是安全必需品。切换模式平衡→停止→平衡时如果不清零PID的积分项上次的积分累积会导致上电瞬间的剧烈抖动。复位函数一次性清零所有状态。为什么角度环从固定目标变为动态目标没有速度环θ_ref 0°只管站稳不关心车速有速度环θ_ref atan(v_ref/g)通过倾角调度重力来实现速度指令这个转变是平衡车从能站住升级为能听话的核心九、完整工程本文涉及的代码变更文件变更内容user/app_control.h新增App_Control_Reset()接口user/app_control.c新增pid_velocity四层串级PID实现编码器反馈值分解atan逆解算user/app_encoder.c/h无变更使用已有的GetSpeed接口user/main.c无变更速度环已集成在App_Control_Proc内部已有基础设施文件功能my_lib/pid.c/h位置式PID库梯形积分 抗饱和my_lib/qmath.c/h快速三角函数库含qatanuser/app_mpu6050.c/hMPU6050驱动 互补滤波user/app_encoder.c/h编码器T法测速 位置累计user/app_motor.c/h电机速度闭环PImy_lib/task.h轻量级周期调度宏 工程路径G:\Balance_Car_New\2.2\ 2026-07-08 前文STM32平衡车教程 — 串级PID倒立摆控制从建模到代码如果你觉得这篇文章对你有帮助欢迎点赞收藏