打通新版Stanley轨迹跟踪闭环并补充实验测试与数据分析脚本

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-06 15:14:12 +08:00
parent 47973cc94b
commit 19b1e49189
27 changed files with 3482 additions and 22 deletions
@@ -0,0 +1,163 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 为新版控制器实验生成不依赖正式规划层的简单世界坐标系参考轨迹。
+    /// </summary>
+    public static class TestTrajectoryFactory
+    {
+        private const double StraightLengthMeters = 4.0;
+
+        /// <summary>
+        /// 从给定车体中心位姿沿当前航向生成带梯形速度规划的4m直线轨迹。
+        /// </summary>
+        public static Trajectory2D CreateStraight4Meters(
+            Pose2D startPoseInWorld,
+            double cruiseSpeedMetersPerSecond = 0.30,
+            double accelerationMetersPerSecondSquared = 0.20,
+            double decelerationMetersPerSecondSquared = 0.20,
+            double pointSpacingMeters = 0.02)
+        {
+            EnsureFinitePose(
+                startPoseInWorld,
+                nameof(startPoseInWorld));
+            EnsureFinitePositive(
+                cruiseSpeedMetersPerSecond,
+                nameof(cruiseSpeedMetersPerSecond));
+            EnsureFinitePositive(
+                accelerationMetersPerSecondSquared,
+                nameof(accelerationMetersPerSecondSquared));
+            EnsureFinitePositive(
+                decelerationMetersPerSecondSquared,
+                nameof(decelerationMetersPerSecondSquared));
+            EnsureFinitePositive(
+                pointSpacingMeters,
+                nameof(pointSpacingMeters));
+
+            if (pointSpacingMeters > StraightLengthMeters)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(pointSpacingMeters),
+                    "直线轨迹点间距不能大于轨迹总长度。");
+            }
+
+            var segmentCount = (int)Math.Ceiling(
+                StraightLengthMeters /
+                pointSpacingMeters);
+            var points = new List<TrajectoryPoint>(
+                segmentCount + 1);
+            var directionX = Math.Cos(
+                startPoseInWorld.YawRadians);
+            var directionY = Math.Sin(
+                startPoseInWorld.YawRadians);
+
+            for (var index = 0;
+                 index <= segmentCount;
+                 index++)
+            {
+                // 均分后最后一个点严格落在4m终点，避免浮点累加越界。
+                var arcLengthMeters =
+                    StraightLengthMeters *
+                    index /
+                    segmentCount;
+                var remainingDistanceMeters =
+                    StraightLengthMeters -
+                    arcLengthMeters;
+                var referenceSpeedMetersPerSecond =
+                    CalculateReferenceSpeed(
+                        arcLengthMeters,
+                        remainingDistanceMeters,
+                        cruiseSpeedMetersPerSecond,
+                        accelerationMetersPerSecondSquared,
+                        decelerationMetersPerSecondSquared);
+
+                points.Add(
+                    new TrajectoryPoint(
+                        arcLengthMeters,
+                        new Pose2D(
+                            startPoseInWorld.XMeters +
+                                directionX * arcLengthMeters,
+                            startPoseInWorld.YMeters +
+                                directionY * arcLengthMeters,
+                            startPoseInWorld.YawRadians),
+                        curvaturePerMeter: 0.0,
+                        referenceSpeedMetersPerSecond:
+                            referenceSpeedMetersPerSecond));
+            }
+
+            return new Trajectory2D(points);
+        }
+
+        /// <summary>
+        /// 根据起步、巡航和制动能力计算指定弧长位置允许的参考速度。
+        /// </summary>
+        private static double CalculateReferenceSpeed(
+            double arcLengthMeters,
+            double remainingDistanceMeters,
+            double cruiseSpeedMetersPerSecond,
+            double accelerationMetersPerSecondSquared,
+            double decelerationMetersPerSecondSquared)
+        {
+            // 由v²=2as分别得到从静止起步和到终点静止允许的速度上限。
+            var accelerationLimitedSpeed = Math.Sqrt(
+                2.0 *
+                accelerationMetersPerSecondSquared *
+                Math.Max(0.0, arcLengthMeters));
+            var brakingLimitedSpeed = Math.Sqrt(
+                2.0 *
+                decelerationMetersPerSecondSquared *
+                Math.Max(0.0, remainingDistanceMeters));
+
+            return Math.Min(
+                cruiseSpeedMetersPerSecond,
+                Math.Min(
+                    accelerationLimitedSpeed,
+                    brakingLimitedSpeed));
+        }
+
+        /// <summary>
+        /// 检查世界坐标系起点位姿是否全部为有限值。
+        /// </summary>
+        private static void EnsureFinitePose(
+            Pose2D pose,
+            string parameterName)
+        {
+            if (!IsFinite(pose.XMeters) ||
+                !IsFinite(pose.YMeters) ||
+                !IsFinite(pose.YawRadians))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "直线测试轨迹的起点位姿必须由有限值组成。");
+            }
+        }
+
+        /// <summary>
+        /// 检查测试轨迹参数是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            if (!IsFinite(value) || value <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "直线测试轨迹的速度、加速度和点间距必须是正有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 判断数值是否可用于轨迹计算。
+        /// </summary>
+        private static bool IsFinite(double value)
+        {
+            return !double.IsNaN(value) &&
+                   !double.IsInfinity(value);
+        }
+    }
+}