feat: add local G2 smoothing contracts

2026-07-30 16:12:06 +08:00
parent f19034db1e
commit 3e1126936d
13 changed files with 364 additions and 9 deletions
@@ -0,0 +1,15 @@
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+/// <summary>局部 G2 五次过渡的可配置阈值。</summary>
+public sealed class LocalG2QuinticOptions
+{
+    public double MinimumWindowLengthMeters { get; set; } = 0.20d;
+    public double PreferredWindowLengthMeters { get; set; } = 0.50d;
+    public double MaximumWindowLengthMeters { get; set; } = 0.80d;
+    public double MaximumDeviationMeters { get; set; } = 0.10d;
+    public double AbsoluteCurvatureJumpFloorPerMeter { get; set; } = 0.001d;
+    public double CurvatureJumpRatioOfMaximum { get; set; } = 0.05d;
+    public double MinimumPeakGradientImprovementRatio { get; set; } = 0.20d;
+    public double MaximumVariationCostRegressionRatio { get; set; } = 0.02d;
+    public int MaximumCandidatesPerRegion { get; set; } = 12;
+}
@@ -5,7 +5,7 @@ public sealed class PathQualityMetrics
 {
    /// <summary>创建全零、不可行的质量指标。</summary>
    public PathQualityMetrics()
-        : this(false, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d)
+        : this(false, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d)
    {
    }

@@ -22,10 +22,41 @@ public sealed class PathQualityMetrics
        double peakCurvatureChangePercent,
        double curvatureVariationChangePercent,
        double minimumClearanceChangeMeters)
+        : this(
+            isFeasible,
+            pathLengthMeters,
+            maximumAbsoluteVehicleCurvaturePerMeter,
+            0d,
+            rootMeanSquareVehicleCurvaturePerMeter,
+            totalAbsoluteCurvatureVariationPerMeter,
+            curvatureVariationEnergy,
+            minimumBodyClearanceMeters,
+            lengthChangePercent,
+            peakCurvatureChangePercent,
+            curvatureVariationChangePercent,
+            minimumClearanceChangeMeters)
+    {
+    }
+
+    /// <summary>创建带有曲率导数峰值的完整质量指标快照。</summary>
+    public PathQualityMetrics(
+        bool isFeasible,
+        double pathLengthMeters,
+        double maximumAbsoluteVehicleCurvaturePerMeter,
+        double maximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
+        double rootMeanSquareVehicleCurvaturePerMeter,
+        double totalAbsoluteCurvatureVariationPerMeter,
+        double curvatureVariationEnergy,
+        double minimumBodyClearanceMeters,
+        double lengthChangePercent,
+        double peakCurvatureChangePercent,
+        double curvatureVariationChangePercent,
+        double minimumClearanceChangeMeters)
    {
        IsFeasible = isFeasible;
        PathLengthMeters = pathLengthMeters;
        MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
+        MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter = maximumAbsoluteVehicleCurvatureDerivativePerSquareMeter;
        RootMeanSquareVehicleCurvaturePerMeter = rootMeanSquareVehicleCurvaturePerMeter;
        TotalAbsoluteCurvatureVariationPerMeter = totalAbsoluteCurvatureVariationPerMeter;
        CurvatureVariationEnergy = curvatureVariationEnergy;
@@ -45,6 +76,9 @@ public sealed class PathQualityMetrics
    /// <summary>绝对车辆曲率峰值，单位 1/m。</summary>
    public double MaximumAbsoluteVehicleCurvaturePerMeter { get; }

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

@@ -54,6 +88,9 @@ public sealed class PathQualityMetrics
    /// <summary>逐方向段计算的曲率变化能量。</summary>
    public double CurvatureVariationEnergy { get; }

+    /// <summary>曲率变化代价的兼容名称。</summary>
+    public double CurvatureVariationCost => CurvatureVariationEnergy;
+
    /// <summary>完整扩大车体的最小保守净空，单位 m。</summary>
    public double MinimumBodyClearanceMeters { get; }

@@ -47,4 +47,7 @@ public sealed class PathSmoothingConfiguration

    /// <summary>分段五次多项式专用参数。</summary>
    public PiecewiseQuinticOptions PiecewiseQuintic { get; } = new PiecewiseQuinticOptions();
+
+    /// <summary>局部 G2 五次过渡专用参数。</summary>
+    public LocalG2QuinticOptions LocalG2Quintic { get; } = new LocalG2QuinticOptions();
 }
