贯通车队运行链并支持轨迹自动推导β

MultiWheelC/Configuration/PilotConfig.ParkingControl.cs

@@ -119,7 +119,7 @@ public partial class PilotConfig
     #region 停车控制-原地自转
 
     [FieldMember(desc = "停车控制：原地自转Kp")]
-    public float InPlaceRotateKp = 1.03f;
+    public float InPlaceRotateKp = 1.0f;
 
     [FieldMember(desc = "停车控制：原地自转Ki")]
     public float InPlaceRotateKi = 0f;
@@ -141,11 +141,11 @@ public partial class PilotConfig
 
     [FieldMember(desc = "停车控制：原地自转最大角速度(deg/s)")]
     // public float InPlaceRotateMaxSpeed = 47.5f;
-    public float InPlaceRotateMaxSpeed = 30f;
+    public float InPlaceRotateMaxSpeed = 45f;
 
     [FieldMember(desc = "停车控制：原地自转角加速度(deg/s²)")]
     // public float InPlaceRotateAcc = 60f;
-    public float InPlaceRotateAcc = 40f;
+    public float InPlaceRotateAcc = 45f;
 
     [FieldMember(desc = "停车控制：原地自转超时(s)")]
     public float InPlaceRotateTimeoutSec = 15f;

MultiWheelC/Experiments/NewControllerTrackingTests.cs

@@ -143,6 +143,12 @@ namespace MultiWheelC
         protected virtual double MotionDirectionInBodyRadians =>
             0.0;
 
+        /// <summary>
+        /// 获取是否由生成后的轨迹自动推导底盘运动坐标系方向。
+        /// </summary>
+        protected virtual bool ResolveMotionDirectionFromTrajectory =>
+            false;
+
         /// <summary>
         /// 获取轨迹完成后是否需要将舵轮主动恢复到车头方向。
         /// </summary>
@@ -244,7 +250,9 @@ namespace MultiWheelC
                     Trajectory = trajectory,
                     StateProvider = _stateProvider,
                     MotionDirectionInBodyRadians =
-                        MotionDirectionInBodyRadians,
+                        ResolveMotionDirectionFromTrajectory
+                            ? (double?)null
+                            : MotionDirectionInBodyRadians,
                     ReturnWheelsForwardAfterCompletion =
                         ReturnWheelsForwardAfterCompletion,
                     CycleObserver = controller =>
@@ -505,6 +513,12 @@ namespace MultiWheelC
         protected override double MotionDirectionInBodyRadians =>
             Math.PI / 4.0;
 
+        /// <summary>
+        /// 只用45°定义参考轨迹，底盘β由轨迹切线和车身参考航向自动推导。
+        /// </summary>
+        protected override bool ResolveMotionDirectionFromTrajectory =>
+            true;
+
         /// <summary>
         /// 蟹行轨迹正常完成后主动将四个舵轮恢复到车头方向。
         /// </summary>
@@ -586,6 +600,12 @@ namespace MultiWheelC
         protected virtual double MotionDirectionInBodyRadians =>
             0.0;
 
+        /// <summary>
+        /// 获取是否由生成后的轨迹自动推导底盘运动坐标系方向。
+        /// </summary>
+        protected virtual bool ResolveMotionDirectionFromTrajectory =>
+            false;
+
         /// <summary>
         /// 获取轨迹完成后是否需要将舵轮主动恢复到车头方向。
         /// </summary>
@@ -698,7 +718,9 @@ namespace MultiWheelC
                     Trajectory = trajectory,
                     StateProvider = _stateProvider,
                     MotionDirectionInBodyRadians =
-                        MotionDirectionInBodyRadians,
+                        ResolveMotionDirectionFromTrajectory
+                            ? (double?)null
+                            : MotionDirectionInBodyRadians,
                     ReturnWheelsForwardAfterCompletion =
                         ReturnWheelsForwardAfterCompletion,
                     CycleObserver = controller =>
@@ -936,6 +958,12 @@ namespace MultiWheelC
         protected override double MotionDirectionInBodyRadians =>
             Math.PI / 4.0;
 
