一、PID 控制的核心问题为什么轮式平衡机器人需要多个 PID普通四轮小车只需要一个速度 PID——设定目标速度测量轮速输出 PWM。但本机器人是轮足混合式结构直立行走依赖 IMU 反馈的动态平衡。这就引入了两个额外的问题自平衡机器人本质上是一个倒立摆——如果不加控制它会向前或向后倾倒。需要一个直立环把机身稳定在俯仰角 ≈ 0° 的位置。速度控制用户希望机器人以 0.5m/s 前进但轮速和机身俯仰角之间存在耦合——加速时机身会后仰减速时机身会前倾。单独的一个 PID 无法同时解决这两个问题。因此本系统采用串级 PID架构——内环负责动态平衡响应快外环负责速度跟踪响应慢内外环协同工作。二、PID 控制器的两种形态本系统中存在两种 PID 实现分别用于不同的控制场景。2.1 PIDIncrement简易 P/PI 控制器structPIDIncrement{floatKp;floatKi;floatKd;};这是一个纯数据结构没有计算逻辑。使用它的地方由调用者手工完成计算controlTarget.forwardPID_Forward.Kp*(robotMotion.forward-0);coordTarget.xcoordTarget.xPID_XCoord.Kp*(controlTarget.forward-coordTarget.x);这种形态适用于P-only 渐进逼近场景——每次只向目标靠近一个比例步长等效于一阶低通滤波。好处是代码直观、无积分饱和风险、无需处理时间戳。2.2 PIDController完整离散 PIDPIDController PID_VEL{0,0,0,1000,50};这是一个完整的增量式 PID 实现适用于需要精确速度跟踪的场景floatPIDController::operator()(floaterror){// 1. 计算实际时间步长 (自适应于控制频率波动)floatTs(timestamp_now-timestamp_prev)*1e-6f;// 2. 比例项floatproportionalP*error;// 3. 积分项 (Tustin 变换, 梯形积分)floatintegralintegral_prevI*Ts*0.5f*(errorerror_prev);integralconstrain(integral,-limit/3,limit/3);// 抗积分饱和// 4. 微分项 (后向差分)floatderivativeD*(error-error_prev)/Ts;// 5. 合成floatoutputproportionalintegralderivative;outputconstrain(output,-limit,limit);// 6. 输出斜率限制 (防止冲击)if(output_ramp0){floatoutput_rate(output-output_prev)/Ts;if(output_rateoutput_ramp)outputoutput_prevoutput_ramp*Ts;elseif(output_rate-output_ramp)outputoutput_prev-output_ramp*Ts;}returnoutput;}两个值得注意的工程细节Tustin 变换的积分项integralintegral_prevI*Ts*0.5f*(errorerror_prev);普通的位置式 PID 使用I * Ts * error(k)这等价于矩形积分左矩形法。这里用的是梯形积分Ts/2 * (e(k) e(k-1))——即 Tustin 变换。梯形积分在高频采样下比矩形积分更准确且对积分器的离散化误差更小。输出斜率限制PID 的输出送入的是 BLDC 电机控制器。如果 PID 输出从 0 瞬间跳到 5V电机会产生冲击电流可能触发 BLDC 驱动器的过流保护。output_ramp参数将输出变化率限制在 1000V/s 以内——在 5ms 的控制周期内每步最大变化1000 × 0.005 5V对应满量程 5.7V恰好约束在电机驱动器的承受范围内。三、串级 PID 架构详解3.1 整体结构目标速度 目标转向 障碍物因子 │ │ │ ▼ ▼ ▼ ┌──────────────┐ ┌────────────┐ ┌──────────────┐ │ 速度环 PID │ │ 转向环 P │ │ 避障系数 │ │ PID_VEL │ │ P_Steer │ │ avoidL/R │ │ (外环,慢) │ │ (独立) │ │ (乘法修正) │ └──────┬───────┘ └─────┬──────┘ └──────┬───────┘ │ │ │ ▼ ▼ ▼ ┌──────────────────────────────────────────────┐ │ 直立环 PD │ │ PID_Stb.Kp × (目标俯仰角 - 实测俯仰角) │ │ - PID_Stb.Kd × 俯仰角速度(GyroY) │ │ 输出: BLDC 目标电压 │ │ (内环, 快) │ └────────────────────┬─────────────────────────┘ │ ▼ ┌──────────────────────────────────────────────┐ │ 轮速分配 │ │ motorLeft dir0 × (targetV differVel) │ │ motorRight dir1 × (targetV - differVel) │ │ motorLeft * avoidL │ │ motorRight * avoidR │ │ clamp(-5.7V, 5.7V) │ └──────────────────────────────────────────────┘为什么速度环是外环、直立环是内环直立环的响应速度必须快于速度环。当机器人受到外力扰动如撞到小石子时内环直立环必须在几十毫秒内恢复平衡而外环速度环只需要在几百毫秒内跟上目标速度。这种时间尺度的分离——内环快、外环慢——正是串级 PID 的核心设计原则。3.2 各环路详细分析速度环 (PID_VEL)PID_VEL.P f(height) // 随腿高自适应 PID_VEL.I 0.000918 // 固定值 (0.00153 * 0.6) PID_VEL.D f(height) // 随腿高自适应 error robotMotion.forward - robotPose.speedAvg output PID_VEL(error)输入目标前进速度robotMotion.forward通常设为 5.0代码中的默认值反馈robotPose.speedAvg (M0Speed M1Speed) / 2两个 BLDC 轮毂电机的平均转速输出controlTarget.velocity作为直立环的输入之一当前调试中将目标速度减去0而非反馈速度controlTarget.forward PID_Forward.Kp * (robotMotion.forward - 0)这意味着速度环实际上是开环的。这是故意的——在腿高较低的蹲姿状态下70-90mm轮速传感器的分辨率不足以提供稳定反馈开环比带噪声反馈更平稳。在正常站立高度下的闭环控制交由 PID_VEL 完成。直立环 (PID_Stb)直立环是一个PD 控制器无积分项控制机器人的前后平衡targetVoltagePID_Stb.Kp*(controlTarget.velocitycontrolTarget.centerAngleOffset-robotPose.pitch)-PID_Stb.Kd*robotPose.GyroY;这个公式本质上就是倒立摆控制器的标准形式u Kp × (θ_desired - θ) - Kd × θ̇其中controlTarget.velocity来自速度环的输出——速度环告诉直立环要加速直立环就主动前倾一个角度来产生加速。这是两轮平衡车的经典控制策略加速靠倾角减速也靠倾角。robotPose.pitch是 MPU6050 实测的俯仰角由互补滤波从加速度计和陀螺仪融合得到robotPose.GyroY是俯仰角速度用于阻尼项controlTarget.centerAngleOffset来自质心自校准下文详述为什么用 PD 不用 PID直立控制中的积分项会引入相位滞后反而降低系统的稳定性。PD 控制对于倒立摆而言已经足够——比例项提供恢复力微分项提供阻尼。转向环 (PID_Steering)转向是一个简单的 P 控制器controlTarget.differVelPID_Steering.Kp*(robotMotion.turn-robotPose.GyroZ);输入目标转向角速率robotMotion.turn正常为 0紧急避障时为 ±8.0反馈robotPose.GyroZ偏航角速度来自 IMU输出左右轮的差速量双轮足结构的零半径转向能力使转向环只需控制角速度而非转向半径降低了控制复杂度。高度环 (PID_Height)controlTarget.legLeftPID_Height.Kp*(robotMotion.updown-controlTarget.legLeft);controlTarget.legRightPID_Height.Kp*(robotMotion.updown-controlTarget.legRight);这是一个 P-only 渐进逼近等效于y(k) y(k-1) Kp × (target - y(k-1)) (1 - Kp) × y(k-1) Kp × target即一阶低通滤波时间常数τ Ts / Kp。在 200Hz 控制频率下Kp0.15对应的滤波时间常数约为0.005/0.15 ≈ 33ms——腿高可以快速变化但不会产生跳变。四、腿高自适应 PID 参数调度轮足机器人在不同腿高下其动力学特性差异显著腿高质心高度等效摆长惯量矩平衡难度70mm (蹲)低短小容易100mm (中)中中中适中130mm (站)高长大最难一套 PID 参数无法覆盖整个腿高范围。因此代码中实现了参数的自适应调度——每次腿高变化时根据当前高度重新计算 PID 增益if(robotPose.height!robotLastHeight){if(height70height110)PID_VEL.P-0.0067*height1.12;elseif(height110height130)PID_VEL.P0.4;PID_Stb.Kp(0.0003*h²-0.0488*h3.5798)*0.8;PID_Stb.Kd(-0.000002*h²0.0005*h-0.0043)*1.6;robotLastHeightrobotPose.height;}参数曲线的来源这些二次函数的系数不是凭空推导的而是通过手动调参 线性回归拟合得到的在 3-5 个离散高度点如 h70, 90, 110, 130分别手工调优 PID_Stb.Kp记录每个高度下的最优 Kp 值以 h 为自变量、Kp 为因变量做二次多项式回归结果在控制效果和代码简洁性之间取得了平衡——相比分段常数调度在每个区间内固定值二次函数提供了连续平滑的增益变化使机器人能在任意腿高下保持一致的稳定性。相比在线自适应如增益调度 系统辨识二次函数调度的代码量仅 5 行且不需要额外的计算开销。五、质心自校准5.1 为什么需要自校准理想情况下当pitch 0时机器人应该站立不动。但实际上由于机械安装误差、重心偏移、地面倾角等因素pitch 0时机器人可能仍会缓慢前滑或后滑。为此引入centerAngleOffset——一个动态调节的俯仰角偏置。当机器人前滑时通过调整偏置使机身略微后仰来产生减速抵消滑行。5.2 实现floatselfCaliCentroid(floatcentral){staticinti0;if(i40){// 每 40 个控制周期执行一次 (~200ms 200Hz)if(fabs(robotPose.speedAvg)1){selfcaliOffset0.8*(-1)*robotPose.speedAvg;selfcaliOffsetconstrain(selfcaliOffset,-0.5,0.5);centralselfcaliOffset;}i0;}else{i;}centralconstrain(central,-4,10);// 限幅 [-4°, 10°]returncentral;}算法逻辑每 200ms 检查一次如果轮子仍有速度speedAvg 1说明当前偏置不合适按速度方向的反向修正单次最大修正量 ±0.5°避免一次性大幅调整导致平衡失稳总偏置限制在 [-4, 10] 度范围这个范围覆盖了各种地面倾角从下坡到上坡和机械安装误差的极端情况实际效果在平坦地面上机器人在启动后约 1-2 秒内完成自校准此后保持稳态零速漂移。六、低通滤波器6.1 作用MPU6050 的陀螺仪输出含有高频噪声直接用GyroY做 PD 控制器的微分项会导致输出抖动。因此在微分项前加入一阶低通滤波LowPassFilter LPFPitch{0.03};// 时间常数 0.03sLowPassFilter LPFRoll{0.05};// 时间常数 0.05s6.2 实现floatLowPassFilter::operator()(floatx){floatdt(timestamp-timestamp_prev)*1e-6f;// 时间间隔自适应floatalphaTf/(Tfdt);floatyalpha*y_prev(1.0f-alpha)*x;y_prevy;timestamp_prevtimestamp;returny;}这里Tf 0.03对应的截止频率为fc 1/(2πTf) ≈ 5.3Hz——即俯仰角速度中 5.3Hz 以上的分量被衰减。这个截止频率低于控制频率200Hz约 40 倍足以有效过滤振动噪声同时又不会引入不可接受的相位滞后。七、完整的控制周期以上所有模块在每个robotRun()中被依次调用组成完整的控制周期voidrobotRun(){// 1. 更新 PID 参数 (腿高变化时)if(height!lastHeight)schedulePIDParams(height);// 2. 速度环 (外环)controlTarget.velocityPID_VEL(defaultSpeed-speedAvg);// 3. 转向环controlTarget.differVelPID_Steering.Kp*(targetTurn-GyroZ);// 4. 直立环 (内环) 质心校准floatangleRefcontrolTarget.velocitycenterAngleOffset;targetVoltagePID_Stb.Kp*(angleRef-pitch)-PID_Stb.Kd*GyroY;// 5. 轮速分配 避障因子motorLeftdir0*(targetVoltagedifferVel)*avoidL;motorRightdir1*(targetVoltage-differVel)*avoidR;// 6. 输出限幅clamp(motorLeft,-5.7,5.7);clamp(motorRight,-5.7,5.7);motors.setTargets(motorLeft,motorRight);}八、调试方法论在调参过程中遵循了一条逐步解锁的步骤第一步只调直立环PD_Stb只让机器人站立, 目标速度 0 依次增大 Kp 直到出现高频抖动, 记录临界值 取 Kp 临界值的 60% 再增大 Kd 提供阻尼, 直到推一下能快速恢复且不超调第二步加入速度环PID_VEL设定目标速度 5.0, Kp 从 0 逐渐增大 观察速度阶跃响应: 上升时间 500ms, 超调 20% 若超调大 → 增大 Kd 或减小 Ki 若稳态误差大 → 加入 Ki (但本系统最终用了 Ki≈0)第三步腿高自适应固定腿高 70mm → 调出最优 Kp_70, Kd_70 固定腿高 100mm → 调出最优 Kp_100, Kd_100 固定腿高 130mm → 调出最优 Kp_130, Kd_130 三个点拟合二次曲线 → 代码中写入系数第四步转向环目标转向速率 5.0, Kp 从 0 起始 观察偏航角速度阶跃响应, 直到机器人能按预期速率旋转