集中停车控制配置并完善终点逼近和周期诊断

MultiWheelC/Control/Execution/ParkingGeometricController.cs

@@ -1,4 +1,5 @@
 using System;
+using System.Diagnostics;
 using MultiWheelC.Control.Abstractions;
 using MultiWheelC.Control.Allocation;
 using MultiWheelC.StateEstimation;
@@ -19,6 +20,52 @@ namespace MultiWheelC.Control.Execution
         Faulted = 4
     }
 
+    /// <summary>
+    /// 保存一个轨迹控制周期各阶段的高精度耗时和定位帧更新状态。
+    /// </summary>
+    public readonly struct ParkingControlCycleTiming
+    {
+        public ParkingControlCycleTiming(
+            long cycleIndex,
+            double cycleIntervalMilliseconds,
+            double stateReadMilliseconds,
+            double projectionMilliseconds,
+            double controllerComputeMilliseconds,
+            double commandSendMilliseconds,
+            double totalCycleMilliseconds,
+            bool hasStateTimestamp,
+            double stateTimestampSeconds,
+            bool stateTimestampChanged,
+            ParkingControlCycleResult result)
+        {
+            CycleIndex = cycleIndex;
+            CycleIntervalMilliseconds =
+                cycleIntervalMilliseconds;
+            StateReadMilliseconds = stateReadMilliseconds;
+            ProjectionMilliseconds = projectionMilliseconds;
+            ControllerComputeMilliseconds =
+                controllerComputeMilliseconds;
+            CommandSendMilliseconds = commandSendMilliseconds;
+            TotalCycleMilliseconds = totalCycleMilliseconds;
+            HasStateTimestamp = hasStateTimestamp;
+            StateTimestampSeconds = stateTimestampSeconds;
+            StateTimestampChanged = stateTimestampChanged;
+            Result = result;
+        }
+
+        public long CycleIndex { get; }
+        public double CycleIntervalMilliseconds { get; }
+        public double StateReadMilliseconds { get; }
+        public double ProjectionMilliseconds { get; }
+        public double ControllerComputeMilliseconds { get; }
+        public double CommandSendMilliseconds { get; }
+        public double TotalCycleMilliseconds { get; }
+        public bool HasStateTimestamp { get; }
+        public double StateTimestampSeconds { get; }
+        public bool StateTimestampChanged { get; }
+        public ParkingControlCycleResult Result { get; }
+    }
+
     /// <summary>
     /// 组织状态读取、轨迹投影、横纵向控制、GCP分配和底盘命令执行。
     /// </summary>
@@ -41,6 +88,10 @@ namespace MultiWheelC.Control.Execution
         private readonly GcpCommandExecutor _commandExecutor;
 
         private Trajectory2D _trajectory;
+        private double _terminalTravelDirection = 1.0;
+        private long _cycleIndex;
+        private bool _hasPreviousStateTimestamp;
+        private double _previousStateTimestampSeconds;
 
         /// <summary>
         /// 创建具有终点判定和轨迹偏离保护的单车轨迹控制器。
@@ -56,7 +107,10 @@ namespace MultiWheelC.Control.Execution
             double finishHeadingToleranceRadians =
                 3.0 * Math.PI / 180.0,
             double maximumDistanceToTrajectoryMeters = 0.30,
-            double terminalBrakingPreviewMeters = 0.02)
+            double terminalBrakingPreviewMeters = 0.02,
+            double terminalApproachDistanceMeters = 0.10,
+            double terminalApproachGainPerSecond = 0.8,
+            double maximumTerminalApproachSpeedMetersPerSecond = 0.05)
         {
             _stateProvider = stateProvider ??
                 throw new ArgumentNullException(
@@ -89,6 +143,23 @@ namespace MultiWheelC.Control.Execution
             EnsureFiniteNonNegative(
                 terminalBrakingPreviewMeters,
                 nameof(terminalBrakingPreviewMeters));
+            EnsureFinitePositive(
+                terminalApproachDistanceMeters,
+                nameof(terminalApproachDistanceMeters));
+            EnsureFinitePositive(
+                terminalApproachGainPerSecond,
+                nameof(terminalApproachGainPerSecond));
+            EnsureFinitePositive(
+                maximumTerminalApproachSpeedMetersPerSecond,
+                nameof(maximumTerminalApproachSpeedMetersPerSecond));
+
+            if (terminalApproachDistanceMeters <=
+                finishDistanceMeters)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(terminalApproachDistanceMeters),
+                    "终点单向逼近范围必须大于终点位置容差。");
+            }
 
             FinishDistanceMeters = finishDistanceMeters;
             FinishSpeedMetersPerSecond =
