向原先的单环结构加入速度环，能够控制轮足机器人的移动速度。除此之外，我们希望速度环能够对地面的阻力有一个比较鲁棒的适应性。

我们将尝试两种常见的控制方法来设计速度环。

4.1 角度环PD+速度环PD

我们尝试将速度环设计为PD控制，因为我们在角度环上取得了显著的见效。

在代码中，你可以这样设计：

angleAdjust = -(Kp_speed * speedError + Kd_speed * speedDerivative)

可以看见我们的机器人能够指定速度并且保持一定的平衡前进。但长时间运行，我们可能看到速度的稳定性愈发不足，这是长时间累计的速度误差导致的。为此，我们需要思考是否更换一种控制方式。

4.2 角度环PD+速度环PI

如果对速度环PD控制的效果不太满意，我们可以将速度环改为PI控制。

在原理上来说，D项是对速度的一阶微分，也就是加速度，然而正常来说加速度不会频繁的变化，有时甚至是恒速前进的。所以，加入D项对速度环的优化并不大。

而I项可以消除稳态误差，这是因为当机器人长时间存在速度误差时，积分项会不断累积，最终消除稳态误差。

代码中，可以这样设计：

float speedError = robotState.filteredSpeed - robotState.speedBias - robotState.targetSpeed;
if (noCommand && imuStable) {
    robotState.speedIntegral += speedError * 0.01f;  // 积分项
    robotState.speedIntegral = constrain(robotState.speedIntegral, 
                                         -RobotConstants::SPEED_INTEGRAL_LIMIT, 
                                         RobotConstants::SPEED_INTEGRAL_LIMIT);
} else {
    robotState.speedIntegral *= 0.95f;  // 积分衰减
}

float angleAdjust = -(robotState.Kp_speed * speedError + robotState.Ki_speed * robotState.speedIntegral);
float dynamicTargetAngle = robotState.targetAngle + angleAdjust;

可以看到我们的机器人在长时间内都能保持一个稳定的速度前进了。

如果您想要更稳定或其他路面的适应性，请自行搜索并设计合适的控制器哦~