MPU6050的速度环与转向环pid调节车子直行（标准库）

Question

我现在要用pid的转向环和速度环来控制小车的直行与差速转弯（左右转向以及原地掉头）我用的是两个编码器电机加上一个万向轮，我现在已经用TIM1的通道1和4来输出pwm波，用TIM2和TIM4的引脚来作为输入补获引脚，我现在想要来写一个pid控制程序，那该怎么写（标准库）

Answer 1

下午好🌅🌅🌅
本答案参考通义千问

要使用 PID 控制 来实现小车的 直行、转向和差速转弯（左右转向、原地掉头），你需要设计两个 PID 控制环：速度环 和 转向环。以下是详细的实现步骤与代码结构建议（基于标准库）。

---

一、系统结构与功能需求

1. 系统组成

• MPU6050：用于获取加速度和角速度信息，作为转向环的反馈。
• 两个编码器电机：用于控制左右轮的速度，实现速度环。
• 万向轮：用于支撑小车并减少摩擦。
• TIM1：输出 PWM 控制电机转速（通道 1 和 4）。
• TIM2 和 TIM4：用于捕获编码器脉冲，计算速度。
• PID 控制器：分别用于速度环和转向环。

---

二、PID 控制原理概述

1. 速度环

• 目标：保持左右轮速度一致，使小车直线行驶。
• 输入：左右轮的实际速度（通过编码器测得）。
• 输出：PWM 占空比调整左右轮的转速。

2. 转向环

• 目标：控制小车的转向角度（如左转、右转、原地掉头）。
• 输入：MPU6050 的角速度（或角度）。
• 输出：调整左右轮速度差（差速控制）。

---

三、PID 控制程序结构

1. 定义全局变量（在 main.c 或 stm32f10x_it.c 中）

// 速度环参数
float Kp_speed = 0.5, Ki_speed = 0.1, Kd_speed = 0.01;
float speed_error_prev = 0, speed_error_sum = 0;

// 转向环参数
float Kp_turn = 1.0, Ki_turn = 0.05, Kd_turn = 0.01;
float turn_error_prev = 0, turn_error_sum = 0;

// 目标速度和目标角度
float target_speed = 0.0;      // 目标速度
float target_angle = 0.0;      // 目标角度

// 实际速度和角度
float actual_speed_left = 0.0, actual_speed_right = 0.0;
float actual_angle = 0.0;      // 从 MPU6050 获取

// 差速控制变量
float left_pwm = 0, right_pwm = 0;

---

2. 编码器速度计算函数（基于 TIM2 和 TIM4 捕获）

void ReadEncoderSpeed(void) {
    static uint16_t last_count_left = 0, last_count_right = 0;
    static uint32_t last_time = 0;
    
    uint32_t current_time = TIM_GetCounter(TIM2); // 假设 TIM2 是左轮编码器
    uint16_t count_left = TIM_GetCapture1(TIM2);
    uint16_t count_right = TIM_GetCapture1(TIM4);

    float time_diff = (current_time - last_time) / (SystemCoreClock / 1000.0); // ms

    actual_speed_left = (count_left - last_count_left) / time_diff;
    actual_speed_right = (count_right - last_count_right) / time_diff;

    last_count_left = count_left;
    last_count_right = count_right;
    last_time = current_time;
}

---

3. MPU6050 角度读取（假设已通过 I2C 获取）

void ReadMPU6050Angle(float *angle) {
    // 这里需要根据实际 MPU6050 数据处理，例如：
    // angle[0] = 角速度 x, y, z
    // angle[1] = 角度 x, y, z
    // 示例：直接读取角度值
    *angle = get_angle_from_mpu(); // 自定义函数
}

---

4. PID 控制函数（速度环）

float PID_Speed(float target, float actual) {
    float error = target - actual;
    float p = Kp_speed * error;
    float i = Ki_speed * (speed_error_sum += error);
    float d = Kd_speed * (error - speed_error_prev);
    speed_error_prev = error;

    return p + i + d;
}

---

5. PID 控制函数（转向环）

float PID_Turn(float target, float actual) {
    float error = target - actual;
    float p = Kp_turn * error;
    float i = Ki_turn * (turn_error_sum += error);
    float d = Kd_turn * (error - turn_error_prev);
    turn_error_prev = error;

    return p + i + d;
}

---

四、主循环逻辑（在 main() 中）

int main(void) {
    // 初始化系统、PWM、编码器、MPU6050 等

    while (1) {
        ReadEncoderSpeed();
        ReadMPU6050Angle(&actual_angle);

        // 速度环控制
        float speed_diff = PID_Speed(target_speed, (actual_speed_left + actual_speed_right) / 2);

        // 转向环控制
        float turn_diff = PID_Turn(target_angle, actual_angle);

        // 差速控制
        left_pwm = target_speed + turn_diff;
        right_pwm = target_speed - turn_diff;

        // 设置 PWM 输出（假设使用 TIM1）
        TIM_SetCompare1(TIM1, left_pwm);
        TIM_SetCompare4(TIM1, right_pwm);

        Delay_ms(10); // 延时
    }
}

---

五、关键点说明

1. PID 参数调优

• Kp：比例系数，影响响应速度。
• Ki：积分系数，消除稳态误差。
• Kd：微分系数，抑制震荡。

建议：先调优 速度环，再调优 转向环。

2. 差速控制

• 当 turn_diff > 0：表示向右转，左轮加速，右轮减速。
• 当 turn_diff < 0：表示向左转，右轮加速，左轮减速。
• target_angle = 0 表示直行；target_angle = 90 表示右转；target_angle = -90 表示左转。

3. 原地掉头

• 设置 target_angle = 180 或 -180，同时将 target_speed = 0，实现 原地旋转。

---

六、完整代码结构建议（基于标准库）

Project/
├── core/
│   └── stm32f10x_conf.h
│   └── stm32f10x_it.c
├── inc/
│   └── main.h
│   └── mpu6050.h
├── src/
│   └── main.c
│   └── mpu6050.c
│   └── encoder.c
│   └── pid.c
└── library/
    └── stm32f10x_stdlib.a

---

七、总结

1. 速度环：控制左右轮速度一致，保证直行。
2. 转向环：通过 MPU6050 获取角度，控制差速。
3. PID 调参：逐个优化，确保稳定性和响应性。
4. 差速控制：根据转向环输出调整左右轮速度。

最终目标：让小车能够自动保持直行、左右转向、原地掉头，具备基本的路径跟踪能力。

---

如有更多问题，比如如何初始化 MPU6050、如何处理编码器脉冲等，欢迎继续提问！