动量轮自平衡自行车的双环PID控制架构解析

1. 动量轮自平衡自行车的控制架构解析

动量轮自平衡自行车并非传统意义上的倒立摆系统，其核心稳定机制依赖于高速旋转的动量轮所产生的陀螺效应与角动量守恒。当车体发生倾斜时，控制系统通过改变动量轮的角加速度，产生一个反向力矩来抵抗倾覆趋势。这一物理过程决定了其控制逻辑必须同时处理姿态稳定与运动控制两个强耦合维度。从工程实现角度看，整个系统可被抽象为一个双闭环反馈结构：外环为直立环（Attitude Loop），内环为速度环（Speed Loop）。这种结构并非简单的功能叠加，而是对经典串级PID控制在特定物理约束下的工程化重构。

直立环的核心任务是维持车体在垂直方向上的动态平衡。它以MPU6050传感器输出的横滚角（Roll Angle）及其角速度（Roll Rate）作为反馈量，目标设定值（Setpoint）通常为零度，即理想的竖直姿态。该环路的输出并非直接驱动执行机构，而是作为速度环的参考输入，形成“姿态误差驱动速度指令”的逻辑链。速度环则负责精确跟踪这一由姿态误差生成的速度指令，其反馈量来源于安装在动量轮电机轴上的增量式编码器脉冲计数值。两个环路的输出最终线性叠加，共同构成施加于动量轮驱动电机的PWM占空比指令。这种并联结构的表象下，隐藏着深刻的物理意义：直立环负责快速响应姿态扰动，提供主要的稳定力矩；速度环则负责抑制稳态误差、改善动态响应，并为系统提供必要的阻尼特性。

理解这一架构的关键在于厘清“为什么是并联而非串联”。从控制理论推导可知，将一个理想的串级PID控制器展开后，其总输出可表示为：

Output = Kp_att * (θ_ref - θ) + Kd_att * (-ω) + Kp_spd * (ω_ref - ω) + Ki_spd * ∫(ω_ref - ω)dt

其中， θ 为横滚角， ω 为横滚角速度， ω_ref 即为直立环的输出。当我们将 ω_ref 代入上式并进行代数整理，即可发现总输出自然分解为两个独立项之和：一项仅含 θ 与 ω ，另一项仅含 ω 的积分项。这正是代码中 straighten_pid() 与 speed_pid() 函数输出直接相加的数学本质。因此，“并联PID”并非设计妥协，而是对物理模型进行合理简化后的最优工程表达，它在保证控制性能的同时，极大降低了参数整定的复杂度与计算开销。

2. 硬件外设配置与信号链分析

系统的硬件平台基于STM32F4系列微控制器，其丰富的定时器资源与专用外设模块为多任务实时控制提供了坚实基础。整个信号采集与执行链路的设计，严格遵循了嵌入式实时系统对确定性、低延迟与抗干扰性的核心要求。

2.1 定时器资源规划与PWM生成

系统共使用了四个高级定时器（TIM1, TIM2, TIM3, TIM4），其分配逻辑完全服务于不同执行机构的物理特性需求：

• TIM3（CH1/CH2） ：配置为10 kHz PWM频率，用于驱动动量轮无刷电机与底盘运动电机。选择10 kHz而非数据手册标称的20 kHz，是经过实测权衡的结果。过高的开关频率虽能减小电流纹波，但会显著增加MOSFET的开关损耗与EMI噪声，而10 kHz已足以满足电机电感的滤波需求，确保转矩输出平滑且系统热稳定性良好。

• TIM2（CH1） ：配置为50 Hz PWM频率，专用于舵机（Steering Servo）控制。此频率是标准舵机协议的硬性要求，其脉宽（0.5 ms - 2.5 ms）直接映射到舵机的机械角度（0° - 180°）。任何偏离都将导致舵机失控或异常抖动。

• TIM1（CH1） ：配置为1 s周期的单脉冲模式，用于超声波测距模块（HC-SR04）的触发。其计数器每递增1对应100 μs时间，因此最大计数值为10,000，恰好覆盖1 s。此设计旨在规避超声波回波信号因障碍物距离过远而丢失的风险。在开放环境中，声速约340 m/s，1 s内声波可传播340米，远超小车工作范围，从而确保每次触发后必有有效回波返回，避免了因超时而引发的中断服务程序（ISR）死锁。

