First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

chore: save current workspace progress

Browse Source
This commit is contained in:
梁薄云
2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.task8-sweep/ParkrobTrajplanner/PathSmoothing/Algorithms/CubicBSplineSmoother.cs
+412
View File
@@ -0,0 +1,412 @@
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 EndpointProbeParameter = 1e-6d;
private const double MinimumTangentHandleLengthMeters = 1e-10d;
/// <inheritdoc />
public SmoothingMethod Method => SmoothingMethod.CubicBSpline;
/// <inheritdoc />
public SmoothingCandidate Smooth(
SmoothingAlgorithmInput input,
double effectiveStrength,
CancellationToken cancellationToken)
{
if (input == null || input.OriginalPath == null ||
input.Options == 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,
input.Options.CubicBSplineEndpointTangentScale,
cancellationToken,
out IReadOnlyList<SmoothingPoint2D> points,
out string reason,
out SmoothingCandidateStatus status))
{
return status == SmoothingCandidateStatus.RetryableInfeasible
? SmoothingCandidate.RetryableInfeasible(reason)
: SmoothingCandidate.Failed(reason);
}
candidateSegments.Add(new PreparedDirectionSegment(
sourceSegment.SegmentIndex,
sourceSegment.Direction,
points,
sourceSegment.StartsAtGearSwitch,
sourceSegment.EndsAtGearSwitch,
sourceSegment.StartVehicleCurvaturePerMeter));
}
return SmoothingCandidate.Success(candidateSegments);
}
private static bool TrySmoothSegment(
PreparedDirectionSegment sourceSegment,
double strength,
double reserveMeters,
double endpointTangentScale,
CancellationToken cancellationToken,
out IReadOnlyList<SmoothingPoint2D> result,
out string reason,
out SmoothingCandidateStatus status)
{
result = null;
reason = string.Empty;
status = SmoothingCandidateStatus.Failed;
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;
}
if (!TryCreateControls(anchors, sourceSegment.Direction, strength, reserveMeters, endpointTangentScale,
cancellationToken, out Point2D[] controls, out reason))
{
return false;
}
double[] knots = CreateClampedKnots(controls.Length);
var sampled = new List<SmoothingPoint2D>();
int spanCount = controls.Length - Degree;
int uniformIntervals = spanCount * SamplesPerSpan;
if (!TryAddSample(0d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason, out status))
return false;
if (!TryAddSample(EndpointProbeParameter, anchors, controls, knots, reserveMeters,
sampled, cancellationToken, out reason, out status))
return false;
for (int index = 1; index < uniformIntervals; index++)
{
if (!TryAddSample((double)index / uniformIntervals, anchors, controls, knots, reserveMeters,
sampled, cancellationToken, out reason, out status))
{
return false;
}
}
if (!TryAddSample(1d - EndpointProbeParameter, anchors, controls, knots, reserveMeters,
sampled, cancellationToken, out reason, out status))
return false;
if (!TryAddSample(1d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason, out status))
return false;
result = sampled;
return true;
}
private static bool TryCreateControls(
IReadOnlyList<SmoothingPoint2D> anchors,
TravelDirection direction,
double strength,
double reserveMeters,
double endpointTangentScale,
CancellationToken cancellationToken,
out Point2D[] controls,
out string reason)
{
reason = string.Empty;
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);
if (!TryConstrainTangentHandle(
Point2D.FromAnchor(anchors[0]),
Math.Cos(startTravelHeading),
Math.Sin(startTravelHeading),
startHandleLength,
anchors[1],
GetAllowedRadius(anchors[1], reserveMeters),
out controls[1]))
{
reason = "B 样条起点切向手柄无法同时满足相邻锚点移动范围。";
return false;
}
int finalIndex = anchors.Count - 1;
double endHandleLength = Distance(anchors[finalIndex - 1], anchors[finalIndex]) * endpointTangentScale * strength;
double endTravelHeading = GetTravelHeading(anchors[finalIndex], direction);
if (!TryConstrainTangentHandle(
Point2D.FromAnchor(anchors[finalIndex]),
-Math.Cos(endTravelHeading),
-Math.Sin(endTravelHeading),
endHandleLength,
anchors[finalIndex - 1],
GetAllowedRadius(anchors[finalIndex - 1], reserveMeters),
out controls[finalIndex - 1]))
{
reason = "B 样条终点切向手柄无法同时满足相邻锚点移动范围。";
return false;
}
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))
{
reason = "B 样条控制点构造产生非法数值。";
return false;
}
}
return true;
}
private static bool TryAddSample(
double parameter,
IReadOnlyList<SmoothingPoint2D> anchors,
Point2D[] controls,
double[] knots,
double reserveMeters,
List<SmoothingPoint2D> output,
CancellationToken cancellationToken,
out string reason,
out SmoothingCandidateStatus status)
{
reason = string.Empty;
status = SmoothingCandidateStatus.Failed;
cancellationToken.ThrowIfCancellationRequested();
Point2D evaluated = Evaluate(controls, knots, parameter);
if (!NumericGuard.IsFinite(evaluated.X) || !NumericGuard.IsFinite(evaluated.Y))
{
reason = "B 样条评估产生非法数值。";
return false;
}
double targetArcLength = parameter * anchors[anchors.Count - 1].ArcLength;
if (!PathReferenceInterpolator.TryInterpolateByArcLength(anchors, targetArcLength,
out SmoothingPoint2D reference, out reason))
{
return false;
}
double displacement = Distance(evaluated, reference);
if (!NumericGuard.IsFinite(displacement))
{
reason = "B 样条评估点位移产生非法数值。";
return false;
}
if (displacement > GetAllowedRadius(reference, reserveMeters))
{
reason = "B 样条评估点超过对应原始参考点的允许移动范围。";
status = SmoothingCandidateStatus.RetryableInfeasible;
return false;
}
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));
return true;
}
private static bool TryConstrainTangentHandle(
Point2D endpoint,
double rayDirectionX,
double rayDirectionY,
double desiredLength,
SmoothingPoint2D adjacentAnchor,
double allowedRadius,
out Point2D control)
{
control = default;
double offsetX = adjacentAnchor.X - endpoint.X;
double offsetY = adjacentAnchor.Y - endpoint.Y;
double projectedLength = offsetX * rayDirectionX + offsetY * rayDirectionY;
double perpendicularX = offsetX - projectedLength * rayDirectionX;
double perpendicularY = offsetY - projectedLength * rayDirectionY;
double discriminant = allowedRadius * allowedRadius -
(perpendicularX * perpendicularX + perpendicularY * perpendicularY);
if (!NumericGuard.IsFinite(discriminant) || discriminant < 0d) return false;
double halfInterval = Math.Sqrt(discriminant);
double minimumLength = Math.Max(MinimumTangentHandleLengthMeters, projectedLength - halfInterval);
double maximumLength = projectedLength + halfInterval;
if (!NumericGuard.IsFinite(maximumLength) || maximumLength < minimumLength) return false;
double constrainedLength = Math.Max(minimumLength, Math.Min(desiredLength, maximumLength));
control = new Point2D(
endpoint.X + constrainedLength * rayDirectionX,
endpoint.Y + constrainedLength * rayDirectionY);
return NumericGuard.IsFinite(control.X) && NumericGuard.IsFinite(control.Y);
}
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 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 static double Distance(Point2D 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);
}
}
}
.task8-sweep/ParkrobTrajplanner/PathSmoothing/Algorithms/IPathSmoother.cs
+14
View File
@@ -0,0 +1,14 @@
using System.Threading;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
/// <summary>单一平滑方法生成原始几何候选的内部契约。</summary>
internal interface IPathSmoother
{
SmoothingMethod Method { get; }
SmoothingCandidate Smooth(
SmoothingAlgorithmInput input,
double effectiveStrength,
CancellationToken cancellationToken);
}
.task8-sweep/ParkrobTrajplanner/PathSmoothing/Algorithms/LocalCubicBezierSmoother.cs
+427
View File
@@ -0,0 +1,427 @@
using System;
using System.Collections.Generic;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
using MultiWheelC.TrajectoryPlanning.Utils;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
/// <summary>在方向段内以局部三次 Bézier 连接替换明显转角。</summary>
internal sealed class LocalCubicBezierSmoother : IPathSmoother
{
private const double WindowToleranceMeters = 1e-9d;
/// <inheritdoc />
public SmoothingMethod Method => SmoothingMethod.LocalCubicBezier;
/// <inheritdoc />
public SmoothingCandidate Smooth(
SmoothingAlgorithmInput input,
double effectiveStrength,
CancellationToken cancellationToken)
{
if (input == null || input.OriginalPath == null || input.Options == null ||
!NumericGuard.IsPositiveFinite(effectiveStrength) ||
!NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
input.MinimumClearanceReserveMeters < 0d)
{
return SmoothingCandidate.Failed("Bézier 输入、强度或净空预留无效。");
}
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,
input.Options.BezierCornerHeadingThresholdRadians,
input.Options.BezierMaximumWindowLengthMeters,
input.Options.BezierHandleLengthRatio,
effectiveStrength,
input.MinimumClearanceReserveMeters,
cancellationToken,
out IReadOnlyList<SmoothingPoint2D> points,
out string reason,
out SmoothingCandidateStatus status))
{
return status == SmoothingCandidateStatus.RetryableInfeasible
? SmoothingCandidate.RetryableInfeasible(reason)
: SmoothingCandidate.Failed(reason);
}
candidateSegments.Add(new PreparedDirectionSegment(
sourceSegment.SegmentIndex,
sourceSegment.Direction,
points,
sourceSegment.StartsAtGearSwitch,
sourceSegment.EndsAtGearSwitch,
sourceSegment.StartVehicleCurvaturePerMeter));
}
return SmoothingCandidate.Success(candidateSegments);
}
private static bool TrySmoothSegment(
PreparedDirectionSegment sourceSegment,
double cornerThresholdRadians,
double maximumWindowLengthMeters,
double handleLengthRatio,
double strength,
double reserveMeters,
CancellationToken cancellationToken,
out IReadOnlyList<SmoothingPoint2D> result,
out string reason,
out SmoothingCandidateStatus status)
{
result = null;
reason = string.Empty;
status = SmoothingCandidateStatus.Failed;
if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count == 0)
{
reason = "Bézier 方向段为空。";
return false;
}
IReadOnlyList<SmoothingPoint2D> anchors = sourceSegment.Points;
if (!ValidateAnchors(anchors, cancellationToken, out reason)) return false;
if (anchors.Count < 3)
{
result = anchors;
return true;
}
if (!TryCreateMergedWindows(
anchors,
cornerThresholdRadians,
maximumWindowLengthMeters,
cancellationToken,
out List<Window> windows,
out reason))
{
return false;
}
if (windows.Count == 0)
{
result = anchors;
return true;
}
var output = new List<SmoothingPoint2D>(anchors.Count);
int anchorIndex = 0;
for (int windowIndex = 0; windowIndex < windows.Count; windowIndex++)
{
cancellationToken.ThrowIfCancellationRequested();
Window window = windows[windowIndex];
while (anchorIndex <= window.StartIndex)
{
output.Add(anchors[anchorIndex]);
anchorIndex++;
}
if (!TryAppendWindowInterior(
anchors,
window,
handleLengthRatio,
strength,
reserveMeters,
output,
cancellationToken,
out reason,
out status))
{
return false;
}
output.Add(anchors[window.EndIndex]);
anchorIndex = window.EndIndex + 1;
}
while (anchorIndex < anchors.Count)
{
output.Add(anchors[anchorIndex]);
anchorIndex++;
}
result = output;
return true;
}
private static bool ValidateAnchors(
IReadOnlyList<SmoothingPoint2D> anchors,
CancellationToken cancellationToken,
out string reason)
{
reason = string.Empty;
for (int index = 0; index < anchors.Count; index++)
{
cancellationToken.ThrowIfCancellationRequested();
SmoothingPoint2D point = anchors[index];
if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) ||
!NumericGuard.IsFinite(point.ArcLength) || point.ArcLength < 0d ||
!NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.UnwrappedHeading) ||
!NumericGuard.IsFinite(point.BodyClearance) || point.BodyClearance < 0d ||
(index > 0 && point.ArcLength <= anchors[index - 1].ArcLength))
{
reason = "Bézier 方向段包含非有限、非递增弧长或无效净空的锚点。";
return false;
}
}
return true;
}
private static bool TryCreateMergedWindows(
IReadOnlyList<SmoothingPoint2D> anchors,
double cornerThresholdRadians,
double maximumWindowLengthMeters,
CancellationToken cancellationToken,
out List<Window> windows,
out string reason)
{
windows = new List<Window>();
var candidates = new List<Window>();
reason = string.Empty;
for (int cornerIndex = 1; cornerIndex < anchors.Count - 1; cornerIndex++)
{
cancellationToken.ThrowIfCancellationRequested();
if (!TryGetTravelTangent(anchors[cornerIndex - 1], anchors[cornerIndex], out Point2D entryTangent) ||
!TryGetTravelTangent(anchors[cornerIndex], anchors[cornerIndex + 1], out Point2D exitTangent))
{
reason = "Bézier 转角包含零长度或非有限行进切向。";
return false;
}
double cross = entryTangent.X * exitTangent.Y - entryTangent.Y * exitTangent.X;
double dot = entryTangent.X * exitTangent.X + entryTangent.Y * exitTangent.Y;
double turnRadians = Math.Atan2(Math.Abs(cross), dot);
if (!NumericGuard.IsFinite(turnRadians))
{
reason = "Bézier 转角计算产生非有限数值。";
return false;
}
if (turnRadians < cornerThresholdRadians) continue;
int startIndex = cornerIndex - 1;
int endIndex = cornerIndex + 1;
double windowLength = anchors[endIndex].ArcLength - anchors[startIndex].ArcLength;
if (!NumericGuard.IsPositiveFinite(windowLength))
{
reason = "Bézier 局部窗口弧长无效。";
return false;
}
if (windowLength > maximumWindowLengthMeters + WindowToleranceMeters) continue;
if (ContainsGearSwitch(anchors, startIndex, endIndex)) continue;
candidates.Add(new Window(startIndex, endIndex));
}
MergeBoundedConnectedWindows(anchors, candidates, maximumWindowLengthMeters, windows);
return true;
}
private static void MergeBoundedConnectedWindows(
IReadOnlyList<SmoothingPoint2D> anchors,
IReadOnlyList<Window> candidates,
double maximumWindowLengthMeters,
List<Window> windows)
{
int candidateIndex = 0;
while (candidateIndex < candidates.Count)
{
Window merged = candidates[candidateIndex];
candidateIndex++;
while (candidateIndex < candidates.Count &&
candidates[candidateIndex].StartIndex <= merged.EndIndex + 1)
{
merged = new Window(merged.StartIndex,
Math.Max(merged.EndIndex, candidates[candidateIndex].EndIndex));
candidateIndex++;
}
double mergedLength = anchors[merged.EndIndex].ArcLength - anchors[merged.StartIndex].ArcLength;
if (mergedLength <= maximumWindowLengthMeters + WindowToleranceMeters)
{
windows.Add(merged);
}
// A connected group that exceeds the cap is declined as a whole. Splitting it into
// adjacent local curves would introduce unrequested joins; accepting it would violate
// the maximum-window contract. Its original anchors therefore remain unchanged.
}
}
private static bool TryAppendWindowInterior(
IReadOnlyList<SmoothingPoint2D> anchors,
Window window,
double handleLengthRatio,
double strength,
double reserveMeters,
List<SmoothingPoint2D> output,
CancellationToken cancellationToken,
out string reason,
out SmoothingCandidateStatus status)
{
reason = string.Empty;
status = SmoothingCandidateStatus.Failed;
SmoothingPoint2D p0 = anchors[window.StartIndex];
SmoothingPoint2D p3 = anchors[window.EndIndex];
if (!TryGetTravelTangent(p0, anchors[window.StartIndex + 1], out Point2D entryTangent) ||
!TryGetTravelTangent(anchors[window.EndIndex - 1], p3, out Point2D exitTangent))
{
reason = "Bézier 窗口端点包含无效行进切向。";
return false;
}
double arcLength = p3.ArcLength - p0.ArcLength;
double chordLength = Distance(p0, p3);
double handleLength = chordLength * handleLengthRatio * strength;
if (!NumericGuard.IsPositiveFinite(arcLength) || !NumericGuard.IsPositiveFinite(chordLength) ||
!NumericGuard.IsPositiveFinite(handleLength))
{
reason = "Bézier 窗口弧长、端点弦长或控制柄长度无效。";
return false;
}
Point2D control1 = new Point2D(
p0.X + entryTangent.X * handleLength,
p0.Y + entryTangent.Y * handleLength);
Point2D control2 = new Point2D(
p3.X - exitTangent.X * handleLength,
p3.Y - exitTangent.Y * handleLength);
if (!IsFinite(control1) || !IsFinite(control2))
{
reason = "Bézier 控制点产生非有限数值。";
return false;
}
for (int index = window.StartIndex + 1; index < window.EndIndex; index++)
{
cancellationToken.ThrowIfCancellationRequested();
SmoothingPoint2D anchor = anchors[index];
double parameter = (anchor.ArcLength - p0.ArcLength) / arcLength;
if (!NumericGuard.IsFinite(parameter) || parameter <= 0d || parameter >= 1d)
{
reason = "Bézier 窗口参数无效。";
return false;
}
Point2D evaluated = Evaluate(p0, control1, control2, p3, parameter);
if (!IsFinite(evaluated))
{
reason = "Bézier 评估产生非有限数值。";
return false;
}
double referenceArcLength = p0.ArcLength + parameter * arcLength;
if (!PathReferenceInterpolator.TryInterpolateByArcLength(
anchors,
referenceArcLength,
out SmoothingPoint2D reference,
out reason))
{
return false;
}
double displacement = Distance(evaluated, reference);
if (!NumericGuard.IsFinite(displacement))
{
reason = "Bézier 评估点位移产生非有限数值。";
return false;
}
double allowedDisplacement = Math.Max(0d, reference.BodyClearance - reserveMeters);
if (displacement > allowedDisplacement)
{
reason = "Bézier 评估点超过对应原始弧长参考点的允许移动范围。";
status = SmoothingCandidateStatus.RetryableInfeasible;
return false;
}
output.Add(new SmoothingPoint2D(
evaluated.X,
evaluated.Y,
reference.ArcLength,
reference.Heading,
reference.UnwrappedHeading,
reference.BodyClearance,
false,
SmoothedPathPointSource.Interpolated));
}
return true;
}
private static bool ContainsGearSwitch(IReadOnlyList<SmoothingPoint2D> anchors, int startIndex, int endIndex)
{
for (int index = startIndex; index <= endIndex; index++)
{
if (anchors[index].IsGearSwitchPoint) return true;
}
return false;
}
private static bool TryGetTravelTangent(SmoothingPoint2D start, SmoothingPoint2D end, out Point2D tangent)
{
tangent = default;
double deltaX = end.X - start.X;
double deltaY = end.Y - start.Y;
double length = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
if (!NumericGuard.IsPositiveFinite(length)) return false;
tangent = new Point2D(deltaX / length, deltaY / length);
return IsFinite(tangent);
}
private static Point2D Evaluate(SmoothingPoint2D p0, Point2D p1, Point2D p2, SmoothingPoint2D p3, double parameter)
{
double oneMinusParameter = 1d - parameter;
double p0Weight = oneMinusParameter * oneMinusParameter * oneMinusParameter;
double p1Weight = 3d * oneMinusParameter * oneMinusParameter * parameter;
double p2Weight = 3d * oneMinusParameter * parameter * parameter;
double p3Weight = parameter * parameter * parameter;
return new Point2D(
p0Weight * p0.X + p1Weight * p1.X + p2Weight * p2.X + p3Weight * p3.X,
p0Weight * p0.Y + p1Weight * p1.Y + p2Weight * p2.Y + p3Weight * p3.Y);
}
private static bool IsFinite(Point2D point)
{
return NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y);
}
private static double Distance(Point2D point, SmoothingPoint2D reference)
{
double deltaX = point.X - reference.X;
double deltaY = point.Y - reference.Y;
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
}
private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right)
{
double deltaX = left.X - right.X;
double deltaY = left.Y - right.Y;
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
}
private readonly struct Window
{
internal Window(int startIndex, int endIndex)
{
StartIndex = startIndex;
EndIndex = endIndex;
}
internal int StartIndex { get; }
internal int EndIndex { get; }
}
private readonly struct Point2D
{
internal Point2D(double x, double y)
{
X = x;
Y = y;
}
internal double X { get; }
internal double Y { get; }
}
}
.task8-sweep/ParkrobTrajplanner/PathSmoothing/Algorithms/PiecewiseQuinticSmoother.cs
+557
View File
@@ -0,0 +1,557 @@
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>按方向段局部弧长构造 C2 连续的分段五次 Hermite 原始几何候选。</summary>
internal sealed class PiecewiseQuinticSmoother : IPathSmoother
{
private const int SamplesPerInterval = 8;
private const double DoubleMachineEpsilon = 2.2204460492503131e-16d;
private const double EndpointNormalizationUlps = 32d;
/// <inheritdoc />
public SmoothingMethod Method => SmoothingMethod.PiecewiseQuintic;
/// <inheritdoc />
public SmoothingCandidate Smooth(
SmoothingAlgorithmInput input,
double effectiveStrength,
CancellationToken cancellationToken)
{
if (input == null || input.OriginalPath == null || input.Options == null ||
!NumericGuard.IsPositiveFinite(effectiveStrength) ||
!NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
input.MinimumClearanceReserveMeters < 0d ||
!NumericGuard.IsPositiveFinite(input.Options.QuinticKnotSpacingMeters) ||
!NumericGuard.IsPositiveFinite(input.Options.QuinticMinimumKnotSpacingMeters) ||
input.Options.QuinticKnotSpacingMeters < input.Options.QuinticMinimumKnotSpacingMeters)
{
return SmoothingCandidate.Failed("五次 Hermite 输入、强度、净空预留或结点间距无效。");
}
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,
input.Options.QuinticKnotSpacingMeters,
input.Options.QuinticMinimumKnotSpacingMeters,
cancellationToken,
out IReadOnlyList<SmoothingPoint2D> points,
out string reason,
out SmoothingCandidateStatus status))
{
return status == SmoothingCandidateStatus.RetryableInfeasible
? SmoothingCandidate.RetryableInfeasible(reason)
: SmoothingCandidate.Failed(reason);
}
candidateSegments.Add(new PreparedDirectionSegment(
sourceSegment.SegmentIndex,
sourceSegment.Direction,
points,
sourceSegment.StartsAtGearSwitch,
sourceSegment.EndsAtGearSwitch,
sourceSegment.StartVehicleCurvaturePerMeter));
}
return SmoothingCandidate.Success(candidateSegments);
}
private static bool TrySmoothSegment(
PreparedDirectionSegment sourceSegment,
double effectiveStrength,
double reserveMeters,
double knotSpacingMeters,
double minimumKnotSpacingMeters,
CancellationToken cancellationToken,
out IReadOnlyList<SmoothingPoint2D> result,
out string reason,
out SmoothingCandidateStatus status)
{
result = null;
reason = string.Empty;
status = SmoothingCandidateStatus.Failed;
if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count < 2)
{
reason = "五次 Hermite 方向段至少需要两个锚点。";
return false;
}
IReadOnlyList<SmoothingPoint2D> anchors = sourceSegment.Points;
if (!ValidateAnchors(anchors, cancellationToken, out reason)) return false;
if (!TryCreateKnots(
anchors,
sourceSegment.Direction,
effectiveStrength,
knotSpacingMeters,
minimumKnotSpacingMeters,
cancellationToken,
out List<Knot> knots,
out reason))
{
return false;
}
// Each physical acceleration is blended once into its shared knot and then scaled by
// the left/right local interval independently. Reusing this value is what makes the
// curve C2 with respect to local arc length, even for nonuniform final intervals.
if (!TryAssignSharedAccelerations(knots, out reason)) return false;
if (!TryCreateIntervals(knots, out List<QuinticInterval> intervals, out reason)) return false;
var sampled = new List<SmoothingPoint2D>(1 + intervals.Count * SamplesPerInterval);
for (int intervalIndex = 0; intervalIndex < intervals.Count; intervalIndex++)
{
cancellationToken.ThrowIfCancellationRequested();
QuinticInterval interval = intervals[intervalIndex];
int firstSample = intervalIndex == 0 ? 0 : 1;
for (int sampleIndex = firstSample; sampleIndex <= SamplesPerInterval; sampleIndex++)
{
cancellationToken.ThrowIfCancellationRequested();
double parameter = (double)sampleIndex / SamplesPerInterval;
double referenceArcLength = interval.Start.ArcLength + parameter * interval.Length;
if (!NumericGuard.IsFinite(referenceArcLength))
{
reason = "五次 Hermite 采样参考弧长无效。";
return false;
}
if (!PathReferenceInterpolator.TryInterpolateByArcLength(
anchors,
referenceArcLength,
out SmoothingPoint2D reference,
out reason))
{
return false;
}
Point2D evaluated;
if (sampleIndex == 0)
evaluated = interval.Start.Position;
else if (sampleIndex == SamplesPerInterval)
evaluated = interval.End.Position;
else if (!interval.TryEvaluate(parameter, out evaluated, out Point2D derivative, out Point2D secondDerivative))
{
reason = "五次 Hermite 采样产生非有限位置或导数。";
return false;
}
double displacement = Distance(evaluated, reference);
if (!NumericGuard.IsFinite(displacement))
{
reason = "五次 Hermite 采样位移无效。";
return false;
}
double allowedDisplacement = Math.Max(0d, reference.BodyClearance - reserveMeters);
if (displacement > allowedDisplacement)
{
reason = "五次 Hermite 采样点超过对应局部弧长参考点的允许移动范围。";
status = SmoothingCandidateStatus.RetryableInfeasible;
return false;
}
bool firstEndpoint = intervalIndex == 0 && sampleIndex == 0;
bool lastEndpoint = intervalIndex == intervals.Count - 1 && sampleIndex == SamplesPerInterval;
if (firstEndpoint)
{
sampled.Add(anchors[0]);
}
else if (lastEndpoint)
{
sampled.Add(anchors[anchors.Count - 1]);
}
else
{
sampled.Add(new SmoothingPoint2D(
evaluated.X,
evaluated.Y,
reference.ArcLength,
reference.Heading,
reference.UnwrappedHeading,
reference.BodyClearance,
false,
SmoothedPathPointSource.Interpolated));
}
}
}
result = sampled;
return true;
}
private static bool ValidateAnchors(
IReadOnlyList<SmoothingPoint2D> anchors,
CancellationToken cancellationToken,
out string reason)
{
reason = string.Empty;
for (int index = 0; index < anchors.Count; index++)
{
cancellationToken.ThrowIfCancellationRequested();
SmoothingPoint2D point = anchors[index];
if (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.ArcLength < 0d || point.BodyClearance < 0d ||
(index > 0 && point.ArcLength <= anchors[index - 1].ArcLength))
{
reason = "五次 Hermite 方向段包含非有限、非递增弧长或无效净空的锚点。";
return false;
}
}
return true;
}
private static bool TryCreateKnots(
IReadOnlyList<SmoothingPoint2D> anchors,
TravelDirection direction,
double effectiveStrength,
double knotSpacingMeters,
double minimumKnotSpacingMeters,
CancellationToken cancellationToken,
out List<Knot> knots,
out string reason)
{
knots = new List<Knot>();
reason = string.Empty;
double startArcLength = anchors[0].ArcLength;
double endArcLength = anchors[anchors.Count - 1].ArcLength;
double totalLength = endArcLength - startArcLength;
if (!NumericGuard.IsPositiveFinite(totalLength) || totalLength < minimumKnotSpacingMeters)
{
reason = "五次 Hermite 方向段短于配置的最小结点间距。";
return false;
}
if (!TryCreateKnot(anchors[0], direction, effectiveStrength, out Knot first))
{
reason = "五次 Hermite 起点结点或行进切向无效。";
return false;
}
knots.Add(first);
double endpointTolerance = GetEndpointNormalizationTolerance(
startArcLength,
endArcLength,
knotSpacingMeters);
double previousArcLength = startArcLength;
for (long knotOrdinal = 1L; ; knotOrdinal++)
{
cancellationToken.ThrowIfCancellationRequested();
double targetArcLength = startArcLength + knotOrdinal * knotSpacingMeters;
if (!NumericGuard.IsFinite(targetArcLength))
{
reason = "五次 Hermite 内部结点弧长无效。";
return false;
}
if (targetArcLength >= endArcLength - endpointTolerance) break;
if (targetArcLength <= previousArcLength)
{
reason = "五次 Hermite 内部结点无法在浮点弧长尺度上保持递增。";
return false;
}
if (!PathReferenceInterpolator.TryInterpolateByArcLength(
anchors,
targetArcLength,
out SmoothingPoint2D reference,
out reason) ||
!TryCreateKnot(reference, direction, effectiveStrength, out Knot knot))
{
if (string.IsNullOrEmpty(reason)) reason = "五次 Hermite 内部结点或行进切向无效。";
return false;
}
knots.Add(knot);
previousArcLength = targetArcLength;
}
if (!TryCreateKnot(anchors[anchors.Count - 1], direction, effectiveStrength, out Knot last))
{
reason = "五次 Hermite 终点结点或行进切向无效。";
return false;
}
knots.Add(last);
for (int index = 1; index < knots.Count; index++)
{
double intervalLength = knots[index].ArcLength - knots[index - 1].ArcLength;
if (!NumericGuard.IsPositiveFinite(intervalLength) || intervalLength < minimumKnotSpacingMeters)
{
reason = "五次 Hermite 结点间隔无效或短于配置的最小间距。";
return false;
}
}
return true;
}
private static double GetEndpointNormalizationTolerance(
double startArcLength,
double endArcLength,
double knotSpacingMeters)
{
double magnitude = Math.Max(
Math.Abs(startArcLength),
Math.Max(Math.Abs(endArcLength), Math.Abs(knotSpacingMeters)));
return EndpointNormalizationUlps * DoubleMachineEpsilon * magnitude;
}
private static bool TryCreateKnot(
SmoothingPoint2D reference,
TravelDirection direction,
double effectiveStrength,
out Knot knot)
{
knot = default;
if (reference == null || !NumericGuard.IsFinite(reference.X) || !NumericGuard.IsFinite(reference.Y) ||
!NumericGuard.IsFinite(reference.ArcLength) || !NumericGuard.IsFinite(reference.Heading))
{
return false;
}
double travelHeading = direction == TravelDirection.Forward
? reference.Heading
: reference.Heading - Math.PI;
double tangentX = Math.Cos(travelHeading);
double tangentY = Math.Sin(travelHeading);
if (!NumericGuard.IsFinite(tangentX) || !NumericGuard.IsFinite(tangentY)) return false;
var velocity = new Point2D(tangentX * effectiveStrength, tangentY * effectiveStrength);
if (!velocity.IsFinite) return false;
knot = new Knot(reference.ArcLength, new Point2D(reference.X, reference.Y), velocity);
return true;
}
private static bool TryAssignSharedAccelerations(List<Knot> knots, out string reason)
{
reason = string.Empty;
for (int index = 0; index < knots.Count; index++)
{
Point2D acceleration;
if (index == 0)
{
if (!TryAcceleration(knots[0], knots[1], out acceleration))
{
reason = "五次 Hermite 起点加速度无效。";
return false;
}
}
else if (index == knots.Count - 1)
{
if (!TryAcceleration(knots[index - 1], knots[index], out acceleration))
{
reason = "五次 Hermite 终点加速度无效。";
return false;
}
}
else
{
if (!TryAcceleration(knots[index - 1], knots[index], out Point2D left) ||
!TryAcceleration(knots[index], knots[index + 1], out Point2D right))
{
reason = "五次 Hermite 共享结点加速度无效。";
return false;
}
acceleration = new Point2D((left.X + right.X) / 2d, (left.Y + right.Y) / 2d);
if (!acceleration.IsFinite)
{
reason = "五次 Hermite 共享结点加速度混合产生非有限数值。";
return false;
}
}
knots[index] = knots[index].WithAcceleration(acceleration);
}
return true;
}
private static bool TryAcceleration(Knot start, Knot end, out Point2D acceleration)
{
acceleration = default;
double intervalLength = end.ArcLength - start.ArcLength;
if (!NumericGuard.IsPositiveFinite(intervalLength)) return false;
acceleration = new Point2D(
(end.Velocity.X - start.Velocity.X) / intervalLength,
(end.Velocity.Y - start.Velocity.Y) / intervalLength);
return acceleration.IsFinite;
}
private static bool TryCreateIntervals(
IReadOnlyList<Knot> knots,
out List<QuinticInterval> intervals,
out string reason)
{
intervals = new List<QuinticInterval>(knots.Count - 1);
reason = string.Empty;
for (int index = 1; index < knots.Count; index++)
{
if (!QuinticInterval.TryCreate(knots[index - 1], knots[index], out QuinticInterval interval))
{
reason = "五次 Hermite 系数、端点导数或结点区间无效。";
return false;
}
intervals.Add(interval);
}
return true;
}
private static double Distance(Point2D point, SmoothingPoint2D reference)
{
double deltaX = point.X - reference.X;
double deltaY = point.Y - reference.Y;
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
}
private readonly struct Knot
{
internal Knot(double arcLength, Point2D position, Point2D velocity)
{
ArcLength = arcLength;
Position = position;
Velocity = velocity;
Acceleration = default;
}
internal double ArcLength { get; }
internal Point2D Position { get; }
internal Point2D Velocity { get; }
internal Point2D Acceleration { get; }
internal Knot WithAcceleration(Point2D acceleration)
{
return new Knot(ArcLength, Position, Velocity, acceleration);
}
private Knot(double arcLength, Point2D position, Point2D velocity, Point2D acceleration)
{
ArcLength = arcLength;
Position = position;
Velocity = velocity;
Acceleration = acceleration;
}
}
private readonly struct QuinticInterval
{
private QuinticInterval(Knot start, Knot end, Point2D c0, Point2D c1, Point2D c2, Point2D c3, Point2D c4, Point2D c5)
{
Start = start;
End = end;
Length = end.ArcLength - start.ArcLength;
_c0 = c0;
_c1 = c1;
_c2 = c2;
_c3 = c3;
_c4 = c4;
_c5 = c5;
}
private readonly Point2D _c0;
private readonly Point2D _c1;
private readonly Point2D _c2;
private readonly Point2D _c3;
private readonly Point2D _c4;
private readonly Point2D _c5;
internal Knot Start { get; }
internal Knot End { get; }
internal double Length { get; }
internal static bool TryCreate(Knot start, Knot end, out QuinticInterval interval)
{
interval = default;
double length = end.ArcLength - start.ArcLength;
if (!NumericGuard.IsPositiveFinite(length) || !start.Position.IsFinite || !end.Position.IsFinite ||
!start.Velocity.IsFinite || !end.Velocity.IsFinite ||
!start.Acceleration.IsFinite || !end.Acceleration.IsFinite)
{
return false;
}
Point2D c0 = start.Position;
Point2D c1 = Scale(start.Velocity, length);
Point2D c2 = Scale(start.Acceleration, length * length / 2d);
Point2D difference = Subtract(end.Position, start.Position);
Point2D endVelocity = Scale(end.Velocity, length);
Point2D startAcceleration = Scale(start.Acceleration, length * length);
Point2D endAcceleration = Scale(end.Acceleration, length * length);
Point2D c3 = Add(
Add(Scale(difference, 10d), Scale(c1, -6d)),
Add(Scale(endVelocity, -4d), Add(Scale(startAcceleration, -1.5d), Scale(endAcceleration, 0.5d))));
Point2D c4 = Add(
Add(Scale(difference, -15d), Scale(c1, 8d)),
Add(Scale(endVelocity, 7d), Add(Scale(startAcceleration, 1.5d), Scale(endAcceleration, -1d))));
Point2D c5 = Add(
Add(Scale(difference, 6d), Add(Scale(c1, -3d), Scale(endVelocity, -3d))),
Add(Scale(startAcceleration, -0.5d), Scale(endAcceleration, 0.5d)));
if (!c0.IsFinite || !c1.IsFinite || !c2.IsFinite || !c3.IsFinite || !c4.IsFinite || !c5.IsFinite)
return false;
interval = new QuinticInterval(start, end, c0, c1, c2, c3, c4, c5);
return interval.TryEvaluate(0d, out _, out _, out _) && interval.TryEvaluate(1d, out _, out _, out _);
}
internal bool TryEvaluate(double parameter, out Point2D position, out Point2D derivative, out Point2D secondDerivative)
{
position = default;
derivative = default;
secondDerivative = default;
if (!NumericGuard.IsFinite(parameter) || parameter < 0d || parameter > 1d) return false;
double t2 = parameter * parameter;
double t3 = t2 * parameter;
double t4 = t3 * parameter;
double t5 = t4 * parameter;
position = Add(Add(Add(_c0, Scale(_c1, parameter)), Add(Scale(_c2, t2), Scale(_c3, t3))),
Add(Scale(_c4, t4), Scale(_c5, t5)));
derivative = Add(Add(_c1, Scale(_c2, 2d * parameter)),
Add(Scale(_c3, 3d * t2), Add(Scale(_c4, 4d * t3), Scale(_c5, 5d * t4))));
secondDerivative = Add(Scale(_c2, 2d),
Add(Scale(_c3, 6d * parameter), Add(Scale(_c4, 12d * t2), Scale(_c5, 20d * t3))));
return position.IsFinite && derivative.IsFinite && secondDerivative.IsFinite;
}
}
private readonly struct Point2D
{
internal Point2D(double x, double y)
{
X = x;
Y = y;
}
internal double X { get; }
internal double Y { get; }
internal bool IsFinite => NumericGuard.IsFinite(X) && NumericGuard.IsFinite(Y);
}
private static Point2D Add(Point2D left, Point2D right)
{
return new Point2D(left.X + right.X, left.Y + right.Y);
}
private static Point2D Subtract(Point2D left, Point2D right)
{
return new Point2D(left.X - right.X, left.Y - right.Y);
}
private static Point2D Scale(Point2D point, double scale)
{
return new Point2D(point.X * scale, point.Y * scale);
}
}
.task8-sweep/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingAlgorithmInput.cs
+44
View File
@@ -0,0 +1,44 @@
using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
/// <summary>单次算法运行共享的已预处理路径和独立复核上下文。</summary>
internal sealed class SmoothingAlgorithmInput
{
internal SmoothingAlgorithmInput(
PreparedPath originalPath,
PlanningGridMap map,
VehicleParameters vehicle,
double maximumCollisionCheckStepMeters,
double minimumClearanceReserveMeters,
SmoothingOptionsSnapshot options)
{
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;
Options = options ?? throw new ArgumentNullException(nameof(options));
}
/// <summary>已校验并按方向分段的原始路径。</summary>
internal PreparedPath OriginalPath { get; }
/// <summary>用于完整车体复核的不可变规划地图。</summary>
internal PlanningGridMap Map { get; }
/// <summary>用于曲率和足迹复核的车辆参数快照。</summary>
internal VehicleParameters Vehicle { get; }
/// <summary>连续车体碰撞检查的最大步长，单位 m。</summary>
internal double MaximumCollisionCheckStepMeters { get; }
/// <summary>候选几何必须从原始保守净空中预留的最小安全余量，单位 m。</summary>
internal double MinimumClearanceReserveMeters { get; }
/// <summary>本次算法运行使用的已验证方法选项快照。</summary>
internal SmoothingOptionsSnapshot Options { get; }
}
.task8-sweep/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingAlgorithmRunner.cs
+443
View File
@@ -0,0 +1,443 @@
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
/// <summary>在有限强度计划内运行单一算法，并以共享分析和安全复核决定是否接受候选。</summary>
internal sealed class SmoothingAlgorithmRunner
{
private static readonly double[] RetryStrengthScales = { 1d, 0.75d, 0.50d, 0.25d };
private readonly PathGeometryAnalyzer _analyzer;
private readonly SmoothedPathValidator _validator;
internal SmoothingAlgorithmRunner()
: this(new PathGeometryAnalyzer(), new SmoothedPathValidator())
{
}
internal SmoothingAlgorithmRunner(PathGeometryAnalyzer analyzer, SmoothedPathValidator validator)
{
_analyzer = analyzer ?? throw new ArgumentNullException(nameof(analyzer));
_validator = validator ?? throw new ArgumentNullException(nameof(validator));
}
/// <summary>
/// 依次尝试配置的有限强度比例。取消直接向上传播，由门面转换为最终状态；
/// 只有算法明确标记为可重试的不可行性才会使用较低强度；终止失败和统一复核失败均不重试。
/// </summary>
internal AlgorithmRunResult Run(
IPathSmoother smoother,
SmoothingAlgorithmInput input,
PathSmoothingConfiguration configuration,
CancellationToken cancellationToken)
{
var attemptedStrengths = new List<double>();
var failureReasons = new List<string>();
if (smoother == null || input == null || input.Options == null || configuration == null)
return AlgorithmRunResult.Failed("平滑算法、输入或配置无效。", attemptedStrengths, failureReasons);
foreach (double scale in RetryStrengthScales)
{
cancellationToken.ThrowIfCancellationRequested();
double effectiveStrength = configuration.SmoothingStrength * scale;
if (!IsPositiveFinite(effectiveStrength))
return AlgorithmRunResult.Failed("平滑强度或重试比例无效。", attemptedStrengths, failureReasons);
attemptedStrengths.Add(effectiveStrength);
SmoothingCandidate candidate = smoother.Smooth(input, effectiveStrength, cancellationToken);
if (candidate == null)
return AlgorithmRunResult.Failed("平滑算法未返回候选。", attemptedStrengths, failureReasons);
if (candidate.Status == SmoothingCandidateStatus.RetryableInfeasible)
{
failureReasons.Add(candidate.Reason);
continue;
}
if (candidate.Status == SmoothingCandidateStatus.Failed)
{
failureReasons.Add(candidate.Reason);
return AlgorithmRunResult.Failed(candidate.Reason, attemptedStrengths, failureReasons);
}
if (candidate.Status != SmoothingCandidateStatus.Success)
{
const string unknownStatusReason = "平滑算法返回未知候选状态。";
failureReasons.Add(unknownStatusReason);
return AlgorithmRunResult.Failed(unknownStatusReason, attemptedStrengths, failureReasons);
}
if (!_analyzer.TryAnalyze(candidate.Segments, configuration.OutputSpacingMeters,
out PathGeometryAnalysis analysis, out string reason))
{
failureReasons.Add(reason);
return AlgorithmRunResult.Failed(reason, attemptedStrengths, failureReasons);
}
if (_validator.TryValidate(analysis.Path, analysis.Segments, input.OriginalPath, input.Map, input.Vehicle,
input.MaximumCollisionCheckStepMeters, out IReadOnlyList<SmoothedPathPoint> safePath,
out double minimumClearanceMeters, out reason))
{
return AlgorithmRunResult.Success(safePath, analysis.Segments,
CreateMetrics(analysis, minimumClearanceMeters), effectiveStrength,
attemptedStrengths, failureReasons);
}
failureReasons.Add(reason);
return AlgorithmRunResult.Failed(reason, attemptedStrengths, failureReasons);
}
return AlgorithmRunResult.Infeasible(null, attemptedStrengths, failureReasons);
}
private static PathQualityMetrics CreateMetrics(PathGeometryAnalysis analysis, double minimumClearanceMeters)
{
return new PathQualityMetrics(
true,
analysis.PathLengthMeters,
analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
analysis.RootMeanSquareVehicleCurvaturePerMeter,
analysis.TotalAbsoluteCurvatureVariationPerMeter,
analysis.CurvatureVariationEnergy,
minimumClearanceMeters,
0d,
0d,
0d,
0d);
}
private static bool IsPositiveFinite(double value)
{
return !double.IsNaN(value) && !double.IsInfinity(value) && value > 0d;
}
/// <summary>
/// Reflection-only deterministic coverage seam. It is nested in an internal runner and intentionally
/// does not construct or register a production smoothing method.
/// </summary>
public static class TestHooks
{
/// <summary>执行一个固定的内部假平滑器场景并返回可反射读取的快照。</summary>
public static RunnerTestSnapshot Execute(string scenario)
{
if (string.IsNullOrWhiteSpace(scenario)) throw new ArgumentException("A scenario is required.", nameof(scenario));
var cancellationSource = new CancellationTokenSource();
var smoother = new DeterministicTestSmoother(ParseScenario(scenario), cancellationSource);
var runner = new SmoothingAlgorithmRunner();
SmoothingAlgorithmInput input = CreateTestInput();
var configuration = new PathSmoothingConfiguration();
try
{
AlgorithmRunResult result = runner.Run(smoother, input, configuration, cancellationSource.Token);
return RunnerTestSnapshot.FromResult(result, false);
}
catch (OperationCanceledException)
{
return new RunnerTestSnapshot(
"OperationCanceledException",
smoother.AttemptedStrengths,
0,
0,
0,
true);
}
finally
{
cancellationSource.Dispose();
}
}
/// <summary>供 PowerShell 断言使用的不可变执行摘要。</summary>
public sealed class RunnerTestSnapshot
{
internal RunnerTestSnapshot(
string status,
IReadOnlyList<double> attemptedStrengths,
int acceptedPathPointCount,
int rejectedComparisonCandidatePointCount,
int failureCount,
bool cancellationPropagated)
{
Status = status ?? string.Empty;
AttemptedStrengths = CopyReadOnly(attemptedStrengths);
AcceptedPathPointCount = acceptedPathPointCount;
RejectedComparisonCandidatePointCount = rejectedComparisonCandidatePointCount;
FailureCount = failureCount;
CancellationPropagated = cancellationPropagated;
}
public string Status { get; }
public IReadOnlyList<double> AttemptedStrengths { get; }
public int AcceptedPathPointCount { get; }
public int RejectedComparisonCandidatePointCount { get; }
public int FailureCount { get; }
public bool CancellationPropagated { get; }
internal static RunnerTestSnapshot FromResult(AlgorithmRunResult result, bool cancellationPropagated)
{
int rejectedPointCount = result.RejectedComparisonCandidate == null
? 0
: CountPoints(result.RejectedComparisonCandidate.Segments);
return new RunnerTestSnapshot(
result.Status.ToString(),
result.AttemptedStrengths,
result.Path.Count,
rejectedPointCount,
result.FailureReasons.Count,
cancellationPropagated);
}
private static IReadOnlyList<T> CopyReadOnly<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);
}
}
private enum TestScenario
{
RetryableInfeasible,
AcceptFirst,
TerminalFailed,
CancelBeforeNextAttempt,
}
private sealed class DeterministicTestSmoother : IPathSmoother
{
private readonly TestScenario _scenario;
private readonly CancellationTokenSource _cancellationSource;
internal DeterministicTestSmoother(TestScenario scenario, CancellationTokenSource cancellationSource)
{
_scenario = scenario;
_cancellationSource = cancellationSource;
AttemptedStrengths = new List<double>();
}
public SmoothingMethod Method => SmoothingMethod.CubicBSpline;
internal List<double> AttemptedStrengths { get; }
public SmoothingCandidate Smooth(
SmoothingAlgorithmInput input,
double effectiveStrength,
CancellationToken cancellationToken)
{
AttemptedStrengths.Add(effectiveStrength);
if (_scenario == TestScenario.TerminalFailed)
return SmoothingCandidate.Failed("确定性数值退化。");
if (_scenario == TestScenario.AcceptFirst)
return CreateAcceptedCandidate();
if (_scenario == TestScenario.CancelBeforeNextAttempt)
_cancellationSource.Cancel();
return SmoothingCandidate.RetryableInfeasible("确定性可重试不可行。" );
}
}
private static TestScenario ParseScenario(string scenario)
{
if (string.Equals(scenario, nameof(TestScenario.RetryableInfeasible), StringComparison.Ordinal)) return TestScenario.RetryableInfeasible;
if (string.Equals(scenario, nameof(TestScenario.AcceptFirst), StringComparison.Ordinal)) return TestScenario.AcceptFirst;
if (string.Equals(scenario, nameof(TestScenario.TerminalFailed), StringComparison.Ordinal)) return TestScenario.TerminalFailed;
if (string.Equals(scenario, nameof(TestScenario.CancelBeforeNextAttempt), StringComparison.Ordinal)) return TestScenario.CancelBeforeNextAttempt;
throw new ArgumentOutOfRangeException(nameof(scenario));
}
private static SmoothingAlgorithmInput CreateTestInput()
{
var mapRequest = new PlanningMapRequest
{
Bounds = new MapBoundsMm(0f, 5000f, 0f, 5000f),
ResolutionMm = 50f,
AllowExplicitEmptyMap = true,
};
PlanningMapBuildResult mapResult = new PlanningMapFactory().Create(mapRequest);
if (!mapResult.Succeeded || mapResult.Map == null)
throw new InvalidOperationException("The runner test hook could not create its empty map.");
var originalSegments = new List<PreparedDirectionSegment>
{
new PreparedDirectionSegment(
0,
TravelDirection.Forward,
new List<SmoothingPoint2D>
{
CreatePoint(0.5d, 0.5d, 0d),
CreatePoint(1.5d, 0.5d, 1d),
},
false,
false),
};
var vehicle = new VehicleParameters
{
LengthMeters = 0.20d,
WidthMeters = 0.20d,
SafetyMarginMeters = 0d,
MaximumCurvaturePerMeter = 100d,
MinimumTurningRadiusMeters = 0.01d,
};
return new SmoothingAlgorithmInput(
new PreparedPath(originalSegments),
mapResult.Map,
vehicle,
0.05d,
0.02d,
new SmoothingOptionsSnapshot(new PathSmoothingConfiguration()));
}
private static SmoothingCandidate CreateAcceptedCandidate()
{
return SmoothingCandidate.Success(new List<PreparedDirectionSegment>
{
new PreparedDirectionSegment(
0,
TravelDirection.Forward,
new List<SmoothingPoint2D>
{
CreatePoint(0.5d, 0.5d, 0d),
CreatePoint(1.5d, 0.5d, 1d),
},
false,
false),
});
}
private static SmoothingPoint2D CreatePoint(double x, double y, double arcLength)
{
return new SmoothingPoint2D(
x,
y,
arcLength,
0d,
0d,
1d,
false,
SmoothedPathPointSource.Anchor);
}
private static int CountPoints(IReadOnlyList<PreparedDirectionSegment> segments)
{
int count = 0;
for (int index = 0; index < segments.Count; index++) count += segments[index].Points.Count;
return count;
}
}
/// <summary>内部运行结果；只有成功路径可被正式门面发布，拒绝候选仅供比较诊断读取。</summary>
internal sealed class AlgorithmRunResult
{
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 AlgorithmRunResult(
PathSmoothingStatus status,
IReadOnlyList<SmoothedPathPoint> path,
IReadOnlyList<SmoothedPathSegment> segments,
PathQualityMetrics metrics,
double acceptedStrength,
SmoothingCandidate rejectedComparisonCandidate,
IReadOnlyList<double> attemptedStrengths,
IReadOnlyList<string> failureReasons,
string reason)
{
Status = status;
Path = path ?? EmptyPath;
Segments = segments ?? EmptySegments;
Metrics = metrics ?? new PathQualityMetrics();
AcceptedStrength = acceptedStrength;
RejectedComparisonCandidate = rejectedComparisonCandidate;
AttemptedStrengths = CopyReadOnly(attemptedStrengths);
FailureReasons = CopyReadOnly(failureReasons);
Reason = reason ?? string.Empty;
}
internal PathSmoothingStatus Status { get; }
internal IReadOnlyList<SmoothedPathPoint> Path { get; }
internal IReadOnlyList<SmoothedPathSegment> Segments { get; }
internal PathQualityMetrics Metrics { get; }
internal double AcceptedStrength { get; }
internal SmoothingCandidate RejectedComparisonCandidate { get; }
internal IReadOnlyList<double> AttemptedStrengths { get; }
internal IReadOnlyList<string> FailureReasons { get; }
internal string Reason { get; }
internal static AlgorithmRunResult Success(
IReadOnlyList<SmoothedPathPoint> path,
IReadOnlyList<SmoothedPathSegment> segments,
PathQualityMetrics metrics,
double acceptedStrength,
IReadOnlyList<double> attemptedStrengths,
IReadOnlyList<string> failureReasons)
{
return new AlgorithmRunResult(
PathSmoothingStatus.Success,
CopyReadOnly(path),
CopyReadOnly(segments),
metrics,
acceptedStrength,
null,
attemptedStrengths,
failureReasons,
string.Empty);
}
internal static AlgorithmRunResult Infeasible(
SmoothingCandidate rejectedComparisonCandidate,
IReadOnlyList<double> attemptedStrengths,
IReadOnlyList<string> failureReasons)
{
string reason = failureReasons == null || failureReasons.Count == 0
? "所有有限平滑尝试均未通过复核。"
: failureReasons[failureReasons.Count - 1];
return new AlgorithmRunResult(
PathSmoothingStatus.Infeasible,
null,
null,
null,
0d,
rejectedComparisonCandidate,
attemptedStrengths,
failureReasons,
reason);
}
internal static AlgorithmRunResult Failed(
string reason,
IReadOnlyList<double> attemptedStrengths,
IReadOnlyList<string> failureReasons)
{
return new AlgorithmRunResult(
PathSmoothingStatus.Failed,
null,
null,
null,
0d,
null,
attemptedStrengths,
failureReasons,
reason);
}
private static IReadOnlyList<T> CopyReadOnly<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);
}
}
}
.task8-sweep/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingCandidate.cs
+83
View File
@@ -0,0 +1,83 @@
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
internal enum SmoothingCandidateStatus
{
Success,
RetryableInfeasible,
Failed,
}
/// <summary>平滑方法产生的原始方向段候选，尚未经过统一几何或安全复核。</summary>
internal sealed class SmoothingCandidate
{
private SmoothingCandidate(
SmoothingCandidateStatus status,
IReadOnlyList<PreparedDirectionSegment> segments,
string reason)
{
Status = status;
if (status == SmoothingCandidateStatus.Success)
{
if (segments == null || segments.Count == 0)
throw new ArgumentException("A successful smoothing candidate requires direction segments.", nameof(segments));
for (int index = 0; index < segments.Count; index++)
{
if (segments[index] == null || segments[index].Points == null || segments[index].Points.Count == 0)
throw new ArgumentException("A successful smoothing candidate requires complete direction segments.", nameof(segments));
}
Segments = CopyReadOnly(segments);
Reason = string.Empty;
return;
}
if (string.IsNullOrWhiteSpace(reason))
throw new ArgumentException("An unsuccessful smoothing candidate requires a reason.", nameof(reason));
Segments = CopyReadOnly<PreparedDirectionSegment>(null);
Reason = reason;
}
/// <summary>候选是否成功产生有限的原始几何。</summary>
internal bool Succeeded => Status == SmoothingCandidateStatus.Success;
/// <summary>候选的可重试性和终止性状态。</summary>
internal SmoothingCandidateStatus Status { get; }
/// <summary>候选方向段；失败候选始终为空。</summary>
internal IReadOnlyList<PreparedDirectionSegment> Segments { get; }
/// <summary>失败或退化时的稳定说明；成功时为空。</summary>
internal string Reason { get; }
/// <summary>创建待统一分析和验证的成功候选。</summary>
internal static SmoothingCandidate Success(IReadOnlyList<PreparedDirectionSegment> segments)
{
return new SmoothingCandidate(SmoothingCandidateStatus.Success, segments, string.Empty);
}
/// <summary>创建可由较低平滑强度重新尝试的不可行候选。</summary>
internal static SmoothingCandidate RetryableInfeasible(string reason)
{
return new SmoothingCandidate(SmoothingCandidateStatus.RetryableInfeasible, null, reason);
}
/// <summary>创建不应重试的数值或构造失败候选。</summary>
internal static SmoothingCandidate Failed(string reason)
{
return new SmoothingCandidate(SmoothingCandidateStatus.Failed, null, reason);
}
private static IReadOnlyList<T> CopyReadOnly<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);
}
}
.task8-sweep/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingOptionsSnapshot.cs
+50
View File
@@ -0,0 +1,50 @@
using System;
using MultiWheelC.TrajectoryPlanning.Utils;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
/// <summary>供单次平滑算法运行使用的、已校验的不可变方法选项快照。</summary>
internal sealed class SmoothingOptionsSnapshot
{
internal SmoothingOptionsSnapshot(PathSmoothingConfiguration configuration)
{
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
CubicBSplineEndpointTangentScale = configuration.CubicBSpline.EndpointTangentScale;
BezierCornerHeadingThresholdRadians = configuration.LocalCubicBezier.CornerHeadingThresholdRadians;
BezierMaximumWindowLengthMeters = configuration.LocalCubicBezier.MaximumWindowLengthMeters;
BezierHandleLengthRatio = configuration.LocalCubicBezier.HandleLengthRatio;
QuinticKnotSpacingMeters = configuration.PiecewiseQuintic.KnotSpacingMeters;
QuinticMinimumKnotSpacingMeters = configuration.PiecewiseQuintic.MinimumKnotSpacingMeters;
ValidatePositiveFinite(CubicBSplineEndpointTangentScale, nameof(CubicBSplineEndpointTangentScale));
if (!NumericGuard.IsFinite(BezierCornerHeadingThresholdRadians) ||
BezierCornerHeadingThresholdRadians <= 0d || BezierCornerHeadingThresholdRadians > Math.PI)
{
throw new ArgumentOutOfRangeException(nameof(BezierCornerHeadingThresholdRadians));
}
ValidatePositiveFinite(BezierMaximumWindowLengthMeters, nameof(BezierMaximumWindowLengthMeters));
ValidatePositiveFinite(BezierHandleLengthRatio, nameof(BezierHandleLengthRatio));
ValidatePositiveFinite(QuinticKnotSpacingMeters, nameof(QuinticKnotSpacingMeters));
ValidatePositiveFinite(QuinticMinimumKnotSpacingMeters, nameof(QuinticMinimumKnotSpacingMeters));
if (QuinticKnotSpacingMeters < QuinticMinimumKnotSpacingMeters)
throw new ArgumentOutOfRangeException(nameof(QuinticKnotSpacingMeters));
}
internal double CubicBSplineEndpointTangentScale { get; }
internal double BezierCornerHeadingThresholdRadians { get; }
internal double BezierMaximumWindowLengthMeters { get; }
internal double BezierHandleLengthRatio { get; }
internal double QuinticKnotSpacingMeters { get; }
internal double QuinticMinimumKnotSpacingMeters { get; }
private static void ValidatePositiveFinite(double value, string name)
{
if (!NumericGuard.IsPositiveFinite(value)) throw new ArgumentOutOfRangeException(name);
}
}