@@ -0,0 +1,17 @@
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+/// <summary>局部 G2 区域未替换原始路径的稳定原因。</summary>
+public enum PathSmoothingRegionFailureReason
+{
+    None,
+    WindowUnavailable,
+    CandidateGenerationFailed,
+    Collision,
+    InsufficientClearance,
+    CurvatureExceeded,
+    CurvatureOvershoot,
+    DeviationExceeded,
+    InsufficientImprovement,
+    VariationCostRegression,
+    GlobalValidationRollback,
+}
@@ -0,0 +1,83 @@
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+/// <summary>单个局部 G2 平滑区域的不可变发布报告。</summary>
+public sealed class PathSmoothingRegionReport
+{
+    public PathSmoothingRegionReport(
+        int segmentIndex,
+        double startArcLengthMeters,
+        double endArcLengthMeters,
+        IReadOnlyList<double> curvatureJumpsPerMeter,
+        double plannedWindowLengthMeters,
+        double actualWindowLengthMeters,
+        double leftWindowLengthMeters,
+        double rightWindowLengthMeters,
+        int candidateCount,
+        int selectedCandidateIndex,
+        PathSmoothingRegionStatus status,
+        PathSmoothingRegionFailureReason failureReason,
+        double rawPeakCurvatureDerivativePerSquareMeter,
+        double resultPeakCurvatureDerivativePerSquareMeter,
+        double rawCurvatureVariationCost,
+        double resultCurvatureVariationCost,
+        double maximumDeviationMeters,
+        double minimumBodyClearanceMeters,
+        double maximumAbsoluteVehicleCurvaturePerMeter)
+    {
+        SegmentIndex = segmentIndex;
+        StartArcLengthMeters = startArcLengthMeters;
+        EndArcLengthMeters = endArcLengthMeters;
+        CurvatureJumpsPerMeter = CopyReadOnly(curvatureJumpsPerMeter);
+        PlannedWindowLengthMeters = plannedWindowLengthMeters;
+        ActualWindowLengthMeters = actualWindowLengthMeters;
+        LeftWindowLengthMeters = leftWindowLengthMeters;
+        RightWindowLengthMeters = rightWindowLengthMeters;
+        CandidateCount = candidateCount;
+        SelectedCandidateIndex = status == PathSmoothingRegionStatus.Improved
+            ? selectedCandidateIndex
+            : -1;
+        Status = status;
+        FailureReason = failureReason;
+        RawPeakCurvatureDerivativePerSquareMeter = rawPeakCurvatureDerivativePerSquareMeter;
+        ResultPeakCurvatureDerivativePerSquareMeter = resultPeakCurvatureDerivativePerSquareMeter;
+        RawCurvatureVariationCost = rawCurvatureVariationCost;
+        ResultCurvatureVariationCost = resultCurvatureVariationCost;
+        MaximumDeviationMeters = maximumDeviationMeters;
+        MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
+        MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
+    }
+
+    public int SegmentIndex { get; }
+    public double StartArcLengthMeters { get; }
+    public double EndArcLengthMeters { get; }
+    public IReadOnlyList<double> CurvatureJumpsPerMeter { get; }
+    public double PlannedWindowLengthMeters { get; }
+    public double ActualWindowLengthMeters { get; }
+    public double LeftWindowLengthMeters { get; }
+    public double RightWindowLengthMeters { get; }
+    public int CandidateCount { get; }
+    public int SelectedCandidateIndex { get; }
+    public PathSmoothingRegionStatus Status { get; }
+    public PathSmoothingRegionFailureReason FailureReason { get; }
+    public double RawPeakCurvatureDerivativePerSquareMeter { get; }
+    public double ResultPeakCurvatureDerivativePerSquareMeter { get; }
+    public double RawCurvatureVariationCost { get; }
+    public double ResultCurvatureVariationCost { get; }
+    public double MaximumDeviationMeters { get; }
+    public double MinimumBodyClearanceMeters { get; }
+    public double MaximumAbsoluteVehicleCurvaturePerMeter { get; }
+
+    private static IReadOnlyList<double> CopyReadOnly(IReadOnlyList<double> source)
+    {
+        var copy = new List<double>(source == null ? 0 : source.Count);
+        if (source != null)
+        {
+            for (int index = 0; index < source.Count; index++)
+                copy.Add(source[index]);
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+}
@@ -0,0 +1,8 @@
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+/// <summary>单个局部 G2 平滑区域的处理结果。</summary>
+public enum PathSmoothingRegionStatus
+{
+    Improved,
+    RetainedOriginal,
+}
@@ -83,6 +83,15 @@ public sealed class PathSmoothingRequest
        copy.LocalCubicBezier.HandleLengthRatio = source.LocalCubicBezier.HandleLengthRatio;
        copy.PiecewiseQuintic.KnotSpacingMeters = source.PiecewiseQuintic.KnotSpacingMeters;
        copy.PiecewiseQuintic.MinimumKnotSpacingMeters = source.PiecewiseQuintic.MinimumKnotSpacingMeters;
+        copy.LocalG2Quintic.MinimumWindowLengthMeters = source.LocalG2Quintic.MinimumWindowLengthMeters;
+        copy.LocalG2Quintic.PreferredWindowLengthMeters = source.LocalG2Quintic.PreferredWindowLengthMeters;
+        copy.LocalG2Quintic.MaximumWindowLengthMeters = source.LocalG2Quintic.MaximumWindowLengthMeters;
+        copy.LocalG2Quintic.MaximumDeviationMeters = source.LocalG2Quintic.MaximumDeviationMeters;
+        copy.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter = source.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter;
+        copy.LocalG2Quintic.CurvatureJumpRatioOfMaximum = source.LocalG2Quintic.CurvatureJumpRatioOfMaximum;
+        copy.LocalG2Quintic.MinimumPeakGradientImprovementRatio = source.LocalG2Quintic.MinimumPeakGradientImprovementRatio;
+        copy.LocalG2Quintic.MaximumVariationCostRegressionRatio = source.LocalG2Quintic.MaximumVariationCostRegressionRatio;
+        copy.LocalG2Quintic.MaximumCandidatesPerRegion = source.LocalG2Quintic.MaximumCandidatesPerRegion;
        return copy;
    }
 }