+        /// <summary>
+        /// 只用45°定义参考轨迹，底盘β由整段轨迹自动推导并检查一致性。
+        /// </summary>
+        protected override bool ResolveMotionDirectionFromTrajectory =>
+            true;
+
         /// <summary>
         /// 蟹行组合轨迹正常完成后主动将四个舵轮恢复到车头方向。
         /// </summary>

MultiWheelC/Fleet/FleetCoordinator.cs

@@ -2,7 +2,7 @@ using System;
 using System.Collections.Generic;
 using MultiWheelC.Trajectory;
 using MyParking.Shared;
-
+// 负责运动中：每周期计算每辆车的速度命令
 namespace MultiWheelC.Fleet
 {
     // 主车单周期车队协调结果，不表示通信或成员底盘执行结果。
@@ -173,6 +173,10 @@ namespace MultiWheelC.Fleet
 
         public string LastFailureReason { get; private set; }
 
+        // 运动前成员β准备必须与车队控制器采用同一个车队运动方向。
+        public double MotionDirectionInFleetRadians =>
+            _fleetController.MotionDirectionInFleetRadians;
+
         public void Start(
             FleetLayout layout,
             Trajectory2D trajectory)

MultiWheelC/Fleet/FleetMemberAgent.cs

@@ -30,6 +30,8 @@ namespace MultiWheelC.Fleet
         private readonly double _alignmentStableSeconds;
 
         private double _alignedDurationSeconds;
+        private double? _lastAcceptedCommandTimeSeconds;
+        private double? _commandDeadlineSeconds;
 
         /// <summary>创建绑定到一辆多舵轮底盘的成员车执行器。</summary>
         public FleetMemberAgent(
@@ -81,6 +83,13 @@ namespace MultiWheelC.Fleet
 
         public string LastFailureReason { get; private set; }
 
+        // 使用从车本机单调时钟记录，不依赖主车或Detour时间戳。
+        public double? LastAcceptedCommandTimeSeconds =>
+            _lastAcceptedCommandTimeSeconds;
+
+        public double? CommandDeadlineSeconds =>
+            _commandDeadlineSeconds;
+
         /// <summary>停车并开始准备本车固定β滚动运动系。</summary>
         public bool BeginRollingPreparation(
             long planId,
@@ -158,8 +167,18 @@ namespace MultiWheelC.Fleet
         }
 
         /// <summary>在主车确认全队Ready后激活本车已经准备好的运动方式。</summary>
-        public bool Activate(long planId)
+        public bool Activate(
+            long planId,
+            double commandReceivedTimeSeconds,
+            double validForSeconds)
         {
+            NumericGuard.EnsureFiniteNonNegative(
+                commandReceivedTimeSeconds,
+                nameof(commandReceivedTimeSeconds));
+            NumericGuard.EnsureFinitePositive(
+                validForSeconds,
+                nameof(validForSeconds));
+
             if (planId != CurrentPlanId)
             {
                 LastFailureReason =
@@ -169,7 +188,9 @@ namespace MultiWheelC.Fleet
 
             if (State == FleetMemberAgentState.Active)
             {
-                return true;
+                // 重复激活只允许幂等确认，不能替代周期运动命令延长车辆运动时间。
+                return UpdateCommandWatchdog(
+                    commandReceivedTimeSeconds);
             }
 
             if (State != FleetMemberAgentState.Ready ||
@@ -227,6 +248,9 @@ namespace MultiWheelC.Fleet
             }
 
             State = FleetMemberAgentState.Active;
+            AcceptCommandDeadline(
+                commandReceivedTimeSeconds,
+                validForSeconds);
             LastFailureReason = string.Empty;
             return true;
         }
@@ -235,12 +259,20 @@ namespace MultiWheelC.Fleet
         public bool Execute(
             long planId,
             FleetMemberCommand command,
+            double commandReceivedTimeSeconds,
+            double validForSeconds,
             TimeSpan? interval = null)
         {
             ValidatePlanId(planId);
             NumericGuard.EnsureFinite(
                 command.TwistInVehicleBody,
                 nameof(command));
+            NumericGuard.EnsureFiniteNonNegative(
+                commandReceivedTimeSeconds,
+                nameof(commandReceivedTimeSeconds));
+            NumericGuard.EnsureFinitePositive(
+                validForSeconds,
+                nameof(validForSeconds));
 
             if (planId != CurrentPlanId)
             {
@@ -262,6 +294,13 @@ namespace MultiWheelC.Fleet
                     "成员车尚未激活，不能执行速度命令。");
             }
 
+            // 先检查上一条命令是否已经过期，禁止失联后由迟到命令自动恢复运动。
+            if (!UpdateCommandWatchdog(
+                    commandReceivedTimeSeconds))
+            {
+                return false;
+            }
+
             if (!IsCommandCompatibleWithPreparation(
                     command.TwistInVehicleBody))
             {
@@ -289,10 +328,59 @@ namespace MultiWheelC.Fleet
                 return Fail(exception.Message);
             }
 
+            AcceptCommandDeadline(
+                commandReceivedTimeSeconds,
+                validForSeconds);
             LastFailureReason = string.Empty;
             return true;
         }
 
+        // 运行循环即使没有收到新命令也必须调用本方法，超时后会本地停车并锁存Faulted。
+        public bool UpdateCommandWatchdog(
+            double currentTimeSeconds)
+        {
+            NumericGuard.EnsureFiniteNonNegative(
+                currentTimeSeconds,
+                nameof(currentTimeSeconds));
+
+            if (State == FleetMemberAgentState.Faulted)
+            {
+                return false;
+            }
+
+            if (State != FleetMemberAgentState.Active)
+            {
+                return true;
+            }
+
+            if (!_lastAcceptedCommandTimeSeconds.HasValue ||
+                !_commandDeadlineSeconds.HasValue)
+            {
+                return Fail(
+                    "成员车已经激活，但本地命令看门狗尚未初始化。");
+            }
+
+            if (currentTimeSeconds <
+                _lastAcceptedCommandTimeSeconds.Value)
+            {
+                return Fail(
+                    "成员车本地单调时钟发生倒退，无法继续校验命令时效。");
+            }
+
+            if (currentTimeSeconds <=
+                _commandDeadlineSeconds.Value)
+            {
+                return true;
+            }
+
+            var commandAgeSeconds =
+                currentTimeSeconds -
+                _lastAcceptedCommandTimeSeconds.Value;
+            return Fail(
+                "成员车等待主车有效命令超时，" +
+                $"最近一次命令距今{commandAgeSeconds:F3}s。");
+        }
+
         /// <summary>正常取消当前任务并立即停止驱动轮。</summary>
         public void Stop()
         {
@@ -302,6 +390,7 @@ namespace MultiWheelC.Fleet
             CurrentPlanId = 0;
             MotionDirectionInBodyRadians = 0.0;
             _alignedDurationSeconds = 0.0;
+            ClearCommandWatchdog();
             LastFailureReason = string.Empty;
         }
 
@@ -323,6 +412,7 @@ namespace MultiWheelC.Fleet
                 State = FleetMemberAgentState.Preparing;
                 LastFailureReason = string.Empty;
                 _alignedDurationSeconds = 0.0;
+                ClearCommandWatchdog();
 
                 var accepted = mode ==
                     FleetMemberPreparationMode.Rolling
@@ -423,6 +513,29 @@ namespace MultiWheelC.Fleet
             return false;
         }
 
+        private void AcceptCommandDeadline(
+            double commandReceivedTimeSeconds,
+            double validForSeconds)
+        {
+            var commandDeadlineSeconds =
+                commandReceivedTimeSeconds +
+                validForSeconds;
+            NumericGuard.EnsureFinite(
+                commandDeadlineSeconds,
+                nameof(validForSeconds));
+
+            _lastAcceptedCommandTimeSeconds =
+                commandReceivedTimeSeconds;
+            _commandDeadlineSeconds =
+                commandDeadlineSeconds;
+        }
+
+        private void ClearCommandWatchdog()
+        {
+            _lastAcceptedCommandTimeSeconds = null;
+            _commandDeadlineSeconds = null;
+        }
+
         /// <summary>锁存成员车故障并立即清零驱动轮速度。</summary>
         private bool Fail(string reason)
         {

MultiWheelC/Fleet/FleetPreparationCoordinator.cs

@@ -2,6 +2,7 @@ using System;
 using System.Collections.Generic;
 using System.Collections.ObjectModel;
 using MyParking.Shared;
+// 负责运动前：所有车辆舵轮是否准备完成
 
 namespace MultiWheelC.Fleet
 {

MultiWheelC/Fleet/FleetSafetySupervisor.cs

@@ -0,0 +1,286 @@
+using System;
+using System.Collections.Generic;
+using MyParking.Shared;
+
+namespace MultiWheelC.Fleet
+{
+    // 主车已经接收并接受的一辆成员车安全状态。
+    public readonly struct FleetMemberSafetyStatus
+    {
+        public FleetMemberSafetyStatus(
+            int vehicleId,
+            long planId,
+            bool isStateAvailable,
+            bool isFaulted,
+            int failureCode,
+            double lastAcceptedReportTimeSeconds)
+        {
+            if (vehicleId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(vehicleId),
+                    "成员车编号必须大于零。");
+            }
+
+            if (planId < 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(planId),
+                    "任务编号不能为负数。");
+            }
+
+            NumericGuard.EnsureFiniteNonNegative(
+                lastAcceptedReportTimeSeconds,
+                nameof(lastAcceptedReportTimeSeconds));
+
+            VehicleId = vehicleId;
+            PlanId = planId;
+            IsStateAvailable = isStateAvailable;
+            IsFaulted = isFaulted;
+            FailureCode = failureCode;
+            LastAcceptedReportTimeSeconds =
+                lastAcceptedReportTimeSeconds;
+        }
+
+        public int VehicleId { get; }
+
+        public long PlanId { get; }
+
+        public bool IsStateAvailable { get; }
+
+        public bool IsFaulted { get; }
+
+        // 零表示成员车没有报告结构化故障。
+        public int FailureCode { get; }
+
+        // 使用主车本地单调时钟，不能直接填写从车上传的时间戳。
+        public double LastAcceptedReportTimeSeconds { get; }
+    }
+
+    // 一次安全检查的结果；ShouldStop可直接作为是否停车的判断标识。
+    public readonly struct FleetSafetyDecision
+    {
+        internal FleetSafetyDecision(
+            bool shouldStop,
+            int sourceVehicleId,
+            string reason)
+        {
+            ShouldStop = shouldStop;
+            SourceVehicleId = sourceVehicleId;
+            Reason = reason ?? string.Empty;
+        }
+
+        public bool ShouldStop { get; }
+
+        // 零表示原因属于整个车队，而不是某一辆成员车。
+        public int SourceVehicleId { get; }
+
+        public string Reason { get; }
+    }
+
+    // 检查成员通信和健康状态，并锁存需要整队停车的首个原因。
+    public sealed class FleetSafetySupervisor
+    {
+        private readonly double _communicationTimeoutSeconds;
+
+        private long _activePlanId;
+        private FleetSafetyDecision _latchedDecision;
+
+        public FleetSafetySupervisor(
+            double communicationTimeoutSeconds)
+        {
+            NumericGuard.EnsureFinitePositive(
+                communicationTimeoutSeconds,
+                nameof(communicationTimeoutSeconds));
+
+            _communicationTimeoutSeconds =
+                communicationTimeoutSeconds;
+            Reset();
+        }
+
+        public double CommunicationTimeoutSeconds =>
+            _communicationTimeoutSeconds;
+
+        public long ActivePlanId => _activePlanId;
+
+        public bool IsActive => _activePlanId > 0;
+
+        public bool IsStopLatched =>
+            _latchedDecision.ShouldStop;
+
+        public FleetSafetyDecision LastDecision =>
+            _latchedDecision;
+
+        // 开始一次新任务，同时清除上一任务留下的停车锁存。
+        public void Start(long planId)
+        {
+            if (planId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(planId),
+                    "活动任务编号必须大于零。");
+            }
+
+            _activePlanId = planId;
+            _latchedDecision = CreateContinueDecision();
+        }
+
+        // 返回ShouldStop；本类只负责判定，实际停车由后续运行入口执行。
+        public FleetSafetyDecision Evaluate(
+            FleetLayout layout,
+            IReadOnlyList<FleetMemberSafetyStatus> memberStatuses,
+            double currentTimeSeconds)
+        {
+            if (layout == null)
+            {
+                throw new ArgumentNullException(nameof(layout));
+            }
+
+            if (memberStatuses == null)
+            {
+                throw new ArgumentNullException(
+                    nameof(memberStatuses));
+            }
+
+            NumericGuard.EnsureFiniteNonNegative(
+                currentTimeSeconds,
+                nameof(currentTimeSeconds));
+
+            if (!IsActive)
+            {
+                return new FleetSafetyDecision(
+                    true,
+                    0,
+                    "车队安全监督器尚未启动活动任务。");
+            }
+
+            if (IsStopLatched)
+            {
+                return _latchedDecision;
+            }
+
+            var statusesByVehicleId =
+                new Dictionary<int, FleetMemberSafetyStatus>();
+
+            for (var index = 0;
+                 index < memberStatuses.Count;
+                 index++)
+            {
+                var status = memberStatuses[index];
+
+                // 非当前编队成员的状态不参与本次任务安全判定。
+                if (!layout.TryGetVehicle(
+                        status.VehicleId,
+                        out _))
+                {
+                    continue;
+                }
+
+                if (statusesByVehicleId.ContainsKey(
+                        status.VehicleId))
+                {
+                    return LatchStop(
+                        status.VehicleId,
+                        $"成员车{status.VehicleId}存在重复状态报告。");
+                }
+
+                statusesByVehicleId.Add(
+                    status.VehicleId,
+                    status);
+            }
+
+            for (var index = 0;
+                 index < layout.Vehicles.Count;
+                 index++)
+            {
+                var vehicleId =
+                    layout.Vehicles[index].VehicleId;
+
+                if (!statusesByVehicleId.TryGetValue(
+                        vehicleId,
+                        out var status))
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"未收到成员车{vehicleId}的状态报告。");
+                }
+
+                if (status.PlanId != _activePlanId)
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"成员车{vehicleId}报告的任务编号" +
+                        $"{status.PlanId}与当前任务" +
+                        $"{_activePlanId}不一致。");
+                }
+
+                if (status.LastAcceptedReportTimeSeconds >
+                    currentTimeSeconds)
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"成员车{vehicleId}的主车接收时间晚于当前时间。");
+                }
+
+                var reportAgeSeconds =
+                    currentTimeSeconds -
+                    status.LastAcceptedReportTimeSeconds;
+                if (reportAgeSeconds >
+                    _communicationTimeoutSeconds)
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"成员车{vehicleId}通信超时，" +
+                        $"最近有效报告距今" +
+                        $"{reportAgeSeconds:F3}s。");
+                }
+
+                if (status.IsFaulted ||
+                    status.FailureCode != 0)
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"成员车{vehicleId}报告故障，" +
+                        $"故障码为{status.FailureCode}。");
+                }
+
+                if (!status.IsStateAvailable)
+                {
+                    return LatchStop(
+                        vehicleId,
+                        $"成员车{vehicleId}状态不可用。");
+                }
+            }
+
+            _latchedDecision = CreateContinueDecision();
+            return _latchedDecision;
+        }
+
+        // 结束当前任务并清除锁存；未开始新任务前Evaluate仍会要求停车。
+        public void Reset()
+        {
+            _activePlanId = 0;
+            _latchedDecision = CreateContinueDecision();
+        }
+
+        private FleetSafetyDecision LatchStop(
+            int sourceVehicleId,
+            string reason)
+        {
+            _latchedDecision = new FleetSafetyDecision(
+                true,
+                sourceVehicleId,
+                reason);
+            return _latchedDecision;
+        }
+
+        private static FleetSafetyDecision
+            CreateContinueDecision()
+        {
+            return new FleetSafetyDecision(
+                false,
+                0,
+                string.Empty);
+        }
+    }
+}

