1. 平衡车三环控制架构解析：从单角度环到速度-转向协同控制

在完成基础直立控制后，仅依赖角度环的平衡车系统存在本质性缺陷：它能维持车身姿态稳定，却无法约束小车的全局运动状态。当外力扰动（如手动推动）施加于小车时，角度环会通过调整电机输出维持倾角恒定，但小车自身将产生持续的位移与速度变化——这正是“直立但不驻停”的典型现象。工程上，这种控制结构被定义为 单闭环系统 ，其反馈量仅为倾角，缺乏对运动学变量（线速度、角速度）的直接观测与调控能力。

真正的平衡车控制系统必须构建 多级串级PID架构 ，形成对外部扰动的分层抑制能力。本节实现的三环结构严格遵循经典控制理论中的主从关系设计：
- 内环（角度环） ：以MPU6050陀螺仪/加速度计融合解算出的实时倾角为反馈量，以电机平均PWM占空比为控制量，负责最快速的姿态稳定。其采样周期设定为10ms，满足姿态动态响应的带宽要求。
- 中环（速度环） ：以左右电机编码器计算出的平均轮速为反馈量，以角度环的目标倾角为控制量，构成外环。其核心作用是将用户指定的行进速度指令转化为对应的目标倾角——例如，要使小车以2rps前进，速度环需计算出约-3.5°的目标倾角，驱动角度环执行该姿态以产生前进加速度。
- 外环（转向环） ：以左右电机编码器差分速度为反馈量，以差分PWM为控制量，独立调节小车的转向角速度。其输出叠加在角度环的PWM输出上，实现行进与转向的解耦控制。

这种三层嵌套结构并非简单叠加，而是通过 物理量映射关系 建立内在耦合：速度环的输出（目标倾角）直接作为角度环的输入指令；转向环的输出（差分PWM）则作为角度环PWM输出的修正项。三者共同构成一个完整的运动控制系统，其数学模型可表述为：

$$
\begin{cases}
\theta_{target} = PID_{speed}(v_{ref} - v_{avg}) \
v_{avg} = \frac{N_{left} + N_{right}}{2} \cdot \frac{1}{K_{enc}} \cdot \frac{1}{T_{sample}} \
\omega_{diff} = PID_{turn}(\omega_{ref} - (N_{left} - N_{right}) \cdot \frac{1}{K_{enc}} \cdot \frac{1}{T_{sample}}) \
PWM_{left} = PWM_{avg} - \omega_{diff} \
PWM_{right} = PWM_{avg} + \omega_{diff}
\end{cases}
$$

其中 $K_{enc}=44$ 为编码器线数，$T_{sample}=50ms$ 为速度环采样周期，$PWM_{avg}$ 由角度环PID计算得出。该模型清晰表明：速度环通过调节倾角间接控制线速度，转向环则通过差分速度直接控制角速度，二者在执行层完全解耦。

2. 编码器信号采集与速度解算实现

2.1 硬件接口与定时器配置

本系统采用STM32F103C8T6微控制器，左右电机编码器分别接入TIM2_CH1（PA0）与TIM3_CH2（PB5）引脚，配置为编码器接口模式（Encoder Interface Mode）。此模式下，定时器自动对AB相正交脉冲进行四倍频计数，无需软件判向，显著降低CPU开销。关键寄存器配置如下：

// TIM2 编码器配置（左电机）
TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, 
                           TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_SetCounter(TIM2, 0); // 清零计数器
TIM_Cmd(TIM2, ENABLE);

// TIM3 编码器配置（右电机）
TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12,
                           TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_SetCounter(TIM3, 0);
TIM_Cmd(TIM3, ENABLE);

此处必须强调：编码器计数器初值必须在初始化时清零，否则首次读取将包含上电累积值，导致速度计算严重失真。

2.2 50ms采样周期的精确实现

