打通新版Stanley轨迹跟踪闭环并补充实验测试与数据分析脚本

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-06 15:14:12 +08:00
parent 47973cc94b
commit 19b1e49189
27 changed files with 3482 additions and 22 deletions
@@ -0,0 +1,307 @@
+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参数和输入必须是有限值。");
+            }
+        }
+    }
+}