优化轨迹制动与轮速反馈并补充实验记录和中英文说明

MultiWheelC/Control/Execution/ParkingGeometricController.cs

@@ -51,7 +51,8 @@ namespace MultiWheelC.Control.Execution
             double finishSpeedMetersPerSecond = 0.02,
             double finishHeadingToleranceRadians =
                 3.0 * Math.PI / 180.0,
-            double maximumDistanceToTrajectoryMeters = 0.30)
+            double maximumDistanceToTrajectoryMeters = 0.30,
+            double terminalBrakingPreviewMeters = 0.02)
         {
             _stateProvider = stateProvider ??
                 throw new ArgumentNullException(
@@ -81,6 +82,9 @@ namespace MultiWheelC.Control.Execution
             EnsureFinitePositive(
                 maximumDistanceToTrajectoryMeters,
                 nameof(maximumDistanceToTrajectoryMeters));
+            EnsureFiniteNonNegative(
+                terminalBrakingPreviewMeters,
+                nameof(terminalBrakingPreviewMeters));
 
             FinishDistanceMeters = finishDistanceMeters;
             FinishSpeedMetersPerSecond =
@@ -89,6 +93,8 @@ namespace MultiWheelC.Control.Execution
                 finishHeadingToleranceRadians;
             MaximumDistanceToTrajectoryMeters =
                 maximumDistanceToTrajectoryMeters;
+            TerminalBrakingPreviewMeters =
+                terminalBrakingPreviewMeters;
         }
 
         /// <summary>
@@ -111,6 +117,11 @@ namespace MultiWheelC.Control.Execution
         /// </summary>
         public double MaximumDistanceToTrajectoryMeters { get; }
 
+        /// <summary>
+        /// 获取沿轨迹提前读取更低制动参考速度的距离，单位为m。
+        /// </summary>
+        public double TerminalBrakingPreviewMeters { get; }
+
         /// <summary>
         /// 获取控制器当前是否持有并正在执行一条轨迹。
         /// </summary>
@@ -288,7 +299,7 @@ namespace MultiWheelC.Control.Execution
         }
 
         /// <summary>
-        /// 在轨迹起点零速固定点处读取前方速度，并限制为低速起步命令。
+        /// 在轨迹起步区域内保持最低释放速度，避免空间速度曲线零速固定点。
         /// </summary>
         private double ResolveReferenceSpeedForControl(
             TrajectoryProjection projection)
@@ -297,15 +308,18 @@ namespace MultiWheelC.Control.Execution
                 projection.ReferencePoint
                     .ReferenceSpeedMetersPerSecond;
 
-            var requiresStartupRelease =
+            currentReferenceSpeed =
+                ApplyTerminalBrakingPreview(
+                    projection,
+                    currentReferenceSpeed);
+
+            var isInStartupRegion =
                 projection.ArcLengthMeters <=
                     StartupRegionMeters &&
                 projection.RemainingDistanceMeters >
-                    FinishDistanceMeters &&
-                Math.Abs(currentReferenceSpeed) <=
-                    ZeroReferenceSpeedToleranceMetersPerSecond;
+                    FinishDistanceMeters;
 
-            if (!requiresStartupRelease)
+            if (!isInStartupRegion)
             {
                 return currentReferenceSpeed;
             }
@@ -314,6 +328,7 @@ namespace MultiWheelC.Control.Execution
                 _trajectory.TotalLengthMeters,
                 projection.ArcLengthMeters +
                 StartupPreviewDistanceMeters);
+
             var previewReferenceSpeed =
                 _trajectory
                     .SampleAtArcLength(
@@ -323,13 +338,67 @@ namespace MultiWheelC.Control.Execution
             if (Math.Abs(previewReferenceSpeed) <=
                 ZeroReferenceSpeedToleranceMetersPerSecond)
             {
-                return 0.0;
+                return currentReferenceSpeed;
             }
 
-            return Math.Sign(previewReferenceSpeed) *
-                   Math.Min(
-                       Math.Abs(previewReferenceSpeed),
-                       MaximumStartupSpeedMetersPerSecond);
+            var startupReleaseSpeed =
+                Math.Sign(previewReferenceSpeed) *
+                Math.Min(
+                    Math.Abs(previewReferenceSpeed),
+                    MaximumStartupSpeedMetersPerSecond);
+
+            // 当前空间速度已经高于起步释放速度时，
+            // 正常交还给原始速度曲线。
+            if (Math.Sign(currentReferenceSpeed) ==
+                    Math.Sign(startupReleaseSpeed) &&
+                Math.Abs(currentReferenceSpeed) >=
+                    Math.Abs(startupReleaseSpeed))
+            {
+                return currentReferenceSpeed;
+            }
+
+            return startupReleaseSpeed;
+        }
+
+        /// <summary>
+        /// 采用前方更低的同方向参考速度，使车辆在终点减速段提前制动。
+        /// </summary>
+        private double ApplyTerminalBrakingPreview(
+            TrajectoryProjection projection,
+            double currentReferenceSpeed)
+        {
+            if (TerminalBrakingPreviewMeters <= 0.0)
+            {
+                return currentReferenceSpeed;
+            }
+
+            var previewArcLengthMeters = Math.Min(
+                _trajectory.TotalLengthMeters,
+                projection.ArcLengthMeters +
+                TerminalBrakingPreviewMeters);
+            var previewReferenceSpeed =
+                _trajectory
+                    .SampleAtArcLength(
+                        previewArcLengthMeters)
+                    .ReferenceSpeedMetersPerSecond;
+
+            var previewIsStop =
+                Math.Abs(previewReferenceSpeed) <=
+                ZeroReferenceSpeedToleranceMetersPerSecond;
+            var hasSameDirection =
+                Math.Sign(previewReferenceSpeed) ==
+                Math.Sign(currentReferenceSpeed);
+            var previewIsSlower =
+                Math.Abs(previewReferenceSpeed) <
+                Math.Abs(currentReferenceSpeed);
+
+            if (previewIsSlower &&
+                (previewIsStop || hasSameDirection))
+            {
+                return previewReferenceSpeed;
+            }
+
+            return currentReferenceSpeed;
         }
 
         /// <summary>