PID控制器积分项导致极端不稳定

我有一个在机器人上运行的PID控制器,旨在使机器人转向罗盘方向.以20Hz的速率重新计算/应用PID校正.

虽然PID控制器在PD模式下运行良好(IE,积分项为零),但即使是最轻微的积分也会迫使输出不稳定,使得转向执行器被推到左或右极限.

码:

        private static void DoPID(object o)
    {
        // Bring the LED up to signify frame start
        BoardLED.Write(true);

        // Get IMU heading
        float currentHeading = (float)RazorIMU.Yaw;

        // We just got the IMU heading, so we need to calculate the time from the last correction to the heading read
        // *immediately*. The units don't so much matter, but we are converting Ticks to milliseconds
        int deltaTime = (int)((LastCorrectionTime - DateTime.Now.Ticks) / 10000);

        // Calculate error
        // (let's just assume CurrentHeading really is the current GPS heading, OK?)
        float error = (TargetHeading - currentHeading);

        LCD.Lines[0].Text = "Heading: "+ currentHeading.ToString("F2");

        // We calculated the error, but we need to make sure the error is set so that we will be correcting in the 
        // direction of least work. For example, if we are flying a heading of 2 degrees and the error is a few degrees
        // to the left of that ( IE, somewhere around 360) there will be a large error and the rover will try to turn all
        // the way around to correct, when it could just turn to the right a few degrees.
        // In short, we are adjusting for the fact that a compass heading wraps around in a circle instead of continuing
        // infinity on a line
        if (error < -180)
            error = error + 360;
        else if (error > 180)
            error = error - 360;

        // Add the error calculated in this frame to the running total
        SteadyError = SteadyError + (error * deltaTime);

        // We need to allow for a certain amount of tolerance.
        // If the abs(error) is less than the set amount, we will
        // set error to 0, effectively telling the equation that the
        // rover is perfectly on course.
        if (MyAbs(error) < AllowError)
            error = 0;

        LCD.Lines[2].Text = "Error:   " + error.ToString("F2");

        // Calculate proportional term
        float proportional = Kp * error;

        // Calculate integral term
        float integral = Ki * (SteadyError * deltaTime);

        // Calculate derivative term
        float derivative = Kd * ((error - PrevError) / deltaTime);

        // Add them all together to get the correction delta
        // Set the steering servo to the correction
        Steering.Degree = 90 + proportional + integral + derivative;

        // We have applied the correction, so we need to *immediately* record the 
        // absolute time for generation of deltaTime in the next frame
        LastCorrectionTime = DateTime.Now.Ticks;

        // At this point, the current PID frame is finished
        // ------------------------------------------------------------
        // Now, we need to setup for the next PID frame and close out

        // The "current" error is now the previous error
        // (Remember, we are done with the current frame, so in
        // relative terms, the previous frame IS the "current" frame)
        PrevError = error;

        // Done
        BoardLED.Write(false);
    }

有谁知道为什么会发生这种情况或如何解决它？

看起来你正在将你的时基用于积分三次.错误已经是自上一个样本以来的累积误差,所以你不需要将deltaTime乘以它.所以我会将代码更改为以下内容.

SteadyError += error ;

SteadyError是误差的积分或总和.

所以积分应该是SteadyError*Ki

float integral = Ki * SteadyError;

编辑:

我已经完成了你的代码,除了上面的修复之外,我还会修复其他几个项目.

1)您不希望以毫秒为单位的增量时间.在正常的采样系统中,delta项将是1,但是对于20Hz的速率,你输入的值为50,这具有通过该因子增加Ki并且还将Kd减小50倍的效果.如果您担心抖动,则需要将增量时间转换为相对采样时间.我会用这个公式代替.

float deltaTime = (LastCorrectionTime - DateTime.Now.Ticks) / 500000.0

500000.0是每个样本的预期滴答数,对于20Hz是50ms.

2)将积分项保持在一个范围内.

if ( SteadyError > MaxSteadyError ) SteadyError = MaxSteadyError;
if ( SteadyError < MinSteadyError ) SteadyError = MinSteadyError;

3)更改以下代码,以便当错误在-180左右时,您只需稍作更改就不会出错.

if (error < -270) error += 360;
if (error >  270) error -= 360;

4)验证Steering.Degree正在接收正确的分辨率和符号.

5)最后你可能只是将deltaTime放在一起并按以下方式计算差分项.

float derivative = Kd * (error - PrevError);

随着所有这些你的代码变成了.

private static void DoPID(object o)
{
    // Bring the LED up to signify frame start
    BoardLED.Write(true);

    // Get IMU heading
    float currentHeading = (float)RazorIMU.Yaw;


    // Calculate error
    // (let's just assume CurrentHeading really is the current GPS heading, OK?)
    float error = (TargetHeading - currentHeading);

    LCD.Lines[0].Text = "Heading: "+ currentHeading.ToString("F2");

    // We calculated the error, but we need to make sure the error is set 
    // so that we will be correcting in the 
    // direction of least work. For example, if we are flying a heading 
    // of 2 degrees and the error is a few degrees
    // to the left of that ( IE, somewhere around 360) there will be a 
    // large error and the rover will try to turn all
    // the way around to correct, when it could just turn to the right 
    // a few degrees.
    // In short, we are adjusting for the fact that a compass heading wraps 
    // around in a circle instead of continuing infinity on a line
    if (error < -270) error += 360;
    if (error >  270) error -= 360;

    // Add the error calculated in this frame to the running total
    SteadyError += error;

    if ( SteadyError > MaxSteadyError ) SteadyError = MaxSteadyError;
    if ( SteadyError < MinSteadyError ) SteadyError = MinSteadyError;

    LCD.Lines[2].Text = "Error:   " + error.ToString("F2");

    // Calculate proportional term
    float proportional = Kp * error;

    // Calculate integral term
    float integral = Ki * SteadyError ;

    // Calculate derivative term
    float derivative = Kd * (error - PrevError) ;

    // Add them all together to get the correction delta
    // Set the steering servo to the correction
    Steering.Degree = 90 + proportional + integral + derivative;

    // At this point, the current PID frame is finished
    // ------------------------------------------------------------
    // Now, we need to setup for the next PID frame and close out

    // The "current" error is now the previous error
    // (Remember, we are done with the current frame, so in
    // relative terms, the previous frame IS the "current" frame)
    PrevError = error;

    // Done
    BoardLED.Write(false);
}

你在初始化SteadyError(奇怪的名字......为什么不是"积分器")？如果它在启动时包含一些随机值,它可能永远不会返回到接近零(1e100 + 1 == 1e100).

您可能会受到积分器饱和的影响,通常应该消失,但是如果它需要更长的时间来减少,而不是完成整个旋转(并再次完成积分器).如果您的系统需要,尽管有更高级的解决方案(PDF,879 kB),但这个简单的解决方案是对集成商施加限制.

是否Ki有正确的标志？

由于它们的任意精度,我强烈反对使用浮点数作为PID参数.使用整数(可能是固定点).你将不得不进行限制检查,但它比使用浮动更加明智.