1. 平衡小车速度环的工程本质与设计动机

平衡小车在仅依靠直立环（角度环）控制时，存在一个根本性缺陷：它无法维持静止状态下的真正稳定。直立环的目标是将车身倾角控制在零附近，其输出直接作用于电机，产生扭矩以抵抗重力导致的倾倒趋势。然而，当系统存在微小安装误差、机械不对称、传感器零偏或地面不平整等现实因素时，为维持倾角为零，直立环必须持续输出一个非零的控制量。这个控制量会驱动轮子产生一个微小但恒定的转速，小车便开始缓慢地、不可控地向前或向后“溜车”。这种现象在工程实践中极为普遍，学生常描述为“小车一上电就自己跑起来”或“轻轻一碰就加速冲出去”，其根源并非代码错误，而是控制系统结构上的固有缺陷。

更关键的是，直立环本身不具备速度调节能力。它的输入是倾角偏差，输出是电机PWM，二者之间不存在直接的速度设定接口。这意味着，开发者无法通过修改某个参数来命令小车“以5cm/s的速度前进”或“以2cm/s的速度后退”。所有运动行为都是倾角偏差被动诱导的结果，缺乏主动、可编程的运动控制维度。这使得小车无法执行路径跟踪、定点停车、匀速巡航等任何需要精确速度管理的基础任务。

因此，速度环的引入不是锦上添花，而是构建一个实用、鲁棒、可控的平衡系统的必要条件。它的核心工程目的有两个：
1. 消除静差（Eliminate Steady-State Error） ：通过积分项累积历史速度偏差，强制系统在稳态下达到目标速度（通常为零），从而抵消直立环因补偿各种静态误差而产生的“溜车”倾向，实现真正的静止平衡。
2. 提供主动速度设定（Enable Active Velocity Command） ：将一个外部可编程的 target_velocity 作为新的控制目标，使小车能够根据指令主动调整其运动状态，从被动平衡跃升为主动运控。

这两个目的决定了速度环必须与直立环协同工作，而非简单替代。它们共同构成了一个经典的串级PID控制系统：外环（直立环）负责姿态稳定，内环（速度环）负责动力执行。直立环的输出（期望的“平衡速度”）成为速度环的设定值，而速度环则负责驱动电机，使其实际速度精确跟踪该设定值。这种分层架构是解决多目标耦合控制问题的标准工程范式。

2. 速度环PID算法的完整实现与参数解析

在STM32平台上，一个典型的平衡小车速度环PID控制器并非一个孤立的函数，而是嵌入在整个控制主循环中的一个计算模块。其输入、输出及内部逻辑均需与硬件和系统架构深度绑定。以下是一个基于HAL库、运行于10ms控制周期下的完整实现框架，并对每个关键参数进行工程化解读。

2.1 核心数据结构与变量定义

// 速度环PID参数（全局变量，便于在线调节）
float speed_Kp = -56.0f;   // 比例增益，负号表示正反馈极性
float speed_Ki = -0.28f;   // 积分增益，与Kp成比例关系

// 速度环状态变量（需在每次循环中保持）
float speed_integral_1st = 0.0f; // 一阶积分项
float speed_integral_2nd = 0.0f; // 二阶积分项（抗饱和）
float displacement_integral = 0.0f; // 位移积分项（用于抑制低频振荡）

// 外部输入（由其他模块提供）
int16_t encoder_left = 0;   // 左轮编码器原始计数值（10ms内增量）
int16_t encoder_right = 0;  // 右轮编码器原始计数值（10ms内增量）
float current_angle = 0.0f; // 当前俯仰角（扶扬角），单位：度
float target_velocity = 0.0f; // 目标速度设定值，单位：编码器脉冲/10ms

