智能车角度闭环控制：从PID设计到卡尔曼滤波实现

1. 角度闭环控制的工程本质与系统定位

在智能车控制系统中，角度闭环控制并非独立存在的功能模块，而是电机速度闭环之上的 外环控制器 ，其核心任务是将“期望姿态”转化为“所需运动学响应”。当小车需要执行原地转向、路径校正或定点旋转等动作时，仅靠速度环无法感知车身朝向偏差——它只关心轮子转得多快，不关心车头指向何方。角度环的引入，正是为了解决这一根本性缺失：它以IMU（惯性测量单元）输出的航向角为反馈量，将目标角度与当前姿态的差值作为控制依据，动态生成左右轮所需的速度指令，从而驱动整车向指定方向旋转。

这种双环嵌套结构在控制理论中称为 串级PID控制 （Cascade PID Control）。外环（角度环）负责宏观姿态调节，内环（速度环）负责微观动力执行。二者职责分明：角度环输出的是“应达到的线速度/角速度”，速度环则确保电机实际输出该速度。这种分层设计带来三大工程优势：
- 抗干扰能力增强 ：轮子打滑、地面摩擦变化等扰动主要影响速度环，角度环通过上层反馈可自动修正；
- 响应特性解耦 ：角度环的P/D参数可专注调整转向灵敏度与稳定性，无需迁就电机电气特性；
- 调试边界清晰 ：可先单独验证速度环的阶跃响应，再叠加角度环，避免参数耦合导致的调试混乱。

必须明确的是，角度环的输出 不是PWM占空比 ，而是速度环的设定值（Setpoint）。若直接将角度环输出映射为占空比，会导致系统在低速区响应迟钝、高速区超调剧烈——因为电机转速与占空比并非严格的线性关系，且存在死区、饱和等非线性因素。正确的信号流应为：
目标角度 → 角度误差 → 角度PID计算 → 左右轮速度设定值 → 速度PID计算 → PWM占空比 → 电机实际转速

这一信号链的每一环节都需严格匹配物理约束。例如，角度环输出值必须限定在电机机械极限对应的线速度范围内（如±0.8 m/s），否则速度环将长期处于饱和状态，丧失调节能力。这正是后续幅值限制环节存在的根本原因。

2. 硬件基础：MPU6050姿态解算与卡尔曼滤波实现

角度闭环的可靠性完全依赖于反馈信号的精度与实时性。本系统采用MPU6050作为姿态传感器，其内部集成三轴陀螺仪与加速度计，但原始数据存在显著缺陷：
- 陀螺仪漂移 ：零偏随温度、时间缓慢变化，积分后角度误差呈二次增长；
- 加速度计噪声 ：高频振动导致瞬时倾角测量失真；
- 轴间耦合 ：车体俯仰/横滚运动会干扰偏航角计算。

因此，直接使用单一传感器数据构建闭环必然失败。工程实践中必须采用 传感器融合算法 ，而卡尔曼滤波（Kalman Filter）因其最优估计特性成为首选。其核心思想是将陀螺仪的高带宽角速度积分值作为预测值，以加速度计解算的静态倾角为观测值，通过协方差矩阵动态分配两者的可信权重。

2.1 卡尔曼滤波状态空间建模

本系统采用一维卡尔曼滤波处理偏航角（Yaw），状态向量定义为：
$$\mathbf{x} k = \begin{bmatrix} \theta_k \ \dot{\theta}_k \end{bmatrix}$$
其中 $\theta_k$ 为$k$时刻偏航角，$\dot{\theta}_k$ 为角速度。状态转移方程基于陀螺仪读数 $g_k$：
$$\mathbf{x}_k = \mathbf{F}_k \mathbf{x} {k-1} + \mathbf{B}_k g_k + \mathbf{w}_k$$
$$\mathbf{F}_k = \begin{bmatrix} 1 & \Delta t \ 0 & 1 \end{bmatrix},\quad \mathbf{B}_k = \begin{bmatrix} \Delta t \ 0 \end{bmatrix}$$