速度环与转向环的控制周期统一设定为50ms，该周期需兼顾三重约束：
- 精度下限 ：小于10ms时，编码器计数值过小（如50ms内仅计数20），量化误差放大，速度分辨率不足；
- 稳定性上限 ：大于100ms时，系统响应迟滞，无法及时抑制速度漂移；
- 计算负载 ：50ms周期内需完成双编码器读取、速度转换、双PID运算、PWM更新，经实测占用CPU约12%。

采用SysTick中断实现精确分频：

volatile uint8_t encoder_sample_flag = 0;
uint16_t sample_counter = 0;

void SysTick_Handler(void) {
    if (++sample_counter >= 50) { // 假设SysTick为1ms中断
        sample_counter = 0;
        encoder_sample_flag = 1;
    }
}

// 在主循环中检测标志位
if (encoder_sample_flag) {
    encoder_sample_flag = 0;
    // 执行编码器采样与速度计算
}

此方案避免了在中断服务程序中执行耗时计算，确保实时性。

2.3 速度解算的工程化实现

编码器原始计数值需经三重转换才能得到物理速度：
1. 线数归一化 ： count / 44.0 将脉冲数转换为电机旋转圈数；
2. 时间归一化 ： / 0.05 将50ms采样窗口内的圈数转换为rps（转每秒）；
3. 机械减速比补偿 ： / 9.27666 消除行星减速箱的传动比影响。

最终左右轮速计算代码如下：

float left_speed, right_speed;
float avg_speed, diff_speed;

// 读取TIM2计数值并清零
int16_t left_count = (int16_t)TIM_GetCounter(TIM2);
TIM_SetCounter(TIM2, 0);

// 读取TIM3计数值并清零
int16_t right_count = (int16_t)TIM_GetCounter(TIM3);
TIM_SetCounter(TIM3, 0);

// 转换为物理速度（rps）
left_speed  = (float)left_count  / 44.0f / 0.05f / 9.27666f;
right_speed = (float)right_count / 44.0f / 0.05f / 9.27666f;

// 计算平均速度与差分速度
avg_speed  = (left_speed + right_speed) * 0.5f;
diff_speed = left_speed - right_speed;

关键实践细节 ：
- 使用 int16_t 强制类型转换防止32位计数器高位溢出导致的符号错误；
- TIM_SetCounter() 必须在读取后立即执行，避免两次采样间丢失脉冲；
- 除法运算使用浮点常量（如 0.05f ）而非整数（ 50 ），避免隐式整型除法截断。

3. 速度环PID控制器的设计与参数整定

3.1 控制器结构与物理意义

速度环采用增量式PID算法，其输出为目标倾角 $\theta_{target}$，直接输入至角度环。控制器结构体定义如下：

typedef struct {
    float kp, ki, kd;      // 比例、积分、微分系数
    float target;          // 目标速度（rps）
    float actual;          // 实际平均速度（rps）
    float prev_error;      // 上次误差
    float integral;        // 积分项
    float out;             // 输出（目标倾角，°）
    float out_max, out_min; // 输出限幅（±20°）
} PID_TypeDef;

PID_TypeDef speed_pid = {
    .kp = 2.0f,
    .ki = 0.05f,
    .kd = 0.0f,
    .out_max = 20.0f,
    .out_min = -20.0f
};

限幅值设定依据 ：实验测得小车在±20°倾角下能达到最大可控加速度，超出此范围将触发机械限位或导致失控。该限幅非安全冗余，而是控制律的固有约束——它定义了速度环可调度的最大动力学潜力。

3.2 单位一致性原理的工程实践

初学者常质疑：“速度环输出单位为rps，角度环输入单位为°，直接赋值岂非单位不匹配？”此疑问源于对PID系数物理意义的误解。实际上，$k_p$ 的单位应为 °/(rps) ，$k_i$ 为 °/(rps·s) ，$k_d$ 为 °·s/(rps) 。以比例环节为例：
$$
\theta_{target} = k_p \cdot (v_{ref} - v_{avg})
$$
当 $k_p = 2.0$ 时，其真实含义是“速度偏差每增加1rps，目标倾角增加2°”。这种比例关系由机械系统动力学决定：轮速偏差→所需加速度→对应倾角。因此，PID系数本质是 物理系统传递函数的离散化近似 ，其数值必须通过实验整定，无法纯理论推导。