MultiWheelC/Fleet/IFleetTransport.cs

@@ -0,0 +1,16 @@
+using MyParking.Shared;
+
+namespace MultiWheelC.Fleet
+{
+    // 隔离车队运行逻辑与具体无线、串口或内存传输实现。
+    public interface IFleetTransport
+    {
+        void SendCommand(FleetCommand command);
+
+        void SendReport(FleetMemberReport report);
+
+        bool TryReceiveCommand(out FleetCommand command);
+
+        bool TryReceiveReport(out FleetMemberReport report);
+    }
+}

MultiWheelC/Movements/TrajectoryTrackingMovement.cs

@@ -21,6 +21,11 @@ namespace MultiWheelC
     public sealed class TrajectoryTrackingMovement
         : MovementDefinition
     {
+        private const double ReferenceSpeedDeadbandMetersPerSecond =
+            1e-6;
+        private const double FixedMotionDirectionToleranceRadians =
+            3.0 * Math.PI / 180.0;
+
         /// <summary>
         /// 获取或设置本次动作需要跟踪的世界坐标系轨迹。
         /// </summary>
@@ -43,9 +48,18 @@ namespace MultiWheelC
         public Action<ParkingGeometricController> CycleObserver;
 
         /// <summary>
-        /// 获取或设置本动作运动坐标系X轴在车体系中的方向，单位为rad；0表示车头方向。
+        /// 获取或设置本动作运动坐标系X轴在车体系中的方向，单位为rad；为空时从轨迹自动推导。
         /// </summary>
-        public double MotionDirectionInBodyRadians;
+        public double? MotionDirectionInBodyRadians = 0.0;
+
+        /// <summary>
+        /// 获取本次执行最终采用的运动坐标系方向，动作尚未开始时为空。
+        /// </summary>
+        public double? ResolvedMotionDirectionInBodyRadians
+        {
+            get;
+            private set;
+        }
 
         /// <summary>
         /// 获取或设置轨迹正常完成后是否停车并将舵轮主动恢复到车头方向。
@@ -270,6 +284,12 @@ namespace MultiWheelC
                 config.ParkingExecutionTimeoutSeconds;
 
             ValidateParameters(executionTimeoutSeconds);
+            var motionDirectionInBodyRadians =
+                MotionDirectionInBodyRadians ??
+                ResolveFixedMotionDirectionInBodyRadians(
+                    Trajectory);
+            ResolvedMotionDirectionInBodyRadians =
+                motionDirectionInBodyRadians;
 
             var chassis =
                 PilotDefinition.Chassis as MultiWheelChassis;
@@ -283,7 +303,7 @@ namespace MultiWheelC
                 new PrepareWheelsForward
                 {
                     DirectionRadians =
-                        MotionDirectionInBodyRadians
+                        motionDirectionInBodyRadians
                 };
             foreach (var keepRunning in wheelPreparation.Get())
             {
@@ -306,7 +326,7 @@ namespace MultiWheelC
                 PilotDefinition.Self.CarNum);
 
             adapter.ActivateMotionFrame(
-                MotionDirectionInBodyRadians);
+                motionDirectionInBodyRadians);
 
             var stateProvider =
                 StateProvider ??
