从 PID 基础到循迹机器人控制算法（以24年电赛H题为例）
文章目录
PID理论讲解
PID 控制器是一种基于反馈的控制算法，其名称来源于三个核心控制环节：比例（Proportional）、积分（Integral） 和微分（Derivative）。它通过计算 "目标值" 与 "实际值" 之间的偏差（Error），并根据偏差的变化规律输出控制量，使系统趋于稳定状态。
u(t)：控制器输出的控制量e(t)：偏差（目标值 - 实际值）Kp：比例系数Ki：积分系数Kd：微分系数∫e(τ)dτ：偏差的积分（累计偏差）de(t)/dt：偏差的微分（偏差变化率）
在单片机中，实际应用的PID离散化公式为
其中k表示当前时刻，k-1表示上一时刻，Σe(i)表示从初始时刻到当前时刻的偏差累积和。
- 比例环节（P）：对当前偏差进行放大或缩小比例系数
Kp越大，系统对偏差的响应越灵敏，但过大会导致系统震荡。例如：当循迹机器人偏离黑线时，比例环节会立即产生转向力，偏差越大，转向幅度越大。 - 积分环节（I）：消除静态误差当系统存在微小的固定偏差（如机械结构不对称导致的偏向），积分环节通过累积偏差来逐步修正，最终使偏差趋于零。
Ki越大，消除静态误差的速度越快，但过大会导致超调。 - 微分环节（D）：抑制系统震荡微分环节反映偏差的变化趋势，提前产生反向调节作用。例如：当机器人快速靠近目标位置时，微分环节会抑制其速度，防止超调，增强系统稳定性。
具体示例
以24电赛H题为例
场地面积不小于 220cm×120cm。图中两个对称半圆弧线的半径为 40cm，弧线为黑色，线宽 1.8cm 左右，弧线的四个顶点分别定义为 A、B、C 和 D 点。（1）将小车放在位置 A 点，小车能自动行驶到 B 点停车，停车时有声光提示。用时不大于 15 秒。（20 分）。（2）将小车放在位置 A 点，小车能自动行驶到 B 点后，沿半弧线行驶到 C点，再由 C 点自动行驶到 D 点，最后沿半弧线行驶到 A 点停车，每经过一个点，声光提示一次。完成一圈用时不大于 30 秒。（20 分）（3）将小车放在位置 A 点，小车能自动行驶到 C 点后，沿半弧线行驶到 B点，再由 B 点自动行驶到 D 点，最后沿半弧线行驶到 A 点停车。每经过一个点，声光提示一次。完成一圈用时不大于 40 秒。（30 分）（4）按要求 3 的路径自动行驶 4 圈停车，用时越少越好（30 分）
- 传感器模块：8 路灰度传感器（检测黑线位置）
- 姿态传感器：MPU6050/JY61P（提供 Yaw 角（偏航角）数据，用于丢线时的方向控制）
- 执行机构：TB6612 电机驱动模块 + 520直流减速电机
算法实现
在这个场地上小车一般有两种运动环境，有一种为正常寻线，第二种没有路线，就需要依靠姿态传感器来决定方向。
（1）在正常循线时，系统采用 PD 控制（因循线场景静态误差较小，可省略积分环节），核心逻辑在follow_line()函数中实现。
8 路灰度传感器的输出为 8 位二进制数据（sensor_value），每一位代表对应通道的检测结果（1 为黑，0 为白）。通过加权求和计算偏差
// 传感器权重分配（左4路正权重，右4路负权重） const int8_t sensor_weights[8] = {4, 3, 2, 1, -1, -2, -3, -4}; int16_t error = 0; for (int i = 0; i < 8; i++) { if (sensor_value & (1 << i)) { // 若第i路检测到黑色 error += sensor_weights[i]; // 累加偏差 } }越靠近外侧的传感器权重绝对值越大，使边缘偏差对控制的影响更显著，正值表示黑线偏左，负值表示黑线偏右，零表示居中
根据计算出的偏差，通过比例和微分环节计算电机速度差：
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; // 更新上一时刻偏差- 比例项（
Kp * error）：根据当前偏差直接调整，偏差越大，转向幅度越大 - 微分项（
Kd * (error - last_error)）：根据偏差变化率调整，抑制震荡（例如：当偏差快速增大时，提前加大反向修正力）
以基础速度（SPEED_MEDIUM = 500）为基准，根据 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 /JY61P的 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 角（0° 和 180°），使机器人以每次开始丢失路径时的方向行驶，提高找回线路的概率。
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为正，左轮加速、右轮减速，实现顺时针转向；反之则逆时针转向
调试方法
- 比例系数（Kp）：
- 先将 Ki 和 Kd 设为 0，逐步增大 Kp，直到系统出现轻微震荡
- 循线场景中，Kp 过大会导致机器人左右摇摆，过小则反应迟钝
- 微分系数（Kd）：
- 在 Kp 基础上，逐步增大 Kd 以抑制震荡，直到系统稳定
- 循线场景中，Kd 过大会导致转向滞后，过小则无法抑制摇摆
- 积分系数（Ki）：
- 最后加入 Ki，用于消除静态偏差（如机器人持续偏向一侧）
- 建议 Ki 值远小于 Kp，避免积分饱和导致系统不稳定
target_yaw为丢线时的目标角度，基于H题有两种角度，一种为跑完整的椭圆形 ，目标角度就在0和180之前切换，另一种为下图所示的"x"型路径
目标角度可以另外计算一下，算法不变。
PID 控制虽简单，但要真正掌握其精髓，需要在实践中不断调试与总结。希望本文能为初学者提供清晰的思路，助力更多控制算法的学习与应用。
