chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -0,0 +1,55 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>同一方向原语边界两侧车辆曲率的离散跳变。</summary>
|
||||
internal sealed class CurvatureTransition
|
||||
{
|
||||
internal CurvatureTransition(
|
||||
int segmentIndex,
|
||||
int leftCoarsePathIndex,
|
||||
int rightCoarsePathIndex,
|
||||
double localArcLengthMeters,
|
||||
double x,
|
||||
double y,
|
||||
double vehicleHeadingRadians,
|
||||
double leftVehicleCurvaturePerMeter,
|
||||
double rightVehicleCurvaturePerMeter)
|
||||
{
|
||||
if (segmentIndex < 0 || leftCoarsePathIndex < 0 || rightCoarsePathIndex != leftCoarsePathIndex + 1)
|
||||
throw new ArgumentOutOfRangeException(nameof(leftCoarsePathIndex));
|
||||
if (!NumericGuard.IsFinite(localArcLengthMeters) || localArcLengthMeters < 0d ||
|
||||
!NumericGuard.IsFinite(x) || !NumericGuard.IsFinite(y) || !NumericGuard.IsFinite(vehicleHeadingRadians) ||
|
||||
!NumericGuard.IsFinite(leftVehicleCurvaturePerMeter) || !NumericGuard.IsFinite(rightVehicleCurvaturePerMeter))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(localArcLengthMeters));
|
||||
}
|
||||
|
||||
double curvatureJumpPerMeter = Math.Abs(rightVehicleCurvaturePerMeter - leftVehicleCurvaturePerMeter);
|
||||
if (!NumericGuard.IsFinite(curvatureJumpPerMeter))
|
||||
throw new ArgumentOutOfRangeException(nameof(rightVehicleCurvaturePerMeter));
|
||||
|
||||
SegmentIndex = segmentIndex;
|
||||
LeftCoarsePathIndex = leftCoarsePathIndex;
|
||||
RightCoarsePathIndex = rightCoarsePathIndex;
|
||||
LocalArcLengthMeters = localArcLengthMeters;
|
||||
X = x;
|
||||
Y = y;
|
||||
VehicleHeadingRadians = vehicleHeadingRadians;
|
||||
LeftVehicleCurvaturePerMeter = leftVehicleCurvaturePerMeter;
|
||||
RightVehicleCurvaturePerMeter = rightVehicleCurvaturePerMeter;
|
||||
CurvatureJumpPerMeter = curvatureJumpPerMeter;
|
||||
}
|
||||
|
||||
internal int SegmentIndex { get; }
|
||||
internal int LeftCoarsePathIndex { get; }
|
||||
internal int RightCoarsePathIndex { get; }
|
||||
internal double LocalArcLengthMeters { get; }
|
||||
internal double X { get; }
|
||||
internal double Y { get; }
|
||||
internal double VehicleHeadingRadians { get; }
|
||||
internal double LeftVehicleCurvaturePerMeter { get; }
|
||||
internal double RightVehicleCurvaturePerMeter { get; }
|
||||
internal double CurvatureJumpPerMeter { get; }
|
||||
}
|
||||
@@ -0,0 +1,260 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>从原始粗路径的同向相邻点中识别恒曲率原语边界。</summary>
|
||||
internal sealed class CurvatureTransitionDetector
|
||||
{
|
||||
internal bool TryDetect(
|
||||
PathSmoothingRequest request,
|
||||
double maximumVehicleCurvaturePerMeter,
|
||||
LocalG2OptionsSnapshot options,
|
||||
out IReadOnlyList<CurvatureTransition> transitions,
|
||||
out string reason)
|
||||
{
|
||||
transitions = Empty<CurvatureTransition>();
|
||||
reason = string.Empty;
|
||||
if (request == null || request.CoarsePath == null || request.Segments == null || options == null ||
|
||||
!NumericGuard.IsPositiveFinite(maximumVehicleCurvaturePerMeter))
|
||||
{
|
||||
reason = "局部 G2 曲率事件检测输入无效。";
|
||||
return false;
|
||||
}
|
||||
if (!ValidatePath(request.CoarsePath, out reason) || !ValidateSegments(request.CoarsePath, request.Segments, out reason))
|
||||
return false;
|
||||
|
||||
double threshold = Math.Max(
|
||||
options.AbsoluteCurvatureJumpFloorPerMeter,
|
||||
options.CurvatureJumpRatioOfMaximum * maximumVehicleCurvaturePerMeter);
|
||||
if (!NumericGuard.IsPositiveFinite(threshold))
|
||||
{
|
||||
reason = "局部 G2 曲率事件阈值无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var detected = new List<CurvatureTransition>();
|
||||
for (int segmentPosition = 0; segmentPosition < request.Segments.Count; segmentPosition++)
|
||||
{
|
||||
PathSegment segment = request.Segments[segmentPosition];
|
||||
double segmentStartArc = request.CoarsePath[segment.StartIndex].ArcLength;
|
||||
for (int leftIndex = segment.StartIndex; leftIndex < segment.EndIndex; leftIndex++)
|
||||
{
|
||||
CoarsePathPoint left = request.CoarsePath[leftIndex];
|
||||
CoarsePathPoint right = request.CoarsePath[leftIndex + 1];
|
||||
double delta = right.VehicleCurvature - left.VehicleCurvature;
|
||||
if (!NumericGuard.IsFinite(delta))
|
||||
{
|
||||
reason = "局部 G2 曲率事件跳变溢出。";
|
||||
return false;
|
||||
}
|
||||
if (Math.Abs(delta) >= threshold)
|
||||
{
|
||||
detected.Add(new CurvatureTransition(
|
||||
segment.SegmentIndex,
|
||||
leftIndex,
|
||||
leftIndex + 1,
|
||||
left.ArcLength - segmentStartArc,
|
||||
left.X,
|
||||
left.Y,
|
||||
left.Heading,
|
||||
left.VehicleCurvature,
|
||||
right.VehicleCurvature));
|
||||
}
|
||||
}
|
||||
}
|
||||
transitions = new ReadOnlyCollection<CurvatureTransition>(detected);
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>反射脚本使用的确定性检测与窗口规划接缝。</summary>
|
||||
public static class TestHooks
|
||||
{
|
||||
public static DetectionTestSnapshot Execute(string scenario)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(scenario)) throw new ArgumentException("A scenario is required.", nameof(scenario));
|
||||
switch (scenario)
|
||||
{
|
||||
case "SingleTransition": return DetectSingleTransition();
|
||||
case "GearSwitch": return DetectGearSwitch();
|
||||
case "Noise": return DetectNoise();
|
||||
case "Overlap": return PlanOverlap();
|
||||
case "NearStart": return PlanNearStart();
|
||||
default: throw new ArgumentOutOfRangeException(nameof(scenario));
|
||||
}
|
||||
}
|
||||
|
||||
public sealed class DetectionTestSnapshot
|
||||
{
|
||||
internal DetectionTestSnapshot(int transitionCount, double maximumJump, int regionCount,
|
||||
int transitionCountInFirstRegion, double startArcLength, double leftWindowLength, double rightWindowLength)
|
||||
{
|
||||
TransitionCount = transitionCount;
|
||||
MaximumJump = maximumJump;
|
||||
RegionCount = regionCount;
|
||||
TransitionCountInFirstRegion = transitionCountInFirstRegion;
|
||||
StartArcLength = startArcLength;
|
||||
LeftWindowLength = leftWindowLength;
|
||||
RightWindowLength = rightWindowLength;
|
||||
}
|
||||
|
||||
public int TransitionCount { get; }
|
||||
public double MaximumJump { get; }
|
||||
public int RegionCount { get; }
|
||||
public int TransitionCountInFirstRegion { get; }
|
||||
public double StartArcLength { get; }
|
||||
public double LeftWindowLength { get; }
|
||||
public double RightWindowLength { get; }
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot DetectSingleTransition()
|
||||
{
|
||||
IReadOnlyList<CurvatureTransition> transitions = Detect(CreateRequest(
|
||||
new[] { Point(0d, 0d), Point(0.1d, 0.4167d), Point(0.2d, 0.4167d) },
|
||||
new[] { new PathSegment(0, TravelDirection.Forward, 0, 2, false, false) }));
|
||||
return Snapshot(transitions);
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot DetectGearSwitch()
|
||||
{
|
||||
IReadOnlyList<CurvatureTransition> transitions = Detect(CreateRequest(
|
||||
new[]
|
||||
{
|
||||
Point(0d, 0d, TravelDirection.Forward), Point(0.2d, 0d, TravelDirection.Forward),
|
||||
Point(0.2d, 0.4167d, TravelDirection.Reverse, true), Point(0.4d, 0.4167d, TravelDirection.Reverse),
|
||||
},
|
||||
new[]
|
||||
{
|
||||
new PathSegment(0, TravelDirection.Forward, 0, 1, false, true),
|
||||
new PathSegment(1, TravelDirection.Reverse, 2, 3, true, false),
|
||||
}));
|
||||
return Snapshot(transitions);
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot DetectNoise()
|
||||
{
|
||||
IReadOnlyList<CurvatureTransition> transitions = Detect(CreateRequest(
|
||||
new[] { Point(0d, 0d), Point(0.1d, 0.01d), Point(0.2d, 0.01d) },
|
||||
new[] { new PathSegment(0, TravelDirection.Forward, 0, 2, false, false) }));
|
||||
return Snapshot(transitions);
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot PlanOverlap()
|
||||
{
|
||||
LocalG2OptionsSnapshot options = CreateOptions();
|
||||
var transitions = new[]
|
||||
{
|
||||
new CurvatureTransition(0, 1, 2, 0.4d, 0.4d, 0d, 0d, 0d, 0.5d),
|
||||
new CurvatureTransition(0, 2, 3, 0.7d, 0.7d, 0d, 0d, 0.5d, 0d),
|
||||
};
|
||||
var planner = new LocalG2WindowPlanner();
|
||||
if (!planner.TryPlan(CreatePreparedPath(1.4d), transitions, options, out IReadOnlyList<LocalG2SmoothingRegion> regions, out string reason))
|
||||
throw new InvalidOperationException(reason);
|
||||
return new DetectionTestSnapshot(2, 0.5d, regions.Count, regions[0].Transitions.Count, 0d, 0d, 0d);
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot PlanNearStart()
|
||||
{
|
||||
LocalG2OptionsSnapshot options = CreateOptions();
|
||||
var transitions = new[] { new CurvatureTransition(0, 0, 1, 0.1d, 0.1d, 0d, 0d, 0d, 0.5d) };
|
||||
var planner = new LocalG2WindowPlanner();
|
||||
if (!planner.TryPlan(CreatePreparedPath(1d), transitions, options, out IReadOnlyList<LocalG2SmoothingRegion> regions, out string reason))
|
||||
throw new InvalidOperationException(reason);
|
||||
LocalG2WindowVariant first = regions[0].WindowVariants[0];
|
||||
return new DetectionTestSnapshot(1, 0.5d, regions.Count, 1,
|
||||
first.StartArcLengthMeters, first.LeftWindowLengthMeters, first.RightWindowLengthMeters);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<CurvatureTransition> Detect(PathSmoothingRequest request)
|
||||
{
|
||||
var detector = new CurvatureTransitionDetector();
|
||||
if (!detector.TryDetect(request, 0.8333d, CreateOptions(), out IReadOnlyList<CurvatureTransition> transitions, out string reason))
|
||||
throw new InvalidOperationException(reason);
|
||||
return transitions;
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot Snapshot(IReadOnlyList<CurvatureTransition> transitions)
|
||||
{
|
||||
double maximum = 0d;
|
||||
for (int index = 0; index < transitions.Count; index++) maximum = Math.Max(maximum, transitions[index].CurvatureJumpPerMeter);
|
||||
return new DetectionTestSnapshot(transitions.Count, maximum, 0, 0, 0d, 0d, 0d);
|
||||
}
|
||||
|
||||
private static PathSmoothingRequest CreateRequest(IReadOnlyList<CoarsePathPoint> points, IReadOnlyList<PathSegment> segments)
|
||||
{
|
||||
return new PathSmoothingRequest(points, segments, null, null, new PathSmoothingConfiguration());
|
||||
}
|
||||
|
||||
private static CoarsePathPoint Point(double arcLength, double curvature, TravelDirection direction = TravelDirection.Forward, bool gearSwitch = false)
|
||||
{
|
||||
return new CoarsePathPoint(arcLength, 0d, 0d, 0d, arcLength, direction, curvature, 1d, gearSwitch,
|
||||
CoarsePathPointSource.MotionPrimitive);
|
||||
}
|
||||
|
||||
private static LocalG2OptionsSnapshot CreateOptions() => new LocalG2OptionsSnapshot(new PathSmoothingConfiguration());
|
||||
|
||||
private static Processing.PreparedPath CreatePreparedPath(double length)
|
||||
{
|
||||
var points = new[]
|
||||
{
|
||||
new Processing.SmoothingPoint2D(0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||
new Processing.SmoothingPoint2D(length, 0d, length, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||
};
|
||||
var segment = new Processing.PreparedDirectionSegment(0, TravelDirection.Forward, points, false, false);
|
||||
return new Processing.PreparedPath(new[] { segment });
|
||||
}
|
||||
}
|
||||
|
||||
private static bool ValidatePath(IReadOnlyList<CoarsePathPoint> path, out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
if (path.Count == 0)
|
||||
{
|
||||
reason = "局部 G2 曲率事件检测需要粗路径点。";
|
||||
return false;
|
||||
}
|
||||
double previousArc = -1d;
|
||||
for (int index = 0; index < path.Count; index++)
|
||||
{
|
||||
CoarsePathPoint point = path[index];
|
||||
if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) ||
|
||||
!NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.ArcLength) || point.ArcLength < 0d ||
|
||||
!NumericGuard.IsFinite(point.VehicleCurvature) || point.ArcLength < previousArc)
|
||||
{
|
||||
reason = "局部 G2 曲率事件检测要求有限且非递减的粗路径。";
|
||||
return false;
|
||||
}
|
||||
previousArc = point.ArcLength;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool ValidateSegments(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments, out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
for (int position = 0; position < segments.Count; position++)
|
||||
{
|
||||
PathSegment segment = segments[position];
|
||||
if (segment == null || segment.SegmentIndex != position || segment.StartIndex < 0 ||
|
||||
segment.EndIndex < segment.StartIndex || segment.EndIndex >= path.Count)
|
||||
{
|
||||
reason = "局部 G2 曲率事件检测的方向分段无效。";
|
||||
return false;
|
||||
}
|
||||
for (int index = segment.StartIndex; index <= segment.EndIndex; index++)
|
||||
{
|
||||
if (path[index].Direction != segment.Direction)
|
||||
{
|
||||
reason = "局部 G2 曲率事件检测的方向分段包含换向点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> Empty<T>() => new ReadOnlyCollection<T>(new List<T>());
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,559 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Linq;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>对一个局部 G2 替换候选执行区域质量门和完整路径安全复核。</summary>
|
||||
internal sealed class LocalG2CandidateEvaluator
|
||||
{
|
||||
private const double CurvatureRangeTolerance = 1e-6d;
|
||||
private const double WindowPointToleranceMeters = 1e-10d;
|
||||
private static readonly IReadOnlyList<SmoothedPathPoint> EmptyPath =
|
||||
new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());
|
||||
private static readonly IReadOnlyList<SmoothedPathSegment> EmptySegments =
|
||||
new ReadOnlyCollection<SmoothedPathSegment>(new List<SmoothedPathSegment>());
|
||||
private readonly PathGeometryAnalyzer _analyzer;
|
||||
private readonly LocalG2PathSplicer _splicer;
|
||||
private readonly SmoothedPathValidator _validator;
|
||||
|
||||
internal LocalG2CandidateEvaluator()
|
||||
: this(new PathGeometryAnalyzer(), new LocalG2PathSplicer(), new SmoothedPathValidator())
|
||||
{
|
||||
}
|
||||
|
||||
internal LocalG2CandidateEvaluator(
|
||||
PathGeometryAnalyzer analyzer,
|
||||
LocalG2PathSplicer splicer,
|
||||
SmoothedPathValidator validator)
|
||||
{
|
||||
_analyzer = analyzer ?? throw new ArgumentNullException(nameof(analyzer));
|
||||
_splicer = splicer ?? throw new ArgumentNullException(nameof(splicer));
|
||||
_validator = validator ?? throw new ArgumentNullException(nameof(validator));
|
||||
}
|
||||
|
||||
internal LocalG2CandidateEvaluation Evaluate(
|
||||
PreparedPath rawPath,
|
||||
PreparedPath currentPath,
|
||||
LocalG2SmoothingRegion region,
|
||||
LocalG2CandidateGeometry candidate,
|
||||
PathSmoothingRequest request,
|
||||
LocalG2OptionsSnapshot options,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
int candidateIndex = candidate == null ? -1 : candidate.CandidateIndex;
|
||||
if (!HasUsableInput(rawPath, currentPath, region, candidate, request, options))
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "局部 G2 候选评价输入无效。");
|
||||
|
||||
PreparedDirectionSegment currentSegment = currentPath.Segments[candidate.SegmentIndex];
|
||||
if (!TryExtractWindow(currentSegment, candidate.StartArcLengthMeters, candidate.EndArcLengthMeters,
|
||||
out IReadOnlyList<SmoothingPoint2D> rawWindow, out string reason) ||
|
||||
!TryAnalyzeWindow(currentSegment, rawWindow, request.Configuration.OutputSpacingMeters,
|
||||
out PathGeometryAnalysis rawAnalysis, out reason) ||
|
||||
!TryAnalyzeWindow(currentSegment, candidate.RegionPoints, request.Configuration.OutputSpacingMeters,
|
||||
out PathGeometryAnalysis candidateAnalysis, out reason) ||
|
||||
!HasFiniteMetrics(rawAnalysis) || !HasFiniteMetrics(candidateAnalysis))
|
||||
{
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed,
|
||||
string.IsNullOrEmpty(reason) ? "局部 G2 区域几何分析失败。" : reason);
|
||||
}
|
||||
|
||||
if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double vehicleMaximumCurvature))
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "车辆曲率约束无效。");
|
||||
if (candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter > vehicleMaximumCurvature + CurvatureRangeTolerance)
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CurvatureExceeded, "局部 G2 候选超过车辆曲率上限。");
|
||||
|
||||
GetCurvatureRange(rawAnalysis.Path, out double rawMinimumCurvature, out double rawMaximumCurvature);
|
||||
if (ExceedsRawCurvatureRange(candidateAnalysis.Path, rawMinimumCurvature, rawMaximumCurvature))
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CurvatureOvershoot, "局部 G2 候选超出原始区域曲率范围。");
|
||||
|
||||
double maximumDeviation = MaximumDistanceToPolyline(candidate.RegionPoints, rawWindow);
|
||||
if (!NumericGuard.IsFinite(maximumDeviation))
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "局部 G2 候选偏差计算失败。");
|
||||
if (maximumDeviation > options.MaximumDeviationMeters)
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.DeviationExceeded, "局部 G2 候选偏离原始窗口过远。");
|
||||
|
||||
if (!_splicer.TryReplace(currentPath, candidate, out PreparedPath spliced, out reason) ||
|
||||
!_analyzer.TryAnalyze(spliced.Segments, request.Configuration.OutputSpacingMeters, out PathGeometryAnalysis fullAnalysis, out reason) ||
|
||||
!HasFiniteMetrics(fullAnalysis))
|
||||
{
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed,
|
||||
string.IsNullOrEmpty(reason) ? "局部 G2 完整路径几何分析失败。" : reason);
|
||||
}
|
||||
|
||||
if (!_validator.TryValidate(fullAnalysis.Path, fullAnalysis.Segments, rawPath, request.Map, request.Vehicle,
|
||||
request.Configuration.MaximumCollisionCheckStepMeters, out IReadOnlyList<SmoothedPathPoint> safePath,
|
||||
out double minimumClearance, out reason))
|
||||
{
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.Collision,
|
||||
string.IsNullOrEmpty(reason) ? "局部 G2 完整路径安全复核失败。" : reason);
|
||||
}
|
||||
if (!NumericGuard.IsFinite(minimumClearance) || minimumClearance < request.Configuration.MinimumClearanceReserveMeters)
|
||||
return new LocalG2CandidateEvaluation(false, PathSmoothingRegionFailureReason.InsufficientClearance, candidateIndex,
|
||||
null, EmptyPath, EmptySegments, rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter, rawAnalysis.CurvatureVariationCost,
|
||||
candidateAnalysis.CurvatureVariationCost, maximumDeviation, minimumClearance,
|
||||
candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter, 0d, "局部 G2 候选净空不足。");
|
||||
|
||||
if (candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter >
|
||||
rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter *
|
||||
(1d - options.MinimumPeakGradientImprovementRatio))
|
||||
{
|
||||
return new LocalG2CandidateEvaluation(false, PathSmoothingRegionFailureReason.InsufficientImprovement, candidateIndex,
|
||||
null, EmptyPath, EmptySegments, rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter, rawAnalysis.CurvatureVariationCost,
|
||||
candidateAnalysis.CurvatureVariationCost, maximumDeviation, minimumClearance,
|
||||
candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter, 0d, "局部 G2 候选曲率导数峰值改善不足。");
|
||||
}
|
||||
if (candidateAnalysis.CurvatureVariationCost > rawAnalysis.CurvatureVariationCost *
|
||||
(1d + options.MaximumVariationCostRegressionRatio))
|
||||
{
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.VariationCostRegression, "局部 G2 候选曲率变化代价回退。");
|
||||
}
|
||||
|
||||
if (!_analyzer.TryAnalyze(rawPath.Segments, request.Configuration.OutputSpacingMeters, out PathGeometryAnalysis rawFullAnalysis, out reason) ||
|
||||
!HasFiniteMetrics(rawFullAnalysis))
|
||||
{
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed,
|
||||
string.IsNullOrEmpty(reason) ? "原始完整路径几何分析失败。" : reason);
|
||||
}
|
||||
|
||||
return new LocalG2CandidateEvaluation(
|
||||
true,
|
||||
PathSmoothingRegionFailureReason.None,
|
||||
candidateIndex,
|
||||
spliced,
|
||||
safePath,
|
||||
fullAnalysis.Segments,
|
||||
rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
rawAnalysis.CurvatureVariationCost,
|
||||
candidateAnalysis.CurvatureVariationCost,
|
||||
maximumDeviation,
|
||||
minimumClearance,
|
||||
candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
Math.Abs(fullAnalysis.PathLengthMeters - rawFullAnalysis.PathLengthMeters),
|
||||
"Accepted");
|
||||
}
|
||||
|
||||
internal static LocalG2CandidateEvaluation SelectBest(IReadOnlyList<LocalG2CandidateEvaluation> evaluations)
|
||||
{
|
||||
if (evaluations == null) throw new ArgumentNullException(nameof(evaluations));
|
||||
LocalG2CandidateEvaluation best = null;
|
||||
for (int index = 0; index < evaluations.Count; index++)
|
||||
{
|
||||
LocalG2CandidateEvaluation evaluation = evaluations[index];
|
||||
if (evaluation == null || !evaluation.Accepted) continue;
|
||||
if (best == null || Compare(evaluation, best) < 0) best = evaluation;
|
||||
}
|
||||
return best ?? Rejected(-1, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "没有通过质量门的局部 G2 候选。");
|
||||
}
|
||||
|
||||
private static int Compare(LocalG2CandidateEvaluation left, LocalG2CandidateEvaluation right)
|
||||
{
|
||||
int result = left.MaximumDeviationMeters.CompareTo(right.MaximumDeviationMeters);
|
||||
if (result != 0) return result;
|
||||
result = left.ResultPeakCurvatureDerivativePerSquareMeter.CompareTo(right.ResultPeakCurvatureDerivativePerSquareMeter);
|
||||
if (result != 0) return result;
|
||||
result = left.ResultCurvatureVariationCost.CompareTo(right.ResultCurvatureVariationCost);
|
||||
if (result != 0) return result;
|
||||
result = left.AbsolutePathLengthChangeMeters.CompareTo(right.AbsolutePathLengthChangeMeters);
|
||||
return result != 0 ? result : left.CandidateIndex.CompareTo(right.CandidateIndex);
|
||||
}
|
||||
|
||||
private static bool HasUsableInput(PreparedPath rawPath, PreparedPath currentPath, LocalG2SmoothingRegion region,
|
||||
LocalG2CandidateGeometry candidate, PathSmoothingRequest request, LocalG2OptionsSnapshot options)
|
||||
{
|
||||
return rawPath != null && currentPath != null && region != null && candidate != null && request != null && options != null &&
|
||||
request.Configuration != null && request.Map != null && request.Vehicle != null && candidate.SegmentIndex == region.SegmentIndex &&
|
||||
candidate.SegmentIndex >= 0 && candidate.SegmentIndex < currentPath.Segments.Count &&
|
||||
NumericGuard.IsPositiveFinite(request.Configuration.OutputSpacingMeters) &&
|
||||
NumericGuard.IsPositiveFinite(request.Configuration.MaximumCollisionCheckStepMeters) &&
|
||||
NumericGuard.IsFinite(request.Configuration.MinimumClearanceReserveMeters) &&
|
||||
request.Configuration.MinimumClearanceReserveMeters >= 0d;
|
||||
}
|
||||
|
||||
private static bool TryExtractWindow(PreparedDirectionSegment segment, double startArcLength, double endArcLength,
|
||||
out IReadOnlyList<SmoothingPoint2D> window, out string reason)
|
||||
{
|
||||
window = null;
|
||||
reason = string.Empty;
|
||||
if (segment == null || !PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, startArcLength, out SmoothingPoint2D start, out reason) ||
|
||||
!PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, endArcLength, out SmoothingPoint2D end, out reason)) return false;
|
||||
var points = new List<SmoothingPoint2D> { start };
|
||||
for (int index = 0; index < segment.Points.Count; index++)
|
||||
{
|
||||
SmoothingPoint2D point = segment.Points[index];
|
||||
if (point.ArcLength > startArcLength && point.ArcLength < endArcLength &&
|
||||
!SamePosition(points[points.Count - 1], point))
|
||||
{
|
||||
points.Add(point);
|
||||
}
|
||||
}
|
||||
if (SamePosition(points[points.Count - 1], end))
|
||||
points[points.Count - 1] = end;
|
||||
else
|
||||
points.Add(end);
|
||||
window = new ReadOnlyCollection<SmoothingPoint2D>(points);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool SamePosition(SmoothingPoint2D left, SmoothingPoint2D right)
|
||||
{
|
||||
if (left == null || right == null) return false;
|
||||
double x = right.X - left.X;
|
||||
double y = right.Y - left.Y;
|
||||
return x * x + y * y <= WindowPointToleranceMeters * WindowPointToleranceMeters;
|
||||
}
|
||||
|
||||
private bool TryAnalyzeWindow(PreparedDirectionSegment source, IReadOnlyList<SmoothingPoint2D> points, double spacing,
|
||||
out PathGeometryAnalysis analysis, out string reason)
|
||||
{
|
||||
var segment = new PreparedDirectionSegment(0, source.Direction, points, false, false,
|
||||
source.Points[0].ArcLength == points[0].ArcLength ? source.StartVehicleCurvaturePerMeter : null);
|
||||
return _analyzer.TryAnalyze(new[] { segment }, spacing, out analysis, out reason);
|
||||
}
|
||||
|
||||
private static bool HasFiniteMetrics(PathGeometryAnalysis analysis)
|
||||
{
|
||||
return analysis != null && NumericGuard.IsFinite(analysis.PathLengthMeters) &&
|
||||
NumericGuard.IsFinite(analysis.MaximumAbsoluteVehicleCurvaturePerMeter) &&
|
||||
NumericGuard.IsFinite(analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter) &&
|
||||
NumericGuard.IsFinite(analysis.CurvatureVariationCost) && analysis.Path != null && analysis.Path.Count >= 2;
|
||||
}
|
||||
|
||||
private static void GetCurvatureRange(IReadOnlyList<SmoothedPathPoint> path, out double minimum, out double maximum)
|
||||
{
|
||||
minimum = double.PositiveInfinity;
|
||||
maximum = double.NegativeInfinity;
|
||||
for (int index = 0; index < path.Count; index++)
|
||||
{
|
||||
minimum = Math.Min(minimum, path[index].VehicleCurvature);
|
||||
maximum = Math.Max(maximum, path[index].VehicleCurvature);
|
||||
}
|
||||
}
|
||||
|
||||
private static bool ExceedsRawCurvatureRange(IReadOnlyList<SmoothedPathPoint> candidate, double minimum, double maximum)
|
||||
{
|
||||
for (int index = 0; index < candidate.Count; index++)
|
||||
{
|
||||
double curvature = candidate[index].VehicleCurvature;
|
||||
if (curvature < minimum - CurvatureRangeTolerance || curvature > maximum + CurvatureRangeTolerance) return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private static double MaximumDistanceToPolyline(IReadOnlyList<SmoothingPoint2D> candidate, IReadOnlyList<SmoothingPoint2D> raw)
|
||||
{
|
||||
if (candidate == null || raw == null || candidate.Count == 0 || raw.Count < 2) return double.NaN;
|
||||
double maximum = 0d;
|
||||
for (int index = 0; index < candidate.Count; index++)
|
||||
{
|
||||
double nearest = double.PositiveInfinity;
|
||||
for (int segment = 1; segment < raw.Count; segment++)
|
||||
nearest = Math.Min(nearest, PointToSegmentDistance(candidate[index], raw[segment - 1], raw[segment]));
|
||||
maximum = Math.Max(maximum, nearest);
|
||||
}
|
||||
return maximum;
|
||||
}
|
||||
|
||||
private static double PointToSegmentDistance(SmoothingPoint2D point, SmoothingPoint2D start, SmoothingPoint2D end)
|
||||
{
|
||||
double dx = end.X - start.X;
|
||||
double dy = end.Y - start.Y;
|
||||
double lengthSquared = dx * dx + dy * dy;
|
||||
if (!NumericGuard.IsPositiveFinite(lengthSquared)) return double.NaN;
|
||||
double projection = ((point.X - start.X) * dx + (point.Y - start.Y) * dy) / lengthSquared;
|
||||
projection = Math.Max(0d, Math.Min(1d, projection));
|
||||
double nearestX = start.X + projection * dx;
|
||||
double nearestY = start.Y + projection * dy;
|
||||
double distanceX = point.X - nearestX;
|
||||
double distanceY = point.Y - nearestY;
|
||||
return Math.Sqrt(distanceX * distanceX + distanceY * distanceY);
|
||||
}
|
||||
|
||||
private static LocalG2CandidateEvaluation Rejected(int candidateIndex, PathSmoothingRegionFailureReason failureReason, string reason)
|
||||
{
|
||||
return new LocalG2CandidateEvaluation(false, failureReason, candidateIndex, null, EmptyPath, EmptySegments,
|
||||
0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, reason);
|
||||
}
|
||||
|
||||
/// <summary>反射脚本使用的窄范围确定性质量门覆盖入口。</summary>
|
||||
public static class TestHooks
|
||||
{
|
||||
public static EvaluationTestSnapshot Execute(string scenario)
|
||||
{
|
||||
LocalG2CandidateEvaluation evaluation;
|
||||
switch (scenario)
|
||||
{
|
||||
case "TooFar":
|
||||
PreparedPath tooFarPath = CreateWavyPath(1d);
|
||||
evaluation = Evaluate(tooFarPath, CreateCandidate(tooFarPath, 0, 1.12d), 0d);
|
||||
break;
|
||||
case "Overshoot":
|
||||
PreparedPath overshootPath = CreateStraightPath(1d);
|
||||
evaluation = Evaluate(overshootPath, CreateCandidate(overshootPath, 0, 1.15d), 0d);
|
||||
break;
|
||||
case "NoOp":
|
||||
PreparedPath noOpPath = CreateWavyPath(1d);
|
||||
evaluation = Evaluate(noOpPath, CreateCandidateFromPath(noOpPath, 0), 0d);
|
||||
break;
|
||||
case "Oscillating":
|
||||
PreparedPath oscillatingPath = CreateOscillationBaseline(1d);
|
||||
evaluation = Evaluate(oscillatingPath, CreateOscillatingCandidate(oscillatingPath), 0d);
|
||||
break;
|
||||
case "LowClearance":
|
||||
PreparedPath lowClearancePath = CreateWavyPath(0.27d);
|
||||
evaluation = Evaluate(lowClearancePath, CreateCandidateFromPath(lowClearancePath, 0), 0.02d, false, true);
|
||||
break;
|
||||
case "Improved":
|
||||
PreparedPath improvedPath = CreateWavyPath(1d);
|
||||
evaluation = Evaluate(improvedPath, CreateCandidate(improvedPath, 0, 1d), 0d);
|
||||
break;
|
||||
case "SmallestDeviation":
|
||||
PreparedPath smallestDeviationPath = CreateWavyPath(1d);
|
||||
evaluation = Evaluate(smallestDeviationPath, CreateCandidate(smallestDeviationPath, 4, 1d), 100d);
|
||||
break;
|
||||
case "Best": evaluation = SelectBest(CreateWavyPath(1d)); break;
|
||||
default: throw new ArgumentOutOfRangeException(nameof(scenario));
|
||||
}
|
||||
return new EvaluationTestSnapshot(evaluation.Accepted ? "Accepted" : "Rejected", evaluation.FailureReason.ToString(), evaluation.CandidateIndex,
|
||||
evaluation.MinimumBodyClearanceMeters, evaluation.Reason);
|
||||
}
|
||||
|
||||
private static LocalG2CandidateEvaluation Evaluate(PreparedPath path, LocalG2CandidateGeometry candidate, double minimumClearance,
|
||||
bool useNearObstacle = false, bool useEmptyMap = false)
|
||||
{
|
||||
PathSmoothingConfiguration configuration = CreateConfiguration(minimumClearance);
|
||||
PathSmoothingRequest request = new PathSmoothingRequest(null, null,
|
||||
useEmptyMap ? CreateEmptyMap() : CreateMap(useNearObstacle), CreateVehicle(), configuration);
|
||||
return new LocalG2CandidateEvaluator().Evaluate(path, path, CreateRegion(), candidate, request,
|
||||
new LocalG2OptionsSnapshot(configuration), CancellationToken.None);
|
||||
}
|
||||
|
||||
private static LocalG2CandidateEvaluation SelectBest(PreparedPath path)
|
||||
{
|
||||
PathSmoothingConfiguration configuration = CreateConfiguration(0d);
|
||||
PathSmoothingRequest request = new PathSmoothingRequest(null, null, CreateMap(false), CreateVehicle(), configuration);
|
||||
var evaluator = new LocalG2CandidateEvaluator();
|
||||
LocalG2CandidateEvaluation smallestDeviation = evaluator.Evaluate(path, path, CreateRegion(),
|
||||
CreateCandidate(path, 4, 1d), request, new LocalG2OptionsSnapshot(configuration), CancellationToken.None);
|
||||
LocalG2CandidateEvaluation smootherButFarther = evaluator.Evaluate(path, path, CreateRegion(),
|
||||
CreateCandidate(path, 5, 1.01d), request, new LocalG2OptionsSnapshot(configuration), CancellationToken.None);
|
||||
return LocalG2CandidateEvaluator.SelectBest(new[] { smootherButFarther, smallestDeviation });
|
||||
}
|
||||
|
||||
private static PreparedPath CreateWavyPath(double startX)
|
||||
{
|
||||
return CreatePath(new[]
|
||||
{
|
||||
Point(startX, 1d), Point(startX + 0.10d, 1.04d), Point(startX + 0.20d, 1.08d),
|
||||
Point(startX + 0.30d, 1.05d), Point(startX + 0.40d, 0.98d), Point(startX + 0.50d, 0.92d),
|
||||
Point(startX + 0.60d, 0.98d), Point(startX + 0.70d, 1.05d), Point(startX + 0.80d, 1.08d),
|
||||
Point(startX + 0.90d, 1.04d), Point(startX + 1d, 1d),
|
||||
});
|
||||
}
|
||||
|
||||
private static PreparedPath CreateStraightPath(double startX) => CreatePath(new[] { Point(startX, 1d), Point(startX + 0.5d, 1d), Point(startX + 1d, 1d) });
|
||||
|
||||
private static PreparedPath CreateOscillationBaseline(double startX)
|
||||
{
|
||||
return CreatePath(new[]
|
||||
{
|
||||
Point(startX, 1d), Point(startX + 0.05d, 1.022d), Point(startX + 0.10d, 1d),
|
||||
Point(startX + 2.03d, 1d),
|
||||
});
|
||||
}
|
||||
|
||||
private static PreparedPath CreatePath(IReadOnlyList<SmoothingPoint2D> points)
|
||||
{
|
||||
var normalized = new List<SmoothingPoint2D>(points.Count);
|
||||
double arcLength = 0d;
|
||||
for (int index = 0; index < points.Count; index++)
|
||||
{
|
||||
if (index > 0)
|
||||
{
|
||||
double dx = points[index].X - points[index - 1].X;
|
||||
double dy = points[index].Y - points[index - 1].Y;
|
||||
arcLength += Math.Sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
normalized.Add(new SmoothingPoint2D(points[index].X, points[index].Y, arcLength, 0d, 0d, 1d,
|
||||
false, SmoothedPathPointSource.LocalG2Transition));
|
||||
}
|
||||
return new PreparedPath(new[] { new PreparedDirectionSegment(0, TravelDirection.Forward, normalized, false, false) });
|
||||
}
|
||||
|
||||
private static LocalG2CandidateGeometry CreateCandidate(PreparedPath path, int index, double middleY)
|
||||
{
|
||||
IReadOnlyList<SmoothingPoint2D> source = path.Segments[0].Points;
|
||||
SmoothingPoint2D start = source[0];
|
||||
SmoothingPoint2D end = source[source.Count - 1];
|
||||
return new LocalG2CandidateGeometry(index, 0, start.ArcLength, end.ArcLength, 0d, 0d,
|
||||
new[] { Point(start.X, start.Y), Point((start.X + end.X) / 2d, middleY), Point(end.X, end.Y) },
|
||||
0d, 0d, 0d, 0d, true);
|
||||
}
|
||||
|
||||
private static LocalG2CandidateGeometry CreateCandidateFromPath(PreparedPath path, int index)
|
||||
{
|
||||
IReadOnlyList<SmoothingPoint2D> points = path.Segments[0].Points;
|
||||
return new LocalG2CandidateGeometry(index, 0, points[0].ArcLength, points[points.Count - 1].ArcLength, 0d, 0d, points,
|
||||
0d, 0d, 0d, 0d, true);
|
||||
}
|
||||
|
||||
private static LocalG2CandidateGeometry CreateOscillatingCandidate(PreparedPath path)
|
||||
{
|
||||
IReadOnlyList<SmoothingPoint2D> source = path.Segments[0].Points;
|
||||
SmoothingPoint2D start = source[0];
|
||||
SmoothingPoint2D end = source[source.Count - 1];
|
||||
var points = new List<SmoothingPoint2D> { Point(start.X, start.Y) };
|
||||
SmoothingPoint2D straightStart = source[source.Count - 2];
|
||||
points.Add(Point(straightStart.X, straightStart.Y));
|
||||
for (int index = 1; index < 20; index++)
|
||||
{
|
||||
double x = straightStart.X + index * (end.X - straightStart.X) / 20d;
|
||||
points.Add(Point(x, straightStart.Y + (index % 2 == 0 ? 0.004d : -0.004d)));
|
||||
}
|
||||
points.Add(Point(end.X, end.Y));
|
||||
return new LocalG2CandidateGeometry(0, 0, start.ArcLength, end.ArcLength, 0d, 0d, points, 0d, 0d, 0d, 0d, true);
|
||||
}
|
||||
|
||||
private static LocalG2SmoothingRegion CreateRegion()
|
||||
{
|
||||
return new LocalG2SmoothingRegion(0,
|
||||
new[] { new CurvatureTransition(0, 1, 2, 0.5d, 0.5d, 0d, 0d, 0d, 0d) },
|
||||
0d, 1d, new[] { new LocalG2WindowVariant(0, 0d, 1d, 0d, 0d) });
|
||||
}
|
||||
|
||||
private static PathSmoothingConfiguration CreateConfiguration(double minimumClearance)
|
||||
{
|
||||
var configuration = new PathSmoothingConfiguration
|
||||
{
|
||||
OutputSpacingMeters = 0.05d,
|
||||
MaximumCollisionCheckStepMeters = 0.05d,
|
||||
MinimumClearanceReserveMeters = minimumClearance,
|
||||
};
|
||||
configuration.LocalG2Quintic.MaximumDeviationMeters = 0.10d;
|
||||
configuration.LocalG2Quintic.MinimumPeakGradientImprovementRatio = 0.20d;
|
||||
configuration.LocalG2Quintic.MaximumVariationCostRegressionRatio = 0.02d;
|
||||
return configuration;
|
||||
}
|
||||
|
||||
private static VehicleParameters CreateVehicle() => new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.20d, WidthMeters = 0.20d, SafetyMarginMeters = 0d,
|
||||
MaximumCurvaturePerMeter = 1000000d, MinimumTurningRadiusMeters = 0.000001d,
|
||||
};
|
||||
|
||||
private static PlanningGridMap CreateMap(bool useNearObstacle)
|
||||
{
|
||||
var request = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 4000f, 0f, 4000f), ResolutionMm = 20f,
|
||||
ObstacleSources = new IMapObstacleSource[]
|
||||
{
|
||||
new ManualObstacleSource("local-g2-evaluator", 1, true, new IMapObstacle[]
|
||||
{
|
||||
useNearObstacle
|
||||
? new AxisAlignedRectangleObstacle(100f, 150f, 900f, 1100f)
|
||||
: new AxisAlignedRectangleObstacle(3000f, 3100f, 3000f, 3100f),
|
||||
}),
|
||||
},
|
||||
};
|
||||
PlanningMapBuildResult result = new PlanningMapFactory().Create(request);
|
||||
if (!result.Succeeded || result.Map == null) throw new InvalidOperationException("测试地图创建失败。");
|
||||
return result.Map;
|
||||
}
|
||||
|
||||
private static PlanningGridMap CreateEmptyMap()
|
||||
{
|
||||
var request = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 4000f, 0f, 4000f), ResolutionMm = 20f,
|
||||
ObstacleSources = Array.Empty<IMapObstacleSource>(), AllowExplicitEmptyMap = true,
|
||||
};
|
||||
PlanningMapBuildResult result = new PlanningMapFactory().Create(request);
|
||||
if (!result.Succeeded || result.Map == null) throw new InvalidOperationException("测试空地图创建失败。");
|
||||
return result.Map;
|
||||
}
|
||||
|
||||
private static SmoothingPoint2D Point(double x, double y) =>
|
||||
new SmoothingPoint2D(x, y, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.LocalG2Transition);
|
||||
|
||||
public sealed class EvaluationTestSnapshot
|
||||
{
|
||||
internal EvaluationTestSnapshot(string status, string failureReason, int candidateIndex, double minimumClearanceMeters, string reason)
|
||||
{
|
||||
Status = status;
|
||||
FailureReason = failureReason;
|
||||
CandidateIndex = candidateIndex;
|
||||
MinimumClearanceMeters = minimumClearanceMeters;
|
||||
Reason = reason;
|
||||
}
|
||||
public string Status { get; }
|
||||
public string FailureReason { get; }
|
||||
public int CandidateIndex { get; }
|
||||
public double MinimumClearanceMeters { get; }
|
||||
public string Reason { get; }
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>不可变的局部 G2 候选质量与安全评价结果。</summary>
|
||||
internal sealed class LocalG2CandidateEvaluation
|
||||
{
|
||||
internal LocalG2CandidateEvaluation(bool accepted, PathSmoothingRegionFailureReason failureReason, int candidateIndex,
|
||||
PreparedPath splicedPreparedPath, IReadOnlyList<SmoothedPathPoint> safePath, IReadOnlyList<SmoothedPathSegment> safeSegments,
|
||||
double rawPeakCurvatureDerivativePerSquareMeter, double resultPeakCurvatureDerivativePerSquareMeter,
|
||||
double rawCurvatureVariationCost, double resultCurvatureVariationCost, double maximumDeviationMeters,
|
||||
double minimumBodyClearanceMeters, double maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
double absolutePathLengthChangeMeters, string reason)
|
||||
{
|
||||
Accepted = accepted;
|
||||
FailureReason = failureReason;
|
||||
CandidateIndex = candidateIndex;
|
||||
SplicedPreparedPath = splicedPreparedPath;
|
||||
SafePath = Copy(safePath);
|
||||
SafeSegments = Copy(safeSegments);
|
||||
RawPeakCurvatureDerivativePerSquareMeter = rawPeakCurvatureDerivativePerSquareMeter;
|
||||
ResultPeakCurvatureDerivativePerSquareMeter = resultPeakCurvatureDerivativePerSquareMeter;
|
||||
RawCurvatureVariationCost = rawCurvatureVariationCost;
|
||||
ResultCurvatureVariationCost = resultCurvatureVariationCost;
|
||||
MaximumDeviationMeters = maximumDeviationMeters;
|
||||
MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
|
||||
MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
|
||||
AbsolutePathLengthChangeMeters = absolutePathLengthChangeMeters;
|
||||
Reason = reason ?? string.Empty;
|
||||
}
|
||||
|
||||
internal bool Accepted { get; }
|
||||
internal PathSmoothingRegionFailureReason FailureReason { get; }
|
||||
internal int CandidateIndex { get; }
|
||||
internal PreparedPath SplicedPreparedPath { get; }
|
||||
internal IReadOnlyList<SmoothedPathPoint> SafePath { get; }
|
||||
internal IReadOnlyList<SmoothedPathSegment> SafeSegments { get; }
|
||||
internal double RawPeakCurvatureDerivativePerSquareMeter { get; }
|
||||
internal double ResultPeakCurvatureDerivativePerSquareMeter { get; }
|
||||
internal double RawCurvatureVariationCost { get; }
|
||||
internal double ResultCurvatureVariationCost { get; }
|
||||
internal double MaximumDeviationMeters { get; }
|
||||
internal double MinimumBodyClearanceMeters { get; }
|
||||
internal double MaximumAbsoluteVehicleCurvaturePerMeter { get; }
|
||||
internal double AbsolutePathLengthChangeMeters { get; }
|
||||
internal string Reason { get; }
|
||||
|
||||
private static IReadOnlyList<T> Copy<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source == null ? 0 : source.Count);
|
||||
if (source != null) for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,76 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>一个尚未经过安全和质量评价的局部 G2 替换几何。</summary>
|
||||
internal sealed class LocalG2CandidateGeometry
|
||||
{
|
||||
internal LocalG2CandidateGeometry(
|
||||
int candidateIndex,
|
||||
int segmentIndex,
|
||||
double startArcLengthMeters,
|
||||
double endArcLengthMeters,
|
||||
double leftWindowLengthMeters,
|
||||
double rightWindowLengthMeters,
|
||||
IReadOnlyList<SmoothingPoint2D> regionPoints,
|
||||
double startVehicleCurvaturePerMeter,
|
||||
double endVehicleCurvaturePerMeter,
|
||||
double startGeometricCurvaturePerMeter,
|
||||
double endGeometricCurvaturePerMeter,
|
||||
bool internalConnectionsAreG2)
|
||||
{
|
||||
if (candidateIndex < 0 || segmentIndex < 0 || !NumericGuard.IsFinite(startArcLengthMeters) ||
|
||||
!NumericGuard.IsFinite(endArcLengthMeters) || startArcLengthMeters < 0d ||
|
||||
endArcLengthMeters < startArcLengthMeters || !NumericGuard.IsFinite(leftWindowLengthMeters) ||
|
||||
!NumericGuard.IsFinite(rightWindowLengthMeters) || leftWindowLengthMeters < 0d ||
|
||||
rightWindowLengthMeters < 0d || regionPoints == null || regionPoints.Count < 2 ||
|
||||
!NumericGuard.IsFinite(startVehicleCurvaturePerMeter) || !NumericGuard.IsFinite(endVehicleCurvaturePerMeter) ||
|
||||
!NumericGuard.IsFinite(startGeometricCurvaturePerMeter) || !NumericGuard.IsFinite(endGeometricCurvaturePerMeter))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(regionPoints));
|
||||
}
|
||||
|
||||
CandidateIndex = candidateIndex;
|
||||
SegmentIndex = segmentIndex;
|
||||
StartArcLengthMeters = startArcLengthMeters;
|
||||
EndArcLengthMeters = endArcLengthMeters;
|
||||
LeftWindowLengthMeters = leftWindowLengthMeters;
|
||||
RightWindowLengthMeters = rightWindowLengthMeters;
|
||||
RegionPoints = Copy(regionPoints);
|
||||
StartVehicleCurvaturePerMeter = startVehicleCurvaturePerMeter;
|
||||
EndVehicleCurvaturePerMeter = endVehicleCurvaturePerMeter;
|
||||
StartGeometricCurvaturePerMeter = startGeometricCurvaturePerMeter;
|
||||
EndGeometricCurvaturePerMeter = endGeometricCurvaturePerMeter;
|
||||
InternalConnectionsAreG2 = internalConnectionsAreG2;
|
||||
}
|
||||
|
||||
internal int CandidateIndex { get; }
|
||||
internal int SegmentIndex { get; }
|
||||
internal double StartArcLengthMeters { get; }
|
||||
internal double EndArcLengthMeters { get; }
|
||||
internal double LeftWindowLengthMeters { get; }
|
||||
internal double RightWindowLengthMeters { get; }
|
||||
internal IReadOnlyList<SmoothingPoint2D> RegionPoints { get; }
|
||||
|
||||
// 这些边界状态供候选评价和窄范围反射验证使用，不能替代统一几何分析器的最终统计。
|
||||
internal double StartVehicleCurvaturePerMeter { get; }
|
||||
internal double EndVehicleCurvaturePerMeter { get; }
|
||||
internal double StartGeometricCurvaturePerMeter { get; }
|
||||
internal double EndGeometricCurvaturePerMeter { get; }
|
||||
internal bool InternalConnectionsAreG2 { get; }
|
||||
|
||||
private static IReadOnlyList<SmoothingPoint2D> Copy(IReadOnlyList<SmoothingPoint2D> source)
|
||||
{
|
||||
var copy = new List<SmoothingPoint2D>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
if (source[index] == null) throw new ArgumentOutOfRangeException(nameof(source));
|
||||
copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<SmoothingPoint2D>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>局部 G2 预平滑一次运行使用的已校验不可变选项。</summary>
|
||||
internal sealed class LocalG2OptionsSnapshot
|
||||
{
|
||||
internal LocalG2OptionsSnapshot(PathSmoothingConfiguration configuration)
|
||||
{
|
||||
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
||||
LocalG2QuinticOptions source = configuration.LocalG2Quintic;
|
||||
if (source == null) throw new ArgumentOutOfRangeException(nameof(configuration));
|
||||
|
||||
MinimumWindowLengthMeters = source.MinimumWindowLengthMeters;
|
||||
PreferredWindowLengthMeters = source.PreferredWindowLengthMeters;
|
||||
MaximumWindowLengthMeters = source.MaximumWindowLengthMeters;
|
||||
MaximumDeviationMeters = source.MaximumDeviationMeters;
|
||||
AbsoluteCurvatureJumpFloorPerMeter = source.AbsoluteCurvatureJumpFloorPerMeter;
|
||||
CurvatureJumpRatioOfMaximum = source.CurvatureJumpRatioOfMaximum;
|
||||
MinimumPeakGradientImprovementRatio = source.MinimumPeakGradientImprovementRatio;
|
||||
MaximumVariationCostRegressionRatio = source.MaximumVariationCostRegressionRatio;
|
||||
MaximumCandidatesPerRegion = source.MaximumCandidatesPerRegion;
|
||||
|
||||
if (!NumericGuard.IsPositiveFinite(MinimumWindowLengthMeters))
|
||||
throw new ArgumentOutOfRangeException(nameof(MinimumWindowLengthMeters));
|
||||
if (!NumericGuard.IsFinite(PreferredWindowLengthMeters) || PreferredWindowLengthMeters < MinimumWindowLengthMeters)
|
||||
throw new ArgumentOutOfRangeException(nameof(PreferredWindowLengthMeters));
|
||||
if (!NumericGuard.IsFinite(MaximumWindowLengthMeters) || MaximumWindowLengthMeters < PreferredWindowLengthMeters)
|
||||
throw new ArgumentOutOfRangeException(nameof(MaximumWindowLengthMeters));
|
||||
if (!NumericGuard.IsPositiveFinite(MaximumDeviationMeters))
|
||||
throw new ArgumentOutOfRangeException(nameof(MaximumDeviationMeters));
|
||||
if (!NumericGuard.IsPositiveFinite(AbsoluteCurvatureJumpFloorPerMeter))
|
||||
throw new ArgumentOutOfRangeException(nameof(AbsoluteCurvatureJumpFloorPerMeter));
|
||||
if (!NumericGuard.IsFinite(CurvatureJumpRatioOfMaximum) || CurvatureJumpRatioOfMaximum <= 0d || CurvatureJumpRatioOfMaximum > 1d)
|
||||
throw new ArgumentOutOfRangeException(nameof(CurvatureJumpRatioOfMaximum));
|
||||
if (!NumericGuard.IsFinite(MinimumPeakGradientImprovementRatio) ||
|
||||
MinimumPeakGradientImprovementRatio <= 0d || MinimumPeakGradientImprovementRatio >= 1d)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(MinimumPeakGradientImprovementRatio));
|
||||
}
|
||||
if (!NumericGuard.IsFinite(MaximumVariationCostRegressionRatio) || MaximumVariationCostRegressionRatio < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(MaximumVariationCostRegressionRatio));
|
||||
if (MaximumCandidatesPerRegion < 1)
|
||||
throw new ArgumentOutOfRangeException(nameof(MaximumCandidatesPerRegion));
|
||||
}
|
||||
|
||||
internal double MinimumWindowLengthMeters { get; }
|
||||
internal double PreferredWindowLengthMeters { get; }
|
||||
internal double MaximumWindowLengthMeters { get; }
|
||||
internal double MaximumDeviationMeters { get; }
|
||||
internal double AbsoluteCurvatureJumpFloorPerMeter { get; }
|
||||
internal double CurvatureJumpRatioOfMaximum { get; }
|
||||
internal double MinimumPeakGradientImprovementRatio { get; }
|
||||
internal double MaximumVariationCostRegressionRatio { get; }
|
||||
internal int MaximumCandidatesPerRegion { get; }
|
||||
}
|
||||
@@ -0,0 +1,134 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>只替换一个方向段内局部窗口，并保持其余方向拓扑不变。</summary>
|
||||
internal sealed class LocalG2PathSplicer
|
||||
{
|
||||
internal bool TryReplace(
|
||||
PreparedPath currentPath,
|
||||
LocalG2CandidateGeometry candidate,
|
||||
out PreparedPath replacedPath,
|
||||
out string reason)
|
||||
{
|
||||
replacedPath = null;
|
||||
reason = string.Empty;
|
||||
if (currentPath == null || candidate == null || candidate.SegmentIndex < 0 ||
|
||||
candidate.SegmentIndex >= currentPath.Segments.Count || candidate.RegionPoints == null ||
|
||||
candidate.RegionPoints.Count < 2)
|
||||
{
|
||||
reason = "局部 G2 拼接输入无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
PreparedDirectionSegment source = currentPath.Segments[candidate.SegmentIndex];
|
||||
if (source == null || source.SegmentIndex != candidate.SegmentIndex ||
|
||||
!PathReferenceInterpolator.TryInterpolateByArcLength(source.Points, candidate.StartArcLengthMeters, out SmoothingPoint2D start, out reason) ||
|
||||
!PathReferenceInterpolator.TryInterpolateByArcLength(source.Points, candidate.EndArcLengthMeters, out SmoothingPoint2D end, out reason) ||
|
||||
!SamePosition(candidate.RegionPoints[0], start) || !SamePosition(candidate.RegionPoints[candidate.RegionPoints.Count - 1], end))
|
||||
{
|
||||
if (string.IsNullOrEmpty(reason)) reason = "局部 G2 候选端点不匹配原始窗口。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var combined = new List<SmoothingPoint2D>();
|
||||
for (int index = 0; index < source.Points.Count; index++)
|
||||
{
|
||||
SmoothingPoint2D point = source.Points[index];
|
||||
if (point.ArcLength < candidate.StartArcLengthMeters) AddWithoutNonGearDuplicates(combined, point);
|
||||
}
|
||||
// Candidate endpoints are only validated within a numerical tolerance. The replacement
|
||||
// itself must use the exact source-window endpoints, including a possible gear marker.
|
||||
AddWithoutNonGearDuplicates(combined, start);
|
||||
for (int index = 1; index < candidate.RegionPoints.Count - 1; index++)
|
||||
AddWithoutNonGearDuplicates(combined, candidate.RegionPoints[index]);
|
||||
AddWithoutNonGearDuplicates(combined, end);
|
||||
for (int index = 0; index < source.Points.Count; index++)
|
||||
{
|
||||
SmoothingPoint2D point = source.Points[index];
|
||||
if (point.ArcLength > candidate.EndArcLengthMeters) AddWithoutNonGearDuplicates(combined, point);
|
||||
}
|
||||
if (!TryRecalculateArcLengths(combined, out IReadOnlyList<SmoothingPoint2D> localPoints, out reason)) return false;
|
||||
|
||||
var segments = new List<PreparedDirectionSegment>(currentPath.Segments.Count);
|
||||
for (int index = 0; index < currentPath.Segments.Count; index++)
|
||||
{
|
||||
PreparedDirectionSegment segment = currentPath.Segments[index];
|
||||
if (index != candidate.SegmentIndex)
|
||||
{
|
||||
segments.Add(segment);
|
||||
continue;
|
||||
}
|
||||
segments.Add(new PreparedDirectionSegment(segment.SegmentIndex, segment.Direction, localPoints,
|
||||
segment.StartsAtGearSwitch, segment.EndsAtGearSwitch, segment.StartVehicleCurvaturePerMeter));
|
||||
}
|
||||
replacedPath = new PreparedPath(segments);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryRecalculateArcLengths(
|
||||
IReadOnlyList<SmoothingPoint2D> points,
|
||||
out IReadOnlyList<SmoothingPoint2D> recalculated,
|
||||
out string reason)
|
||||
{
|
||||
recalculated = null;
|
||||
reason = string.Empty;
|
||||
if (points == null || points.Count < 2)
|
||||
{
|
||||
reason = "局部 G2 拼接后方向段没有足够点。";
|
||||
return false;
|
||||
}
|
||||
var output = new List<SmoothingPoint2D>(points.Count);
|
||||
double arcLength = 0d;
|
||||
for (int index = 0; index < points.Count; index++)
|
||||
{
|
||||
SmoothingPoint2D point = points[index];
|
||||
if (!IsValid(point))
|
||||
{
|
||||
reason = "局部 G2 拼接点包含非法数值。";
|
||||
return false;
|
||||
}
|
||||
if (index > 0)
|
||||
{
|
||||
double distance = Distance(points[index - 1], point);
|
||||
if (!NumericGuard.IsPositiveFinite(distance))
|
||||
{
|
||||
reason = "局部 G2 拼接后存在重复非换向点。";
|
||||
return false;
|
||||
}
|
||||
arcLength += distance;
|
||||
}
|
||||
output.Add(new SmoothingPoint2D(point.X, point.Y, arcLength, point.Heading, point.UnwrappedHeading,
|
||||
point.BodyClearance, point.IsGearSwitchPoint, point.Source));
|
||||
}
|
||||
recalculated = output;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static void AddWithoutNonGearDuplicates(List<SmoothingPoint2D> output, SmoothingPoint2D point)
|
||||
{
|
||||
if (output.Count == 0)
|
||||
{
|
||||
output.Add(point);
|
||||
return;
|
||||
}
|
||||
SmoothingPoint2D previous = output[output.Count - 1];
|
||||
if (!previous.IsGearSwitchPoint && !point.IsGearSwitchPoint && SamePosition(previous, point)) return;
|
||||
output.Add(point);
|
||||
}
|
||||
|
||||
private static bool IsValid(SmoothingPoint2D point) => point != null && NumericGuard.IsFinite(point.X) &&
|
||||
NumericGuard.IsFinite(point.Y) && NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
|
||||
NumericGuard.IsFinite(point.BodyClearance) && point.BodyClearance >= 0d;
|
||||
private static bool SamePosition(SmoothingPoint2D left, SmoothingPoint2D right) =>
|
||||
left != null && right != null && Distance(left, right) <= 1e-9d;
|
||||
private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right)
|
||||
{
|
||||
double dx = right.X - left.X;
|
||||
double dy = right.Y - left.Y;
|
||||
return Math.Sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,234 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>Publishes independently validated Local G2 regional replacements through the dedicated service route.</summary>
|
||||
internal sealed class LocalG2PreSmoothingPipeline
|
||||
{
|
||||
private readonly CurvatureTransitionDetector _detector = new CurvatureTransitionDetector();
|
||||
private readonly LocalG2WindowPlanner _windowPlanner = new LocalG2WindowPlanner();
|
||||
private readonly LocalG2CandidateBuilder _builder = new LocalG2CandidateBuilder();
|
||||
private readonly LocalG2CandidateEvaluator _evaluator = new LocalG2CandidateEvaluator();
|
||||
private readonly LocalG2RegionWorkOrder _workOrder = new LocalG2RegionWorkOrder();
|
||||
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
|
||||
private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
|
||||
|
||||
internal PathSmoothingResult Smooth(
|
||||
PathSmoothingRequest request,
|
||||
PreparedPath preparedPath,
|
||||
RawPathBaseline rawBaseline,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
try
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
if (request == null || preparedPath == null || rawBaseline == null ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double maximumCurvature))
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, "局部 G2 预平滑输入无效。");
|
||||
}
|
||||
|
||||
var options = new LocalG2OptionsSnapshot(request.Configuration);
|
||||
if (!_detector.TryDetect(request, maximumCurvature, options, out IReadOnlyList<CurvatureTransition> transitions, out string reason) ||
|
||||
!_windowPlanner.TryPlan(preparedPath, transitions, options, out IReadOnlyList<LocalG2SmoothingRegion> regions, out reason))
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, reason);
|
||||
}
|
||||
|
||||
IReadOnlyList<LocalG2SmoothingRegion> reportOrder =
|
||||
new ReadOnlyCollection<LocalG2SmoothingRegion>(new List<LocalG2SmoothingRegion>(regions));
|
||||
if (!_workOrder.TryCreate(reportOrder, out IReadOnlyList<LocalG2SmoothingRegion> workRegions, out string orderReason))
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, orderReason);
|
||||
|
||||
PreparedPath current = preparedPath;
|
||||
var reportsByRegion = new Dictionary<LocalG2SmoothingRegion, PathSmoothingRegionReport>();
|
||||
var accepted = new List<AcceptedRegion>();
|
||||
int improvedCount = 0;
|
||||
|
||||
foreach (LocalG2SmoothingRegion region in workRegions)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
IReadOnlyList<LocalG2CandidateGeometry> candidates = _builder.Build(
|
||||
preparedPath.Segments[region.SegmentIndex], region,
|
||||
request.Configuration.OutputSpacingMeters, options, cancellationToken);
|
||||
var evaluations = new List<LocalG2CandidateEvaluation>();
|
||||
for (int candidateIndex = 0; candidateIndex < candidates.Count; candidateIndex++)
|
||||
evaluations.Add(_evaluator.Evaluate(
|
||||
preparedPath, current, region, candidates[candidateIndex], request, options, cancellationToken));
|
||||
|
||||
LocalG2CandidateEvaluation best = LocalG2CandidateEvaluator.SelectBest(evaluations);
|
||||
if (best.Accepted)
|
||||
{
|
||||
accepted.Add(new AcceptedRegion(region, current, best));
|
||||
current = best.SplicedPreparedPath;
|
||||
improvedCount++;
|
||||
reportsByRegion.Add(region, CreateImprovedReport(region, candidates.Count, best));
|
||||
}
|
||||
else
|
||||
{
|
||||
reportsByRegion.Add(region, CreateRetainedReport(region, candidates.Count, best));
|
||||
}
|
||||
}
|
||||
|
||||
if (!TryValidateFinal(current, preparedPath, rawBaseline, request, options, out IReadOnlyList<SmoothedPathPoint> path,
|
||||
out IReadOnlyList<SmoothedPathSegment> segments, out PathQualityMetrics metrics, out reason))
|
||||
{
|
||||
for (int index = accepted.Count - 1; index >= 0; index--)
|
||||
{
|
||||
AcceptedRegion rollback = accepted[index];
|
||||
current = rollback.Before;
|
||||
reportsByRegion[rollback.Region] = CreateRollbackReport(rollback.Region, rollback.Evaluation);
|
||||
improvedCount--;
|
||||
if (TryValidateFinal(current, preparedPath, rawBaseline, request, options, out path, out segments, out metrics, out reason))
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (metrics == null)
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, reason);
|
||||
|
||||
var reports = new List<PathSmoothingRegionReport>(reportOrder.Count);
|
||||
for (int reportIndex = 0; reportIndex < reportOrder.Count; reportIndex++)
|
||||
reports.Add(reportsByRegion[reportOrder[reportIndex]]);
|
||||
|
||||
PathSmoothingStatus status;
|
||||
if (transitions.Count == 0) status = PathSmoothingStatus.NotNeeded;
|
||||
else if (improvedCount == regions.Count) status = PathSmoothingStatus.Complete;
|
||||
else if (improvedCount > 0) status = PathSmoothingStatus.PartialImprovement;
|
||||
else status = PathSmoothingStatus.Unchanged;
|
||||
return PathSmoothingResult.PublishLocalG2(
|
||||
status,
|
||||
path,
|
||||
segments,
|
||||
new PathSmoothingDiagnostics(metrics, stopwatch.Elapsed, 0, 0d, reason ?? string.Empty),
|
||||
reports);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Cancelled, stopwatch, "路径平滑已取消。");
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, exception.Message);
|
||||
}
|
||||
}
|
||||
|
||||
private bool TryValidateFinal(
|
||||
PreparedPath current,
|
||||
PreparedPath rawPath,
|
||||
RawPathBaseline rawBaseline,
|
||||
PathSmoothingRequest request,
|
||||
LocalG2OptionsSnapshot options,
|
||||
out IReadOnlyList<SmoothedPathPoint> path,
|
||||
out IReadOnlyList<SmoothedPathSegment> segments,
|
||||
out PathQualityMetrics metrics,
|
||||
out string reason)
|
||||
{
|
||||
path = null;
|
||||
segments = null;
|
||||
metrics = null;
|
||||
reason = string.Empty;
|
||||
if (!_analyzer.TryAnalyze(current.Segments, request.Configuration.OutputSpacingMeters, out PathGeometryAnalysis analysis, out reason) ||
|
||||
!_validator.TryValidate(analysis.Path, analysis.Segments, rawPath, request.Map, request.Vehicle,
|
||||
request.Configuration.MaximumCollisionCheckStepMeters, out IReadOnlyList<SmoothedPathPoint> safePath,
|
||||
out double minimumClearance, out reason) ||
|
||||
minimumClearance < request.Configuration.MinimumClearanceReserveMeters ||
|
||||
analysis.CurvatureVariationCost > rawBaseline.Metrics.CurvatureVariationCost *
|
||||
(1d + options.MaximumVariationCostRegressionRatio))
|
||||
{
|
||||
if (string.IsNullOrEmpty(reason)) reason = "局部 G2 完整路径复核未通过。";
|
||||
return false;
|
||||
}
|
||||
|
||||
path = safePath;
|
||||
segments = analysis.Segments;
|
||||
metrics = new PathQualityMetrics(
|
||||
true,
|
||||
analysis.PathLengthMeters,
|
||||
analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
analysis.RootMeanSquareVehicleCurvaturePerMeter,
|
||||
analysis.TotalAbsoluteCurvatureVariationPerMeter,
|
||||
analysis.CurvatureVariationCost,
|
||||
minimumClearance,
|
||||
0d, 0d, 0d, 0d);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static PathSmoothingRegionReport CreateImprovedReport(
|
||||
LocalG2SmoothingRegion region,
|
||||
int candidateCount,
|
||||
LocalG2CandidateEvaluation evaluation) =>
|
||||
CreateReport(region, candidateCount, evaluation, PathSmoothingRegionStatus.Improved, PathSmoothingRegionFailureReason.None);
|
||||
|
||||
private static PathSmoothingRegionReport CreateRetainedReport(
|
||||
LocalG2SmoothingRegion region,
|
||||
int candidateCount,
|
||||
LocalG2CandidateEvaluation evaluation) =>
|
||||
CreateReport(region, candidateCount, evaluation, PathSmoothingRegionStatus.RetainedOriginal, evaluation.FailureReason);
|
||||
|
||||
private static PathSmoothingRegionReport CreateRollbackReport(
|
||||
LocalG2SmoothingRegion region,
|
||||
LocalG2CandidateEvaluation evaluation) =>
|
||||
CreateReport(region, region.WindowVariants.Count, evaluation, PathSmoothingRegionStatus.RetainedOriginal,
|
||||
PathSmoothingRegionFailureReason.GlobalValidationRollback);
|
||||
|
||||
private static PathSmoothingRegionReport CreateReport(
|
||||
LocalG2SmoothingRegion region,
|
||||
int candidateCount,
|
||||
LocalG2CandidateEvaluation evaluation,
|
||||
PathSmoothingRegionStatus status,
|
||||
PathSmoothingRegionFailureReason failureReason)
|
||||
{
|
||||
LocalG2WindowVariant window = region.WindowVariants[0];
|
||||
var jumps = new List<double>(region.Transitions.Count);
|
||||
for (int index = 0; index < region.Transitions.Count; index++)
|
||||
jumps.Add(region.Transitions[index].RightVehicleCurvaturePerMeter - region.Transitions[index].LeftVehicleCurvaturePerMeter);
|
||||
return new PathSmoothingRegionReport(
|
||||
region.SegmentIndex,
|
||||
window.StartArcLengthMeters,
|
||||
window.EndArcLengthMeters,
|
||||
jumps,
|
||||
window.EndArcLengthMeters - window.StartArcLengthMeters,
|
||||
window.EndArcLengthMeters - window.StartArcLengthMeters,
|
||||
window.LeftWindowLengthMeters,
|
||||
window.RightWindowLengthMeters,
|
||||
candidateCount,
|
||||
evaluation.CandidateIndex,
|
||||
status,
|
||||
failureReason,
|
||||
evaluation.RawPeakCurvatureDerivativePerSquareMeter,
|
||||
evaluation.ResultPeakCurvatureDerivativePerSquareMeter,
|
||||
evaluation.RawCurvatureVariationCost,
|
||||
evaluation.ResultCurvatureVariationCost,
|
||||
evaluation.MaximumDeviationMeters,
|
||||
evaluation.MinimumBodyClearanceMeters,
|
||||
evaluation.MaximumAbsoluteVehicleCurvaturePerMeter);
|
||||
}
|
||||
|
||||
private static PathSmoothingResult Failure(PathSmoothingStatus status, Stopwatch stopwatch, string reason) =>
|
||||
PathSmoothingResult.Failure(status,
|
||||
new PathSmoothingDiagnostics(new PathQualityMetrics(), stopwatch.Elapsed, 0, 0d, reason));
|
||||
|
||||
private sealed class AcceptedRegion
|
||||
{
|
||||
internal AcceptedRegion(LocalG2SmoothingRegion region, PreparedPath before, LocalG2CandidateEvaluation evaluation)
|
||||
{
|
||||
Region = region;
|
||||
Before = before;
|
||||
Evaluation = evaluation;
|
||||
}
|
||||
|
||||
internal LocalG2SmoothingRegion Region { get; }
|
||||
internal PreparedPath Before { get; }
|
||||
internal LocalG2CandidateEvaluation Evaluation { get; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,107 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>把稳定报告顺序转换为不会使后续原始局部弧长错位的工作顺序。</summary>
|
||||
internal sealed class LocalG2RegionWorkOrder
|
||||
{
|
||||
internal bool TryCreate(
|
||||
IReadOnlyList<LocalG2SmoothingRegion> reportOrder,
|
||||
out IReadOnlyList<LocalG2SmoothingRegion> workOrder,
|
||||
out string reason)
|
||||
{
|
||||
workOrder = Empty();
|
||||
reason = string.Empty;
|
||||
if (reportOrder == null)
|
||||
{
|
||||
reason = "局部 G2 区域工作顺序输入无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var indexed = new List<IndexedRegion>(reportOrder.Count);
|
||||
for (int index = 0; index < reportOrder.Count; index++)
|
||||
{
|
||||
LocalG2SmoothingRegion region = reportOrder[index];
|
||||
if (!TryValidate(region, out double firstArc, out reason))
|
||||
return false;
|
||||
indexed.Add(new IndexedRegion(region, index, firstArc));
|
||||
}
|
||||
|
||||
indexed.Sort(Compare);
|
||||
var ordered = new List<LocalG2SmoothingRegion>(indexed.Count);
|
||||
for (int index = 0; index < indexed.Count; index++)
|
||||
ordered.Add(indexed[index].Region);
|
||||
workOrder = new ReadOnlyCollection<LocalG2SmoothingRegion>(ordered);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryValidate(
|
||||
LocalG2SmoothingRegion region,
|
||||
out double firstArc,
|
||||
out string reason)
|
||||
{
|
||||
firstArc = 0d;
|
||||
reason = string.Empty;
|
||||
if (region == null || region.SegmentIndex < 0 ||
|
||||
region.Transitions == null || region.Transitions.Count == 0 ||
|
||||
region.WindowVariants == null || region.WindowVariants.Count == 0)
|
||||
{
|
||||
reason = "局部 G2 工作顺序包含空区域或空窗口集。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double previousArc = -1d;
|
||||
for (int index = 0; index < region.Transitions.Count; index++)
|
||||
{
|
||||
CurvatureTransition transition = region.Transitions[index];
|
||||
if (transition == null ||
|
||||
transition.SegmentIndex != region.SegmentIndex ||
|
||||
!NumericGuard.IsFinite(transition.LocalArcLengthMeters) ||
|
||||
transition.LocalArcLengthMeters < previousArc)
|
||||
{
|
||||
reason = "局部 G2 工作顺序要求区域事件有限、同段且按弧长升序。";
|
||||
return false;
|
||||
}
|
||||
previousArc = transition.LocalArcLengthMeters;
|
||||
}
|
||||
|
||||
firstArc = region.Transitions[0].LocalArcLengthMeters;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static int Compare(IndexedRegion left, IndexedRegion right)
|
||||
{
|
||||
int segment = left.Region.SegmentIndex.CompareTo(right.Region.SegmentIndex);
|
||||
if (segment != 0) return segment;
|
||||
int descendingArc = right.FirstArc.CompareTo(left.FirstArc);
|
||||
return descendingArc != 0
|
||||
? descendingArc
|
||||
: left.ReportIndex.CompareTo(right.ReportIndex);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<LocalG2SmoothingRegion> Empty()
|
||||
{
|
||||
return new ReadOnlyCollection<LocalG2SmoothingRegion>(
|
||||
new List<LocalG2SmoothingRegion>());
|
||||
}
|
||||
|
||||
private sealed class IndexedRegion
|
||||
{
|
||||
internal IndexedRegion(
|
||||
LocalG2SmoothingRegion region,
|
||||
int reportIndex,
|
||||
double firstArc)
|
||||
{
|
||||
Region = region;
|
||||
ReportIndex = reportIndex;
|
||||
FirstArc = firstArc;
|
||||
}
|
||||
|
||||
internal LocalG2SmoothingRegion Region { get; }
|
||||
internal int ReportIndex { get; }
|
||||
internal double FirstArc { get; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,71 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>一个局部 G2 候选可替换的弧长窗口。</summary>
|
||||
internal sealed class LocalG2WindowVariant
|
||||
{
|
||||
internal LocalG2WindowVariant(int candidateIndex, double startArcLengthMeters, double endArcLengthMeters,
|
||||
double leftWindowLengthMeters, double rightWindowLengthMeters)
|
||||
{
|
||||
if (candidateIndex < 0 || startArcLengthMeters < 0d || endArcLengthMeters < startArcLengthMeters ||
|
||||
leftWindowLengthMeters < 0d || rightWindowLengthMeters < 0d)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(candidateIndex));
|
||||
}
|
||||
CandidateIndex = candidateIndex;
|
||||
StartArcLengthMeters = startArcLengthMeters;
|
||||
EndArcLengthMeters = endArcLengthMeters;
|
||||
LeftWindowLengthMeters = leftWindowLengthMeters;
|
||||
RightWindowLengthMeters = rightWindowLengthMeters;
|
||||
}
|
||||
|
||||
internal int CandidateIndex { get; }
|
||||
internal double StartArcLengthMeters { get; }
|
||||
internal double EndArcLengthMeters { get; }
|
||||
internal double LeftWindowLengthMeters { get; }
|
||||
internal double RightWindowLengthMeters { get; }
|
||||
}
|
||||
|
||||
/// <summary>至少存在一个联合合法生成窗口变体的一组曲率过渡。</summary>
|
||||
internal sealed class LocalG2SmoothingRegion
|
||||
{
|
||||
internal LocalG2SmoothingRegion(
|
||||
int segmentIndex,
|
||||
IReadOnlyList<CurvatureTransition> transitions,
|
||||
double maximumStartArcLengthMeters,
|
||||
double maximumEndArcLengthMeters,
|
||||
IReadOnlyList<LocalG2WindowVariant> windowVariants)
|
||||
{
|
||||
if (segmentIndex < 0 || transitions == null || transitions.Count == 0 ||
|
||||
!NumericGuard.IsFinite(maximumStartArcLengthMeters) ||
|
||||
!NumericGuard.IsFinite(maximumEndArcLengthMeters) ||
|
||||
maximumStartArcLengthMeters < 0d ||
|
||||
maximumEndArcLengthMeters < maximumStartArcLengthMeters ||
|
||||
windowVariants == null || windowVariants.Count == 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(transitions));
|
||||
}
|
||||
SegmentIndex = segmentIndex;
|
||||
Transitions = Copy(transitions);
|
||||
MaximumStartArcLengthMeters = maximumStartArcLengthMeters;
|
||||
MaximumEndArcLengthMeters = maximumEndArcLengthMeters;
|
||||
WindowVariants = Copy(windowVariants);
|
||||
}
|
||||
|
||||
internal int SegmentIndex { get; }
|
||||
internal IReadOnlyList<CurvatureTransition> Transitions { get; }
|
||||
internal double MaximumStartArcLengthMeters { get; }
|
||||
internal double MaximumEndArcLengthMeters { get; }
|
||||
internal IReadOnlyList<LocalG2WindowVariant> WindowVariants { get; }
|
||||
|
||||
private static IReadOnlyList<T> Copy<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,378 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>按硬方向边界生成并合并局部 G2 曲率事件的候选窗口。</summary>
|
||||
internal sealed class LocalG2WindowPlanner
|
||||
{
|
||||
private const double MergeToleranceMeters = 1e-9d;
|
||||
|
||||
internal bool TryPlan(
|
||||
PreparedPath originalPath,
|
||||
IReadOnlyList<CurvatureTransition> transitions,
|
||||
LocalG2OptionsSnapshot options,
|
||||
out IReadOnlyList<LocalG2SmoothingRegion> regions,
|
||||
out string reason)
|
||||
{
|
||||
regions = Empty<LocalG2SmoothingRegion>();
|
||||
reason = string.Empty;
|
||||
if (originalPath == null || transitions == null || options == null)
|
||||
{
|
||||
reason = "局部 G2 窗口规划输入无效。";
|
||||
return false;
|
||||
}
|
||||
if (!TryGetSegmentLengths(originalPath, out Dictionary<int, double> segmentLengths, out reason)) return false;
|
||||
|
||||
var ordered = new List<CurvatureTransition>(transitions.Count);
|
||||
for (int index = 0; index < transitions.Count; index++)
|
||||
{
|
||||
CurvatureTransition transition = transitions[index];
|
||||
if (transition == null || !segmentLengths.TryGetValue(transition.SegmentIndex, out double length) ||
|
||||
!NumericGuard.IsFinite(transition.LocalArcLengthMeters) || transition.LocalArcLengthMeters < 0d ||
|
||||
transition.LocalArcLengthMeters > length + MergeToleranceMeters)
|
||||
{
|
||||
reason = "局部 G2 曲率事件不属于有效方向分段。";
|
||||
return false;
|
||||
}
|
||||
ordered.Add(transition);
|
||||
}
|
||||
ordered.Sort(CompareTransitions);
|
||||
|
||||
var planned = new List<LocalG2SmoothingRegion>();
|
||||
int cursor = 0;
|
||||
while (cursor < ordered.Count)
|
||||
{
|
||||
CurvatureTransition first = ordered[cursor];
|
||||
double segmentLength = segmentLengths[first.SegmentIndex];
|
||||
var group = new List<CurvatureTransition> { first };
|
||||
IReadOnlyList<LocalG2WindowVariant> variants =
|
||||
BuildVariants(group, segmentLength, options);
|
||||
if (variants.Count == 0)
|
||||
{
|
||||
reason = "局部 G2 单事件无法生成满足总长度约束的窗口。";
|
||||
return false;
|
||||
}
|
||||
cursor++;
|
||||
|
||||
while (cursor < ordered.Count &&
|
||||
ordered[cursor].SegmentIndex == first.SegmentIndex)
|
||||
{
|
||||
var tentative = new List<CurvatureTransition>(group)
|
||||
{
|
||||
ordered[cursor],
|
||||
};
|
||||
IReadOnlyList<LocalG2WindowVariant> tentativeVariants =
|
||||
BuildVariants(tentative, segmentLength, options);
|
||||
if (tentativeVariants.Count == 0) break;
|
||||
group = tentative;
|
||||
variants = tentativeVariants;
|
||||
cursor++;
|
||||
}
|
||||
|
||||
double minimumStart = double.PositiveInfinity;
|
||||
double maximumEnd = double.NegativeInfinity;
|
||||
for (int variantIndex = 0; variantIndex < variants.Count; variantIndex++)
|
||||
{
|
||||
minimumStart = Math.Min(
|
||||
minimumStart,
|
||||
variants[variantIndex].StartArcLengthMeters);
|
||||
maximumEnd = Math.Max(
|
||||
maximumEnd,
|
||||
variants[variantIndex].EndArcLengthMeters);
|
||||
}
|
||||
|
||||
planned.Add(new LocalG2SmoothingRegion(
|
||||
first.SegmentIndex,
|
||||
group,
|
||||
minimumStart,
|
||||
maximumEnd,
|
||||
variants));
|
||||
}
|
||||
regions = new ReadOnlyCollection<LocalG2SmoothingRegion>(planned);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<LocalG2WindowVariant> BuildVariants(
|
||||
IReadOnlyList<CurvatureTransition> transitions,
|
||||
double segmentLength,
|
||||
LocalG2OptionsSnapshot options)
|
||||
{
|
||||
var variants = new List<LocalG2WindowVariant>();
|
||||
double firstEvent = transitions[0].LocalArcLengthMeters;
|
||||
double lastEvent = transitions[transitions.Count - 1].LocalArcLengthMeters;
|
||||
double anchor = (firstEvent + lastEvent) / 2d;
|
||||
foreach (double target in BuildTargets(options, segmentLength))
|
||||
{
|
||||
if (variants.Count >= options.MaximumCandidatesPerRegion) break;
|
||||
AddIfLegal(variants, target, 0.5d, anchor, firstEvent, lastEvent, segmentLength, true, options);
|
||||
AddIfLegal(variants, target, 0.4d, anchor, firstEvent, lastEvent, segmentLength, false, options);
|
||||
AddIfLegal(variants, target, 0.6d, anchor, firstEvent, lastEvent, segmentLength, false, options);
|
||||
}
|
||||
return new ReadOnlyCollection<LocalG2WindowVariant>(variants);
|
||||
}
|
||||
|
||||
private static void AddIfLegal(List<LocalG2WindowVariant> variants, double target, double leftRatio,
|
||||
double anchor, double firstEvent, double lastEvent, double segmentLength, bool permitBoundaryShift,
|
||||
LocalG2OptionsSnapshot options)
|
||||
{
|
||||
if (variants.Count >= options.MaximumCandidatesPerRegion) return;
|
||||
double left = target * leftRatio;
|
||||
double right = target - left;
|
||||
double availableLeft = anchor;
|
||||
double availableRight = segmentLength - anchor;
|
||||
if (permitBoundaryShift)
|
||||
{
|
||||
left = Math.Min(left, availableLeft);
|
||||
right = Math.Min(right, availableRight);
|
||||
double missing = target - left - right;
|
||||
double addRight = Math.Min(missing, availableRight - right);
|
||||
right += addRight;
|
||||
left += Math.Min(missing - addRight, availableLeft - left);
|
||||
}
|
||||
else if (left > availableLeft + MergeToleranceMeters || right > availableRight + MergeToleranceMeters)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
double start = anchor - left;
|
||||
double end = anchor + right;
|
||||
if (start > firstEvent + MergeToleranceMeters || end + MergeToleranceMeters < lastEvent || end - start + MergeToleranceMeters < target)
|
||||
return;
|
||||
double actualLength = end - start;
|
||||
if (actualLength + MergeToleranceMeters < options.MinimumWindowLengthMeters ||
|
||||
actualLength > options.MaximumWindowLengthMeters + MergeToleranceMeters)
|
||||
{
|
||||
return;
|
||||
}
|
||||
variants.Add(new LocalG2WindowVariant(variants.Count, start, end, left, right));
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> BuildTargets(LocalG2OptionsSnapshot options, double segmentLength)
|
||||
{
|
||||
if (segmentLength + MergeToleranceMeters < options.MinimumWindowLengthMeters)
|
||||
return new ReadOnlyCollection<double>(new List<double>());
|
||||
|
||||
double[] requested =
|
||||
{
|
||||
options.PreferredWindowLengthMeters,
|
||||
0.75d * options.PreferredWindowLengthMeters,
|
||||
1.25d * options.PreferredWindowLengthMeters,
|
||||
options.MinimumWindowLengthMeters,
|
||||
options.MaximumWindowLengthMeters,
|
||||
};
|
||||
var targets = new List<double>(requested.Length);
|
||||
for (int index = 0; index < requested.Length; index++)
|
||||
{
|
||||
double target = Math.Min(segmentLength,
|
||||
Math.Max(options.MinimumWindowLengthMeters, Math.Min(options.MaximumWindowLengthMeters, requested[index])));
|
||||
bool duplicate = false;
|
||||
for (int prior = 0; prior < targets.Count; prior++)
|
||||
{
|
||||
if (Math.Abs(targets[prior] - target) <= MergeToleranceMeters)
|
||||
{
|
||||
duplicate = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!duplicate) targets.Add(target);
|
||||
}
|
||||
return targets;
|
||||
}
|
||||
|
||||
private static bool TryGetSegmentLengths(PreparedPath originalPath, out Dictionary<int, double> lengths, out string reason)
|
||||
{
|
||||
lengths = new Dictionary<int, double>();
|
||||
reason = string.Empty;
|
||||
for (int position = 0; position < originalPath.Segments.Count; position++)
|
||||
{
|
||||
PreparedDirectionSegment segment = originalPath.Segments[position];
|
||||
if (segment == null || segment.SegmentIndex != position || segment.Points == null || segment.Points.Count == 0)
|
||||
{
|
||||
reason = "局部 G2 窗口规划的预处理方向分段无效。";
|
||||
return false;
|
||||
}
|
||||
double previousArc = -1d;
|
||||
for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
|
||||
{
|
||||
double arc = segment.Points[pointIndex].ArcLength;
|
||||
if (!NumericGuard.IsFinite(arc) || arc < 0d || arc < previousArc)
|
||||
{
|
||||
reason = "局部 G2 窗口规划要求分段弧长有限且非递减。";
|
||||
return false;
|
||||
}
|
||||
previousArc = arc;
|
||||
}
|
||||
lengths.Add(segment.SegmentIndex, previousArc);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static int CompareTransitions(CurvatureTransition left, CurvatureTransition right)
|
||||
{
|
||||
int segment = left.SegmentIndex.CompareTo(right.SegmentIndex);
|
||||
if (segment != 0) return segment;
|
||||
int arc = left.LocalArcLengthMeters.CompareTo(right.LocalArcLengthMeters);
|
||||
return arc != 0 ? arc : left.LeftCoarsePathIndex.CompareTo(right.LeftCoarsePathIndex);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> Empty<T>() => new ReadOnlyCollection<T>(new List<T>());
|
||||
|
||||
public static class TestHooks
|
||||
{
|
||||
public static WindowPlanningTestSnapshot Execute(string scenario)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(scenario))
|
||||
throw new ArgumentException("A scenario is required.", nameof(scenario));
|
||||
|
||||
IReadOnlyList<CurvatureTransition> transitions;
|
||||
double segmentLength;
|
||||
switch (scenario)
|
||||
{
|
||||
case "SeparatedByOneMeter":
|
||||
transitions = new[]
|
||||
{
|
||||
Transition(0.2d, 0),
|
||||
Transition(1.2d, 1),
|
||||
};
|
||||
segmentLength = 1.4d;
|
||||
break;
|
||||
case "Mergeable":
|
||||
transitions = new[]
|
||||
{
|
||||
Transition(0.4d, 0),
|
||||
Transition(0.7d, 1),
|
||||
};
|
||||
segmentLength = 1.4d;
|
||||
break;
|
||||
case "ThreeEventPartition":
|
||||
transitions = new[]
|
||||
{
|
||||
Transition(0.2d, 0),
|
||||
Transition(0.6d, 1),
|
||||
Transition(1.2d, 2),
|
||||
};
|
||||
segmentLength = 1.4d;
|
||||
break;
|
||||
case "NearBoundary":
|
||||
transitions = new[] { Transition(0.1d, 0) };
|
||||
segmentLength = 1d;
|
||||
break;
|
||||
default:
|
||||
throw new ArgumentOutOfRangeException(nameof(scenario));
|
||||
}
|
||||
|
||||
var planner = new LocalG2WindowPlanner();
|
||||
if (!planner.TryPlan(
|
||||
CreatePreparedPath(segmentLength),
|
||||
transitions,
|
||||
new LocalG2OptionsSnapshot(new PathSmoothingConfiguration()),
|
||||
out IReadOnlyList<LocalG2SmoothingRegion> regions,
|
||||
out string reason))
|
||||
{
|
||||
throw new InvalidOperationException(reason);
|
||||
}
|
||||
|
||||
double maximumLength = 0d;
|
||||
bool exactEnvelope = true;
|
||||
var counts = new List<string>(regions.Count);
|
||||
var signature = new List<string>();
|
||||
for (int regionIndex = 0; regionIndex < regions.Count; regionIndex++)
|
||||
{
|
||||
LocalG2SmoothingRegion region = regions[regionIndex];
|
||||
counts.Add(region.Transitions.Count.ToString());
|
||||
double minimumStart = double.PositiveInfinity;
|
||||
double maximumEnd = double.NegativeInfinity;
|
||||
for (int variantIndex = 0; variantIndex < region.WindowVariants.Count; variantIndex++)
|
||||
{
|
||||
LocalG2WindowVariant variant = region.WindowVariants[variantIndex];
|
||||
maximumLength = Math.Max(
|
||||
maximumLength,
|
||||
variant.EndArcLengthMeters - variant.StartArcLengthMeters);
|
||||
minimumStart = Math.Min(minimumStart, variant.StartArcLengthMeters);
|
||||
maximumEnd = Math.Max(maximumEnd, variant.EndArcLengthMeters);
|
||||
signature.Add(
|
||||
region.SegmentIndex + ":" +
|
||||
variant.CandidateIndex + ":" +
|
||||
variant.StartArcLengthMeters.ToString("R") + ":" +
|
||||
variant.EndArcLengthMeters.ToString("R"));
|
||||
}
|
||||
exactEnvelope &= Math.Abs(region.MaximumStartArcLengthMeters - minimumStart) <= 1e-9d;
|
||||
exactEnvelope &= Math.Abs(region.MaximumEndArcLengthMeters - maximumEnd) <= 1e-9d;
|
||||
}
|
||||
|
||||
LocalG2WindowVariant first = regions[0].WindowVariants[0];
|
||||
return new WindowPlanningTestSnapshot(
|
||||
regions.Count,
|
||||
string.Join(",", counts),
|
||||
maximumLength,
|
||||
exactEnvelope,
|
||||
first.LeftWindowLengthMeters,
|
||||
first.RightWindowLengthMeters,
|
||||
string.Join("|", signature));
|
||||
}
|
||||
|
||||
public sealed class WindowPlanningTestSnapshot
|
||||
{
|
||||
internal WindowPlanningTestSnapshot(
|
||||
int regionCount,
|
||||
string transitionCounts,
|
||||
double maximumWindowLength,
|
||||
bool exactEnvelope,
|
||||
double firstLeftLength,
|
||||
double firstRightLength,
|
||||
string signature)
|
||||
{
|
||||
RegionCount = regionCount;
|
||||
TransitionCounts = transitionCounts;
|
||||
MaximumWindowLength = maximumWindowLength;
|
||||
ExactEnvelope = exactEnvelope;
|
||||
FirstLeftLength = firstLeftLength;
|
||||
FirstRightLength = firstRightLength;
|
||||
Signature = signature;
|
||||
}
|
||||
|
||||
public int RegionCount { get; }
|
||||
public string TransitionCounts { get; }
|
||||
public double MaximumWindowLength { get; }
|
||||
public bool ExactEnvelope { get; }
|
||||
public double FirstLeftLength { get; }
|
||||
public double FirstRightLength { get; }
|
||||
public string Signature { get; }
|
||||
}
|
||||
|
||||
private static CurvatureTransition Transition(double arcLength, int index)
|
||||
{
|
||||
return new CurvatureTransition(
|
||||
0,
|
||||
index,
|
||||
index + 1,
|
||||
arcLength,
|
||||
arcLength,
|
||||
0d,
|
||||
0d,
|
||||
index % 2 == 0 ? 0d : 0.5d,
|
||||
index % 2 == 0 ? 0.5d : 0d);
|
||||
}
|
||||
|
||||
private static PreparedPath CreatePreparedPath(double length)
|
||||
{
|
||||
var points = new[]
|
||||
{
|
||||
new SmoothingPoint2D(
|
||||
0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||
new SmoothingPoint2D(
|
||||
length, 0d, length, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||
};
|
||||
return new PreparedPath(new[]
|
||||
{
|
||||
new PreparedDirectionSegment(
|
||||
0, TravelDirection.Forward, points, false, false),
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,124 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>满足两个二维端点二阶边界条件的参数五次 Hermite 曲线。</summary>
|
||||
internal sealed class QuinticHermiteCurve2D
|
||||
{
|
||||
private readonly double _x0;
|
||||
private readonly double _x1;
|
||||
private readonly double _x2;
|
||||
private readonly double _x3;
|
||||
private readonly double _x4;
|
||||
private readonly double _x5;
|
||||
private readonly double _y0;
|
||||
private readonly double _y1;
|
||||
private readonly double _y2;
|
||||
private readonly double _y3;
|
||||
private readonly double _y4;
|
||||
private readonly double _y5;
|
||||
|
||||
private QuinticHermiteCurve2D(
|
||||
double x0, double x1, double x2, double x3, double x4, double x5,
|
||||
double y0, double y1, double y2, double y3, double y4, double y5)
|
||||
{
|
||||
_x0 = x0;
|
||||
_x1 = x1;
|
||||
_x2 = x2;
|
||||
_x3 = x3;
|
||||
_x4 = x4;
|
||||
_x5 = x5;
|
||||
_y0 = y0;
|
||||
_y1 = y1;
|
||||
_y2 = y2;
|
||||
_y3 = y3;
|
||||
_y4 = y4;
|
||||
_y5 = y5;
|
||||
}
|
||||
|
||||
internal static bool TryCreate(
|
||||
double x0, double y0, double dx0, double dy0, double ddx0, double ddy0,
|
||||
double x1, double y1, double dx1, double dy1, double ddx1, double ddy1,
|
||||
out QuinticHermiteCurve2D curve,
|
||||
out string reason)
|
||||
{
|
||||
curve = null;
|
||||
reason = string.Empty;
|
||||
if (!AreFinite(x0, y0, dx0, dy0, ddx0, ddy0, x1, y1, dx1, dy1, ddx1, ddy1))
|
||||
{
|
||||
reason = "五次 Hermite 曲线需要有限的边界条件。";
|
||||
return false;
|
||||
}
|
||||
if (IsZeroVector(dx0, dy0) || IsZeroVector(dx1, dy1))
|
||||
{
|
||||
reason = "五次 Hermite 曲线端点一阶导数不能为零。";
|
||||
return false;
|
||||
}
|
||||
|
||||
SolveCoordinate(x0, dx0, ddx0, x1, dx1, ddx1,
|
||||
out double ax0, out double ax1, out double ax2, out double ax3, out double ax4, out double ax5);
|
||||
SolveCoordinate(y0, dy0, ddy0, y1, dy1, ddy1,
|
||||
out double ay0, out double ay1, out double ay2, out double ay3, out double ay4, out double ay5);
|
||||
if (!AreFinite(ax0, ax1, ax2, ax3, ax4, ax5, ay0, ay1, ay2, ay3, ay4, ay5))
|
||||
{
|
||||
reason = "五次 Hermite 曲线系数溢出。";
|
||||
return false;
|
||||
}
|
||||
|
||||
curve = new QuinticHermiteCurve2D(ax0, ax1, ax2, ax3, ax4, ax5, ay0, ay1, ay2, ay3, ay4, ay5);
|
||||
return true;
|
||||
}
|
||||
|
||||
internal void Evaluate(
|
||||
double u,
|
||||
out double x, out double y,
|
||||
out double dx, out double dy,
|
||||
out double ddx, out double ddy)
|
||||
{
|
||||
if (!IsFinite(u) || u < 0d || u > 1d)
|
||||
throw new ArgumentOutOfRangeException(nameof(u), "The curve parameter must be finite and within [0, 1].");
|
||||
|
||||
x = EvaluateValue(_x0, _x1, _x2, _x3, _x4, _x5, u);
|
||||
y = EvaluateValue(_y0, _y1, _y2, _y3, _y4, _y5, u);
|
||||
dx = EvaluateFirstDerivative(_x1, _x2, _x3, _x4, _x5, u);
|
||||
dy = EvaluateFirstDerivative(_y1, _y2, _y3, _y4, _y5, u);
|
||||
ddx = EvaluateSecondDerivative(_x2, _x3, _x4, _x5, u);
|
||||
ddy = EvaluateSecondDerivative(_y2, _y3, _y4, _y5, u);
|
||||
}
|
||||
|
||||
private static void SolveCoordinate(double p0, double v0, double acceleration0, double p1, double v1, double acceleration1,
|
||||
out double a0, out double a1, out double a2, out double a3, out double a4, out double a5)
|
||||
{
|
||||
a0 = p0;
|
||||
a1 = v0;
|
||||
a2 = acceleration0 / 2d;
|
||||
double c0 = p1 - (a0 + a1 + a2);
|
||||
double c1 = v1 - (a1 + 2d * a2);
|
||||
double c2 = acceleration1 - 2d * a2;
|
||||
a3 = 10d * c0 - 4d * c1 + 0.5d * c2;
|
||||
a4 = -15d * c0 + 7d * c1 - c2;
|
||||
a5 = 6d * c0 - 3d * c1 + 0.5d * c2;
|
||||
}
|
||||
|
||||
private static double EvaluateValue(double a0, double a1, double a2, double a3, double a4, double a5, double u) =>
|
||||
(((((a5 * u + a4) * u + a3) * u + a2) * u + a1) * u) + a0;
|
||||
|
||||
private static double EvaluateFirstDerivative(double a1, double a2, double a3, double a4, double a5, double u) =>
|
||||
((((5d * a5 * u + 4d * a4) * u + 3d * a3) * u + 2d * a2) * u) + a1;
|
||||
|
||||
private static double EvaluateSecondDerivative(double a2, double a3, double a4, double a5, double u) =>
|
||||
(((20d * a5 * u + 12d * a4) * u + 6d * a3) * u) + 2d * a2;
|
||||
|
||||
private static bool IsZeroVector(double x, double y) => x == 0d && y == 0d;
|
||||
|
||||
private static bool AreFinite(params double[] values)
|
||||
{
|
||||
for (int index = 0; index < values.Length; index++)
|
||||
{
|
||||
if (!IsFinite(values[index])) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
Reference in New Issue
Block a user