@@ -348,7 +368,7 @@ namespace MultiWheelC
                 new GcpCommandExecutor(
                     adapter,
                     maximumGcpAngleRateRadiansPerSecond,
-                    MotionDirectionInBodyRadians);
+                    motionDirectionInBodyRadians);
 
             Controller = new ParkingGeometricController(
                 stateProvider,
@@ -366,7 +386,7 @@ namespace MultiWheelC
                 maximumTerminalApproachSpeedMetersPerSecond,
                 stanleyCurvaturePreviewSeconds,
                 stanleyMaximumCurvaturePreviewMeters,
-                MotionDirectionInBodyRadians);
+                motionDirectionInBodyRadians);
 
             var clock = Stopwatch.StartNew();
             var previousCycleSeconds =
@@ -478,9 +498,12 @@ namespace MultiWheelC
                     "新版轨迹跟踪动作没有设置Trajectory。");
             }
 
-            NumericGuard.EnsureFinite(
-                MotionDirectionInBodyRadians,
-                nameof(MotionDirectionInBodyRadians));
+            if (MotionDirectionInBodyRadians.HasValue)
+            {
+                NumericGuard.EnsureFinite(
+                    MotionDirectionInBodyRadians.Value,
+                    nameof(MotionDirectionInBodyRadians));
+            }
 
             if (double.IsNaN(executionTimeoutSeconds) ||
                 double.IsInfinity(executionTimeoutSeconds) ||
@@ -491,5 +514,114 @@ namespace MultiWheelC
                     "轨迹跟踪超时时间必须是正有限值。");
             }
         }