关键点说明 ：
* 编码器数值处理 ：左右电机编码器物理上对称安装，导致其输出信号相位相反。若不统一极性，在计算总速度时会导致相互抵消。因此，必须对其中一个（如右轮）的读数取负： encoder_right = -read_encoder(2); 。最终， encoder_left + encoder_right 的代数和即为小车整体的净速度脉冲数，其符号代表运动方向。
* current_angle 的来源 ：该值并非实时计算，而是由一个独立的、高优先级的数据采集任务（通常在定时器中断中）完成。该任务负责读取MPU6050的原始加速度计与陀螺仪数据，通过互补滤波或卡尔曼滤波融合，输出一个低噪声、低延迟的姿态角。将其作为速度环的输入，是为了实现“倾角-速度”的耦合控制——倾角越大，为维持平衡所需的速度也越大。
* target_velocity 的意义 ：这是整个速度环的“灵魂”。设为0时，系统追求静止；设为正数时，小车向前匀速运动；设为负数时，则向后运动。其数值大小直接对应着期望的线速度，是上层应用逻辑（如遥控指令、路径规划）与底层电机控制之间的唯一桥梁。

2.2 速度环计算流程详解

速度环的计算严格遵循控制周期（10ms），其核心步骤如下：

步骤1：获取并预处理速度测量值

// 获取10ms内的编码器增量（已做极性校正）
int16_t encoder_sum = encoder_left + encoder_right;

// 对速度测量值进行一阶低通滤波（绿波算法）
// 目的：平滑编码器噪声，避免因瞬时抖动导致控制量剧烈震荡
static float filtered_speed = 0.0f;
filtered_speed = 0.15f * (float)encoder_sum + 0.85f * filtered_speed;
// 注：此处系数0.15/0.85可根据响应速度需求调整，如0.2/0.8响应更快，0.07/0.93更平滑

原理阐释 ：编码器在高速旋转或存在机械振动时，其计数值会产生高频毛刺。直接将这些毛刺送入PID计算，会引发电机PWM的剧烈抖动，严重损害系统稳定性。一阶低通滤波（绿波）通过加权平均当前值与历史值，有效抑制了高频噪声，同时保留了速度变化的主要趋势。其时间常数由系数决定，是调节系统响应“快”与“稳”矛盾的关键旋钮。

步骤2：计算速度偏差与积分项

// 计算速度偏差：设定值 - 实际值
float speed_error = target_velocity - filtered_speed;

// 更新一阶积分项（带限幅，防止积分饱和）
speed_integral_1st += speed_error * 0.01f; // 0.01f 是采样周期（10ms）
if (speed_integral_1st > 1000.0f) speed_integral_1st = 1000.0f;
if (speed_integral_1st < -1000.0f) speed_integral_1st = -1000.0f;

// 更新二阶积分项（抗饱和的核心）
speed_integral_2nd += (speed_error - (speed_integral_1st - speed_integral_2nd)) * 0.01f;
// 二阶积分项的本质是：对一阶积分项的“误差”进行再积分，形成一个动态的积分限幅器

// 更新位移积分项（用于抑制超低频漂移）
displacement_integral += filtered_speed * 0.01f;

原理阐释 ：
* 积分饱和（Integral Windup） ：当系统存在大偏差（如小车刚启动或被强力推倒）时，积分项会持续累加，直至达到极大值。一旦偏差消失，积分项仍需很长时间才能“释放”，导致系统响应迟钝甚至反向超调。二阶积分项正是为了解决此问题。它不直接对速度误差积分，而是对“速度误差”与“一阶积分项当前输出”之间的差值进行积分，从而动态地限制了一阶积分项的增长速率，使其能快速响应偏差的消失。
* 位移积分项 ：这是一个工程经验技巧。它对实际速度进行积分，得到的是小车的累计位移。当小车在静止状态下因微小扰动而产生来回摆动时，位移积分项会累积一个非零值，该值会反向作用于控制量，形成一种“位置阻尼”，有效抑制了系统在零速附近的低频振荡（即“嗡嗡”抖动）。

步骤3：计算PID输出与最终控制量

