feat: add piecewise quintic path smoother
This commit is contained in:
梁薄云
@@ -0,0 +1,532 @@
|
||||
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;
|
||||
|
||||
/// <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));
|
||||
}
|
||||
|
||||
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 targetArcLength = startArcLength + knotSpacingMeters;
|
||||
while (targetArcLength < endArcLength)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
if (!NumericGuard.IsFinite(targetArcLength))
|
||||
{
|
||||
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);
|
||||
targetArcLength += knotSpacingMeters;
|
||||
}
|
||||
|
||||
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 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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user