12

ロボットをコンパスの方向に向けるように設計された PID コントローラーをロボットで実行しています。PID 補正は、20Hz のレートで再計算/適用されます。

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);
    }

なぜこれが起こっているのか、それを修正する方法を知っている人はいますか?

4

4 に答える 4

8

タイムベースを積分に3回適用しているようです。エラーは、最後のサンプル以降に累積されたエラーであるため、deltaTimeにそれを掛ける必要はありません。したがって、コードを次のように変更します。

SteadyError += error ;

SteadyErrorは、エラーの積分または合計です。

したがって、積分はSteadyError*Kiである必要があります

float integral = Ki * SteadyError;

編集：

私はあなたのコードをもう一度調べました、そして上記の修正に加えて私が修正するであろう他のいくつかの項目があります。

1）ミリ秒単位のデルタ時間は必要ありません。通常のサンプルシステムでは、デルタ項は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);
}
于 2010-10-11T04:14:30.823 に答える
5

初期化していSteadyErrorますか（奇妙な名前...なぜ「インテグレーター」ではないのですか）？起動時にランダムな値が含まれている場合、ほぼゼロに戻らない可能性があります ( 1e100 + 1 == 1e100)。

インテグレーター ワインドアップに悩まされている可能性がありますが、通常は解消されるはずですが、車両が完全に回転する (そしてインテグレーターを再度ワインドアップする) よりも減少するのに時間がかかる場合はそうではありません。システムで必要な場合は、より高度なソリューション(PDF、879 kB)がありますが、簡単な解決策はインテグレーターに制限を課すことです。

Ki正しい記号はありますか？

任意の精度のため、PID パラメータに float を使用することは強くお勧めしません。整数を使用します (おそらく固定小数点)。制限チェックを課す必要がありますが、フロートを使用するよりもはるかに適切です。

于 2010-10-11T14:43:21.763 に答える
4

積分項は時間の経過とともにすでに累積されており、deltaTimeを掛けると、時間の2乗の割合で累積されます。実際、SteadyErrorは、エラーにdeltaTimeを掛けることによってすでに誤って計算されているため、時間3倍になります。

SteadyErrorで、非周期的な更新を補正しようとしている場合は、非周期性を修正することをお勧めします。ただし、いずれの場合も計算に欠陥があります。エラー/時間の単位で計算しましたが、エラーの単位だけが必要です。本当に必要な場合にタイミングジッタを補償するための算術的に正しい方法は次のとおりです。

SteadyError += (error * 50.0f/deltaTime);

deltaTimeがミリ秒単位のままで、公称更新レートが20Hzの場合。ただし、検出しようとしているタイミングジッターの場合、deltaTimeはfloatとして計算するか、ミリ秒に変換しない方が適切です。あなたは不必要に精度を捨てています。いずれにせよ、必要なのは、実際の時間に対する公称時間の比率によってエラー値を変更することです。

良い読み物は博士号のないPIDです

于 2010-10-11T16:26:32.517 に答える
1

コードが機能しない理由はわかりませんが、理由を確認するためにコードをテストできないことはほぼ間違いありません。タイマー サービスを挿入して、モックを作成して何が起こっているかを確認できます。

public interace ITimer 
{
     long GetCurrentTicks()
}

public class Timer : ITimer
{
    public long GetCurrentTicks() 
    {
        return DateTime.Now.Ticks;
    }
}

public class TestTimer : ITimer
{
    private bool firstCall = true;
    private long last;
    private int counter = 1000000000;

    public long GetCurrentTicks()
    {
        if (firstCall)
            last = counter * 10000;
        else
            last += 3500;  //ticks; not sure what a good value is here

        //set up for next call;
        firstCall = !firstCall;
        counter++;

        return last;
    }
}

次に、両方の呼び出しを に置き換えDateTime.Now.TicksますGetCurrentTicks()。コードをステップ実行して、値がどのように見えるかを確認できます。

于 2010-10-11T01:08:41.537 に答える