ParkingRobot/ClumsyPilot/Control/Common/PidController.cs

using System;

namespace MultiWheelC.Control.Common
{
    /// <summary>
    /// 使用真实控制周期计算带积分限幅、输出限幅和抗饱和的通用有状态PID输出。
    /// </summary>
    public sealed class PidController
    {
        private double _integralState;
        private double _previousError;
        private double _previousMeasurement;
        private bool _hasPreviousSample;

        /// <summary>
        /// 创建具有指定增益、积分输出限制和微分形式的PID控制器。
        /// </summary>
        public PidController(
            double proportionalGain,
            double integralGainPerSecond,
            double derivativeGainSeconds,
            double maximumIntegralOutput,
            bool derivativeOnMeasurement = true)
        {
            EnsureFiniteNonNegative(
                proportionalGain,
                nameof(proportionalGain));
            EnsureFiniteNonNegative(
                integralGainPerSecond,
                nameof(integralGainPerSecond));
            EnsureFiniteNonNegative(
                derivativeGainSeconds,
                nameof(derivativeGainSeconds));
            EnsureFiniteNonNegative(
                maximumIntegralOutput,
                nameof(maximumIntegralOutput));

            ProportionalGain = proportionalGain;
            IntegralGainPerSecond = integralGainPerSecond;
            DerivativeGainSeconds = derivativeGainSeconds;
            MaximumIntegralOutput = maximumIntegralOutput;
            DerivativeOnMeasurement = derivativeOnMeasurement;
        }

        /// <summary>
        /// 获取比例增益。
        /// </summary>
        public double ProportionalGain { get; }

        /// <summary>
        /// 获取积分增益，单位为1/s。
        /// </summary>
        public double IntegralGainPerSecond { get; }

        /// <summary>
        /// 获取微分增益，单位为s。
        /// </summary>
        public double DerivativeGainSeconds { get; }

        /// <summary>
        /// 获取积分项允许产生的最大输出绝对值。
        /// </summary>
        public double MaximumIntegralOutput { get; }

        /// <summary>
        /// 获取微分项是否作用于测量值，以避免设定值变化产生微分冲击。
        /// </summary>
        public bool DerivativeOnMeasurement { get; }

        /// <summary>
        /// 获取最近一次设定值减测量值的误差。
        /// </summary>
        public double LastError { get; private set; }

        /// <summary>
        /// 获取最近一次比例项输出。
        /// </summary>
        public double LastProportionalOutput { get; private set; }

        /// <summary>
        /// 获取最近一次积分项输出。
        /// </summary>
        public double LastIntegralOutput { get; private set; }

        /// <summary>
        /// 获取最近一次微分项输出。
        /// </summary>
        public double LastDerivativeOutput { get; private set; }

        /// <summary>
        /// 获取最近一次经过输出范围限制后的PID输出。
        /// </summary>
        public double LastOutput { get; private set; }

        /// <summary>
        /// 根据设定值、测量值、真实时间间隔和本周期输出范围更新PID。
        /// </summary>
        public double Update(
            double setPoint,
            double measurement,
            double deltaTimeSeconds,
            double minimumOutput,
            double maximumOutput)
        {
            EnsureFinite(setPoint, nameof(setPoint));
            EnsureFinite(measurement, nameof(measurement));
            EnsureFinitePositive(
                deltaTimeSeconds,
                nameof(deltaTimeSeconds));
            EnsureFinite(minimumOutput, nameof(minimumOutput));
            EnsureFinite(maximumOutput, nameof(maximumOutput));

            if (minimumOutput > maximumOutput)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(minimumOutput),
                    "PID最小输出不能大于最大输出。");
            }

            var error = setPoint - measurement;
            var proportionalOutput =
                ProportionalGain * error;
            var derivativeOutput = CalculateDerivativeOutput(
                error,
                measurement,
                deltaTimeSeconds);

            var candidateIntegralState =
                _integralState +
                error * deltaTimeSeconds;
            var integralOutput = CalculateIntegralOutput(
                candidateIntegralState);

