fix: unify path smoothing geometry metrics

2026-07-30 16:39:08 +08:00
parent d7ceb761b7
commit d6b34f88ce
16 changed files with 259 additions and 115 deletions
@@ -111,7 +111,8 @@ public sealed class ArcLengthResampler
            segment.Direction,
            points,
            segment.StartsAtGearSwitch,
-            segment.EndsAtGearSwitch);
+            segment.EndsAtGearSwitch,
+            segment.StartVehicleCurvaturePerMeter);
        return true;
    }

@@ -13,6 +13,7 @@ public sealed class PathGeometryAnalysis
        IReadOnlyList<SmoothedPathSegment> segments,
        double pathLengthMeters,
        double maximumAbsoluteVehicleCurvaturePerMeter,
+        double maximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
        double rootMeanSquareVehicleCurvaturePerMeter,
        double totalAbsoluteCurvatureVariationPerMeter,
        double curvatureVariationEnergy,
@@ -22,6 +23,7 @@ public sealed class PathGeometryAnalysis
        Segments = CopyReadOnly(segments);
        PathLengthMeters = pathLengthMeters;
        MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
+        MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter = maximumAbsoluteVehicleCurvatureDerivativePerSquareMeter;
        RootMeanSquareVehicleCurvaturePerMeter = rootMeanSquareVehicleCurvaturePerMeter;
        TotalAbsoluteCurvatureVariationPerMeter = totalAbsoluteCurvatureVariationPerMeter;
        CurvatureVariationEnergy = curvatureVariationEnergy;
@@ -40,15 +42,21 @@ public sealed class PathGeometryAnalysis
    /// <summary>车辆曲率绝对值峰值，单位 1/m。</summary>
    public double MaximumAbsoluteVehicleCurvaturePerMeter { get; }

+    /// <summary>车辆曲率导数绝对值峰值，单位 1/m²。</summary>
+    public double MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter { get; }
+
    /// <summary>车辆曲率均方根，单位 1/m。</summary>
    public double RootMeanSquareVehicleCurvaturePerMeter { get; }

    /// <summary>不跨换向点累计的绝对曲率变化，单位 1/m。</summary>
    public double TotalAbsoluteCurvatureVariationPerMeter { get; }

-    /// <summary>不跨换向点累计的曲率变化能量。</summary>
+    /// <summary>不跨换向点累计的曲率变化代价。</summary>
    public double CurvatureVariationEnergy { get; }

+    /// <summary>曲率变化代价的面向用户名称；保留 <see cref="CurvatureVariationEnergy"/> 以兼容既有调用方。</summary>
+    public double CurvatureVariationCost => CurvatureVariationEnergy;
+
    /// <summary>输入点携带的最小保守净空，单位 m。</summary>
    public double MinimumBodyClearanceMeters { get; }

@@ -37,6 +37,7 @@ public sealed class PathGeometryAnalyzer
        double previousOutputUnwrappedHeading = 0d;
        bool hasPreviousOutputHeading = false;
        double maximumAbsoluteVehicleCurvature = 0d;
+        double maximumAbsoluteVehicleCurvatureDerivative = 0d;
        double curvatureSquareSum = 0d;
        int curvatureSampleCount = 0;
        double totalCurvatureVariation = 0d;
@@ -61,6 +62,7 @@ public sealed class PathGeometryAnalyzer
                    ref hasPreviousOutputHeading,
                    outputPath,
                    out double segmentMaximumCurvature,
+                    out double segmentMaximumCurvatureDerivative,
                    out double segmentCurvatureSquareSum,
                    out int segmentCurvatureSampleCount,
                    out double segmentVariation,
@@ -72,6 +74,9 @@ public sealed class PathGeometryAnalyzer
            }

            maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, segmentMaximumCurvature);
+            maximumAbsoluteVehicleCurvatureDerivative = Math.Max(
+                maximumAbsoluteVehicleCurvatureDerivative,
+                segmentMaximumCurvatureDerivative);
            curvatureSquareSum += segmentCurvatureSquareSum;
            curvatureSampleCount += segmentCurvatureSampleCount;
            totalCurvatureVariation += segmentVariation;
@@ -94,6 +99,7 @@ public sealed class PathGeometryAnalyzer
            outputSegments,
            cumulativeArcLength,
            maximumAbsoluteVehicleCurvature,
+            maximumAbsoluteVehicleCurvatureDerivative,
            rmsCurvature,
            totalCurvatureVariation,
            curvatureVariationEnergy,