• TIM4（编码器接口） ：工作于编码器接口模式，直接连接动量轮电机的A/B相正交编码器信号。该模式利用硬件自动完成四倍频计数与方向判别，无需软件干预，将CPU从高频脉冲计数的繁重任务中解放出来。其计数器溢出值被设定为足够大，以防止在小车高速运行时发生意外溢出。

2.2 传感器信号调理与ADC采样

电池电压监测是系统安全运行的生命线。3S锂电池标称电压为11.1 V，满电可达12.6 V，远超STM32F4 ADC的3.3 V参考电压上限。为此，硬件采用高精度电阻分压网络进行信号调理：R1 = 10 kΩ（接电池正极），R2 = 1 kΩ（接地），分压比为1/11。理论上，12.6 V电池电压在ADC采样点呈现为1.145 V。然而，实际电路中电阻公差、PCB走线寄生参数及ADC内部参考电压漂移均会引入系统误差。因此，在软件层面，校准系数被设定为11.09而非理论值11.00。该系数需通过高精度数字万用表（DMM）实测电池电压与ADC读数后，通过最小二乘法拟合得出，确保全量程范围内电压读取误差小于±0.1 V。

MPU6050的初始化流程是系统启动可靠性的关键一环。其内部集成的DMP（Digital Motion Processor）协处理器可直接输出经卡尔曼滤波融合后的姿态解算结果，大幅降低主CPU负担。初始化函数 MPU_Init() 的返回值是唯一可信的完成标志：仅当DMP初始化成功（返回值为0）后，OLED屏幕才显示“MPU-6050 OK”，并启动主控制循环；若失败（返回非0值），则显示“MPU-6050 ERROR”，并进入故障保护状态。此设计杜绝了因传感器未就绪而导致的无效控制输出，是嵌入式系统“Fail-Safe”原则的典型体现。

2.3 外设抽象层设计

sys.h 头文件扮演着系统资源总线的角色。它并非简单地包含所有外设头文件，而是通过精心设计的宏定义与条件编译，实现了硬件抽象层（HAL）的雏形。例如，所有GPIO引脚定义均采用 GPIOx_PinY 的标准命名（如 GPIOA_Pin5 ），所有外设句柄（Handle）均在此处统一声明。更重要的是，它封装了关键的全局变量与函数原型，如 extern float battery_voltage; 与 void system_init(void); 。这种设计使得上层应用代码（如 ctrl.c ）完全不依赖具体硬件细节，只需包含 sys.h 即可访问全部系统资源。当需要更换MCU型号或调整引脚分配时，仅需修改 sys.h 及其对应的 .c 实现文件，应用逻辑代码无需任何改动，极大地提升了工程的可维护性与可移植性。

3. 核心控制算法实现详解

控制算法的实现位于 ctrl.c 文件中，其核心是 straighten_pid() 与 speed_pid() 两个函数。它们并非教科书式的通用PID模板，而是针对动量轮自行车这一特定物理对象进行深度定制的工程化实现，每一行代码都承载着明确的物理意义与工程考量。

3.1 直立环（Straighten PID）实现

直立环采用PD控制策略，省略了积分项。这是由其物理任务决定的：直立环的目标是瞬时抵消倾覆力矩，其输出应与当前姿态偏差及偏差变化率成正比。引入积分项会导致系统对持续微小偏差产生过度累积响应，反而在平衡点附近诱发低频振荡（Hunting），破坏稳定性。

float straighten_pid(float angle, float gyro_rate) {
    static float last_angle = 0.0f;
    float angle_error = angle_setpoint - angle; // 姿态误差：理想角 - 实际角
    float rate_error = -gyro_rate;              // 角速度误差：理想角速度（0）- 实际角速度

    float output = Kp_straighten * angle_error + Kd_straighten * rate_error;

    // 防止积分饱和的等效措施：对输出进行硬限幅
    if (output > OUTPUT_MAX) output = OUTPUT_MAX;
    if (output < -OUTPUT_MAX) output = -OUTPUT_MAX;

    return output;
}

此处 Kp_straighten 与 Kd_straighten 的符号至关重要。在本系统中， Kp_straighten 被赋值为负值（如-820）， Kd_straighten 亦为负值。这意味着：当车体向右倾斜（ angle > 0 ）时， angle_error 为负，负的 Kp 乘以负的 error 得到正的输出，驱动动量轮向右加速，产生向左的陀螺力矩以扶正车身。反之亦然。这种极性设置必须与MPU6050的安装朝向、编码器的相位关系以及电机的物理转向严格匹配。任何一环的极性错误，都会导致控制力矩方向相反，系统瞬间失稳。因此， Kp 的符号整定是调参的第一步，也是最关键的一步，它决定了控制系统的“本能”是否正确。