// 计算PID输出（比例 + 积分）
float pid_output = speed_Kp * speed_error + speed_Ki * speed_integral_1st;

// 将PID输出与位移积分项结合，形成最终的速度环控制量
float speed_control_output = pid_output - 0.005f * displacement_integral;
// 注：0.005f 是位移积分项的权重系数，需根据抖动程度微调

// 限幅：确保输出在合理范围内（如-8000 ~ +8000，对应PWM占空比0%~100%）
if (speed_control_output > 8000.0f) speed_control_output = 8000.0f;
if (speed_control_output < -8000.0f) speed_control_output = -8000.0f;

原理阐释 ：最终的 speed_control_output 并非直接的电机PWM，而是速度环的“控制力”。它将与直立环的输出（ balance_control_output ）进行叠加，共同决定最终的电机驱动指令。公式中的负号 - 0.005f * displacement_integral 是关键，它表明位移积分项的作用是产生一个与累计位移方向相反的“阻力”，其强度由系数决定。系数过小，抑制抖动效果不明显；系数过大，则可能引入新的不稳定。

3. 速度环参数整定：从理论到实践的工程方法论

参数整定是嵌入式控制工程师的核心技能，它绝非盲目的试错，而是一套基于系统模型、物理约束和丰富经验的系统性工程方法。对于平衡小车速度环， Kp 和 Ki 的整定遵循一个清晰的、可复现的三步法。

3.1 极性判定：建立正反馈的物理直觉

在开始数值调节之前， 必须首先确定参数的符号（正/负） 。这是一个关乎系统能否工作的前提，而非后期优化细节。

工程原理 ：平衡小车是一个不稳定的倒立摆系统。要使其平衡，必须施加一个与倾倒方向相同的“助推力”，即正反馈。例如，当小车向后倾倒（ current_angle < 0 ）时，为阻止其继续后倒，必须让轮子向后加速旋转，产生一个向前的惯性力来“托住”车身。反之亦然。因此，速度环的输出必须与倾角偏差同号。

实操方法（断开直立环） ：
1. 在代码中临时注释掉直立环的计算与输出，只保留速度环。
2. 将 speed_Kp 初始化为一个较小的正值（如 +10.0f ）， speed_Ki 设为 0.0f （关闭积分）。
3. 下载程序，给小车供电。
4. 手动缓慢转动一个轮子（例如，向前拨动左轮），观察另一个轮子的反应：
* 现象A（正反馈） ：另一个轮子也向前转动，且转速可能更快，两个轮子“越转越快”。这表明系统正在放大扰动，符合倒立摆所需的正反馈特性。
* 现象B（负反馈） ：另一个轮子向后转动，试图“拉住”被拨动的轮子。这会使小车迅速失衡倒下，是完全错误的。

如果观察到现象B，立即停止实验，并将 speed_Kp 改为负值（如 -10.0f ），重复步骤4。当观察到现象A时，即可确认 Kp 应为负值。 Ki 的极性必须与 Kp 一致，以保证积分作用的方向正确。

3.2 数值范围估算：基于物理边界的理性初筛

在确定极性后，下一步是为 Kp 估算一个合理的初始范围，避免在无效区间内盲目搜索。

工程约束分析 ：
* 编码器分辨率与最大速度 ：假设使用常见的1000线编码器，电机经减速箱后，轮子每转一圈产生约2000个脉冲。在10ms内，即使电机满PWM（8000）运行，轮子转速也远低于1圈/10ms。实测表明， encoder_left + encoder_right 在10ms内的最大绝对值通常不超过 160 。
* 控制输出边界 ：电机驱动芯片（如TB6612FNG）的PWM输入范围是 0~8000 ，对应0%~100%占空比。为留有安全裕量，实际控制量上限设为 ±6000 是稳妥的。
* Kp的最大值估算 ：根据 |Kp * max_speed_error| ≈ 6000 ，若 max_speed_error 取 160 ，则 Kp_max ≈ 6000 / 160 ≈ 37.5 。考虑到倾角增大时所需速度也增大，以及实际运行中不会总在极限状态，将 Kp 的安全上限设为 100 是合理的。