@@ -110,6 +116,7 @@ public sealed class PathGeometryAnalyzer
        ref bool hasPreviousOutputHeading,
        List<SmoothedPathPoint> output,
        out double maximumAbsoluteVehicleCurvature,
+        out double maximumAbsoluteVehicleCurvatureDerivative,
        out double curvatureSquareSum,
        out int curvatureSampleCount,
        out double totalCurvatureVariation,
@@ -118,6 +125,7 @@ public sealed class PathGeometryAnalyzer
        out string reason)
    {
        maximumAbsoluteVehicleCurvature = 0d;
+        maximumAbsoluteVehicleCurvatureDerivative = 0d;
        curvatureSquareSum = 0d;
        curvatureSampleCount = 0;
        totalCurvatureVariation = 0d;
@@ -129,6 +137,8 @@ public sealed class PathGeometryAnalyzer
        var headings = new double[count];
        var unwrappedHeadings = new double[count];
        var geometricCurvatures = new double[count];
+        var vehicleCurvatures = new double[count];
+        var curvatureDerivatives = new double[count];

        for (int index = 1; index < count; index++)
        {
@@ -214,9 +224,53 @@ public sealed class PathGeometryAnalyzer

        for (int index = 0; index < count; index++)
        {
-            SmoothingPoint2D sample = samples[index];
            double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
-            double vehicleCurvature = directionSign * geometricCurvatures[index];
+            vehicleCurvatures[index] = directionSign * geometricCurvatures[index];
+        }
+
+        if (segment.StartVehicleCurvaturePerMeter.HasValue)
+        {
+            double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
+            vehicleCurvatures[0] = segment.StartVehicleCurvaturePerMeter.Value;
+            geometricCurvatures[0] = directionSign * vehicleCurvatures[0];
+        }
+
+        for (int index = 0; index < count; index++)
+        {
+            if (count == 1)
+            {
+                curvatureDerivatives[index] = 0d;
+            }
+            else if (index == 0)
+            {
+                curvatureDerivatives[index] =
+                    (vehicleCurvatures[1] - vehicleCurvatures[0]) /
+                    (localArcLengths[1] - localArcLengths[0]);
+            }
+            else if (index == count - 1)
+            {
+                curvatureDerivatives[index] =
+                    (vehicleCurvatures[index] - vehicleCurvatures[index - 1]) /
+                    (localArcLengths[index] - localArcLengths[index - 1]);
+            }
+            else
+            {
+                curvatureDerivatives[index] =
+                    (vehicleCurvatures[index + 1] - vehicleCurvatures[index - 1]) /
+                    (localArcLengths[index + 1] - localArcLengths[index - 1]);
+            }
+
+            if (!NumericGuard.IsFinite(curvatureDerivatives[index]))
+            {
+                reason = "候选路径曲率导数计算产生了非法数值。";
+                return false;
+            }
+        }
+
+        for (int index = 0; index < count; index++)
+        {
+            SmoothingPoint2D sample = samples[index];
+            double vehicleCurvature = vehicleCurvatures[index];
            double arcLength = cumulativeArcLength + localArcLengths[index];
            bool isGearSwitch = index == 0 && segment.StartsAtGearSwitch;
            SmoothedPathPointSource source = isGearSwitch ? SmoothedPathPointSource.GearSwitch : sample.Source;
@@ -229,11 +283,15 @@ public sealed class PathGeometryAnalyzer
                segment.Direction,
                geometricCurvatures[index],
                vehicleCurvature,
+                curvatureDerivatives[index],
                sample.BodyClearance,
                isGearSwitch,
                source));

            maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(vehicleCurvature));
+            maximumAbsoluteVehicleCurvatureDerivative = Math.Max(
+                maximumAbsoluteVehicleCurvatureDerivative,
+                Math.Abs(curvatureDerivatives[index]));
            curvatureSquareSum += vehicleCurvature * vehicleCurvature;
            curvatureSampleCount++;
            minimumClearance = Math.Min(minimumClearance, sample.BodyClearance);
@@ -75,10 +75,17 @@ public sealed class PathSmoothingPreprocessor
                sourceSegment.Direction,
                segmentPoints,
                sourceSegment.StartsAtGearSwitch,
-                sourceSegment.EndsAtGearSwitch);
+                sourceSegment.EndsAtGearSwitch,
+                request.CoarsePath[sourceSegment.StartIndex].VehicleCurvature);
            if (!_resampler.TryResample(unresampled, configuration.OutputSpacingMeters, out PreparedDirectionSegment resampled, out reason))
                return false;
