前面我们提到了用PID实现机器人直立需要角度环，现在我们对角度环实现的代码进行详细解释。

3.1 角度环：P

对角度环单环实现P控制，是平衡调参中最基础的实现。

让我们回顾一下PID的核心公式：

$ Output = Kpe(t) + Ki∫e(t)dt + Kd*\frac{d e(t)}{dt} $

其中，$ Output $代表系统输出，右式第一项是比例项，第二项是积分项，第三项是微分项，$ Kp $表示比例系数，$ e(t) $表示误差项，$ Ki $表示积分系数，$ Kd $表示微分系数。

在P控制中，我们忽略后面两项，保留比例项。则上述公式变式为：

$ Output = Kp*e(t) $

我们可以实现一个误差项：

float angleError = pitch - dynamicTargetAngle;

其中pitch代表当前俯仰角，它来自于IMU的角度寄存器的数值读取；dynamicTargetAngle代表角度期望值，这是需要我们自己设定的，在这里，该变量定义是我们的目标角度＋偏差项，我们的控制输出需要尽可能接近这个值。

这样，我们就可以实现一个P项：

float pitchControlOutput = robotState.Kp_angle * angleError;

你需要调整Kp_Angle，观察机器人的平衡情况，以达到一个合适的值。调参原则来说，这个值应该是Kp/Ki/Kd中最大的。

此时机器人已能基本实现站立，但是振荡较大，很可能倒下。为此，我们需要引入D项，调节其稳定性。

3.2 角度环：PD

实现一个最基础的P控制之后，加入D项可以让我们的直立环更稳定。

在PD控制中，我们忽略积分项，保留比例项和微分项。则上述公式变式为：

$ Output = Kpe(t) +Kd\frac{d e(t)}{dt} $

通过读取IMU寄存器值，并减去零漂，来实现一个微分误差项：

float pitchRate = imuData.pitchRate;

代入公式：

float pitchControlOutput = robotState.Kp_angle * angleError + robotState.Kd_angle * pitchRate;

调节Kd_angle，达到一个合适的值。调参原则来说，Kd_angle的值不应太大，只需要一个较小的值就可以使系统稳定，如果太大了反而会增大振荡。

此时机器人已能实现较稳定的站立。读取IMU数据时我们可能需要使用卡尔曼滤波来减少累积误差，实现更为实时、精确的控制。

卡尔曼滤波实现示例：

bool IMUHandler::update() {
    inv_imu_sensor_event_t imuEvent;
    
    // 读取IMU数据
    int ret = imu.getDataFromRegisters(imuEvent);
    if (ret != 0) {
        return false;
    }
    
    // 计算时间间隔
    unsigned long currentTime = millis();
    float dt = (currentTime - lastUpdateTime) / 1000.0f;
    
    // 限制dt范围，防止第一次调用或异常情况下dt过大
    if (dt > 0.1f || dt < 0.001f) {
        dt = 0.01f;  // 默认100Hz更新率
    }
    
    lastUpdateTime = currentTime;
    
    // 提取加速度数据(单位：g)，不需要转换为m/s²
    float ax = imuEvent.accel[0] * ACCEL_SCALE;
    float ay = imuEvent.accel[1] * ACCEL_SCALE;
    float az = imuEvent.accel[2] * ACCEL_SCALE;
    
    // 提取陀螺仪数据(单位：度/秒) 并减去零漂
    float gx = imuEvent.gyro[0] * GYRO_SCALE - gyroOffsetX;
    float gy = imuEvent.gyro[1] * GYRO_SCALE - gyroOffsetY;
    float gz = imuEvent.gyro[2] * GYRO_SCALE - gyroOffsetZ;
    
    // 存储原始数据（加速度转换为m/s²供外部使用）
    data.accelX = ax;
    data.accelY = ay;
    data.accelZ = az;
    data.pitchRate = gx;
    data.rollRate = gy;
    data.yawRate = gz;
    
    // 卡尔曼滤波计算姿态角
    kalmanFilterUpdate(ax, ay, az, gx, gy, gz, dt);
    
    data.timestamp = currentTime;
    
    return true;
}


float KalmanFilter::update(float newAngle, float newRate, float dt) {
    // 限制dt范围，防止异常
    if (dt > 0.1f || dt < 0.001f) {
        dt = 0.01f;
    }

    // ===== 预测步骤 =====
    // 1. 状态预测
    rate = newRate - bias;                    // 去除零漂的角速度
    angle += dt * rate;                       // 角度预测

    // 防止角度积分发散，限制在合理范围内
    if (angle > 180.0f) angle = 180.0f;
    if (angle < -180.0f) angle = -180.0f;

    // 2. 误差协方差预测
    // P = A * P * A^T + Q
    P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
    P[0][1] -= dt * P[1][1];
    P[1][0] -= dt * P[1][1];
    P[1][1] += Q_bias * dt;

    // ===== 更新步骤 =====
    // 3. 计算卡尔曼增益
    // K = P * H^T / (H * P * H^T + R)
    float S = P[0][0] + R_measure;            // 新息协方差
    float K[2];                               // 卡尔曼增益
    K[0] = P[0][0] / S;
    K[1] = P[1][0] / S;

    // 4. 状态更新
    // x = x + K * (z - H * x)
    float y = newAngle - angle;               // 新息(innovation)

    // 限制新息范围，防止异常值
    if (y > 90.0f) y = 90.0f;
    if (y < -90.0f) y = -90.0f;

    angle += K[0] * y;                        // 更新角度
    bias += K[1] * y;                         // 更新零漂

    // 限制bias的范围，防止过度变化（更严格）
    if (bias > 2.0f) bias = 2.0f;
    if (bias < -2.0f) bias = -2.0f;

    // 5. 误差协方差更新
    // P = (I - K * H) * P
    float P00_temp = P[0][0];
    float P01_temp = P[0][1];

    P[0][0] -= K[0] * P00_temp;
    P[0][1] -= K[0] * P01_temp;
    P[1][0] -= K[1] * P00_temp;
    P[1][1] -= K[1] * P01_temp;

    return angle;
}

在站立稳定后，我们希望它发挥移动机器人该有的移动性，因此我们需要引入速度和位移。对于这一目标来说，实现直立环是不足够的，我们还需要实现速度环，以控制它的移动速度。

3. PID：角度环

3.1 角度环：P

3.2 角度环：PD

相关文档