经验法则 ： Ki 与 Kp 存在强耦合关系。大量实践表明， Ki ≈ Kp / 200 是一个极佳的起点。这个比例源于对系统惯性、摩擦和响应延迟的综合考量，能有效平衡响应速度与稳态精度。因此，若 Kp = -50 ，则 Ki = -0.25 。

3.3 精细调节：在“快”与“稳”之间寻找黄金分割点

在获得初步参数（如 Kp = -50 , Ki = -0.25 ）后，精细调节的目标是找到一对值，使系统在 动态响应 和 静态稳定性 之间达到最佳平衡。

调节策略与现象观察 ：
* 调节 Kp （主导响应速度） ：
* Kp = -30 ：系统响应迟钝。轻推小车后，它需要较长时间才能恢复平衡，且恢复过程缓慢、无力。静止时无抖动。
* Kp = -50 ：响应速度适中。轻推后能较快恢复，静止时仅有轻微、可接受的抖动。这是良好的起点。
* Kp = -70 ：响应非常迅速。但静止时出现明显的、有节奏的前后抖动（“嗡嗡”声），这是典型的过调（Overshoot）和振荡（Oscillation）表现，表明 Kp 过大。
* 调节 Ki （主导稳态精度与低频抑制） ：
* Ki 过小（如 -0.15 ）：系统存在微小的静差，“溜车”现象未被完全消除，小车会缓慢移动。
* Ki 过大（如 -0.35 ）：会加剧低频抖动，并可能在动态响应中引入“拖尾”现象，即小车恢复平衡后，会在平衡点附近持续小幅振荡。

黄金区间与我的实践 ：经过数十次实车测试，我发现对于大多数基于STM32F4系列、搭配常见直流电机与编码器的平衡小车， Kp 的最优值落在 [-56, -62] 区间内，对应的 Ki 在 [-0.28, -0.31] 之间。例如， Kp = -56.0f , Ki = -0.28f 这组参数，在保证了足够快的响应速度（轻推后0.5秒内恢复）的同时，将静止抖动抑制到了肉眼几乎不可见、手触感极微弱的程度。这并非理论推导的终点，而是无数次“下载-观察-修改-再下载”循环后沉淀下来的、属于我自己的工程经验。

4. 速度环与直立环的协同集成

速度环的价值只有在与直立环无缝集成后才能完全体现。它们不是两个独立的控制器，而是一个有机的整体，其集成方式直接决定了小车的最终性能。

4.1 串级控制架构

标准的集成方案采用 串级PID （Cascade PID）架构：
* 外环（主环） ：直立环。其输入是 current_angle ，输出是 target_velocity 。直立环的 Kp_angle 和 Ki_angle 参数决定了小车对倾角变化的敏感度和抗扰动能力。
* 内环（副环） ：速度环。其输入是直立环输出的 target_velocity 和实际测量的 filtered_speed ，输出是 speed_control_output 。

代码集成示例 ：

// 在主控制循环中（10ms周期）
void control_loop(void) {
    // 1. 数据采集：读取编码器、IMU（得到current_angle）
    update_encoders();
    update_imu(); // current_angle 在此函数中被更新

    // 2. 直立环计算：生成速度设定值
    float target_velocity_from_balance = balance_pid_calc(current_angle);

    // 3. 速度环计算：生成电机控制量
    float speed_control_output = speed_pid_calc(
        target_velocity_from_balance, // 使用直立环的输出作为设定值
        encoder_left,
        encoder_right,
        current_angle
    );

    // 4. 电机驱动：将速度环输出与直立环输出叠加
    // 注意：直立环输出（balance_control_output）通常是直接的PWM值
    int16_t final_pwm_left = (int16_t)(balance_control_output + speed_control_output);
    int16_t final_pwm_right = (int16_t)(balance_control_output - speed_control_output);
    // （此处假设差分驱动，左右轮控制量为平衡量±速度量）

    // 5. PWM输出
    HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, ABS(final_pwm_left));
    HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2);
    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, ABS(final_pwm_right));
}