3.2 速度环（Speed PID）实现

速度环采用PI控制，其目标是精确跟踪由直立环生成的、随姿态动态变化的速度指令 ω_ref 。由于 ω_ref 本身就是一个模拟量（来自 straighten_pid() 的浮点输出），其值可能在正负大范围内快速跳变，因此速度环必须具备强大的抗扰动能力与快速跟踪性能。

float speed_pid(int16_t encoder_count, float speed_ref) {
    static int32_t integral_sum = 0;
    static int16_t last_count = 0;

    // 计算本次10ms周期内的脉冲增量
    int16_t delta_count = encoder_count - last_count;
    last_count = encoder_count;

    // 将脉冲增量转换为速度（单位：rpm 或 任意归一化单位）
    // 此处假设10ms内1个脉冲代表1单位速度
    float speed_actual = (float)delta_count;

    // 对实际速度进行一阶低通滤波，截止频率约50Hz
    static float speed_filtered = 0.0f;
    speed_filtered = 0.9f * speed_filtered + 0.1f * speed_actual;

    float speed_error = speed_ref - speed_filtered;

    // 积分项累加，并进行防饱和处理
    integral_sum += (int32_t)(speed_error * Ki_speed);
    if (integral_sum > INTEGRAL_MAX) integral_sum = INTEGRAL_MAX;
    if (integral_sum < -INTEGRAL_MAX) integral_sum = -INTEGRAL_MAX;

    float output = Kp_speed * speed_error + (float)integral_sum;

    return output;
}

该实现的关键创新点在于速度的计算方式。由于编码器计数器在每次10 ms中断中被读取后即清零（或采用差分方式）， delta_count 直接代表了该采样周期内的脉冲数。在固定采样周期（T=10ms）下，脉冲数 N 与平均转速 ω 成正比（ ω ∝ N/T ）。因此，将 delta_count 直接作为 speed_actual 的量化值，是一种高效且物理意义明确的简化。后续的低通滤波（系数0.1）有效抑制了由机械振动、编码器接触噪声等引起的高频干扰，避免了速度环对虚假扰动的误响应。

积分项的防饱和采用“积分限幅”而非“积分分离”，因为速度环的稳态误差必须被彻底消除，以保证在各种姿态下动量轮都能精确达到所需的角速度。 INTEGRAL_MAX 被设定为10000，这是一个经验阈值，既能保证足够的积分作用，又能在系统遭遇大扰动（如突然推搡）时，防止积分项过度累积导致恢复过程缓慢。

3.3 主控制循环与中断服务

整个控制律的执行节奏由一个100 Hz（10 ms周期）的外部中断严格驱动。该中断源来自MPU6050的DMP数据就绪引脚（INT Pin），其频率由DMP固件配置决定。在 EXTI15_10_IRQHandler() 中，执行以下原子操作：

1. 数据采集 ：立即读取TIM4的编码器计数值（ HAL_TIM_ReadCounter(&htim4) ）与MPU6050的DMP融合姿态数据（通过I2C或SPI）。
2. 控制计算 ：依次调用 straighten_pid() 与 speed_pid() ，获得两个环路的输出。
3. 输出合成与加载 ：将两路输出相加，并通过 __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, pwm_duty) 将最终PWM占空比写入TIM3的捕获/比较寄存器。
4. 安全保护 ：在加载PWM前，执行 motor_safety_check(angle) 函数。该函数检查当前横滚角 angle 的绝对值是否超过预设的安全阈值（如20°）。一旦超限，立即强制将PWM输出置零，切断电机动力，防止小车倾倒时动量轮继续高速旋转造成机械损伤。这是一种典型的“硬件看门狗”式软件保护。

主循环（ while(1) ）则承担着非实时性任务：刷新OLED屏幕显示当前角度、速度、电池电压等关键参数；通过串口接收上位机发送的PID参数更新指令；执行用户交互逻辑（如按键切换模式）。这种“中断驱动控制+主循环处理”的架构，完美分离了实时性要求严苛的控制任务与实时性要求宽松的监控任务，是嵌入式实时系统设计的黄金法则。

4. PID参数整定方法论与工程实践