@@ -11,18 +11,22 @@ public sealed class PathSmoothingResult
        new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());
    private static readonly IReadOnlyList<SmoothedPathSegment> EmptySegments =
        new ReadOnlyCollection<SmoothedPathSegment>(new List<SmoothedPathSegment>());
+    private static readonly IReadOnlyList<PathSmoothingRegionReport> EmptyRegionReports =
+        new ReadOnlyCollection<PathSmoothingRegionReport>(new List<PathSmoothingRegionReport>());

    private PathSmoothingResult(
        PathSmoothingStatus status,
        SmoothingMethod? method,
        IReadOnlyList<SmoothedPathPoint> path,
        IReadOnlyList<SmoothedPathSegment> segments,
-        PathSmoothingDiagnostics diagnostics)
+        PathSmoothingDiagnostics diagnostics,
+        IReadOnlyList<PathSmoothingRegionReport> regionReports)
    {
        Status = status;
        Method = method;
        Path = path;
        Segments = segments;
+        RegionReports = regionReports;
        Diagnostics = diagnostics ?? new PathSmoothingDiagnostics(
            new PathQualityMetrics(),
            TimeSpan.Zero,
@@ -43,6 +47,9 @@ public sealed class PathSmoothingResult
    /// <summary>覆盖 <see cref="Path"/> 的方向分段；其他状态始终为空且不可变。</summary>
    public IReadOnlyList<SmoothedPathSegment> Segments { get; }

+    /// <summary>局部 G2 各检测区域的不可变报告；传统算法结果为空。</summary>
+    public IReadOnlyList<PathSmoothingRegionReport> RegionReports { get; }
+
    /// <summary>本次平滑的质量和终止诊断；始终非空。</summary>
    public PathSmoothingDiagnostics Diagnostics { get; }

@@ -59,7 +66,8 @@ public sealed class PathSmoothingResult
            method,
            CopyReadOnly(path),
            CopyReadOnly(segments),
-            diagnostics);
+            diagnostics,
+            EmptyRegionReports);
    }

    /// <summary>创建经过完整复核的原始粗路径回退结果。</summary>
@@ -75,17 +83,49 @@ public sealed class PathSmoothingResult
            attemptedMethod,
            CopyReadOnly(path),
            CopyReadOnly(segments),
