PID 循迹机器人控制算法实战：24 电赛 H 题解析

越靠近外侧的传感器权重绝对值越大，这样边缘偏差对控制的影响更显著。正值表示黑线偏左，负值表示黑线偏右，零表示居中。

计算出偏差后，利用比例和微分环节计算电机速度差：

const float Kp = 25.0; // 比例系数
const float Kd = 26.0; // 微分系数
static int16_t last_error = 0; // 保存上一时刻偏差

// 计算 PD 输出（速度补偿量）
int16_t speed_diff = Kp * error + Kd * (error - last_error);
last_error = error; // 更新上一时刻偏差

• 比例项：根据当前偏差直接调整，偏差越大，转向幅度越大。
• 微分项：根据偏差变化率调整，抑制震荡。例如当偏差快速增大时，提前加大反向修正力。

最后以基础速度为基准，根据 PD 输出的速度差分配左右轮速：

// 左轮速度 = 基础速度 - 速度差（偏差为正时，左轮减速，右轮加速，实现右转）
// 右轮速度 = 基础速度 + 速度差（偏差为负时，右轮减速，左轮加速，实现左转）
int16_t left_speed = SPEED_MEDIUM - speed_diff;
int16_t right_speed = SPEED_MEDIUM + speed_diff;

// 速度限幅（防止超出 PWM 范围 0-1000）
left_speed = constrain(left_speed, 0, 1000);
right_speed = constrain(right_speed, 0, 1000);

// 控制电机转动
TB6612_SetMotorSpeed(TB6612_FORWARD, right_speed, TB6612_FORWARD, left_speed);

注意速度限幅是为了避免电机驱动模块因输入过大而损坏，同时保证控制的稳定性。

2. 丢失路线模式

当传感器检测到全黑（sensor_value == 0xFF）时，系统进入丢线模式。此时依靠 MPU6050 的 Yaw 角进行 PID 控制，实现定向行驶。

状态切换逻辑

我们需要检测从非丢线到丢线的上升沿，以及从丢线到复现的下降沿：

uint8_t all_black = (sensor_value == 0xFF); // 全黑判断
static uint8_t was_all_black_before = 0;    // 上一状态记录

// 从非丢线→丢线的上升沿检测
if (!was_all_black_before && all_black) {
    lost_count++;           // 丢线计数递增
    is_lost = 1;            // 进入丢线模式
    
    // 交替设置目标偏航角（0°和 180°），实现左右转向寻线
    target_yaw = (lost_count % 2 == 1) ? 0.0f : 180.0f;
    
    // 重置 PID 积分项和历史误差，避免累积误差影响
    yaw_integral = 0.0f;
    yaw_last_error = 0.0f;
}
// 从丢线→复现的下降沿检测（退出丢线模式）
else if (was_all_black_before && !all_black) {
    is_lost = 0;
}
was_all_black_before = all_black; // 更新上一状态

通过交替设置目标 Yaw 角，使机器人以每次开始丢失路径时的方向行驶，提高找回线路的概率。

角度环 PID 计算

int16_t UpdateYawPID(float current_yaw, float target_yaw) {
    float error = target_yaw - current_yaw;
    
    // 角度环误差处理（确保误差在±180°内，避免绕远路）
    if (error > 180.0f) error -= 360.0f;
    else if (error < -180.0f) error += 360.0f;
    
    yaw_integral += error; // 积分项累积（需根据实际场景限幅）
    float derivative = error - yaw_last_error; // 微分项计算
    yaw_last_error = error; // 更新上一时刻误差
    
    // PID 输出：比例 + 积分 + 微分
    return (int16_t)(yaw_Kp * error + yaw_Ki * yaw_integral + yaw_Kd * derivative);
}

这里的积分项主要用于消除因电机转速差异、地面摩擦不均等导致的静态偏差。

获取补偿量后，分配电机速度（注意此处逻辑与正常模式相反，补偿量直接叠加到基础速度）：

int16_t yaw_correction = UpdateYawPID(IMUDMP.Yaw, target_yaw); // 获取 PID 补偿量

// 电机速度分配
int16_t left_speed = SPEED_MEDIUM + yaw_correction;
int16_t right_speed = SPEED_MEDIUM - yaw_correction;

// 速度限幅
left_speed = constrain(left_speed, 0, 1000);
right_speed = constrain(right_speed, 0, 1000);

TB6612_SetMotorSpeed(TB6612_FORWARD, right_speed, TB6612_FORWARD, left_speed);

该段代码用于丢线时的电机控制。当实际 Yaw 角小于目标角时，yaw_correction 为正，左轮加速、右轮减速，实现顺时针转向；反之则逆时针转向。

调试经验

PID 参数并非一成不变，需要根据实际机械结构和地面摩擦力进行调试。

1. 比例系数（Kp）：先将 Ki 和 Kd 设为 0，逐步增大 Kp，直到系统出现轻微震荡。循线场景中，Kp 过大会导致机器人左右摇摆，过小则反应迟钝。
2. 微分系数（Kd）：在 Kp 基础上，逐步增大 Kd 以抑制震荡，直到系统稳定。Kd 过大会导致转向滞后，过小则无法抑制摇摆。
3. 积分系数（Ki）：最后加入 Ki，用于消除静态偏差（如机器人持续偏向一侧）。建议 Ki 值远小于 Kp，避免积分饱和导致系统不稳定。

对于丢线时的目标角度 target_yaw，基于 H 题有两种常见路径：一种是跑完整的椭圆形，目标角度在 0 和 180 之间切换；另一种是"X"型路径。具体角度计算可根据路径规划另行设计，核心算法保持不变。

PID 控制虽简单，但要真正掌握其精髓，需要在实践中不断调试与总结。希望这些思路能为你的控制算法学习提供参考。