@@ -99,6 +170,12 @@ namespace MultiWheelC.Control.Execution
                 maximumDistanceToTrajectoryMeters;
             TerminalBrakingPreviewMeters =
                 terminalBrakingPreviewMeters;
+            TerminalApproachDistanceMeters =
+                terminalApproachDistanceMeters;
+            TerminalApproachGainPerSecond =
+                terminalApproachGainPerSecond;
+            MaximumTerminalApproachSpeedMetersPerSecond =
+                maximumTerminalApproachSpeedMetersPerSecond;
         }
 
         /// <summary>
@@ -126,6 +203,21 @@ namespace MultiWheelC.Control.Execution
         /// </summary>
         public double TerminalBrakingPreviewMeters { get; }
 
+        /// <summary>
+        /// 获取切换到终点单向低速逼近的最大剩余弧长，单位为m。
+        /// </summary>
+        public double TerminalApproachDistanceMeters { get; }
+
+        /// <summary>
+        /// 获取由终点纵向剩余距离生成低速参考的比例增益，单位为1/s。
+        /// </summary>
+        public double TerminalApproachGainPerSecond { get; }
+
+        /// <summary>
+        /// 获取终点单向逼近参考速度允许的最大绝对值，单位为m/s。
+        /// </summary>
+        public double MaximumTerminalApproachSpeedMetersPerSecond { get; }
+
         /// <summary>
         /// 获取控制器当前是否持有并正在执行一条轨迹。
         /// </summary>
@@ -167,6 +259,11 @@ namespace MultiWheelC.Control.Execution
         /// </summary>
         public double? LastControlReferenceSpeedMetersPerSecond { get; private set; }
 
+        /// <summary>
+        /// 获取最近一次控制周期的分阶段耗时诊断；尚未执行时为空。
+        /// </summary>
+        public ParkingControlCycleTiming? LastCycleTiming { get; private set; }
+
         /// <summary>
         /// 停止当前底盘并从起点开始执行指定二维轨迹。
         /// </summary>
@@ -180,6 +277,8 @@ namespace MultiWheelC.Control.Execution
 
             StopAndResetControllers();
             _trajectory = trajectory;
+            _terminalTravelDirection =
+                ResolveTerminalTravelDirection(trajectory);
             IsActive = true;
             IsCompleted = false;
             ClearDiagnostics();
@@ -200,40 +299,110 @@ namespace MultiWheelC.Control.Execution
                 return ParkingControlCycleResult.Inactive;
             }
 
