feat: add cubic b-spline path smoother

2026-07-29 10:45:11 +08:00
parent 83940f9749
commit 24b46732e3
4 changed files with 546 additions and 2 deletions
@@ -0,0 +1,322 @@
+using System;
+using System.Collections.Generic;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
+
+/// <summary>按方向段独立生成夹持三次 B 样条原始几何候选。</summary>
+internal sealed class CubicBSplineSmoother : IPathSmoother
+{
+    private const int Degree = 3;
+    private const int SamplesPerSpan = 64;
+    private const double StraightToleranceMeters = 1e-9d;
+    private const double EndpointTangentScale = 1d / 3d;
+    private const double EndpointProbeParameter = 1e-6d;
+
+    /// <inheritdoc />
+    public SmoothingMethod Method => SmoothingMethod.CubicBSpline;
+
+    /// <inheritdoc />
+    public SmoothingCandidate Smooth(
+        SmoothingAlgorithmInput input,
+        double effectiveStrength,
+        CancellationToken cancellationToken)
+    {
+        if (input == null || input.OriginalPath == null ||
+            !NumericGuard.IsPositiveFinite(effectiveStrength) ||
+            !NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
+            input.MinimumClearanceReserveMeters < 0d)
+        {
+            return SmoothingCandidate.Failed("B 样条输入、强度或净空预留无效。");
+        }
+
+        var candidateSegments = new List<PreparedDirectionSegment>(input.OriginalPath.Segments.Count);
+        for (int segmentIndex = 0; segmentIndex < input.OriginalPath.Segments.Count; segmentIndex++)
+        {
+            cancellationToken.ThrowIfCancellationRequested();
+            PreparedDirectionSegment sourceSegment = input.OriginalPath.Segments[segmentIndex];
+            if (!TrySmoothSegment(sourceSegment, effectiveStrength, input.MinimumClearanceReserveMeters,
+                    cancellationToken, out IReadOnlyList<SmoothingPoint2D> points, out string reason))
+            {
+                return SmoothingCandidate.Failed(reason);
+            }
+
+            candidateSegments.Add(new PreparedDirectionSegment(
+                sourceSegment.SegmentIndex,
+                sourceSegment.Direction,
+                points,
+                sourceSegment.StartsAtGearSwitch,
+                sourceSegment.EndsAtGearSwitch));
+        }
+
+        return SmoothingCandidate.Success(candidateSegments);
+    }
+
+    private static bool TrySmoothSegment(
+        PreparedDirectionSegment sourceSegment,
+        double strength,
+        double reserveMeters,
+        CancellationToken cancellationToken,
+        out IReadOnlyList<SmoothingPoint2D> result,
+        out string reason)
+    {
+        result = null;
+        reason = string.Empty;
+        if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count == 0)
+        {
+            reason = "B 样条方向段为空。";
+            return false;
+        }
+
+        IReadOnlyList<SmoothingPoint2D> anchors = sourceSegment.Points;
+        for (int index = 0; index < anchors.Count; index++)
+        {
+            cancellationToken.ThrowIfCancellationRequested();
+            if (!IsValidAnchor(anchors[index]))
+            {
+                reason = "B 样条方向段包含非法锚点。";
+                return false;
+            }
+        }
+
+        if (anchors.Count <= Degree || IsStraight(anchors))
+        {
+            result = anchors;
+            return true;
+        }
+
+        Point2D[] controls = CreateControls(anchors, sourceSegment.Direction, strength, reserveMeters,
+            cancellationToken);
+        if (controls == null)
+        {
+            reason = "B 样条控制点构造产生非法数值。";
+            return false;
+        }
+
+        double[] knots = CreateClampedKnots(controls.Length);
+        var sampled = new List<SmoothingPoint2D>();
+        int spanCount = controls.Length - Degree;
+        int uniformIntervals = spanCount * SamplesPerSpan;
+        AddSample(0d, anchors, controls, knots, sampled, cancellationToken);
+        AddSample(EndpointProbeParameter, anchors, controls, knots, sampled, cancellationToken);
+        for (int index = 1; index < uniformIntervals; index++)
+        {
+            AddSample((double)index / uniformIntervals, anchors, controls, knots, sampled, cancellationToken);
+        }
+        AddSample(1d - EndpointProbeParameter, anchors, controls, knots, sampled, cancellationToken);
+        AddSample(1d, anchors, controls, knots, sampled, cancellationToken);
+
+        result = sampled;
+        return true;
+    }
+
+    private static Point2D[] CreateControls(
+        IReadOnlyList<SmoothingPoint2D> anchors,
+        TravelDirection direction,
+        double strength,
+        double reserveMeters,
+        CancellationToken cancellationToken)
+    {
+        var controls = new Point2D[anchors.Count];
+        controls[0] = Point2D.FromAnchor(anchors[0]);
+        controls[controls.Length - 1] = Point2D.FromAnchor(anchors[anchors.Count - 1]);
+
+        double startHandleLength = Distance(anchors[0], anchors[1]) * EndpointTangentScale * strength;
+        double startTravelHeading = GetTravelHeading(anchors[0], direction);
+        Point2D startProposed = new Point2D(
+            anchors[0].X + startHandleLength * Math.Cos(startTravelHeading),
+            anchors[0].Y + startHandleLength * Math.Sin(startTravelHeading));
+        controls[1] = ClampDisplacement(anchors[1], startProposed, GetAllowedRadius(anchors[1], reserveMeters));
+
+        int finalIndex = anchors.Count - 1;
+        double endHandleLength = Distance(anchors[finalIndex - 1], anchors[finalIndex]) * EndpointTangentScale * strength;
+        double endTravelHeading = GetTravelHeading(anchors[finalIndex], direction);
+        Point2D endProposed = new Point2D(
+            anchors[finalIndex].X - endHandleLength * Math.Cos(endTravelHeading),
+            anchors[finalIndex].Y - endHandleLength * Math.Sin(endTravelHeading));
+        controls[finalIndex - 1] = ClampDisplacement(
+            anchors[finalIndex - 1], endProposed, GetAllowedRadius(anchors[finalIndex - 1], reserveMeters));
+
+        for (int index = 2; index < finalIndex - 1; index++)
+        {
+            cancellationToken.ThrowIfCancellationRequested();
+            SmoothingPoint2D previous = anchors[index - 1];
+            SmoothingPoint2D current = anchors[index];
+            SmoothingPoint2D next = anchors[index + 1];
+            Point2D target = new Point2D(
+                (previous.X + current.X + next.X) / 3d,
+                (previous.Y + current.Y + next.Y) / 3d);
+            Point2D proposed = new Point2D(
+                current.X + strength * (target.X - current.X),
+                current.Y + strength * (target.Y - current.Y));
+            controls[index] = ClampDisplacement(current, proposed, GetAllowedRadius(current, reserveMeters));
+        }
+
+        for (int index = 0; index < controls.Length; index++)
+        {
+            if (!NumericGuard.IsFinite(controls[index].X) || !NumericGuard.IsFinite(controls[index].Y))
+                return null;
+        }
+        return controls;
+    }
+
+    private static void AddSample(
+        double parameter,
+        IReadOnlyList<SmoothingPoint2D> anchors,
+        Point2D[] controls,
+        double[] knots,
+        List<SmoothingPoint2D> output,
+        CancellationToken cancellationToken)
+    {
+        cancellationToken.ThrowIfCancellationRequested();
+        Point2D evaluated = Evaluate(controls, knots, parameter);
+        SmoothingPoint2D reference = InterpolateAnchor(anchors, parameter);
+        bool endpoint = parameter == 0d || parameter == 1d;
+        output.Add(new SmoothingPoint2D(
+            evaluated.X,
+            evaluated.Y,
+            reference.ArcLength,
+            reference.Heading,
+            reference.UnwrappedHeading,
+            reference.BodyClearance,
+            endpoint && reference.IsGearSwitchPoint,
+            endpoint ? reference.Source : SmoothedPathPointSource.Interpolated));
+    }
+
+    private static Point2D Evaluate(Point2D[] controls, double[] knots, double parameter)
+    {
+        if (parameter <= 0d) return controls[0];
+        if (parameter >= 1d) return controls[controls.Length - 1];
+
+        var point = new Point2D(0d, 0d);
+        for (int index = 0; index < controls.Length; index++)
+        {
+            double basis = EvaluateBasis(index, Degree, parameter, knots);
+            point = new Point2D(point.X + basis * controls[index].X, point.Y + basis * controls[index].Y);
+        }
+        return point;
+    }
+
+    private static double EvaluateBasis(int index, int degree, double parameter, double[] knots)
+    {
+        if (degree == 0)
+            return knots[index] <= parameter && parameter < knots[index + 1] ? 1d : 0d;
+
+        double left = 0d;
+        double leftDenominator = knots[index + degree] - knots[index];
+        if (leftDenominator > 0d)
+            left = (parameter - knots[index]) / leftDenominator * EvaluateBasis(index, degree - 1, parameter, knots);
+
+        double right = 0d;
+        double rightDenominator = knots[index + degree + 1] - knots[index + 1];
+        if (rightDenominator > 0d)
+            right = (knots[index + degree + 1] - parameter) / rightDenominator *
+                EvaluateBasis(index + 1, degree - 1, parameter, knots);
+        return left + right;
+    }
+
+    private static double[] CreateClampedKnots(int controlCount)
+    {
+        var knots = new double[controlCount + Degree + 1];
+        for (int index = Degree + 1; index < controlCount; index++)
+            knots[index] = (double)(index - Degree) / (controlCount - Degree);
+        for (int index = controlCount; index < knots.Length; index++) knots[index] = 1d;
+        return knots;
+    }
+
+    private static SmoothingPoint2D InterpolateAnchor(IReadOnlyList<SmoothingPoint2D> anchors, double parameter)
+    {
+        if (parameter <= 0d) return anchors[0];
+        if (parameter >= 1d) return anchors[anchors.Count - 1];
+
+        double scaled = parameter * (anchors.Count - 1);
+        int leftIndex = (int)Math.Floor(scaled);
+        double ratio = scaled - leftIndex;
+        SmoothingPoint2D left = anchors[leftIndex];
+        SmoothingPoint2D right = anchors[leftIndex + 1];
+        return new SmoothingPoint2D(
+            left.X + ratio * (right.X - left.X),
+            left.Y + ratio * (right.Y - left.Y),
+            left.ArcLength + ratio * (right.ArcLength - left.ArcLength),
+            left.Heading + ratio * (right.Heading - left.Heading),
+            left.UnwrappedHeading + ratio * (right.UnwrappedHeading - left.UnwrappedHeading),
+            left.BodyClearance + ratio * (right.BodyClearance - left.BodyClearance),
+            false,
+            SmoothedPathPointSource.Interpolated);
+    }
+
+    private static Point2D ClampDisplacement(SmoothingPoint2D anchor, Point2D proposed, double allowedRadius)
+    {
+        double deltaX = proposed.X - anchor.X;
+        double deltaY = proposed.Y - anchor.Y;
+        double distance = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
+        if (!NumericGuard.IsFinite(distance) || distance <= allowedRadius) return proposed;
+        if (distance == 0d || allowedRadius == 0d) return Point2D.FromAnchor(anchor);
+        double scale = allowedRadius / distance;
+        return new Point2D(anchor.X + deltaX * scale, anchor.Y + deltaY * scale);
+    }
+
+    private static bool IsStraight(IReadOnlyList<SmoothingPoint2D> anchors)
+    {
+        if (anchors.Count < 3) return true;
+        SmoothingPoint2D first = anchors[0];
+        SmoothingPoint2D last = anchors[anchors.Count - 1];
+        double directionX = last.X - first.X;
+        double directionY = last.Y - first.Y;
+        double length = Math.Sqrt(directionX * directionX + directionY * directionY);
+        if (!NumericGuard.IsFinite(length) || length <= StraightToleranceMeters) return false;
+        for (int index = 1; index < anchors.Count - 1; index++)
+        {
+            double offsetX = anchors[index].X - first.X;
+            double offsetY = anchors[index].Y - first.Y;
+            double perpendicularDeviation = Math.Abs(directionX * offsetY - directionY * offsetX) / length;
+            if (perpendicularDeviation >= StraightToleranceMeters) return false;
+        }
+        return true;
+    }
+
+    private static bool IsValidAnchor(SmoothingPoint2D point)
+    {
+        return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
+            NumericGuard.IsFinite(point.ArcLength) && NumericGuard.IsFinite(point.Heading) &&
+            NumericGuard.IsFinite(point.UnwrappedHeading) && NumericGuard.IsFinite(point.BodyClearance) &&
+            point.BodyClearance >= 0d;
+    }
+
+    private static double GetAllowedRadius(SmoothingPoint2D anchor, double reserveMeters)
+    {
+        return Math.Max(0d, anchor.BodyClearance - reserveMeters);
+    }
+
+    private static double GetTravelHeading(SmoothingPoint2D point, TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward ? point.Heading : point.Heading - Math.PI;
+    }
+
+    private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right)
+    {
+        double deltaX = right.X - left.X;
+        double deltaY = right.Y - left.Y;
+        return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
+    }
+
+    private readonly struct Point2D
+    {
+        internal Point2D(double x, double y)
+        {
+            X = x;
+            Y = y;
+        }
+
+        internal double X { get; }
+        internal double Y { get; }
+
+        internal static Point2D FromAnchor(SmoothingPoint2D anchor)
+        {
+            return new Point2D(anchor.X, anchor.Y);
+        }
+    }
+}
@@ -12,12 +12,14 @@ internal sealed class SmoothingAlgorithmInput
        PreparedPath originalPath,
        PlanningGridMap map,
        VehicleParameters vehicle,