            // 同步截断积分状态本身，避免积分输出虽已限幅、内部状态仍继续增长。
            candidateIntegralState =
                IntegralGainPerSecond > 0.0 &&
                MaximumIntegralOutput > 0.0
                    ? integralOutput /
                      IntegralGainPerSecond
                    : 0.0;

            var unlimitedOutput =
                proportionalOutput +
                integralOutput +
                derivativeOutput;
            var output = Clamp(
                unlimitedOutput,
                minimumOutput,
                maximumOutput);

            // 根据实际允许输出反算积分项，避免执行器饱和期间继续积累误差。
            if (IntegralGainPerSecond > 0.0 &&
                output != unlimitedOutput)
            {
                integralOutput = Clamp(
                    output -
                    proportionalOutput -
                    derivativeOutput,
                    -MaximumIntegralOutput,
                    MaximumIntegralOutput);
                candidateIntegralState =
                    integralOutput /
                    IntegralGainPerSecond;
            }

            _integralState =
                IntegralGainPerSecond > 0.0 &&
                MaximumIntegralOutput > 0.0
                    ? candidateIntegralState
                    : 0.0;
            _previousError = error;
            _previousMeasurement = measurement;
            _hasPreviousSample = true;

            LastError = error;
            LastProportionalOutput = proportionalOutput;
            LastIntegralOutput = integralOutput;
            LastDerivativeOutput = derivativeOutput;
            LastOutput = output;

            return output;
        }

        /// <summary>
        /// 清除积分、历史采样和最近一次PID诊断输出。
        /// </summary>
        public void Reset()
        {
            _integralState = 0.0;
            _previousError = 0.0;
            _previousMeasurement = 0.0;
            _hasPreviousSample = false;
            LastError = 0.0;
            LastProportionalOutput = 0.0;
            LastIntegralOutput = 0.0;
            LastDerivativeOutput = 0.0;
            LastOutput = 0.0;
        }

        /// <summary>
        /// 使用测量值微分或误差微分计算本周期微分项输出。
        /// </summary>
        private double CalculateDerivativeOutput(
            double error,
            double measurement,
            double deltaTimeSeconds)
        {
            if (!_hasPreviousSample ||
                DerivativeGainSeconds <= 0.0)
            {
                return 0.0;
            }

            if (DerivativeOnMeasurement)
            {
                return -DerivativeGainSeconds *
                    (measurement - _previousMeasurement) /
                    deltaTimeSeconds;
            }

            return DerivativeGainSeconds *
                (error - _previousError) /
                deltaTimeSeconds;
        }

        /// <summary>
        /// 根据积分状态计算经过绝对值限制的积分项输出。
        /// </summary>
        private double CalculateIntegralOutput(
            double integralState)
        {
            if (IntegralGainPerSecond <= 0.0 ||
                MaximumIntegralOutput <= 0.0)
            {
                return 0.0;
            }

            return Clamp(
                IntegralGainPerSecond * integralState,
                -MaximumIntegralOutput,
                MaximumIntegralOutput);
        }

        /// <summary>
        /// 将数值限制在指定闭区间内。
        /// </summary>
        private static double Clamp(
            double value,
            double minimum,
            double maximum)
        {
            return Math.Max(
                minimum,
                Math.Min(maximum, value));
        }

        /// <summary>
        /// 检查参数是否为正有限值。
        /// </summary>
        private static void EnsureFinitePositive(
            double value,
            string parameterName)
        {
            EnsureFinite(value, parameterName);

            if (value <= 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "PID时间间隔必须是正有限值。");
            }
        }

        /// <summary>
        /// 检查参数是否为非负有限值。
        /// </summary>
        private static void EnsureFiniteNonNegative(
            double value,
            string parameterName)
        {
            EnsureFinite(value, parameterName);

            if (value < 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "PID增益和积分输出限幅必须是非负有限值。");
            }
        }

        /// <summary>
        /// 检查参数是否为有限值。
        /// </summary>
        private static void EnsureFinite(
            double value,
            string parameterName)
        {
            if (double.IsNaN(value) ||
                double.IsInfinity(value))
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "PID参数和输入必须是有限值。");
            }
        }
    }
}