-            preparedSegments.Add(resampled);
+            preparedSegments.Add(new PreparedDirectionSegment(
+                resampled.SegmentIndex,
+                resampled.Direction,
+                resampled.Points,
+                resampled.StartsAtGearSwitch,
+                resampled.EndsAtGearSwitch,
+                unresampled.StartVehicleCurvaturePerMeter));
        }

        preparedPath = new PreparedPath(preparedSegments);
@@ -15,15 +15,30 @@ public sealed class PreparedDirectionSegment
        IReadOnlyList<SmoothingPoint2D> points,
        bool startsAtGearSwitch,
        bool endsAtGearSwitch)
+        : this(segmentIndex, direction, points, startsAtGearSwitch, endsAtGearSwitch, null)
+    {
+    }
+
+    /// <summary>创建带有真实起始车辆曲率边界状态的不可变方向段。</summary>
+    public PreparedDirectionSegment(
+        int segmentIndex,
+        TravelDirection direction,
+        IReadOnlyList<SmoothingPoint2D> points,
+        bool startsAtGearSwitch,
+        bool endsAtGearSwitch,
+        double? startVehicleCurvaturePerMeter)
    {
        if (segmentIndex < 0) throw new ArgumentOutOfRangeException(nameof(segmentIndex));
        if (points == null || points.Count == 0) throw new ArgumentException("A prepared segment requires points.", nameof(points));
+        if (startVehicleCurvaturePerMeter.HasValue && !IsFinite(startVehicleCurvaturePerMeter.Value))
+            throw new ArgumentOutOfRangeException(nameof(startVehicleCurvaturePerMeter));

        SegmentIndex = segmentIndex;
        Direction = direction;
        Points = CopyReadOnly(points);
        StartsAtGearSwitch = startsAtGearSwitch;
        EndsAtGearSwitch = endsAtGearSwitch;
+        StartVehicleCurvaturePerMeter = startVehicleCurvaturePerMeter;
    }

    /// <summary>从零开始的分段序号；在 <see cref="PreparedPath.Segments"/> 中必须与其位置一致。</summary>
@@ -41,6 +56,11 @@ public sealed class PreparedDirectionSegment
    /// <summary>本段末点之后是否紧邻换向点。</summary>
    public bool EndsAtGearSwitch { get; }