PID参数整定是将理论控制律转化为实际稳定性能的最后也是最关键的环节。对于动量轮自平衡自行车，参数整定绝非盲目试错，而是一个遵循严格物理逻辑与工程经验的系统性过程。本节将详细阐述一套已被验证有效的、分阶段、有依据的整定方法。

4.1 机械零点（Mechanical Zero Point）标定

这是所有后续整定工作的基石。机械零点并非理论上的0°，而是小车在静止、水平地面上，其重心投影恰好落在两轮接触点连线上的那个特定角度。由于车架加工公差、传感器安装偏斜、轮胎形变等因素，这个真实零点必然与理论值存在偏差。标定方法如下：

1. 将小车平稳放置于绝对水平的大理石平台或高精度水平仪校准的台面上。
2. 上电运行程序，观察OLED屏幕第一行显示的实时横滚角 angle 。
3. 待数值稳定（约10秒），记录下该稳定值。此值即为 angle_setpoint 的初始值。例如，若屏幕稳定显示 -3.5° ，则 angle_setpoint = -3.5f 。

此步骤的重要性在于，它将整个控制系统的参考系锚定在真实的物理平衡点上。若跳过此步，直接设 angle_setpoint = 0.0f ，则系统将试图将车身“拉”到一个并不存在的理论垂直位置，导致持续的、无法消除的稳态偏差，使速度环长期处于积分饱和状态，最终引发不可控振荡。

4.2 直立环KP极性与幅值整定

在 angle_setpoint 标定完成后，将 Kp_straighten 、 Kd_straighten 、 Kp_speed 、 Ki_speed 等所有其他参数暂时置零，仅保留 Kp_straighten 为一个较小的非零值（如-10）。

1. 极性验证 ：手动将小车向右轻轻倾斜约5°，观察动量轮的转向。若动量轮随之向右加速，则 Kp_straighten 的负号正确；若向左加速，则符号错误，需改为正值。此验证必须在小车即将倾倒前迅速松手，以避免摔车。
2. 幅值整定 ：确认极性正确后，逐步增大 |Kp_straighten| 的绝对值（如-20, -50, -100…）。随着增益增大，小车的“扶正”反应会越来越快、越来越有力。继续增大，直至小车出现明显的、大幅度的低频左右摇摆（类似钟摆）。此时， Kp_straighten 已接近临界稳定点。记录下此时的值（如-800），然后将其减小约10%-20%（如-720），作为初步的 Kp_straighten 值。此留出的裕度是为后续加入微分项与速度环预留的稳定空间。

4.3 直立环KD与速度环KP/Ki整定

在 Kp_straighten 初步确定后，开始引入微分项以抑制振荡：

1. 引入KD ：将 Kd_straighten 从0开始缓慢增大（负值，如-0.1, -0.5, -1.0…）。 KD 的作用是给系统“刹车”，它能显著抑制 KP 过大引起的超调与振荡。最佳 KD 值应使小车在受到扰动后，能以最短的时间、最少的振荡次数回到平衡点。过大的 KD 会使系统反应迟钝，甚至引入新的高频噪声。
2. 引入速度环 ：当直立环已能基本稳定（小幅、快速振荡）后，再开启速度环。首先赋予 Kp_speed 一个较小的正值（如1.0），观察小车在平衡状态下的“沉稳度”。 Kp_speed 越大，小车抵抗扰动的能力越强，但过大会导致系统刚性过强，易受噪声影响。随后，缓慢增加 Ki_speed ，观察稳态偏差是否消失。 Ki_speed 的引入会使小车在长时间运行后，即使有微小的 KP / KD 误差，也能通过积分作用将姿态误差彻底归零。

整定过程中，务必遵循“一次只调一个参数”的铁律。每次调整后，至少观察30秒，让系统充分展现其动态特性。切忌心急求成，将多个参数同时大幅调整，这只会让系统行为变得混沌不堪，无法追溯问题根源。

5. 系统调试与常见问题排查

在完成参数整定后，系统仍可能面临各种现实世界的挑战。这些挑战往往源于硬件、环境或软件实现中的细微偏差，而非控制算法本身的缺陷。掌握一套高效的调试与排查方法，是工程师区别于初学者的核心能力。

5.1 硬件级问题诊断

• MPU6050初始化失败（显示“MPU-6050 ERROR”） ：首要检查I2C总线的硬件连接。确认SDA/SCL线上是否已正确焊接4.7 kΩ上拉电阻至3.3 V。使用示波器观察I2C波形，若无信号，检查MCU的I2C引脚是否被意外配置为其他复用功能（如SWD调试引脚）。若波形存在但通信失败，检查MPU6050的电源（VCC）与地（GND）是否稳定，用电压表测量其引脚电压是否为精确的3.3 V。

