0

我正在用 C++ 实现 PID 控制，以使差动驱动机器人转动准确的度数，但我遇到了很多问题。

由于快速循环运行时间而提前退出控制循环

如果机器人测量其误差小于 0.5 度，它会退出控制循环并认为转弯“完成”（0.5 是我可能会在某个时候更改的随机值）。看起来控制回路运行得如此之快，以至于机器人可以以非常高的速度转动，转过设定点，然后退出回路/切断电机功率，因为​​它是在设定点短时间。我知道这是 PID 控制的全部目的，即准确地达到设定值而不会超调，但这个问题使得调整 PID 常数变得非常困难。例如，我试图找到一个 kp 的值，使得有稳定的振荡，但从来没有任何振荡，因为机器人认为它一旦通过设定点就“完成”了。为了解决这个问题，我实现了一个系统，机器人在退出之前必须处于设定点一段时间，这很有效，允许发生振荡，但提前退出循环的问题似乎很不寻常问题，我的解决方案可能不正确。

由于运行时间快，D 项没有影响

一旦我让机器人仅使用 P 以受控方式振荡，我就尝试添加 D 以防止过冲。但是，这在大多数情况下都没有效果，因为控制循环运行得如此之快，以至于 20 个循环中有 19 个循环，误差变化率为 0：机器人没有移动或移动不足到那个时候去衡量。我打印了每个循环的误差变化和导数项来确认这一点，我可以看到在取一个合理的值之前，它们在大约 20 个循环周期内都为 0，然后在另外 20 个循环内回到 0。就像我说的那样，我认为这是因为循环周期太快了，以至于机器人实际上没有足够的移动来产生任何明显的错误变化。

注意：我还对静态摩擦系数使用了一个小的前馈，我称这个前馈为“f”

我应该添加延迟吗？

我意识到我认为这两个问题的根源是循环运行得非常快，所以我想到的是在循环结束时添加一个等待语句。但是，故意减慢循环似乎是一个整体糟糕的解决方案。这是一个好主意吗？

turnHeading(double finalAngle, double kp, double ki, double kd, double f){
    std::clock_t timer;
    timer = std::clock();

    double pastTime = 0;
    double currentTime = ((std::clock() - timer) / (double)CLOCKS_PER_SEC);

    const double initialHeading = getHeading();
    finalAngle = angleWrapDeg(finalAngle);

    const double initialAngleDiff = initialHeading - finalAngle;
    double error = angleDiff(getHeading(), finalAngle);
    double pastError = error;

    double firstTimeAtSetpoint = 0;
    double timeAtSetPoint = 0;
    bool atSetpoint = false;

    double integral = 0;
    double derivative = 0;
    double lastNonZeroD = 0;

    while (timeAtSetPoint < .05)
    {
        updatePos(encoderL.read(), encoderR.read());
        error = angleDiff(getHeading(), finalAngle);

        currentTime = ((std::clock() - timer) / (double)CLOCKS_PER_SEC);
        double dt = currentTime - pastTime;

        double proportional = error / fabs(initialAngleDiff);
        integral += dt * ((error + pastError) / 2.0);
        double derivative = (error - pastError) / dt;
        
        //FAILED METHOD OF USING LAST NON-0 VALUE OF DERIVATIVE
        // if(epsilonEquals(derivative, 0))
        // {
        //     derivative = lastNonZeroD;
        // }
        // else
        // {
        //     lastNonZeroD = derivative;
        // }

        double power = kp * proportional + ki * integral + kd * derivative;

        if (power > 0)
        {
            setMotorPowers(-power - f, power + f);
        }
        else
        {
            setMotorPowers(-power + f, power - f);
        }

        if (fabs(error) < 2)
        {
            if (!atSetpoint)
            {
                atSetpoint = true;
                firstTimeAtSetpoint = currentTime;
            }
            else //at setpoint
            {
                timeAtSetPoint = currentTime - firstTimeAtSetpoint;
            }
        }
        else //no longer at setpoint
        {
            atSetpoint = false;
            timeAtSetPoint = 0;
        }
        pastTime = currentTime;
        pastError = error;
    }
    setMotorPowers(0, 0);
}

turnHeading(90, .37, 0, .00004, .12);
4

0 回答 0