+    /// <summary>原始车辆在本段物理起点的曲率边界状态，单位 1/m。</summary>
+    public double? StartVehicleCurvaturePerMeter { get; }
+
+    private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
+
    private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
    {
        var copy = new List<T>(source.Count);
@@ -1,19 +1,16 @@
-using System;
 using System.Collections.Generic;
-using System.Collections.ObjectModel;
-using MultiWheelC.TrajectoryPlanning.CoarsePath;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;

 namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;

-/// <summary>将已验证的粗路径转换为不改变任何锚点位姿或车辆曲率的安全比较基线。</summary>
+/// <summary>通过候选路径同一几何分析器和验证器构建安全、可公平比较的原始基线。</summary>
 internal static class RawPathBaselineBuilder
 {
-    private const double MinimumArcDeltaMeters = 1e-12d;
-
    internal static bool TryCreate(
        PathSmoothingRequest request,
        PreparedPath preparedPath,
+        PathGeometryAnalyzer analyzer,
+        double outputSpacingMeters,
        SmoothedPathValidator validator,
        double maximumCollisionCheckStepMeters,
        out RawPathBaseline baseline,
@@ -21,17 +18,18 @@ internal static class RawPathBaselineBuilder
    {
        baseline = null;
        reason = string.Empty;
-        if (request == null || preparedPath == null || validator == null)
+        if (request == null || preparedPath == null || analyzer == null || validator == null)
        {
-            reason = "原始粗路径基线缺少请求、预处理路径或安全验证器。";
+            reason = "原始粗路径基线缺少请求、预处理路径、几何分析器或安全验证器。";
            return false;
        }

-        IReadOnlyList<SmoothedPathPoint> candidatePath = CreatePoints(request.CoarsePath);
-        IReadOnlyList<SmoothedPathSegment> candidateSegments = CreateSegments(request.Segments);
+        if (!analyzer.TryAnalyze(preparedPath.Segments, outputSpacingMeters, out PathGeometryAnalysis analysis, out reason))
+            return false;
+
        if (!validator.TryValidate(
-                candidatePath,
-                candidateSegments,
+                analysis.Path,
+                analysis.Segments,
                preparedPath,
                request.Map,
                request.Vehicle,
@@ -43,102 +41,22 @@ internal static class RawPathBaselineBuilder
            return false;
        }

-        baseline = new RawPathBaseline(safePath, candidateSegments, CreateMetrics(safePath, candidateSegments, minimumClearanceMeters));
+        baseline = new RawPathBaseline(safePath, analysis.Segments, CreateMetrics(analysis, minimumClearanceMeters));
        return true;
    }

-    private static IReadOnlyList<SmoothedPathPoint> CreatePoints(IReadOnlyList<CoarsePathPoint> coarsePath)
-    {
-        var points = new List<SmoothedPathPoint>(coarsePath == null ? 0 : coarsePath.Count);
-        if (coarsePath != null)
-        {
-            for (int index = 0; index < coarsePath.Count; index++)
-            {
-                CoarsePathPoint point = coarsePath[index];
-                points.Add(new SmoothedPathPoint(
-                    point.X,
-                    point.Y,
-                    point.Heading,
-                    point.UnwrappedHeading,
-                    point.ArcLength,
-                    point.Direction,
-                    point.VehicleCurvature,
-                    point.VehicleCurvature,
-                    point.BodyClearance,
-                    point.IsGearSwitchPoint,
-                    point.IsGearSwitchPoint ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor));
-            }
-        }
-        return new ReadOnlyCollection<SmoothedPathPoint>(points);
-    }
-
-    private static IReadOnlyList<SmoothedPathSegment> CreateSegments(IReadOnlyList<PathSegment> coarseSegments)
-    {
-        var segments = new List<SmoothedPathSegment>(coarseSegments == null ? 0 : coarseSegments.Count);
-        if (coarseSegments != null)
-        {
-            for (int index = 0; index < coarseSegments.Count; index++)
-            {
-                PathSegment segment = coarseSegments[index];
-                segments.Add(new SmoothedPathSegment(
-                    segment.SegmentIndex,
-                    segment.Direction,
-                    segment.StartIndex,
-                    segment.EndIndex,
-                    segment.StartsAtGearSwitch,
-                    segment.EndsAtGearSwitch));
-            }
-        }
-        return new ReadOnlyCollection<SmoothedPathSegment>(segments);
-    }
-
    private static PathQualityMetrics CreateMetrics(
-        IReadOnlyList<SmoothedPathPoint> path,
-        IReadOnlyList<SmoothedPathSegment> segments,
+        PathGeometryAnalysis analysis,
        double minimumClearanceMeters)
    {
-        double maximumAbsoluteVehicleCurvature = 0d;
-        double curvatureSquareSum = 0d;
-        int curvatureSampleCount = 0;
-        double totalAbsoluteCurvatureVariation = 0d;
-        double curvatureVariationEnergy = 0d;
-
-        for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
-        {
-            SmoothedPathSegment segment = segments[segmentIndex];
-            SmoothedPathPoint previous = null;
-            for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
-            {
-                SmoothedPathPoint current = path[pointIndex];
-                double curvature = current.VehicleCurvature;
-                maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(curvature));
-                curvatureSquareSum += curvature * curvature;
-                curvatureSampleCount++;
-                if (previous != null)
-                {
-                    double arcDelta = current.ArcLength - previous.ArcLength;
-                    if (arcDelta > MinimumArcDeltaMeters)
-                    {
-                        double curvatureDelta = curvature - previous.VehicleCurvature;
-                        totalAbsoluteCurvatureVariation += Math.Abs(curvatureDelta);
-                        curvatureVariationEnergy += curvatureDelta * curvatureDelta / arcDelta;
-                    }
-                }
-                previous = current;
-            }
-        }
-
-        double rootMeanSquareVehicleCurvature = curvatureSampleCount == 0
-            ? 0d
-            : Math.Sqrt(curvatureSquareSum / curvatureSampleCount);
-        double pathLengthMeters = path.Count == 0 ? 0d : path[path.Count - 1].ArcLength;
        return new PathQualityMetrics(
            true,
-            pathLengthMeters,
-            maximumAbsoluteVehicleCurvature,
-            rootMeanSquareVehicleCurvature,
-            totalAbsoluteCurvatureVariation,
-            curvatureVariationEnergy,
+            analysis.PathLengthMeters,
+            analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
+            analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
+            analysis.RootMeanSquareVehicleCurvaturePerMeter,
+            analysis.TotalAbsoluteCurvatureVariationPerMeter,
+            analysis.CurvatureVariationEnergy,
            minimumClearanceMeters,
            0d,
            0d,