观测方程利用加速度计数据 $a_x, a_y$ 计算倾角：
$$z_k = \arctan2(-a_y, a_x)$$
观测矩阵 $\mathbf{H}_k = \begin{bmatrix} 1 & 0 \end{bmatrix}$，仅观测角度状态。

2.2 实际部署关键参数配置

在STM32 HAL库环境下，卡尔曼滤波需在定时器中断中周期执行（推荐100Hz）。关键参数设置经验如下：
- 过程噪声协方差 Q ：反映陀螺仪积分不确定性，典型值 $Q = \text{diag}([0.001, 0.01])$；
- 观测噪声协方差 R ：反映加速度计静态精度，典型值 $R = 0.1$；
- 初始协方差 P ：设为较大值（如 $P = \text{diag}([1, 1])$），使滤波器初期快速收敛；
- 陀螺仪零偏补偿 ：上电静止时采集1000次样本求均值，从原始读数中减去该偏置。

实践提示 ：MPU6050的DMP（数字运动处理器）虽可硬件解算姿态，但其输出延迟达10ms以上，且无法获取中间状态变量。在实时性要求严苛的闭环控制中，软件卡尔曼滤波+原始传感器数据的方案更具可控性与可调试性。

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

角度环采用 位置式PID 而非增量式，根本原因在于其控制目标是绝对角度定位。增量式算法输出的是控制量的增量 $\Delta u(k)$，适用于连续调节场景（如温度控制），而角度环需在每次采样周期内计算出完整的速度设定值，位置式公式可直接给出：
$$u(k) = K_p e(k) + K_i \sum_{i=0}^{k} e(i) \Delta t + K_d \frac{e(k)-e(k-1)}{\Delta t}$$

其中 $e(k) = \theta_{ref}(k) - \theta_{fb}(k)$ 为角度误差。本系统舍弃积分项（$K_i = 0$），原因在于：
- 小车原地旋转时无外部持续扰动，积分饱和风险低；
- 偏航角存在周期性（±180°），积分累加易引发相位跳变；
- P+D组合已能提供足够动态性能，简化调试流程。

3.1 参数物理意义与工程取值

关键洞察 ：$K_d$ 的作用本质是引入阻尼。当小车接近目标角度时，角度误差趋近于零，但角速度仍较大，此时微分项产生负向修正力，迫使减速。若 $K_d$ 过小，小车会因惯性冲过目标点；若过大，则对MPU6050的陀螺仪噪声过度敏感，导致电机频繁启停。

3.2 代码实现要点解析

// 角度环PID结构体定义（HAL库环境）
typedef struct {
    float Kp;           // 比例系数
    float Ki;           // 积分系数（本系统置0）
    float Kd;           // 微分系数
    float setpoint;     // 目标角度（度）
    float input;        // 当前角度反馈（度）
    float output;       // PID输出（速度设定值，m/s）
    float error;        // 当前误差
    float last_error;   // 上次误差
    float d_error;      // 误差微分
    float integrator;   // 积分项（本系统未使用）
} PID_StructTypeDef;

PID_StructTypeDef angle_pid = {0};

// 角度环初始化
void Angle_PID_Init(void) {
    angle_pid.Kp = 1.8f;   // 快速性主导
    angle_pid.Ki = 0.0f;   // 禁用积分
    angle_pid.Kd = 0.15f;  // 平衡稳定性
    angle_pid.setpoint = 0.0f;
    angle_pid.last_error = 0.0f;
}

// 角度环计算函数（在主控循环中调用）
void Angle_PID_Calculate(void) {
    // 1. 获取当前角度反馈（来自卡尔曼滤波器输出）
    angle_pid.input = kalman_yaw_angle; // 单位：度

    // 2. 计算误差（注意：角度需归一化到[-180,180]）
    float error_unwrapped = angle_pid.setpoint - angle_pid.input;
    angle_pid.error = NormalizeAngle(error_unwrapped); // 归一化函数见附录

    // 3. 计算微分项（防微分冲击：仅对反馈微分）
    angle_pid.d_error = (angle_pid.input - angle_pid.last_error) / 0.01f;
    angle_pid.last_error = angle_pid.input;

    // 4. 位置式PID计算（Ki=0，故无积分项）
    angle_pid.output = angle_pid.Kp * angle_pid.error 
                     + angle_pid.Kd * angle_pid.d_error;
}

