558 lines
22 KiB
C#
558 lines
22 KiB
C#
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);
|
|
}
|
|
}
|