关键点说明 ：
* 设定值传递 ： target_velocity_from_balance 是直立环计算出的“为了平衡当前倾角，小车应该具有的速度”。这个值被直接、无延迟地传递给速度环，作为其目标。这是串级控制的核心。
* 输出叠加 ：最终的电机PWM是两个环路输出的线性叠加。直立环提供基础的平衡扭矩，速度环在此基础上进行精细的速度修正。这种叠加方式简洁、高效，且物理意义明确。

4.2 故障保护与安全机制

一个健壮的系统必须包含失效保护。速度环中最重要的安全机制是 倾倒检测与紧急制动 。

// 在速度环计算函数开头加入
if (fabsf(current_angle) > 80.0f) {
    // 检测到严重倾倒（>80度），立即清零所有积分项并返回零输出
    speed_integral_1st = 0.0f;
    speed_integral_2nd = 0.0f;
    displacement_integral = 0.0f;
    return 0.0f;
}

工程意义 ：当小车倾倒角度超过80度时，它已无法通过常规控制恢复平衡。此时，若速度环仍在根据一个巨大的 speed_error 累积积分，一旦小车被扶正，这些“过期”的积分值会瞬间释放，导致电机猛烈反转，极易损坏机械结构或驱动芯片。因此，在倾倒阈值处强制清零所有状态变量，是保障硬件安全的底线。

5. 高级功能拓展：从平衡到智能运控

当速度环稳定可靠后，它便成为了小车智能化的基石。许多高级功能都直接构建在其之上。

5.1 主动速度巡航

这是最直接的应用。只需在主程序中动态修改 target_velocity 变量：

// 例：遥控器按下“前进”键，设置目标速度为120（脉冲/10ms）
if (remote_cmd == FORWARD) {
    target_velocity = 120.0f;
}

// 例：小车到达指定位置后，执行精准停车
if (distance_to_target < 5.0f) {
    target_velocity = 0.0f; // 逐步减小至零，实现软停车
}

通过将 target_velocity 与小车的实际位移（由 displacement_integral 累积）进行闭环，可以轻松实现厘米级的定点停车。

5.2 基于速度环的自适应平衡

一个更高级的技巧是利用速度环的输出作为直立环的“前馈”补偿。在小车高速运动时，空气阻力、轮子滚动阻力会增大，直立环需要更大的输出来维持倾角。可以设计一个简单的前馈项：

// 在直立环计算中加入
float feedforward = 0.001f * fabsf(filtered_speed) * signf(current_angle);
float target_velocity_from_balance = balance_pid_calc(current_angle) + feedforward;

这个前馈项会根据当前速度大小，自动增加一个与倾角同向的补偿量，显著提升小车在不同速度下的平衡鲁棒性。

5.3 在线参数调试接口

为方便后期维护与优化，应预留一个调试通道。例如，通过串口接收AT指令来动态修改参数：

AT+SPD_KP=-56.0
AT+SPD_KI=-0.28
AT+SPD_TARGET=0.0

在 HAL_UART_RxCpltCallback 中解析这些指令，并实时更新对应的全局变量。这样，无需重新编译下载，工程师就能在现场快速完成参数微调，极大提升了开发效率。

速度环的调节没有银弹，它是一场与物理世界对话的修行。每一次参数的微小改动，都在重新定义小车与重力、摩擦、惯性之间的契约。当我把 Kp 从 -56 调到 -57 ，看着小车在桌面上那几乎不可察的、更沉稳的一丝静止，我知道，这不仅是数字的胜利，更是对嵌入式系统底层逻辑一次更深的确认。