我有一个在机器人上运行的 PID Controller ,旨在使机器人转向罗盘航向。 PID 校正以 20Hz 的速率重新计算/应用。
尽管 PID Controller 在 PD 模式(即,积分项归零)下运行良好,但即使是最少量的积分也会迫使输出不稳定,从而导致转向执行器被推向左侧或左侧右极端。
代码:
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);
}
有没有人知道为什么会发生这种情况或如何解决?
最佳答案
看起来您正在将时基应用于积分 3 次。 错误已经是自上次样本以来的累积错误,因此您不需要将 deltaTime 乘以它。所以我将代码更改为以下内容。
SteadyError += 错误;
SteadyError 是误差的积分或总和。
所以积分应该只是 SteadyError * Ki
浮点积分 = 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);
}
关于c# - PID Controller 积分项导致极度不稳定,我们在Stack Overflow上找到一个类似的问题: https://stackoverflow.com/questions/3902713/