归一化函数实现 （避免跨±180°时误差突变）：

float NormalizeAngle(float angle) {
    while (angle > 180.0f) angle -= 360.0f;
    while (angle <= -180.0f) angle += 360.0f;
    return angle;
}

4. 双轮差速转向的运动学映射与输出约束

角度环输出值需转换为左右轮速度指令，这涉及差速转向的运动学模型。设小车轴距为 $L$（单位：m），目标角速度为 $\omega$（单位：rad/s），则左右轮线速度为：
$$v_{left} = \omega \cdot \frac{L}{2},\quad v_{right} = -\omega \cdot \frac{L}{2}$$

但在实际工程中，需进行三次关键映射：
1. 角度环输出 → 角速度 ：角度PID输出 $u$ 直接作为 $\omega$，单位为 rad/s；
2. 角速度 → 线速度 ：乘以 $L/2$ 得到左右轮速度设定值（m/s）；
3. 线速度 → PWM占空比 ：由速度环完成，此处仅传递设定值。

4.1 输出幅值限制的必要性

角度环输出无天然物理上限，但电机存在硬性约束：
- 机械极限 ：轮子最大线速度（如0.8 m/s）；
- 电气安全 ：过高的PWM占空比导致电流超限；
- 控制有效性 ：当输出远超电机能力时，速度环将持续饱和，角度环失去调节意义。

因此，必须对角度环输出施加 软限幅 （Soft Saturation）：

// 幅值限制函数（放置于Angle_PID_Calculate之后）
#define MAX_ANGULAR_SPEED 2.5f  // rad/s，对应约143°/s
#define MIN_ANGULAR_SPEED -2.5f

void Angle_Output_Limit(void) {
    if (angle_pid.output > MAX_ANGULAR_SPEED) {
        angle_pid.output = MAX_ANGULAR_SPEED;
    } else if (angle_pid.output < MIN_ANGULAR_SPEED) {
        angle_pid.output = MIN_ANGULAR_SPEED;
    }
}

为什么不能复用速度环的限幅？
速度环限幅针对的是PWM占空比（0~100%），而角度环输出是角速度（rad/s）。二者量纲不同、数值范围不同、饱和机理不同。若共用同一限幅逻辑，会导致：当角度环输出为2.0 rad/s时，经运动学换算后左右轮速度可能已达0.4 m/s，但速度环限幅却按100%占空比生效，造成控制指令被错误截断。

4.2 左右轮速度设定值生成

// 在主控循环中调用
void Generate_Wheel_Setpoints(void) {
    // 1. 对角度环输出进行限幅
    Angle_Output_Limit();

    // 2. 根据轴距L计算左右轮线速度设定值（单位：m/s）
    // 假设轴距L = 0.16m（16cm），则 L/2 = 0.08m
    const float HALF_TRACK = 0.08f;
    float left_speed_setpoint  =  angle_pid.output * HALF_TRACK;
    float right_speed_setpoint = -angle_pid.output * HALF_TRACK;

    // 3. 将速度设定值传递给速度环PID控制器
    speed_pid_left.setpoint  = left_speed_setpoint;
    speed_pid_right.setpoint = right_speed_setpoint;
}

转向方向校正技巧 ：若实测小车转向与预期相反，切勿修改PID参数符号，而应调整运动学映射中的正负号：
- 当前： left = +ω·L/2 , right = -ω·L/2 → 逆时针旋转
- 反向： left = -ω·L/2 , right = +ω·L/2 → 顺时针旋转
此方法保持控制逻辑一致性，避免因符号混乱引入难以排查的bug。