3.3 参数整定的系统化流程

参数整定严格遵循“由内而外、先比例后积分”的工业标准流程：

步骤1：验证反馈极性
- 将 $k_p=1.0$, $k_i=k_d=0$，手动推动小车前进
- 观察角度环目标值：若 $\theta_{target}$ 变为负值（车身后仰），说明反馈极性正确（负反馈抑制速度增长）
- 若极性错误，需交换编码器AB相接线或修改 $k_p$ 符号

步骤2：整定比例增益 $k_p$
| $k_p$ 值 | 现象 | 工程判断 |
|---------|------|----------|
| 1.0 | 推动后小车缓慢减速，存在明显超调 | 增益偏低，响应迟钝 |
| 2.0 | 减速过程平稳，无超调，停止位置准确 | 推荐初始值 |
| 3.0 | 快速制动但伴随高频振荡 | 增益偏高，需降低 |
| 4.0 | 持续振荡无法收敛 | 增益严重过调 |

步骤3：引入积分消除稳态误差
- 在 $k_p=2.0$ 基础上，逐步增加 $k_i$
- $k_i=0.05$：松开摇杆后小车能在2秒内完全停止，无位置漂移
- $k_i=0.1$：出现低频摆动（积分饱和），需减小
- 关键经验 ：积分项仅在小车运动时显现效果，静态测试无法验证其价值

步骤4：微分项的取舍
- 尝试 $k_d=0.1$：制动过程出现尖锐抖动
- 根本原因：编码器速度信号含高频噪声，微分项放大噪声导致控制量突变
- 工程结论 ：在50ms采样周期下，速度环无需微分项，$k_d=0$ 为最优选择

4. 转向环PID控制器的实现与协同控制

4.1 差分速度的物理建模

转向环的反馈量为差分速度 $v_{diff} = v_{left} - v_{right}$，其物理意义是小车绕垂直轴的角速度 $\omega_z$。根据运动学关系：
$$
\omega_z = \frac{v_{diff}}{2L} \cdot r
$$
其中 $L$ 为轮距（本车0.12m），$r$ 为轮半径（0.03m）。但实际控制中无需显式计算 $\omega_z$，因为：
- 用户通过摇杆指定的是相对转向强度，而非绝对角速度；
- 差分速度 $v_{diff}$ 与转向响应呈线性关系，直接作为反馈量更鲁棒；
- 避免引入 $L,r$ 等易受装配误差影响的参数。

4.2 转向环控制器设计

转向环采用PI控制（$k_d=0$），其输出为差分PWM $\Delta PWM$，叠加至左右轮PWM：

PID_TypeDef turn_pid = {
    .kp = 4.0f,
    .ki = 3.0f,
    .kd = 0.0f,
    .out_max = 50.0f,   // 差分PWM限幅±50
    .out_min = -50.0f
};

// 在50ms采样周期内执行
turn_pid.actual = diff_speed; // 反馈量为差分速度
PID_Update(&turn_pid);
float delta_pwm = turn_pid.out;

// PWM输出计算
pwm_left  = pwm_avg - delta_pwm;
pwm_right = pwm_avg + delta_pwm;

限幅值设定逻辑 ：±50的限幅预留了50%的PWM动态范围用于行进控制（角度环+速度环输出），避免转向与行进控制相互抢占执行机构。

4.3 转向环参数整定策略

转向环整定需解决的核心矛盾是 响应速度与稳定性 ：
- 纯比例控制失效 ：$k_p=2$ 时转向迟钝，$k_p=4$ 时仍存在跟踪滞后
- 积分主导响应 ：$k_i=3$ 使绿线（实际差分速度）紧密跟随红线（目标差分速度），上升时间缩短60%
- 抗饱和设计 ：在PID_Update函数中加入积分抗饱和：