+
+        /// <summary>
+        /// 根据轨迹切线、参考车身航向和速度符号推导整段轨迹共同使用的固定运动方向。
+        /// </summary>
+        private static double ResolveFixedMotionDirectionInBodyRadians(
+            Trajectory2D trajectory)
+        {
+            double? resolvedDirectionRadians = null;
+
+            for (var index = 0;
+                 index < trajectory.Count - 1;
+                 index++)
+            {
+                var segmentStart = trajectory[index];
+                var segmentEnd = trajectory[index + 1];
+                var travelDirection = ResolveSegmentTravelDirection(
+                    segmentStart.ReferenceSpeedMetersPerSecond,
+                    segmentEnd.ReferenceSpeedMetersPerSecond,
+                    index);
+                if (travelDirection == 0.0)
+                {
+                    continue;
+                }
+
+                var tangentYawRadians = Math.Atan2(
+                    segmentEnd.PoseInWorld.YMeters -
+                        segmentStart.PoseInWorld.YMeters,
+                    segmentEnd.PoseInWorld.XMeters -
+                        segmentStart.PoseInWorld.XMeters);
+                var positiveMotionAxisYawRadians =
+                    travelDirection > 0.0
+                        ? tangentYawRadians
+                        : AngleMath.NormalizeRadians(
+                            tangentYawRadians + Math.PI);
+                var referenceBodyYawRadians =
+                    AngleMath.LerpRadians(
+                        segmentStart.PoseInWorld.YawRadians,
+                        segmentEnd.PoseInWorld.YawRadians,
+                        0.5);
+                var candidateDirectionRadians =
+                    AngleMath.ShortestDifferenceRadians(
+                        positiveMotionAxisYawRadians,
+                        referenceBodyYawRadians);
+
+                if (!resolvedDirectionRadians.HasValue)
+                {
+                    resolvedDirectionRadians =
+                        candidateDirectionRadians;
+                    continue;
+                }
+
+                var directionDifferenceRadians = Math.Abs(
+                    AngleMath.ShortestDifferenceRadians(
+                        candidateDirectionRadians,
+                        resolvedDirectionRadians.Value));
+                if (directionDifferenceRadians >
+                    FixedMotionDirectionToleranceRadians)
+                {
+                    throw new InvalidOperationException(
+                        "轨迹无法由一个固定运动坐标系执行：" +
+                        $"第{index + 1}段需要的方向与起始方向相差" +
+                        $"{AngleMath.RadiansToDegrees(directionDifferenceRadians):F2}°。" +
+                        "请拆分轨迹，或显式指定并验证MotionDirectionInBodyRadians。");
+                }
+            }
+
+            if (!resolvedDirectionRadians.HasValue)
+            {
+                throw new InvalidOperationException(
+                    "轨迹没有非零参考速度线段，无法自动确定运动坐标系方向。");
+            }
+
+            return resolvedDirectionRadians.Value;
+        }
+
+        /// <summary>
+        /// 从相邻轨迹点的有符号参考速度确定该线段的执行方向。
+        /// </summary>
+        private static double ResolveSegmentTravelDirection(
+            double startSpeedMetersPerSecond,
+            double endSpeedMetersPerSecond,
+            int segmentStartIndex)
+        {
+            var hasStartDirection =
+                Math.Abs(startSpeedMetersPerSecond) >
+                ReferenceSpeedDeadbandMetersPerSecond;
+            var hasEndDirection =
+                Math.Abs(endSpeedMetersPerSecond) >
+                ReferenceSpeedDeadbandMetersPerSecond;
+
+            if (hasStartDirection &&
+                hasEndDirection &&
+                Math.Sign(startSpeedMetersPerSecond) !=
+                Math.Sign(endSpeedMetersPerSecond))
+            {
+                throw new InvalidOperationException(
+                    $"轨迹第{segmentStartIndex + 1}段内参考速度发生正负切换，" +
+                    "无法自动确定固定运动坐标系；请在零速点拆分动作段。");
+            }
+
+            if (hasStartDirection)
+            {
+                return Math.Sign(startSpeedMetersPerSecond);
+            }
+
+            return hasEndDirection
+                ? Math.Sign(endSpeedMetersPerSecond)
+                : 0.0;
+        }
     }
 }