5. 速度环与角度环的协同调度与时序优化

双环系统的稳定性高度依赖于 采样周期匹配 与 计算负载均衡 。若角度环频率过高（如500Hz），而速度环仅100Hz，则角度环输出在两次速度环计算间被多次更新，导致指令抖动；反之，若角度环过慢（如10Hz），则无法及时响应姿态变化。

5.1 推荐时序架构

为什么所有环路统一100Hz？
控制理论表明，采样频率需至少为系统带宽的10倍。小车机械系统带宽约5~10Hz，100Hz采样完全满足香农定理。统一频率可避免多定时器中断嵌套导致的时序紊乱，且便于在单个中断服务程序中完成全部控制计算，减少上下文切换开销。

5.2 中断服务程序（ISR）精简设计

// TIM3中断服务程序（100Hz）
void TIM3_IRQHandler(void) {
    if (__HAL_TIM_GET_FLAG(&htim3, TIM_FLAG_UPDATE) != RESET) {
        if (__HAL_TIM_GET_IT_SOURCE(&htim3, TIM_IT_UPDATE) != RESET) {
            __HAL_TIM_CLEAR_IT(&htim3, TIM_IT_UPDATE);

            // 1. 读取MPU6050原始数据（I2C非阻塞模式）
            MPU6050_Read_Raw_Data();

            // 2. 执行卡尔曼滤波（耗时<50μs）
            Kalman_Filter_Update();

            // 3. 角度环计算（耗时<10μs）
            Angle_PID_Calculate();
            Angle_Output_Limit();

            // 4. 生成轮速设定值（耗时<5μs）
            Generate_Wheel_Setpoints();

            // 5. 速度环计算（耗时<20μs）
            Speed_PID_Calculate_Left();
            Speed_PID_Calculate_Right();
        }
    }
}

关键优化点 ：
- I2C读取采用DMA+中断模式，避免总线等待；
- 卡尔曼滤波使用浮点运算，但STM32F4/F7系列具备硬件FPU，单次计算仅需数十周期；
- 所有PID计算均在中断中完成，确保控制指令的确定性延迟（恒为10ms）；
- 主循环（while(1)）仅负责LED状态指示、串口调试输出等非实时任务。

6. 实验验证与典型问题排查

角度闭环的调试必须遵循 分层验证 原则：先确认底层传感器数据可信，再验证单环性能，最后整合双环。以下是经过实战检验的验证流程：

6.1 分步验证清单

6.2 典型故障与根因分析

现象：小车旋转时持续超调，反复穿越目标角度
- 根因 ：$K_d$ 过小，无法提供足够阻尼；或速度环响应过慢，导致角度环指令无法及时执行。
- 解决 ：先增大 $K_d$ 至0.25，若仍超调则检查速度环采样周期是否与角度环同步；用示波器抓取PWM波形，确认占空比能否快速响应设定值变化。

现象：小车静止时角度环输出缓慢漂移
- 根因 ：卡尔曼滤波中陀螺仪零偏未准确补偿，导致积分漂移；或加速度计静态校准不充分。
- 解决 ：上电后静置30秒，采集陀螺仪1000次读数取均值作为新零偏；重新执行加速度计六面校准。

现象：小车转向方向与代码逻辑相反
- 根因 ：电机接线相序错误，或MPU6050坐标系定义与小车物理坐标系不一致。
- 解决 ：不修改代码，直接交换左右电机的A/B相接线；或在MPU6050初始化中调用 mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000) 后，检查 getRotation() 返回值的符号约定。

我的实战经验 ：在2023年电赛备赛中，团队曾因MPU6050的Y轴与小车前进方向不重合，导致角度环始终输出反向指令。最终通过在卡尔曼滤波输出后添加 final_yaw = -kalman_yaw_angle 一行代码解决——这比重新焊接传感器或修改机械结构高效得多。记住：嵌入式调试的本质是 快速定位矛盾点并施加最小干预 ，而非追求理论完美。