-        double maximumCollisionCheckStepMeters)
+        double maximumCollisionCheckStepMeters,
+        double minimumClearanceReserveMeters)
    {
        OriginalPath = originalPath ?? throw new ArgumentNullException(nameof(originalPath));
        Map = map ?? throw new ArgumentNullException(nameof(map));
        Vehicle = vehicle ?? throw new ArgumentNullException(nameof(vehicle));
        MaximumCollisionCheckStepMeters = maximumCollisionCheckStepMeters;
+        MinimumClearanceReserveMeters = minimumClearanceReserveMeters;
    }

    /// <summary>已校验并按方向分段的原始路径。</summary>
@@ -31,4 +33,7 @@ internal sealed class SmoothingAlgorithmInput

    /// <summary>连续车体碰撞检查的最大步长，单位 m。</summary>
    internal double MaximumCollisionCheckStepMeters { get; }
+
+    /// <summary>候选几何必须从原始保守净空中预留的最小安全余量，单位 m。</summary>
+    internal double MinimumClearanceReserveMeters { get; }
 }
@@ -277,7 +277,7 @@ internal sealed class SmoothingAlgorithmRunner
                MaximumCurvaturePerMeter = 100d,
                MinimumTurningRadiusMeters = 0.01d,
            };
-            return new SmoothingAlgorithmInput(new PreparedPath(originalSegments), mapResult.Map, vehicle, 0.05d);
+            return new SmoothingAlgorithmInput(new PreparedPath(originalSegments), mapResult.Map, vehicle, 0.05d, 0.02d);
        }

        private static SmoothingCandidate CreateAcceptedCandidate()