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

2026-08-25 17:59:18 +08:00
parent 95c0b19a26
commit 17092c2766
37 changed files with 3632 additions and 37 deletions
@@ -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;
+        }
    }
 }