void PID_Update(PID_TypeDef* pid) {
    float error = pid->target - pid->actual;

    // 积分项抗饱和
    float integral_temp = pid->integral + error * pid->ki * 0.05f;
    if (integral_temp > pid->out_max) 
        integral_temp = pid->out_max;
    else if (integral_temp < pid->out_min) 
        integral_temp = pid->out_min;
    pid->integral = integral_temp;

    // 微分项（此处为0）
    float derivative = 0.0f;

    pid->out = pid->kp * error + pid->integral + pid->kd * derivative;

    // 输出限幅
    if (pid->out > pid->out_max) pid->out = pid->out_max;
    if (pid->out < pid->out_min) pid->out = pid->out_min;
}

4.4 行进-转向协同控制验证

协同效果通过三组实验验证：
1. 直线行进测试 ：$v_{ref}=1.5$rps, $\omega_{ref}=0$ → 小车以恒定速度沿直线前进，轨迹偏移量<5cm/10m
2. 原地转向测试 ：$v_{ref}=0$, $\omega_{ref}=2.0$rps → 小车绕中心点旋转，角速度波动<±0.1rps
3. 弧线行进测试 ：$v_{ref}=1.0$rps, $\omega_{ref}=1.0$rps → 小车沿半径约1.2m的圆弧运动，无侧滑

关键发现 ：当 $k_i>3$ 时，小车在低速转向（$\omega_{ref}<0.5$rps）下出现“爬行”现象——电机输出PWM在阈值附近反复启停。解决方案是添加死区补偿：

if (fabsf(turn_pid.target) < 0.1f) {
    turn_pid.out = 0.0f; // 目标值过小时强制输出零
}

5. 远程调参系统的集成与实战调试

5.1 蓝牙通信协议栈适配

本系统采用HC-05蓝牙模块，通过USART2与STM32通信。为支持实时调参，需扩展原有通信协议：
- 数据帧格式 ： 0xFF 0xFE [CMD] [LEN] [DATA...] [CHK]
- 命令定义 ：
- CMD=0x01 ：设置PID参数（$k_p,k_i,k_d$ 各占4字节float）
- CMD=0x02 ：获取实时变量（发送目标值、实际值、输出值）
- CMD=0x03 ：启动/停止波形上传

在USART2中断中解析命令：

#define CMD_SET_PID 0x01
#define CMD_GET_VAR 0x02

uint8_t rx_buffer[16];
uint8_t rx_index = 0;

void USART2_IRQHandler(void) {
    if (USART_GetITStatus(USART2, USART_IT_RXNE) != RESET) {
        uint8_t data = USART_ReceiveData(USART2);
        if (rx_index < sizeof(rx_buffer)) {
            rx_buffer[rx_index++] = data;
            if (rx_index == 16) process_command();
        }
    }
}

5.2 滑杆参数映射的线性化处理

手机APP端滑杆值（0-100）需映射为PID系数，但直接线性映射会导致低值区调节过于敏感。采用分段映射：

// 滑杆值→k_p映射（0-100 → 0-10）
float map_kp(uint8_t slider_val) {
    if (slider_val <= 20) return (float)slider_val * 0.1f;      // 0-2
    else if (slider_val <= 60) return 2.0f + (slider_val-20)*0.1f; // 2-6
    else return 6.0f + (slider_val-60)*0.05f;                    // 6-10
}

此映射使工程师在精细调节（如$k_p=2.1$）时操作更精准。

5.3 实战调试中的典型问题与对策

问题1：小车行进时左右轮速不一致
- 现象 ：$v_{left}=1.2$rps, $v_{right}=0.9$rps，差分速度持续为0.3rps
- 根因 ：两电机机械特性差异（摩擦力、磁隙不均）导致相同PWM下转速不同
- 对策 ：在速度解算前加入硬件补偿：

left_speed  = left_speed  * 1.02f; // 左轮补偿系数
right_speed = right_speed * 0.98f; // 右轮补偿系数

问题2：转向响应存在延迟
- 现象 ：摇杆移动后，小车转向滞后约300ms
- 根因 ：50ms采样周期导致的固有延迟，叠加PID积分项的累积效应
- 对策 ：在转向环中引入一阶惯性环节：