• 编码器计数异常（速度显示为0或乱跳） ：断开电机，用手匀速转动动量轮，用逻辑分析仪抓取A/B相信号。正常情况下，两路信号应为严格的90°相位差方波。若信号缺失、畸变或相位差严重偏离，问题出在编码器本身或其与MCU间的信号线（检查是否有虚焊、短路）。若信号正常，检查 TIM4 的编码器模式配置寄存器（ SMCR ）是否正确设置了 SMS=3 （编码器模式）及 CCMR1/2 中通道的输入滤波与极性。

• 电机不转或转向错误 ：首先确认“启动信号”（Enable Pin）是否被MCU正确拉高（3.3 V）。这是无刷电机驱动板的使能条件，未使能则PWM信号无效。其次，用万用表直流电压档测量PWM引脚在程序运行时的平均电压，若为0 V，检查 TIM3 的初始化（ HAL_TIM_PWM_Start() ）是否被正确调用；若为3.3 V（高电平），则检查电机驱动板是否支持3.3 V逻辑电平，部分驱动板要求5 V，此时需加电平转换电路。

5.2 软件与参数级问题诊断

• 小车能短暂平衡但很快摔倒 ：这通常是直立环 KP 不足或 KD 过大所致。 KP 不足导致扶正力矩太小，无法抗衡重力矩； KD 过大则过度抑制了系统的自然响应，使其“麻木”。解决方案是小幅增加 |Kp_straighten| ，或减小 |Kd_straighten| 。

• 小车在平衡点附近高频颤抖（“嗡嗡”声） ：这是典型的 KP 过大或 KD 不足的表现。系统对微小的姿态变化反应过度，形成正反馈振荡。应立即减小 |Kp_straighten| ，并适当增大 |Kd_straighten| 。

• 小车向一侧持续缓慢倾倒 ：这表明存在未被补偿的系统偏差。首先检查 angle_setpoint 是否准确标定；其次，检查MPU6050的零偏（Zero-Rate Level）是否已通过DMP的校准功能消除；最后，检查动量轮电机是否存在单向的机械阻力（如轴承卡滞、皮带过紧），导致其在某一方向上响应滞后。

• OLED屏幕显示乱码或不刷新 ：检查 SSD1306 驱动库的I2C地址是否与硬件匹配（常见为 0x3C 或 0x3D ）。若使用SPI接口，则检查DC（Data/Command）引脚的电平控制逻辑是否正确。乱码也可能是由于在中断服务程序中进行了耗时的OLED刷新操作，导致主循环无法及时更新显示缓冲区，应将显示更新移至主循环中。

5.3 实战经验与避坑指南

在我个人的实际项目中，曾踩过几次印象深刻的坑：

• “静默”中断冲突 ：最初将MPU6050的INT引脚配置为 EXTI Line 0 ，并与系统默认的 SysTick 中断共用同一优先级组。结果在高速运算时， SysTick 的高频率抢占导致MPU6050的10 ms中断偶尔被延迟，造成控制周期抖动，小车出现间歇性失稳。解决方法是将MPU6050的EXTI中断优先级设为最高（ NVIC_SetPriority(EXTI0_IRQn, 0) ），并确保其在 HAL_MspInit() 中被正确使能。

• ADC采样时机错位 ：早期版本中，电池电压采样放在主循环中，导致其读数受OLED刷新、串口通信等任务影响，波动剧烈。后来将其移至一个独立的、低优先级的 TIM6 更新中断中，每500 ms采样一次，并启用ADC的DMA传输，彻底解决了采样时机不确定的问题。

• 参数“记忆”丢失 ：在调试后期，频繁烧录程序导致Flash擦写次数过多，某次烧录后发现 Kp_straighten 的值被意外改写。为避免此问题，我将所有可调PID参数存储在STM32F4的备份寄存器（Backup Registers）中，这些寄存器由VBAT供电，在系统掉电后数据不丢失，且擦写寿命远高于Flash。

这些经验无一不是在一次次“摔车”与“重烧”中积累而来。它们提醒我们，一个成功的嵌入式控制项目，其成功不仅在于精妙的算法，更在于对硬件特性的深刻理解、对实时系统行为的精准把握，以及在无数个细节中所付出的耐心与严谨。