-            diagnostics);
+            diagnostics,
+            EmptyRegionReports);
+    }
+
+    /// <summary>发布经过完整复核的局部 G2 预平滑结果。</summary>
+    public static PathSmoothingResult PublishLocalG2(
+        PathSmoothingStatus status,
+        IReadOnlyList<SmoothedPathPoint> path,
+        IReadOnlyList<SmoothedPathSegment> segments,
+        PathSmoothingDiagnostics diagnostics,
+        IReadOnlyList<PathSmoothingRegionReport> regionReports)
+    {
+        if (status != PathSmoothingStatus.Complete &&
+            status != PathSmoothingStatus.PartialImprovement &&
+            status != PathSmoothingStatus.NotNeeded &&
+            status != PathSmoothingStatus.Unchanged)
+            throw new ArgumentException("Use a Local G2 publication status.", nameof(status));
+        if (regionReports == null)
+            throw new ArgumentNullException(nameof(regionReports));
+
+        ValidatePublishedResult(SmoothingMethod.LocalG2Quintic, path, segments, diagnostics);
+        return new PathSmoothingResult(
+            status,
+            SmoothingMethod.LocalG2Quintic,
+            CopyReadOnly(path),
+            CopyReadOnly(segments),
+            diagnostics,
+            CopyReadOnly(regionReports));
    }

    /// <summary>创建不发布路径的失败、不可行、取消或输入无效结果。</summary>
    public static PathSmoothingResult Failure(PathSmoothingStatus status, PathSmoothingDiagnostics diagnostics)
    {
-        if (status == PathSmoothingStatus.Success || status == PathSmoothingStatus.FallbackToCoarsePath)
+        if (status == PathSmoothingStatus.Success ||
+            status == PathSmoothingStatus.FallbackToCoarsePath ||
+            status == PathSmoothingStatus.Complete ||
+            status == PathSmoothingStatus.PartialImprovement ||
+            status == PathSmoothingStatus.NotNeeded ||
+            status == PathSmoothingStatus.Unchanged)
            throw new ArgumentException("Use Success or Fallback to publish a path.", nameof(status));
        if (!Enum.IsDefined(typeof(PathSmoothingStatus), status))
            throw new ArgumentOutOfRangeException(nameof(status));
-        return new PathSmoothingResult(status, null, EmptyPath, EmptySegments, diagnostics);
+        return new PathSmoothingResult(status, null, EmptyPath, EmptySegments, diagnostics, EmptyRegionReports);
    }

    private static void ValidatePublishedResult(
@@ -9,4 +9,8 @@ public enum PathSmoothingStatus
    Infeasible,
    Failed,
    Cancelled,
+    Complete,
+    PartialImprovement,
+    NotNeeded,
+    Unchanged,
 }
@@ -17,6 +17,36 @@ public sealed class SmoothedPathPoint
        double bodyClearanceMeters,
        bool isGearSwitchPoint,
        SmoothedPathPointSource source)
+        : this(
+            xMeters,
+            yMeters,
+            headingRadians,
+            unwrappedHeadingRadians,
+            arcLengthMeters,
+            direction,
+            geometricCurvaturePerMeter,
+            vehicleCurvaturePerMeter,
+            0d,
+            bodyClearanceMeters,
+            isGearSwitchPoint,
+            source)
+    {
+    }
+
+    /// <summary>创建带有车辆曲率对弧长导数的不可变采样点。</summary>
+    public SmoothedPathPoint(
+        double xMeters,
+        double yMeters,
+        double headingRadians,
+        double unwrappedHeadingRadians,
+        double arcLengthMeters,
+        TravelDirection direction,
+        double geometricCurvaturePerMeter,
+        double vehicleCurvaturePerMeter,
+        double vehicleCurvatureDerivativePerSquareMeter,
+        double bodyClearanceMeters,
+        bool isGearSwitchPoint,
+        SmoothedPathPointSource source)
    {
        X = xMeters;
        Y = yMeters;
@@ -26,6 +56,7 @@ public sealed class SmoothedPathPoint
        Direction = direction;
        GeometricCurvature = geometricCurvaturePerMeter;
        VehicleCurvature = vehicleCurvaturePerMeter;
+        VehicleCurvatureDerivative = vehicleCurvatureDerivativePerSquareMeter;
        BodyClearance = bodyClearanceMeters;
        IsGearSwitchPoint = isGearSwitchPoint;
        Source = source;
@@ -55,6 +86,9 @@ public sealed class SmoothedPathPoint
    /// <summary>车辆模型使用的有符号曲率，单位 1/m。</summary>
    public double VehicleCurvature { get; }

+    /// <summary>车辆曲率对弧长的导数 dκ/ds，单位 1/m²。</summary>
+    public double VehicleCurvatureDerivative { get; }
+
    /// <summary>扩大车体后的保守净空下界，单位 m。</summary>
    public double BodyClearance { get; }

@@ -7,4 +7,5 @@ public enum SmoothedPathPointSource
    Interpolated,
    GearSwitch,
    CoarsePathFallback,
+    LocalG2Transition,
 }
@@ -6,4 +6,5 @@ public enum SmoothingMethod
    CubicBSpline,
    LocalCubicBezier,
    PiecewiseQuintic,
+    LocalG2Quintic,
 }