static float turn_output_filtered = 0.0f;
turn_output_filtered = 0.7f * turn_output_filtered + 0.3f * turn_pid.out;
delta_pwm = turn_output_filtered;

问题3：高速行进时出现高频抖动
- 现象 ：$v_{ref}>2.5$rps时，小车车身高频振动（≈15Hz）
- 根因 ：编码器信号在高速下受电磁干扰，脉冲丢失导致速度计算跳变
- 对策 ：在速度解算中加入中值滤波：

#define SPEED_FILTER_DEPTH 3
float speed_filter_buffer[SPEED_FILTER_DEPTH];
int filter_index = 0;

void update_speed_filter(float new_speed) {
    speed_filter_buffer[filter_index] = new_speed;
    filter_index = (filter_index + 1) % SPEED_FILTER_DEPTH;
}

float get_filtered_speed(void) {
    float buf[SPEED_FILTER_DEPTH];
    for (int i = 0; i < SPEED_FILTER_DEPTH; i++) {
        buf[i] = speed_filter_buffer[(filter_index + i) % SPEED_FILTER_DEPTH];
    }
    // 简单排序取中值（实际项目用快速选择算法）
    qsort(buf, SPEED_FILTER_DEPTH, sizeof(float), cmp_float);
    return buf[1];
}

6. 三环控制系统的性能边界与优化方向

6.1 当前系统的性能实测数据

在标准实验室环境下（水泥地面，温度25℃），三环系统达到以下性能指标：

指标	数值	测试条件
零速保持精度	±0.02rps	静止5分钟内速度漂移
阶跃响应时间	1.2s	$v_{ref}$ 从0→1.5rps，超调<5%
转向角速度跟踪误差	±0.08rps	$\omega_{ref}=1.0$rps正弦输入
最大稳定行进速度	2.8rps	对应线速度0.53m/s
抗扰动能力	3.5N·s	施加瞬时侧向冲击后恢复直立

瓶颈分析 ：最大行进速度受限于角度环带宽。当速度提升时，为维持倾角稳定需更大的$PWM_{avg}$，但电机驱动能力已达极限（TB6612FNG芯片最大持续电流2A），进一步提速将导致角度环饱和失稳。

6.2 下一代优化的技术路径

基于当前架构的局限性，提出三个可落地的优化方向：

方向1：PID算法增强
- 积分分离 ：当误差 $|e|>0.3$rps 时禁用积分，避免大偏差下的积分饱和
- 微分先行 ：对设定值而非误差进行微分，消除突加给定引起的冲击
- 变速积分 ：积分增益随误差绝对值增大而减小，提升大偏差响应速度

方向2：传感器融合升级
- 当前仅用MPU6050的陀螺仪积分计算倾角，存在漂移。可引入：
- 卡尔曼滤波 ：融合加速度计低频信息与陀螺仪高频信息
- 轮式里程计辅助 ：利用编码器累计距离修正倾角估计
- 实测表明，融合后倾角估计误差从±0.5°降至±0.15°

方向3：执行机构动态补偿
- 电机存在电枢电感导致的电流响应延迟，可在PID输出后添加预补偿：

// 基于电机电气时间常数τ=0.02s的预估补偿
float tau = 0.02f;
float pwm_compensated = pwm_out + tau * (pwm_out - pwm_prev) / 0.05f;
pwm_prev = pwm_out;

此补偿使PWM响应带宽提升约40%，显著改善高速动态性能。

在实际项目中，我曾遇到一个典型案例：某客户要求小车在斜坡（5°）上保持静止。原三环系统在坡道上产生持续的-0.15rps速度漂移。通过在速度环中增加 坡度前馈补偿 解决：利用MPU6050的俯仰角$\theta_{pitch}$，计算重力分量 $v_{feedforward} = k_f \cdot \sin(\theta_{pitch})$，将其叠加至速度环目标值。$k_f=0.8$ 时，坡道静止精度提升至±0.01rps。这印证了一个原则：再完美的反馈控制，也需结合系统已知的动力学模型进行前馈补偿。