完善原地自转控制逻辑并加入纵向速度死区与实验绘图改进

Co-authored-by: Cursor <cursoragent@cursor.com>

MultiWheelC/Control/Execution/ParkingGeometricController.cs

@@ -28,7 +28,7 @@ namespace MultiWheelC.Control.Execution
             1e-6;
         private const double StartupRegionMeters = 0.02;
         private const double StartupPreviewDistanceMeters = 0.05;
-        private const double MaximumStartupSpeedMetersPerSecond = 0.05;
+        private const double MaximumStartupSpeedMetersPerSecond = 0.08;
 
         private readonly IVehicleStateProvider _stateProvider;
         private readonly ILateralController _lateralController;

MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs

@@ -23,11 +23,15 @@ namespace MultiWheelC.Control.Longitudinal
             double integralGainPerSecond,
             double derivativeGainSeconds,
             double maximumIntegralCorrectionMetersPerSecond,
-            double maximumCommandSpeedMetersPerSecond)
+            double maximumCommandSpeedMetersPerSecond,
+            double speedErrorDeadbandMetersPerSecond = 0.025)
         {
             EnsureFinitePositive(
                 maximumCommandSpeedMetersPerSecond,
                 nameof(maximumCommandSpeedMetersPerSecond));
+            EnsureFiniteNonNegative(
+                speedErrorDeadbandMetersPerSecond,
+                nameof(speedErrorDeadbandMetersPerSecond));
 
             _feedbackPid = new PidController(
                 proportionalGain,
@@ -37,6 +41,8 @@ namespace MultiWheelC.Control.Longitudinal
                 derivativeOnMeasurement: true);
             MaximumCommandSpeedMetersPerSecond =
                 maximumCommandSpeedMetersPerSecond;
+            SpeedErrorDeadbandMetersPerSecond =
+                speedErrorDeadbandMetersPerSecond;
         }
 
         /// <summary>
@@ -49,6 +55,11 @@ namespace MultiWheelC.Control.Longitudinal
         /// </summary>
         public double MaximumCommandSpeedMetersPerSecond { get; }
 
+        /// <summary>
+        /// 获取不触发纵向PID修正的速度误差死区，单位为m/s。
+        /// </summary>
+        public double SpeedErrorDeadbandMetersPerSecond { get; }
+
         /// <summary>
         /// 获取最近一次有效控制周期的参考速度减实际速度，单位为m/s。
         /// </summary>
@@ -98,6 +109,20 @@ namespace MultiWheelC.Control.Longitudinal
                     referenceSpeedMetersPerSecond);
             }
 
+            var speedErrorMetersPerSecond =
+                referenceSpeedMetersPerSecond -
+                context.ActualLongitudinalSpeedMetersPerSecond;
+
+            // Detour差分速度在参考速度附近会有小幅波动；死区内只使用速度前馈，
+            // 同时清除PID历史，避免噪声持续积累后产生突发修正。
+            if (Math.Abs(speedErrorMetersPerSecond) <=
+                SpeedErrorDeadbandMetersPerSecond)
+            {
+                Reset();
+                return LimitReferenceSpeed(
+                    referenceSpeedMetersPerSecond);
+            }
+
             GetCorrectionOutputRange(
                 referenceSpeedMetersPerSecond,
                 out var minimumCorrectionMetersPerSecond,
@@ -178,5 +203,22 @@ namespace MultiWheelC.Control.Longitudinal
                     "纵向控制器最大命令速度必须是正有限值。");
             }
         }
+
+        /// <summary>
+        /// 检查速度误差死区是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "纵向控制器速度误差死区必须是非负有限值。");
+            }
+        }
     }
 }