拆分MultiWheelC并新增轨迹投影、Detour状态估计与Stanley跟踪控制

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-05 18:07:01 +08:00
parent 7447317812
commit 47973cc94b
38 changed files with 3982 additions and 976 deletions
@@ -0,0 +1,219 @@
+using System;
+using MyParking.Shared;
+
+namespace MultiWheelC.Trajectory
+{
+    /// <summary>
+    /// 将Detour给出的实际车体中心位姿投影到二维离散参考轨迹。
+    /// </summary>
+    public static class TrajectoryProjector
+    {
+        /// <summary>
+        /// 在整条轨迹上查找距离实际车体中心最近的线段投影结果。
+        /// </summary>
+        public static TrajectoryProjection Project(
+            Trajectory2D trajectory,
+            Pose2D vehiclePoseInWorld)
+        {
+            if (trajectory == null)
+            {
+                throw new ArgumentNullException(
+                    nameof(trajectory));
+            }
+
+            EnsureFinitePose(
+                vehiclePoseInWorld,
+                nameof(vehiclePoseInWorld));
+
+            var bestSegmentStartIndex = 0;
+            var bestInterpolationRatio = 0.0;
+            var bestProjectedX = 0.0;
+            var bestProjectedY = 0.0;
+            var bestDistanceSquared =
+                double.PositiveInfinity;
+
+            for (var segmentStartIndex = 0;
+                 segmentStartIndex < trajectory.Count - 1;
+                 segmentStartIndex++)
+            {
+                var segmentStart =
+                    trajectory[segmentStartIndex];
+                var segmentEnd =
+                    trajectory[segmentStartIndex + 1];
+
+                var segmentX =
+                    segmentEnd.PoseInWorld.XMeters -
+                    segmentStart.PoseInWorld.XMeters;
+                var segmentY =
+                    segmentEnd.PoseInWorld.YMeters -
+                    segmentStart.PoseInWorld.YMeters;
+                var segmentLengthSquared =
+                    segmentX * segmentX +
+                    segmentY * segmentY;
+
+                var vehicleFromSegmentStartX =
+                    vehiclePoseInWorld.XMeters -
+                    segmentStart.PoseInWorld.XMeters;
+                var vehicleFromSegmentStartY =
+                    vehiclePoseInWorld.YMeters -
+                    segmentStart.PoseInWorld.YMeters;
+
+                var interpolationRatio =
+                    InterpolationMath.Clamp01(
+                        (vehicleFromSegmentStartX * segmentX +
+                         vehicleFromSegmentStartY * segmentY) /
+                        segmentLengthSquared);
+
+                var projectedX =
+                    InterpolationMath.Lerp(
+                        segmentStart.PoseInWorld.XMeters,
+                        segmentEnd.PoseInWorld.XMeters,
+                        interpolationRatio);
+                var projectedY =
+                    InterpolationMath.Lerp(
+                        segmentStart.PoseInWorld.YMeters,
+                        segmentEnd.PoseInWorld.YMeters,
+                        interpolationRatio);
+
+                var projectionErrorX =
+                    projectedX -
+                    vehiclePoseInWorld.XMeters;
+                var projectionErrorY =
+                    projectedY -
+                    vehiclePoseInWorld.YMeters;
+                var distanceSquared =
+                    projectionErrorX * projectionErrorX +
+                    projectionErrorY * projectionErrorY;
+
+                if (distanceSquared >= bestDistanceSquared)
+                {
+                    continue;
+                }
+
+                bestSegmentStartIndex =
+                    segmentStartIndex;
+                bestInterpolationRatio =
+                    interpolationRatio;
+                bestProjectedX = projectedX;
+                bestProjectedY = projectedY;
+                bestDistanceSquared = distanceSquared;
+            }
+
+            return BuildProjection(
+                trajectory,
+                vehiclePoseInWorld,
+                bestSegmentStartIndex,
+                bestInterpolationRatio,
+                bestProjectedX,
+                bestProjectedY,
+                bestDistanceSquared);
+        }
+
+        /// <summary>
+        /// 根据最近线段和插值比例生成控制器使用的完整投影结果。
+        /// </summary>
+        private static TrajectoryProjection BuildProjection(
+            Trajectory2D trajectory,
+            Pose2D vehiclePoseInWorld,
+            int segmentStartIndex,
+            double interpolationRatio,
+            double projectedX,
+            double projectedY,
+            double distanceSquared)
+        {
+            var segmentStart =
+                trajectory[segmentStartIndex];
+            var segmentEnd =
+                trajectory[segmentStartIndex + 1];
+
+            var referenceYawRadians =
+                AngleMath.LerpRadians(
+                    segmentStart.PoseInWorld.YawRadians,
+                    segmentEnd.PoseInWorld.YawRadians,
+                    interpolationRatio);
+            var referenceArcLengthMeters =
+                InterpolationMath.Lerp(
+                    segmentStart.ArcLengthMeters,
+                    segmentEnd.ArcLengthMeters,
+                    interpolationRatio);
+            var referenceCurvaturePerMeter =
+                InterpolationMath.Lerp(
+                    segmentStart.CurvaturePerMeter,
+                    segmentEnd.CurvaturePerMeter,
+                    interpolationRatio);
+            var referenceSpeedMetersPerSecond =
+                InterpolationMath.Lerp(
+                    segmentStart.ReferenceSpeedMetersPerSecond,
+                    segmentEnd.ReferenceSpeedMetersPerSecond,
+                    interpolationRatio);
+
+            var referencePoint =
+                new TrajectoryPoint(
+                    referenceArcLengthMeters,
+                    new Pose2D(
+                        projectedX,
+                        projectedY,
+                        referenceYawRadians),
+                    referenceCurvaturePerMeter,
+                    referenceSpeedMetersPerSecond);
+
+            var segmentX =
+                segmentEnd.PoseInWorld.XMeters -
+                segmentStart.PoseInWorld.XMeters;
+            var segmentY =
+                segmentEnd.PoseInWorld.YMeters -
+                segmentStart.PoseInWorld.YMeters;
+            var segmentLength =
+                Math.Sqrt(
+                    segmentX * segmentX +
+                    segmentY * segmentY);
+
+            // 以轨迹线段的前进方向判断左右：
+            // 从车辆指向参考轨迹的向量位于轨迹左侧时为正。
+            var vehicleToProjectionX =
+                projectedX -
+                vehiclePoseInWorld.XMeters;
+            var vehicleToProjectionY =
+                projectedY -
+                vehiclePoseInWorld.YMeters;
+            var lateralErrorMeters =
+                (segmentX * vehicleToProjectionY -
+                 segmentY * vehicleToProjectionX) /
+                segmentLength;
+
+            var headingErrorRadians =
+                AngleMath.ShortestDifferenceRadians(
+                    referenceYawRadians,
+                    vehiclePoseInWorld.YawRadians);
+
+            return new TrajectoryProjection(
+                segmentStartIndex,
+                referencePoint,
+                lateralErrorMeters,
+                headingErrorRadians,
+                Math.Sqrt(distanceSquared),
+                trajectory.GetRemainingDistanceMeters(
+                    referenceArcLengthMeters));
+        }
+
+        /// <summary>
+        /// 检查用于投影的实际车体中心位姿是否包含有限数值。
+        /// </summary>
+        private static void EnsureFinitePose(
+            Pose2D pose,
+            string parameterName)
+        {
+            if (double.IsNaN(pose.XMeters) ||
+                double.IsInfinity(pose.XMeters) ||
+                double.IsNaN(pose.YMeters) ||
+                double.IsInfinity(pose.YMeters) ||
+                double.IsNaN(pose.YawRadians) ||
+                double.IsInfinity(pose.YawRadians))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "用于轨迹投影的车体位姿必须是有限值。");
+            }
+        }
+    }
+}