打通新版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
@@ -2,6 +2,7 @@ using System;
 using System.Collections.Generic;
 using System.Collections.ObjectModel;
 using System.Linq;
+using MyParking.Shared;

 namespace MultiWheelC.Trajectory
 {
@@ -114,6 +115,93 @@ namespace MultiWheelC.Trajectory
            return TotalLengthMeters - arcLengthMeters;
        }

+        /// <summary>
+        /// 按累计弧长在线性位置、航向、曲率和参考速度之间插值得到轨迹点。
+        /// </summary>
+        public TrajectoryPoint SampleAtArcLength(
+            double arcLengthMeters)
+        {
+            EnsureFinite(
+                arcLengthMeters,
+                nameof(arcLengthMeters));
+
+            if (arcLengthMeters <= 0.0)
+            {
+                return StartPoint;
+            }
+
+            if (arcLengthMeters >= TotalLengthMeters)
+            {
+                return EndPoint;
+            }
+
+            var segmentStartIndex =
+                FindSegmentStartIndex(
+                    arcLengthMeters);
+            var segmentStart =
+                _points[segmentStartIndex];
+            var segmentEnd =
+                _points[segmentStartIndex + 1];
+            var interpolationRatio =
+                (arcLengthMeters -
+                 segmentStart.ArcLengthMeters) /
+                (segmentEnd.ArcLengthMeters -
+                 segmentStart.ArcLengthMeters);
+
+            return new TrajectoryPoint(
+                arcLengthMeters,
+                new Pose2D(
+                    InterpolationMath.Lerp(
+                        segmentStart.PoseInWorld.XMeters,
+                        segmentEnd.PoseInWorld.XMeters,
+                        interpolationRatio),
+                    InterpolationMath.Lerp(
+                        segmentStart.PoseInWorld.YMeters,
+                        segmentEnd.PoseInWorld.YMeters,
+                        interpolationRatio),
+                    AngleMath.LerpRadians(
+                        segmentStart.PoseInWorld.YawRadians,
+                        segmentEnd.PoseInWorld.YawRadians,
+                        interpolationRatio)),
+                InterpolationMath.Lerp(
+                    segmentStart.CurvaturePerMeter,
+                    segmentEnd.CurvaturePerMeter,
+                    interpolationRatio),
+                InterpolationMath.Lerp(
+                    segmentStart.ReferenceSpeedMetersPerSecond,
+                    segmentEnd.ReferenceSpeedMetersPerSecond,
+                    interpolationRatio));
+        }
+
+        /// <summary>
+        /// 使用二分查找获取包含指定累计弧长的线段起点索引。
+        /// </summary>
+        private int FindSegmentStartIndex(
+            double arcLengthMeters)
+        {
+            var lowerIndex = 0;
+            var upperIndex = _points.Length - 1;
+
+            while (upperIndex - lowerIndex > 1)
+            {
+                var middleIndex =
+                    lowerIndex +
+                    (upperIndex - lowerIndex) / 2;
+
+                if (_points[middleIndex].ArcLengthMeters <=
+                    arcLengthMeters)
+                {
+                    lowerIndex = middleIndex;
+                }
+                else
+                {
+                    upperIndex = middleIndex;
+                }
+            }
+
+            return lowerIndex;
+        }
+
        /// <summary>
        /// 检查相邻轨迹点是否构成有效的非零长度有序线段。
        /// </summary>