+            var cycleIndex = ++_cycleIndex;
+            var cycleStartTimestamp =
+                Stopwatch.GetTimestamp();
+            var stateReadMilliseconds = 0.0;
+            var projectionMilliseconds = 0.0;
+            var controllerComputeMilliseconds = 0.0;
+            var commandSendMilliseconds = 0.0;
+            var controllerComputeStartTimestamp = 0L;
+            var controllerComputeCompleted = false;
+            var hasStateTimestamp = false;
+            var stateTimestampSeconds = 0.0;
+            var stateTimestampChanged = false;
+            var cycleResult =
+                ParkingControlCycleResult.Faulted;
+
             try
             {
-                if (!_stateProvider.TryGetState(
-                        out var vehicleState))
+                var stateReadStartTimestamp =
+                    Stopwatch.GetTimestamp();
+                bool stateAvailable;
+                VehicleState vehicleState;
+                try
                 {
-                    StopForUnavailableState();
-                    return ParkingControlCycleResult
+                    stateAvailable =
+                        _stateProvider.TryGetState(
+                            out vehicleState);
+                }
+                finally
+                {
+                    stateReadMilliseconds =
+                        GetElapsedMilliseconds(
+                            stateReadStartTimestamp);
+                }
+
+                if (!stateAvailable)
+                {
+                    var stopCommandStartTimestamp =
+                        Stopwatch.GetTimestamp();
+                    try
+                    {
+                        StopForUnavailableState();
+                    }
+                    finally
+                    {
+                        commandSendMilliseconds =
+                            GetElapsedMilliseconds(
+                                stopCommandStartTimestamp);
+                    }
+
+                    cycleResult = ParkingControlCycleResult
                         .StateUnavailable;
+                    return cycleResult;
                 }
 
                 LastVehicleState = vehicleState;
+                hasStateTimestamp = true;
+                stateTimestampSeconds =
+                    vehicleState.SampleTimestampSeconds;
+                stateTimestampChanged =
+                    !_hasPreviousStateTimestamp ||
+                    stateTimestampSeconds !=
+                    _previousStateTimestampSeconds;
+                _previousStateTimestampSeconds =
+                    stateTimestampSeconds;
+                _hasPreviousStateTimestamp = true;
 
                 // 首周期允许全局定位轨迹进度；后续周期仅在上次进度
                 // 前后有限物理距离内搜索，避免交叉或平行轨迹间跳段。
-                var projection = LastProjection.HasValue
-                    ? TrajectoryProjector.Project(
-                        _trajectory,
-                        vehicleState.PoseInWorld,
-                        LastProjection.Value.ArcLengthMeters,
-                        ProjectionBackwardSearchDistanceMeters,
-                        ProjectionForwardSearchDistanceMeters)
-                    : TrajectoryProjector.Project(
-                        _trajectory,
-                        vehicleState.PoseInWorld);
+                var projectionStartTimestamp =
+                    Stopwatch.GetTimestamp();
+                TrajectoryProjection projection;
+                try
+                {
+                    projection = LastProjection.HasValue
+                        ? TrajectoryProjector.Project(
+                            _trajectory,
+                            vehicleState.PoseInWorld,
+                            LastProjection.Value.ArcLengthMeters,
+                            ProjectionBackwardSearchDistanceMeters,
+                            ProjectionForwardSearchDistanceMeters)
+                        : TrajectoryProjector.Project(
+                            _trajectory,
+                            vehicleState.PoseInWorld);
+                }
+                finally
+                {
+                    projectionMilliseconds =
+                        GetElapsedMilliseconds(
+                            projectionStartTimestamp);
+                }
+
                 LastProjection = projection;
+                controllerComputeStartTimestamp =
+                    Stopwatch.GetTimestamp();
 
                 if (projection.DistanceToTrajectoryMeters >
                     MaximumDistanceToTrajectoryMeters)
                 {
-                    return EnterFault(
+                    cycleResult = EnterFault(
                         "车辆距离参考轨迹" +
                         $"{projection.DistanceToTrajectoryMeters:F3}m，" +
                         "超过允许值" +
                         $"{MaximumDistanceToTrajectoryMeters:F3}m。");
+                    return cycleResult;
                 }
 
                 if (HasReachedEnd(
@@ -241,7 +410,9 @@ namespace MultiWheelC.Control.Execution
                         projection))
                 {
                     CompleteTrajectory();
-                    return ParkingControlCycleResult.Completed;
+                    cycleResult =
+                        ParkingControlCycleResult.Completed;
+                    return cycleResult;
                 }
 
                 if (HasStoppedAtUnsatisfiedTerminal(
@@ -249,12 +420,14 @@ namespace MultiWheelC.Control.Execution
                         projection,
                         out var terminalFailureReason))
                 {
-                    return EnterFault(
+                    cycleResult = EnterFault(
                         terminalFailureReason);
+                    return cycleResult;
                 }
 
                 var controlReferenceSpeedMetersPerSecond =
-                    ResolveReferenceSpeedForControl(
+                    ResolveControlReferenceSpeed(
+                        vehicleState,
                         projection);
                 LastControlReferenceSpeedMetersPerSecond =
                     controlReferenceSpeedMetersPerSecond;
@@ -272,15 +445,36 @@ namespace MultiWheelC.Control.Execution
                     commandSpeedMetersPerSecond,
                     lateralCommand);
 
-                if (!_commandExecutor.Execute(
-                        gcpCommand,
-                        deltaTimeSeconds))
+                controllerComputeMilliseconds =
+                    GetElapsedMilliseconds(
+                        controllerComputeStartTimestamp);
+                controllerComputeCompleted = true;
+
+                var commandSendStartTimestamp =
+                    Stopwatch.GetTimestamp();
+                bool commandSucceeded;
+                try
                 {
-                    return EnterFault(
+                    commandSucceeded =
+                        _commandExecutor.Execute(
+                            gcpCommand,
+                            deltaTimeSeconds);
+                }
+                finally
+                {
+                    commandSendMilliseconds =
+                        GetElapsedMilliseconds(
+                            commandSendStartTimestamp);
+                }
+
+                if (!commandSucceeded)
+                {
+                    cycleResult = EnterFault(
                         string.IsNullOrWhiteSpace(
                             _commandExecutor.LastFailureReason)
                             ? "GCP底盘命令执行失败。"
                             : _commandExecutor.LastFailureReason);
+                    return cycleResult;
                 }
 
                 LastCommand =
@@ -288,14 +482,42 @@ namespace MultiWheelC.Control.Execution
 
                 LastFailureReason = string.Empty;
                 LastException = null;
-                return ParkingControlCycleResult.CommandSent;
+                cycleResult =
+                    ParkingControlCycleResult.CommandSent;
+                return cycleResult;
             }
             catch (Exception exception)
             {
-                return EnterFault(
+                cycleResult = EnterFault(
                     "停车机器人轨迹控制周期异常：" +
                     exception.Message,
                     exception);
+                return cycleResult;
+            }
+            finally
+            {
+                if (controllerComputeStartTimestamp != 0L &&
+                    !controllerComputeCompleted)
+                {
+                    controllerComputeMilliseconds =
+                        GetElapsedMilliseconds(
+                            controllerComputeStartTimestamp);
+                }
+
+                LastCycleTiming =
+                    new ParkingControlCycleTiming(
+                        cycleIndex,
+                        deltaTimeSeconds * 1000.0,
+                        stateReadMilliseconds,
+                        projectionMilliseconds,
+                        controllerComputeMilliseconds,
+                        commandSendMilliseconds,
+                        GetElapsedMilliseconds(
+                            cycleStartTimestamp),
+                        hasStateTimestamp,
+                        stateTimestampSeconds,
+                        stateTimestampChanged,
+                        cycleResult);
             }
         }
 
@@ -306,11 +528,84 @@ namespace MultiWheelC.Control.Execution
         {
             StopAndResetControllers();
             _trajectory = null;
+            _terminalTravelDirection = 1.0;
             IsActive = false;
             IsCompleted = false;
             ClearDiagnostics();
         }
 
+        /// <summary>
+        /// 在普通空间速度规划和终点单向低速逼近之间选择本周期参考速度。
+        /// </summary>
+        private double ResolveControlReferenceSpeed(
+            VehicleState vehicleState,
+            TrajectoryProjection projection)
+        {
+            if (projection.RemainingDistanceMeters >
+                TerminalApproachDistanceMeters)
+            {
+                return ResolveReferenceSpeedForControl(
+                    projection);
+            }
+
+            return ResolveTerminalApproachSpeed(
+                vehicleState);
+        }
+
+        /// <summary>
+        /// 根据终点车头方向上的有符号剩余距离生成只保持原轨迹行驶方向的低速参考。
+        /// </summary>
+        private double ResolveTerminalApproachSpeed(
+            VehicleState vehicleState)
+        {
+            var distanceToEndMeters =
+                CalculateDistanceToEndMeters(
+                    vehicleState);
+            var headingErrorToEndRadians =
+                CalculateHeadingErrorToEndRadians(
+                    vehicleState);
+
+            // 一旦位置和航向已经进入完成容差，先要求纵向停车；
+            // 后续周期在实际速度也满足条件后完成轨迹。
+            if (distanceToEndMeters <=
+                    FinishDistanceMeters &&
+                headingErrorToEndRadians <=
+                    FinishHeadingToleranceRadians)
+            {
+                return 0.0;
+            }
+
+            var endPose =
+                _trajectory.EndPoint.PoseInWorld;
+            var deltaX =
+                endPose.XMeters -
+                vehicleState.PoseInWorld.XMeters;
+            var deltaY =
+                endPose.YMeters -
+                vehicleState.PoseInWorld.YMeters;
+            var longitudinalErrorMeters =
+                deltaX * Math.Cos(endPose.YawRadians) +
+                deltaY * Math.Sin(endPose.YawRadians);
+            var remainingAlongTravelMeters =
+                _terminalTravelDirection *
+                longitudinalErrorMeters;
+
+            // 到达或越过终点后只停车，禁止生成与原轨迹方向相反的修正速度。
+            if (remainingAlongTravelMeters <= 0.0)
+            {
+                return 0.0;
+            }
+
+            var speedMagnitudeMetersPerSecond =
+                Math.Min(
+                    MaximumTerminalApproachSpeedMetersPerSecond,
+                    TerminalApproachGainPerSecond *
+                    remainingAlongTravelMeters);
+
+            return _terminalTravelDirection *
+                   speedMagnitudeMetersPerSecond;
+        }
+
         /// <summary>
         /// 在轨迹起步区域内保持最低释放速度，避免空间速度曲线零速固定点。
         /// </summary>
@@ -414,6 +709,33 @@ namespace MultiWheelC.Control.Execution
             return currentReferenceSpeed;
         }
 
+        /// <summary>
+        /// 从轨迹终点前最后一个非零参考速度解析单向逼近允许的行驶方向。
+        /// </summary>
+        private static double ResolveTerminalTravelDirection(
+            Trajectory2D trajectory)
+        {
+            for (var index = trajectory.Count - 1;
+                 index >= 0;
+                 index--)
+            {
+                var referenceSpeedMetersPerSecond =
+                    trajectory[index]
+                        .ReferenceSpeedMetersPerSecond;
+
+                if (Math.Abs(referenceSpeedMetersPerSecond) >
+                    ZeroReferenceSpeedToleranceMetersPerSecond)
+                {
+                    return Math.Sign(
+                        referenceSpeedMetersPerSecond);
+                }
+            }
+
+            throw new ArgumentException(
+                "轨迹必须在终点前包含至少一个非零参考速度，才能确定终点单向逼近方向。",
+                nameof(trajectory));
+        }
+
         /// <summary>
         /// 根据终点距离、剩余弧长和实际线速度判断轨迹是否完成。
         /// </summary>
@@ -585,14 +907,29 @@ namespace MultiWheelC.Control.Execution
         /// </summary>
         private void ClearDiagnostics()
         {
+            _cycleIndex = 0;
+            _hasPreviousStateTimestamp = false;
+            _previousStateTimestampSeconds = 0.0;
             LastVehicleState = null;
             LastProjection = null;
             LastCommand = null;
             LastControlReferenceSpeedMetersPerSecond = null;
+            LastCycleTiming = null;
             LastFailureReason = string.Empty;
             LastException = null;
         }
 
+        /// <summary>
+        /// 将Stopwatch高精度时间戳差转换为毫秒。
+        /// </summary>
+        private static double GetElapsedMilliseconds(
+            long startTimestamp)
+        {
+            return (Stopwatch.GetTimestamp() - startTimestamp) *
+                   1000.0 /
+                   Stopwatch.Frequency;
+        }
+
         /// <summary>
         /// 检查控制参数是否为正有限值。
         /// </summary>