打通新版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,182 @@
+using System;
+using MultiWheelC.Control.Abstractions;
+using MultiWheelC.Control.Common;
+
+namespace MultiWheelC.Control.Longitudinal
+{
+    /// <summary>
+    /// 将轨迹参考速度前馈与通用PID速度反馈组合为有符号底盘命令速度。
+    /// </summary>
+    public sealed class PidLongitudinalController
+        : ILongitudinalController
+    {
+        private const double ReferenceStopDeadbandMetersPerSecond =
+            1e-6;
+
+        private readonly PidController _feedbackPid;
+
+        /// <summary>
+        /// 创建具有积分抗饱和和命令速度限幅的纵向速度外环。
+        /// </summary>
+        public PidLongitudinalController(
+            double proportionalGain,
+            double integralGainPerSecond,
+            double derivativeGainSeconds,
+            double maximumIntegralCorrectionMetersPerSecond,
+            double maximumCommandSpeedMetersPerSecond)
+        {
+            EnsureFinitePositive(
+                maximumCommandSpeedMetersPerSecond,
+                nameof(maximumCommandSpeedMetersPerSecond));
+
+            _feedbackPid = new PidController(
+                proportionalGain,
+                integralGainPerSecond,
+                derivativeGainSeconds,
+                maximumIntegralCorrectionMetersPerSecond,
+                derivativeOnMeasurement: true);
+            MaximumCommandSpeedMetersPerSecond =
+                maximumCommandSpeedMetersPerSecond;
+        }
+
+        /// <summary>
+        /// 获取负责计算速度误差修正量的通用PID控制器。
+        /// </summary>
+        public PidController FeedbackPid => _feedbackPid;
+
+        /// <summary>
+        /// 获取底盘命令速度的最大绝对值，单位为m/s。
+        /// </summary>
+        public double MaximumCommandSpeedMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取最近一次有效控制周期的参考速度减实际速度，单位为m/s。
+        /// </summary>
+        public double LastSpeedErrorMetersPerSecond =>
+            _feedbackPid.LastError;
+
+        /// <summary>
+        /// 获取最近一次比例项产生的速度修正，单位为m/s。
+        /// </summary>
+        public double LastProportionalCorrectionMetersPerSecond =>
+            _feedbackPid.LastProportionalOutput;
+
+        /// <summary>
+        /// 获取最近一次积分项产生的速度修正，单位为m/s。
+        /// </summary>
+        public double LastIntegralCorrectionMetersPerSecond =>
+            _feedbackPid.LastIntegralOutput;
+
+        /// <summary>
+        /// 获取最近一次微分项产生的速度修正，单位为m/s。
+        /// </summary>
+        public double LastDerivativeCorrectionMetersPerSecond =>
+            _feedbackPid.LastDerivativeOutput;
+
+        /// <summary>
+        /// 根据轨迹参考速度和Detour实际纵向速度计算底盘命令速度。
+        /// </summary>
+        public double ComputeSpeedMetersPerSecond(
+            PathTrackingContext context)
+        {
+            var referenceSpeedMetersPerSecond =
+                context.ReferenceSpeedMetersPerSecond;
+
+            // 轨迹明确要求停车时直接输出零，防止速度反馈使车辆在终点反向纠偏。
+            if (Math.Abs(referenceSpeedMetersPerSecond) <=
+                ReferenceStopDeadbandMetersPerSecond)
+            {
+                Reset();
+                return 0.0;
+            }
+
+            // 定位速度尚不可用时只透传参考速度，不使用无效反馈更新PID状态。
+            if (!context.HasValidVelocityEstimate)
+            {
+                Reset();
+                return LimitReferenceSpeed(
+                    referenceSpeedMetersPerSecond);
+            }
+
+            GetCorrectionOutputRange(
+                referenceSpeedMetersPerSecond,
+                out var minimumCorrectionMetersPerSecond,
+                out var maximumCorrectionMetersPerSecond);
+
+            var correctionMetersPerSecond =
+                _feedbackPid.Update(
+                    referenceSpeedMetersPerSecond,
+                    context
+                        .ActualLongitudinalSpeedMetersPerSecond,
+                    context.DeltaTimeSeconds,
+                    minimumCorrectionMetersPerSecond,
+                    maximumCorrectionMetersPerSecond);
+
+            return referenceSpeedMetersPerSecond +
+                   correctionMetersPerSecond;
+        }
+
+        /// <summary>
+        /// 清除纵向速度外环的积分、历史测量值和诊断输出。
+        /// </summary>
+        public void Reset()
+        {
+            _feedbackPid.Reset();
+        }
+
+        /// <summary>
+        /// 根据参考行驶方向计算PID修正量允许使用的动态输出范围。
+        /// </summary>
+        private void GetCorrectionOutputRange(
+            double referenceSpeedMetersPerSecond,
+            out double minimumCorrectionMetersPerSecond,
+            out double maximumCorrectionMetersPerSecond)
+        {
+            if (referenceSpeedMetersPerSecond > 0.0)
+            {
+                minimumCorrectionMetersPerSecond =
+                    -referenceSpeedMetersPerSecond;
+                maximumCorrectionMetersPerSecond =
+                    MaximumCommandSpeedMetersPerSecond -
+                    referenceSpeedMetersPerSecond;
+                return;
+            }
+
+            minimumCorrectionMetersPerSecond =
+                -MaximumCommandSpeedMetersPerSecond -
+                referenceSpeedMetersPerSecond;
+            maximumCorrectionMetersPerSecond =
+                -referenceSpeedMetersPerSecond;
+        }
+
+        /// <summary>
+        /// 在没有有效速度反馈时限制参考速度的绝对值。
+        /// </summary>
+        private double LimitReferenceSpeed(
+            double referenceSpeedMetersPerSecond)
+        {
+            return Math.Max(
+                -MaximumCommandSpeedMetersPerSecond,
+                Math.Min(
+                    MaximumCommandSpeedMetersPerSecond,
+                    referenceSpeedMetersPerSecond));
+        }
+
+        /// <summary>
+        /// 检查最大命令速度是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                value <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "纵向控制器最大命令速度必须是正有限值。");
+            }
+        }
+    }
+}