feat: add path smoothing geometry foundation
This commit is contained in:
梁薄云
@@ -0,0 +1,144 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>按单一方向段的弧长线性插值并保留精确锚点的确定性重采样器。</summary>
|
||||
public sealed class ArcLengthResampler
|
||||
{
|
||||
private const double Tolerance = 1e-10d;
|
||||
|
||||
/// <summary>以目标间距重采样一个方向段；段末锚点始终原样保留。</summary>
|
||||
public bool TryResample(
|
||||
IReadOnlyList<SmoothingPoint2D> points,
|
||||
double spacingMeters,
|
||||
out IReadOnlyList<SmoothingPoint2D> resampled,
|
||||
out string reason)
|
||||
{
|
||||
resampled = EmptyPoints();
|
||||
reason = string.Empty;
|
||||
if (points == null || points.Count == 0 || !NumericGuard.IsPositiveFinite(spacingMeters))
|
||||
{
|
||||
reason = "重采样点集或采样间距无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int index = 0; index < points.Count; index++)
|
||||
{
|
||||
if (!IsValidPoint(points[index]))
|
||||
{
|
||||
reason = "重采样输入包含非法数值。";
|
||||
return false;
|
||||
}
|
||||
if (index == 0) continue;
|
||||
|
||||
SmoothingPoint2D previous = points[index - 1];
|
||||
SmoothingPoint2D current = points[index];
|
||||
if (current.ArcLength <= previous.ArcLength + Tolerance)
|
||||
{
|
||||
reason = "同一方向段的弧长必须严格增加。";
|
||||
return false;
|
||||
}
|
||||
double distance = Distance(previous, current);
|
||||
if (!NumericGuard.IsFinite(distance) || distance <= Tolerance)
|
||||
{
|
||||
reason = "同一方向段中不允许重复位姿或数值溢出的距离。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
if (points.Count == 1)
|
||||
{
|
||||
resampled = CopyReadOnly(points);
|
||||
return true;
|
||||
}
|
||||
|
||||
var output = new List<SmoothingPoint2D> { points[0] };
|
||||
double firstArc = points[0].ArcLength;
|
||||
double finalArc = points[points.Count - 1].ArcLength;
|
||||
int rightIndex = 1;
|
||||
for (double targetArc = firstArc + spacingMeters;
|
||||
targetArc < finalArc - Tolerance;
|
||||
targetArc += spacingMeters)
|
||||
{
|
||||
while (rightIndex < points.Count - 1 && points[rightIndex].ArcLength < targetArc)
|
||||
rightIndex++;
|
||||
|
||||
SmoothingPoint2D left = points[rightIndex - 1];
|
||||
SmoothingPoint2D right = points[rightIndex];
|
||||
double ratio = (targetArc - left.ArcLength) / (right.ArcLength - left.ArcLength);
|
||||
double unwrappedHeading = left.UnwrappedHeading +
|
||||
ratio * (right.UnwrappedHeading - left.UnwrappedHeading);
|
||||
output.Add(new SmoothingPoint2D(
|
||||
left.X + ratio * (right.X - left.X),
|
||||
left.Y + ratio * (right.Y - left.Y),
|
||||
targetArc,
|
||||
AngleMath.NormalizeRadians(unwrappedHeading),
|
||||
unwrappedHeading,
|
||||
Math.Min(left.BodyClearance, right.BodyClearance),
|
||||
false,
|
||||
SmoothedPathPointSource.Interpolated));
|
||||
}
|
||||
|
||||
// Appending the original object, rather than interpolating at the final arc, preserves the exact endpoint.
|
||||
output.Add(points[points.Count - 1]);
|
||||
resampled = new ReadOnlyCollection<SmoothingPoint2D>(output);
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>重采样一个完整方向段并保留其换向拓扑标记。</summary>
|
||||
public bool TryResample(
|
||||
PreparedDirectionSegment segment,
|
||||
double spacingMeters,
|
||||
out PreparedDirectionSegment resampled,
|
||||
out string reason)
|
||||
{
|
||||
resampled = null;
|
||||
reason = string.Empty;
|
||||
if (segment == null)
|
||||
{
|
||||
reason = "待重采样方向段为空。";
|
||||
return false;
|
||||
}
|
||||
if (!TryResample(segment.Points, spacingMeters, out IReadOnlyList<SmoothingPoint2D> points, out reason))
|
||||
return false;
|
||||
|
||||
resampled = new PreparedDirectionSegment(
|
||||
segment.SegmentIndex,
|
||||
segment.Direction,
|
||||
points,
|
||||
segment.StartsAtGearSwitch,
|
||||
segment.EndsAtGearSwitch);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidPoint(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 Distance(SmoothingPoint2D left, SmoothingPoint2D right)
|
||||
{
|
||||
double x = right.X - left.X;
|
||||
double y = right.Y - left.Y;
|
||||
return Math.Sqrt(x * x + y * y);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingPoint2D> EmptyPoints()
|
||||
{
|
||||
return new ReadOnlyCollection<SmoothingPoint2D>(new List<SmoothingPoint2D>());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>同一几何分析器产生的路径、方向段和未验证质量统计。</summary>
|
||||
public sealed class PathGeometryAnalysis
|
||||
{
|
||||
internal PathGeometryAnalysis(
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
double pathLengthMeters,
|
||||
double maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
double rootMeanSquareVehicleCurvaturePerMeter,
|
||||
double totalAbsoluteCurvatureVariationPerMeter,
|
||||
double curvatureVariationEnergy,
|
||||
double minimumBodyClearanceMeters)
|
||||
{
|
||||
Path = CopyReadOnly(path);
|
||||
Segments = CopyReadOnly(segments);
|
||||
PathLengthMeters = pathLengthMeters;
|
||||
MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
|
||||
RootMeanSquareVehicleCurvaturePerMeter = rootMeanSquareVehicleCurvaturePerMeter;
|
||||
TotalAbsoluteCurvatureVariationPerMeter = totalAbsoluteCurvatureVariationPerMeter;
|
||||
CurvatureVariationEnergy = curvatureVariationEnergy;
|
||||
MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
|
||||
}
|
||||
|
||||
/// <summary>完成几何重计算的不可变路径。</summary>
|
||||
public IReadOnlyList<SmoothedPathPoint> Path { get; }
|
||||
|
||||
/// <summary>完整覆盖 <see cref="Path"/> 的不可变方向段。</summary>
|
||||
public IReadOnlyList<SmoothedPathSegment> Segments { get; }
|
||||
|
||||
/// <summary>路径总长度，单位 m。</summary>
|
||||
public double PathLengthMeters { get; }
|
||||
|
||||
/// <summary>车辆曲率绝对值峰值，单位 1/m。</summary>
|
||||
public double MaximumAbsoluteVehicleCurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>车辆曲率均方根，单位 1/m。</summary>
|
||||
public double RootMeanSquareVehicleCurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>不跨换向点累计的绝对曲率变化，单位 1/m。</summary>
|
||||
public double TotalAbsoluteCurvatureVariationPerMeter { get; }
|
||||
|
||||
/// <summary>不跨换向点累计的曲率变化能量。</summary>
|
||||
public double CurvatureVariationEnergy { get; }
|
||||
|
||||
/// <summary>输入点携带的最小保守净空，单位 m。</summary>
|
||||
public double MinimumBodyClearanceMeters { get; }
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,401 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>在每个单独方向段内统一重采样、恢复航向并计算几何曲率。</summary>
|
||||
public sealed class PathGeometryAnalyzer
|
||||
{
|
||||
private const double MinimumDistanceMeters = 1e-10d;
|
||||
private const double BoundaryToleranceMeters = 1e-8d;
|
||||
|
||||
/// <summary>
|
||||
/// 对候选方向段进行确定性几何分析。换向点两侧永不参与同一次差分。
|
||||
/// </summary>
|
||||
public bool TryAnalyze(
|
||||
IReadOnlyList<PreparedDirectionSegment> candidateSegments,
|
||||
double spacingMeters,
|
||||
out PathGeometryAnalysis analysis,
|
||||
out string reason)
|
||||
{
|
||||
analysis = null;
|
||||
reason = string.Empty;
|
||||
if (candidateSegments == null || candidateSegments.Count == 0 ||
|
||||
!NumericGuard.IsPositiveFinite(spacingMeters))
|
||||
{
|
||||
reason = "候选方向段或输出采样间距无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var outputPath = new List<SmoothedPathPoint>();
|
||||
var outputSegments = new List<SmoothedPathSegment>();
|
||||
double cumulativeArcLength = 0d;
|
||||
double previousOutputHeading = 0d;
|
||||
double previousOutputUnwrappedHeading = 0d;
|
||||
bool hasPreviousOutputHeading = false;
|
||||
double maximumAbsoluteVehicleCurvature = 0d;
|
||||
double curvatureSquareSum = 0d;
|
||||
int curvatureSampleCount = 0;
|
||||
double totalCurvatureVariation = 0d;
|
||||
double curvatureVariationEnergy = 0d;
|
||||
double minimumClearance = double.PositiveInfinity;
|
||||
|
||||
for (int segmentIndex = 0; segmentIndex < candidateSegments.Count; segmentIndex++)
|
||||
{
|
||||
PreparedDirectionSegment segment = candidateSegments[segmentIndex];
|
||||
if (!IsValidSegment(segment, segmentIndex, out reason)) return false;
|
||||
if (!TryValidateBoundary(candidateSegments, segmentIndex, out reason)) return false;
|
||||
|
||||
if (!TryResampleByGeometry(segment.Points, spacingMeters, out IReadOnlyList<SmoothingPoint2D> samples, out reason))
|
||||
return false;
|
||||
|
||||
if (!TryAnalyzeSegment(
|
||||
segment,
|
||||
samples,
|
||||
ref cumulativeArcLength,
|
||||
ref previousOutputHeading,
|
||||
ref previousOutputUnwrappedHeading,
|
||||
ref hasPreviousOutputHeading,
|
||||
outputPath,
|
||||
out double segmentMaximumCurvature,
|
||||
out double segmentCurvatureSquareSum,
|
||||
out int segmentCurvatureSampleCount,
|
||||
out double segmentVariation,
|
||||
out double segmentVariationEnergy,
|
||||
out double segmentMinimumClearance,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, segmentMaximumCurvature);
|
||||
curvatureSquareSum += segmentCurvatureSquareSum;
|
||||
curvatureSampleCount += segmentCurvatureSampleCount;
|
||||
totalCurvatureVariation += segmentVariation;
|
||||
curvatureVariationEnergy += segmentVariationEnergy;
|
||||
minimumClearance = Math.Min(minimumClearance, segmentMinimumClearance);
|
||||
int endIndex = outputPath.Count - 1;
|
||||
int startIndex = endIndex - samples.Count + 1;
|
||||
outputSegments.Add(new SmoothedPathSegment(
|
||||
segment.SegmentIndex,
|
||||
segment.Direction,
|
||||
startIndex,
|
||||
endIndex,
|
||||
segment.StartsAtGearSwitch,
|
||||
segment.EndsAtGearSwitch));
|
||||
}
|
||||
|
||||
double rmsCurvature = curvatureSampleCount == 0 ? 0d : Math.Sqrt(curvatureSquareSum / curvatureSampleCount);
|
||||
analysis = new PathGeometryAnalysis(
|
||||
outputPath,
|
||||
outputSegments,
|
||||
cumulativeArcLength,
|
||||
maximumAbsoluteVehicleCurvature,
|
||||
rmsCurvature,
|
||||
totalCurvatureVariation,
|
||||
curvatureVariationEnergy,
|
||||
minimumClearance);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryAnalyzeSegment(
|
||||
PreparedDirectionSegment segment,
|
||||
IReadOnlyList<SmoothingPoint2D> samples,
|
||||
ref double cumulativeArcLength,
|
||||
ref double previousOutputHeading,
|
||||
ref double previousOutputUnwrappedHeading,
|
||||
ref bool hasPreviousOutputHeading,
|
||||
List<SmoothedPathPoint> output,
|
||||
out double maximumAbsoluteVehicleCurvature,
|
||||
out double curvatureSquareSum,
|
||||
out int curvatureSampleCount,
|
||||
out double totalCurvatureVariation,
|
||||
out double curvatureVariationEnergy,
|
||||
out double minimumClearance,
|
||||
out string reason)
|
||||
{
|
||||
maximumAbsoluteVehicleCurvature = 0d;
|
||||
curvatureSquareSum = 0d;
|
||||
curvatureSampleCount = 0;
|
||||
totalCurvatureVariation = 0d;
|
||||
curvatureVariationEnergy = 0d;
|
||||
minimumClearance = double.PositiveInfinity;
|
||||
reason = string.Empty;
|
||||
int count = samples.Count;
|
||||
var localArcLengths = new double[count];
|
||||
var headings = new double[count];
|
||||
var unwrappedHeadings = new double[count];
|
||||
var geometricCurvatures = new double[count];
|
||||
|
||||
for (int index = 1; index < count; index++)
|
||||
{
|
||||
double distance = Distance(samples[index - 1], samples[index]);
|
||||
if (!NumericGuard.IsFinite(distance) || distance <= MinimumDistanceMeters)
|
||||
{
|
||||
reason = "同一方向段中包含重复或退化的路径点。";
|
||||
return false;
|
||||
}
|
||||
localArcLengths[index] = localArcLengths[index - 1] + distance;
|
||||
}
|
||||
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
double travelHeading;
|
||||
if (count == 1)
|
||||
{
|
||||
travelHeading = segment.Direction == TravelDirection.Forward
|
||||
? samples[index].Heading
|
||||
: samples[index].Heading - Math.PI;
|
||||
}
|
||||
else if (index == 0)
|
||||
{
|
||||
travelHeading = Math.Atan2(samples[1].Y - samples[0].Y, samples[1].X - samples[0].X);
|
||||
}
|
||||
else if (index == count - 1)
|
||||
{
|
||||
travelHeading = Math.Atan2(samples[index].Y - samples[index - 1].Y,
|
||||
samples[index].X - samples[index - 1].X);
|
||||
}
|
||||
else
|
||||
{
|
||||
travelHeading = Math.Atan2(samples[index + 1].Y - samples[index - 1].Y,
|
||||
samples[index + 1].X - samples[index - 1].X);
|
||||
}
|
||||
|
||||
double heading = AngleMath.NormalizeRadians(
|
||||
segment.Direction == TravelDirection.Forward ? travelHeading : travelHeading + Math.PI);
|
||||
if (!NumericGuard.IsFinite(heading))
|
||||
{
|
||||
reason = "候选路径航向无法归一化。";
|
||||
return false;
|
||||
}
|
||||
|
||||
headings[index] = heading;
|
||||
if (index == 0 && !hasPreviousOutputHeading)
|
||||
{
|
||||
unwrappedHeadings[index] = heading;
|
||||
}
|
||||
else if (index == 0)
|
||||
{
|
||||
unwrappedHeadings[index] = previousOutputUnwrappedHeading +
|
||||
AngleMath.ShortestSignedDifference(previousOutputHeading, heading);
|
||||
}
|
||||
else
|
||||
{
|
||||
unwrappedHeadings[index] = unwrappedHeadings[index - 1] +
|
||||
AngleMath.ShortestSignedDifference(headings[index - 1], heading);
|
||||
}
|
||||
}
|
||||
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
if (count == 1)
|
||||
{
|
||||
geometricCurvatures[index] = 0d;
|
||||
}
|
||||
else if (index == 0)
|
||||
{
|
||||
geometricCurvatures[index] = (unwrappedHeadings[1] - unwrappedHeadings[0]) /
|
||||
(localArcLengths[1] - localArcLengths[0]);
|
||||
}
|
||||
else if (index == count - 1)
|
||||
{
|
||||
geometricCurvatures[index] = (unwrappedHeadings[index] - unwrappedHeadings[index - 1]) /
|
||||
(localArcLengths[index] - localArcLengths[index - 1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
geometricCurvatures[index] = (unwrappedHeadings[index + 1] - unwrappedHeadings[index - 1]) /
|
||||
(localArcLengths[index + 1] - localArcLengths[index - 1]);
|
||||
}
|
||||
|
||||
if (!NumericGuard.IsFinite(geometricCurvatures[index]))
|
||||
{
|
||||
reason = "候选路径曲率计算产生了非法数值。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
SmoothingPoint2D sample = samples[index];
|
||||
double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
|
||||
double vehicleCurvature = directionSign * geometricCurvatures[index];
|
||||
double arcLength = cumulativeArcLength + localArcLengths[index];
|
||||
bool isGearSwitch = index == 0 && segment.StartsAtGearSwitch;
|
||||
SmoothedPathPointSource source = isGearSwitch ? SmoothedPathPointSource.GearSwitch : sample.Source;
|
||||
output.Add(new SmoothedPathPoint(
|
||||
sample.X,
|
||||
sample.Y,
|
||||
headings[index],
|
||||
unwrappedHeadings[index],
|
||||
arcLength,
|
||||
segment.Direction,
|
||||
geometricCurvatures[index],
|
||||
vehicleCurvature,
|
||||
sample.BodyClearance,
|
||||
isGearSwitch,
|
||||
source));
|
||||
|
||||
maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(vehicleCurvature));
|
||||
curvatureSquareSum += vehicleCurvature * vehicleCurvature;
|
||||
curvatureSampleCount++;
|
||||
minimumClearance = Math.Min(minimumClearance, sample.BodyClearance);
|
||||
if (index > 0)
|
||||
{
|
||||
double deltaCurvature = geometricCurvatures[index] - geometricCurvatures[index - 1];
|
||||
double deltaArc = localArcLengths[index] - localArcLengths[index - 1];
|
||||
totalCurvatureVariation += Math.Abs(deltaCurvature);
|
||||
curvatureVariationEnergy += (deltaCurvature / deltaArc) * (deltaCurvature / deltaArc) * deltaArc;
|
||||
}
|
||||
}
|
||||
|
||||
cumulativeArcLength += localArcLengths[count - 1];
|
||||
previousOutputHeading = headings[count - 1];
|
||||
previousOutputUnwrappedHeading = unwrappedHeadings[count - 1];
|
||||
hasPreviousOutputHeading = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryResampleByGeometry(
|
||||
IReadOnlyList<SmoothingPoint2D> input,
|
||||
double spacingMeters,
|
||||
out IReadOnlyList<SmoothingPoint2D> samples,
|
||||
out string reason)
|
||||
{
|
||||
samples = null;
|
||||
reason = string.Empty;
|
||||
var normalized = new List<SmoothingPoint2D>(input.Count);
|
||||
double localArcLength = 0d;
|
||||
for (int index = 0; index < input.Count; index++)
|
||||
{
|
||||
SmoothingPoint2D point = input[index];
|
||||
if (!IsValidPoint(point))
|
||||
{
|
||||
reason = "候选路径点包含非法数值。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (index > 0)
|
||||
{
|
||||
double distance = Distance(input[index - 1], point);
|
||||
if (!NumericGuard.IsFinite(distance) || distance <= MinimumDistanceMeters)
|
||||
{
|
||||
reason = "同一方向段中包含重复或退化的路径点。";
|
||||
return false;
|
||||
}
|
||||
localArcLength += distance;
|
||||
}
|
||||
|
||||
normalized.Add(new SmoothingPoint2D(
|
||||
point.X, point.Y, localArcLength, point.Heading, point.UnwrappedHeading,
|
||||
point.BodyClearance, point.IsGearSwitchPoint, point.Source));
|
||||
}
|
||||
|
||||
if (normalized.Count == 1)
|
||||
{
|
||||
samples = normalized;
|
||||
return true;
|
||||
}
|
||||
|
||||
var result = new List<SmoothingPoint2D> { normalized[0] };
|
||||
double finalArc = normalized[normalized.Count - 1].ArcLength;
|
||||
int rightIndex = 1;
|
||||
for (double targetArc = spacingMeters; targetArc < finalArc - MinimumDistanceMeters; targetArc += spacingMeters)
|
||||
{
|
||||
while (rightIndex < normalized.Count - 1 && normalized[rightIndex].ArcLength < targetArc)
|
||||
rightIndex++;
|
||||
SmoothingPoint2D left = normalized[rightIndex - 1];
|
||||
SmoothingPoint2D right = normalized[rightIndex];
|
||||
double ratio = (targetArc - left.ArcLength) / (right.ArcLength - left.ArcLength);
|
||||
double unwrappedHeading = left.UnwrappedHeading + ratio * (right.UnwrappedHeading - left.UnwrappedHeading);
|
||||
result.Add(new SmoothingPoint2D(
|
||||
left.X + ratio * (right.X - left.X),
|
||||
left.Y + ratio * (right.Y - left.Y),
|
||||
targetArc,
|
||||
AngleMath.NormalizeRadians(unwrappedHeading),
|
||||
unwrappedHeading,
|
||||
Math.Min(left.BodyClearance, right.BodyClearance),
|
||||
false,
|
||||
SmoothedPathPointSource.Interpolated));
|
||||
}
|
||||
|
||||
result.Add(normalized[normalized.Count - 1]);
|
||||
samples = result;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidSegment(PreparedDirectionSegment segment, int expectedIndex, out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
if (segment == null || segment.SegmentIndex != expectedIndex ||
|
||||
(segment.Direction != TravelDirection.Forward && segment.Direction != TravelDirection.Reverse) ||
|
||||
segment.Points == null || segment.Points.Count == 0)
|
||||
{
|
||||
reason = "候选方向段索引、方向或点集无效。";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryValidateBoundary(
|
||||
IReadOnlyList<PreparedDirectionSegment> segments,
|
||||
int segmentIndex,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
PreparedDirectionSegment current = segments[segmentIndex];
|
||||
if (segmentIndex == segments.Count - 1 && current.EndsAtGearSwitch)
|
||||
{
|
||||
reason = "末个方向段不得声明不存在的后续换向点。";
|
||||
return false;
|
||||
}
|
||||
if (segmentIndex == 0)
|
||||
{
|
||||
if (current.StartsAtGearSwitch)
|
||||
{
|
||||
reason = "首个方向段不得从换向点开始。";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
PreparedDirectionSegment previous = segments[segmentIndex - 1];
|
||||
if (previous == null || !previous.EndsAtGearSwitch || !current.StartsAtGearSwitch ||
|
||||
previous.Direction == current.Direction)
|
||||
{
|
||||
reason = "方向段边界必须是前后成对且方向相反的换向点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
SmoothingPoint2D previousEnd = previous.Points[previous.Points.Count - 1];
|
||||
SmoothingPoint2D currentStart = current.Points[0];
|
||||
if (!currentStart.IsGearSwitchPoint ||
|
||||
Math.Abs(previousEnd.X - currentStart.X) > BoundaryToleranceMeters ||
|
||||
Math.Abs(previousEnd.Y - currentStart.Y) > BoundaryToleranceMeters ||
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(previousEnd.Heading, currentStart.Heading)) > BoundaryToleranceMeters)
|
||||
{
|
||||
reason = "换向点两侧必须保留同一位姿和航向。";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidPoint(SmoothingPoint2D point)
|
||||
{
|
||||
return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
|
||||
NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
|
||||
NumericGuard.IsFinite(point.ArcLength) && NumericGuard.IsFinite(point.BodyClearance) &&
|
||||
point.BodyClearance >= 0d;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,194 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>校验粗路径契约、保护换向拓扑并产生统一间距的平滑输入。</summary>
|
||||
public sealed class PathSmoothingPreprocessor
|
||||
{
|
||||
private const double Tolerance = 1e-8d;
|
||||
private readonly ArcLengthResampler _resampler;
|
||||
|
||||
/// <summary>创建使用默认确定性重采样器的预处理器。</summary>
|
||||
public PathSmoothingPreprocessor()
|
||||
: this(new ArcLengthResampler())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定重采样器的预处理器。</summary>
|
||||
public PathSmoothingPreprocessor(ArcLengthResampler resampler)
|
||||
{
|
||||
_resampler = resampler ?? throw new ArgumentNullException(nameof(resampler));
|
||||
}
|
||||
|
||||
/// <summary>将一条粗路径请求校验、按方向拆分并按配置间距重采样。</summary>
|
||||
public bool TryPrepare(PathSmoothingRequest request, out PreparedPath preparedPath, out string reason)
|
||||
{
|
||||
preparedPath = null;
|
||||
reason = string.Empty;
|
||||
if (request == null || request.Map == null || request.Vehicle == null || request.Configuration == null ||
|
||||
request.CoarsePath == null || request.Segments == null || request.CoarsePath.Count == 0 || request.Segments.Count == 0)
|
||||
{
|
||||
reason = "平滑请求缺少粗路径、方向分段、地图、车辆或配置。";
|
||||
return false;
|
||||
}
|
||||
|
||||
PathSmoothingConfiguration configuration = request.Configuration;
|
||||
if (!NumericGuard.IsPositiveFinite(configuration.OutputSpacingMeters))
|
||||
{
|
||||
reason = "平滑输出采样间距无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!ValidatePathPoints(request.CoarsePath, out reason) ||
|
||||
!ValidateSegments(request.CoarsePath, request.Segments, out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var preparedSegments = new List<PreparedDirectionSegment>(request.Segments.Count);
|
||||
for (int segmentIndex = 0; segmentIndex < request.Segments.Count; segmentIndex++)
|
||||
{
|
||||
PathSegment sourceSegment = request.Segments[segmentIndex];
|
||||
double segmentStartArcLength = request.CoarsePath[sourceSegment.StartIndex].ArcLength;
|
||||
var segmentPoints = new List<SmoothingPoint2D>(sourceSegment.EndIndex - sourceSegment.StartIndex + 1);
|
||||
for (int pointIndex = sourceSegment.StartIndex; pointIndex <= sourceSegment.EndIndex; pointIndex++)
|
||||
{
|
||||
CoarsePathPoint point = request.CoarsePath[pointIndex];
|
||||
bool isGearSwitch = point.IsGearSwitchPoint;
|
||||
segmentPoints.Add(new SmoothingPoint2D(
|
||||
point.X,
|
||||
point.Y,
|
||||
point.ArcLength - segmentStartArcLength,
|
||||
point.Heading,
|
||||
point.UnwrappedHeading,
|
||||
point.BodyClearance,
|
||||
isGearSwitch,
|
||||
isGearSwitch ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor));
|
||||
}
|
||||
|
||||
var unresampled = new PreparedDirectionSegment(
|
||||
sourceSegment.SegmentIndex,
|
||||
sourceSegment.Direction,
|
||||
segmentPoints,
|
||||
sourceSegment.StartsAtGearSwitch,
|
||||
sourceSegment.EndsAtGearSwitch);
|
||||
if (!_resampler.TryResample(unresampled, configuration.OutputSpacingMeters, out PreparedDirectionSegment resampled, out reason))
|
||||
return false;
|
||||
preparedSegments.Add(resampled);
|
||||
}
|
||||
|
||||
preparedPath = new PreparedPath(preparedSegments);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool ValidatePathPoints(IReadOnlyList<CoarsePathPoint> path, out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
CoarsePathPoint first = path[0];
|
||||
if (!IsValidPoint(first) || first.IsGearSwitchPoint || Math.Abs(first.ArcLength) > Tolerance)
|
||||
{
|
||||
reason = "粗路径首点无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int index = 1; index < path.Count; index++)
|
||||
{
|
||||
CoarsePathPoint previous = path[index - 1];
|
||||
CoarsePathPoint current = path[index];
|
||||
if (!IsValidPoint(current) || current.ArcLength + Tolerance < previous.ArcLength)
|
||||
{
|
||||
reason = "粗路径包含非法数值或非递增弧长。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double expectedHeadingDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
|
||||
if (!NumericGuard.IsFinite(expectedHeadingDelta) ||
|
||||
Math.Abs((current.UnwrappedHeading - previous.UnwrappedHeading) - expectedHeadingDelta) > Tolerance)
|
||||
{
|
||||
reason = "粗路径展开航向不连续。";
|
||||
return false;
|
||||
}
|
||||
|
||||
bool duplicatePoseAndArc = Math.Abs(current.X - previous.X) <= Tolerance &&
|
||||
Math.Abs(current.Y - previous.Y) <= Tolerance &&
|
||||
Math.Abs(current.ArcLength - previous.ArcLength) <= Tolerance &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(previous.Heading, current.Heading)) <= Tolerance;
|
||||
if (duplicatePoseAndArc)
|
||||
{
|
||||
if (previous.Direction == current.Direction || !current.IsGearSwitchPoint)
|
||||
{
|
||||
reason = "粗路径包含非法的重复点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if (current.IsGearSwitchPoint || current.ArcLength <= previous.ArcLength + Tolerance)
|
||||
{
|
||||
reason = "粗路径普通点必须有正弧长增量且不得标记为换向点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool ValidateSegments(
|
||||
IReadOnlyList<CoarsePathPoint> path,
|
||||
IReadOnlyList<PathSegment> segments,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
int expectedStartIndex = 0;
|
||||
for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
|
||||
{
|
||||
PathSegment segment = segments[segmentIndex];
|
||||
if (segment == null || segment.SegmentIndex != segmentIndex || segment.StartIndex != expectedStartIndex ||
|
||||
segment.StartIndex < 0 || segment.EndIndex < segment.StartIndex || segment.EndIndex >= path.Count ||
|
||||
segment.StartsAtGearSwitch != path[segment.StartIndex].IsGearSwitchPoint)
|
||||
{
|
||||
reason = "粗路径方向分段索引或起始换向标记无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
|
||||
{
|
||||
if (path[pointIndex].Direction != segment.Direction)
|
||||
{
|
||||
reason = "粗路径方向分段包含不同方向的点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool hasNextSegment = segmentIndex + 1 < segments.Count;
|
||||
bool expectedEndsAtGearSwitch = hasNextSegment && segment.EndIndex + 1 < path.Count &&
|
||||
path[segment.EndIndex + 1].IsGearSwitchPoint;
|
||||
if (segment.EndsAtGearSwitch != expectedEndsAtGearSwitch)
|
||||
{
|
||||
reason = "粗路径方向分段末尾换向标记无效。";
|
||||
return false;
|
||||
}
|
||||
expectedStartIndex = segment.EndIndex + 1;
|
||||
}
|
||||
|
||||
if (expectedStartIndex != path.Count)
|
||||
{
|
||||
reason = "粗路径方向分段未完整覆盖全部点。";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidPoint(CoarsePathPoint point)
|
||||
{
|
||||
return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
|
||||
NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
|
||||
NumericGuard.IsFinite(point.ArcLength) && point.ArcLength >= 0d &&
|
||||
NumericGuard.IsFinite(point.VehicleCurvature) && NumericGuard.IsFinite(point.BodyClearance) &&
|
||||
point.BodyClearance >= 0d &&
|
||||
(point.Direction == TravelDirection.Forward || point.Direction == TravelDirection.Reverse) &&
|
||||
Enum.IsDefined(typeof(CoarsePathPointSource), point.Source) &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= Tolerance;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>已校验、已按单一行驶方向分割并重采样的路径段。</summary>
|
||||
public sealed class PreparedDirectionSegment
|
||||
{
|
||||
/// <summary>创建不可变方向段。</summary>
|
||||
public PreparedDirectionSegment(
|
||||
int segmentIndex,
|
||||
TravelDirection direction,
|
||||
IReadOnlyList<SmoothingPoint2D> points,
|
||||
bool startsAtGearSwitch,
|
||||
bool endsAtGearSwitch)
|
||||
{
|
||||
if (segmentIndex < 0) throw new ArgumentOutOfRangeException(nameof(segmentIndex));
|
||||
if (points == null || points.Count == 0) throw new ArgumentException("A prepared segment requires points.", nameof(points));
|
||||
|
||||
SegmentIndex = segmentIndex;
|
||||
Direction = direction;
|
||||
Points = CopyReadOnly(points);
|
||||
StartsAtGearSwitch = startsAtGearSwitch;
|
||||
EndsAtGearSwitch = endsAtGearSwitch;
|
||||
}
|
||||
|
||||
/// <summary>从零开始的分段序号；在 <see cref="PreparedPath.Segments"/> 中必须与其位置一致。</summary>
|
||||
public int SegmentIndex { get; }
|
||||
|
||||
/// <summary>该段的唯一行驶方向。</summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>不包含相邻段点的本段不可变采样点。</summary>
|
||||
public IReadOnlyList<SmoothingPoint2D> Points { get; }
|
||||
|
||||
/// <summary>本段首点是否为换向后保留的新方向点。</summary>
|
||||
public bool StartsAtGearSwitch { get; }
|
||||
|
||||
/// <summary>本段末点之后是否紧邻换向点。</summary>
|
||||
public bool EndsAtGearSwitch { get; }
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,49 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>已校验并按方向拆分的粗路径输入快照。</summary>
|
||||
public sealed class PreparedPath
|
||||
{
|
||||
/// <summary>创建不可变预处理路径。</summary>
|
||||
public PreparedPath(IReadOnlyList<PreparedDirectionSegment> segments)
|
||||
{
|
||||
if (segments == null || segments.Count == 0)
|
||||
throw new ArgumentException("A prepared path requires direction segments.", nameof(segments));
|
||||
for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
|
||||
{
|
||||
if (segments[segmentIndex] == null)
|
||||
throw new ArgumentException("A prepared path cannot contain null direction segments.", nameof(segments));
|
||||
}
|
||||
|
||||
Segments = CopyReadOnly(segments);
|
||||
Points = Flatten(Segments);
|
||||
}
|
||||
|
||||
/// <summary>按原始前进/倒车拓扑排列的方向段。</summary>
|
||||
public IReadOnlyList<PreparedDirectionSegment> Segments { get; }
|
||||
|
||||
/// <summary>将所有方向段顺序拼接后的点；换向重复点保留两次。</summary>
|
||||
public IReadOnlyList<SmoothingPoint2D> Points { get; }
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingPoint2D> Flatten(IReadOnlyList<PreparedDirectionSegment> segments)
|
||||
{
|
||||
var points = new List<SmoothingPoint2D>();
|
||||
for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
|
||||
{
|
||||
PreparedDirectionSegment segment = segments[segmentIndex];
|
||||
for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
|
||||
points.Add(segment.Points[pointIndex]);
|
||||
}
|
||||
return new ReadOnlyCollection<SmoothingPoint2D>(points);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,53 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>供平滑算法处理的二维路径采样点；所有长度单位均为 m，航向单位为 rad。</summary>
|
||||
public sealed class SmoothingPoint2D
|
||||
{
|
||||
/// <summary>创建不可变二维路径点。</summary>
|
||||
public SmoothingPoint2D(
|
||||
double xMeters,
|
||||
double yMeters,
|
||||
double arcLengthMeters,
|
||||
double headingRadians,
|
||||
double unwrappedHeadingRadians,
|
||||
double bodyClearanceMeters,
|
||||
bool isGearSwitchPoint,
|
||||
SmoothedPathPointSource source)
|
||||
{
|
||||
X = xMeters;
|
||||
Y = yMeters;
|
||||
ArcLength = arcLengthMeters;
|
||||
Heading = headingRadians;
|
||||
UnwrappedHeading = unwrappedHeadingRadians;
|
||||
BodyClearance = bodyClearanceMeters;
|
||||
IsGearSwitchPoint = isGearSwitchPoint;
|
||||
Source = source;
|
||||
}
|
||||
|
||||
/// <summary>世界 X 坐标，单位 m。</summary>
|
||||
public double X { get; }
|
||||
|
||||
/// <summary>世界 Y 坐标，单位 m。</summary>
|
||||
public double Y { get; }
|
||||
|
||||
/// <summary>本方向段中的累计弧长，单位 m。</summary>
|
||||
public double ArcLength { get; }
|
||||
|
||||
/// <summary>归一化的车辆航向，单位 rad。</summary>
|
||||
public double Heading { get; }
|
||||
|
||||
/// <summary>连续展开的车辆航向，单位 rad。</summary>
|
||||
public double UnwrappedHeading { get; }
|
||||
|
||||
/// <summary>输入路径携带的保守净空，单位 m。</summary>
|
||||
public double BodyClearance { get; }
|
||||
|
||||
/// <summary>该点是否为新方向段开始处的换向点。</summary>
|
||||
public bool IsGearSwitchPoint { get; }
|
||||
|
||||
/// <summary>该点在平滑流程中的来源。</summary>
|
||||
public SmoothedPathPointSource Source { get; }
|
||||
}
|
||||
Reference in New Issue
Block a user