402 lines
16 KiB
C#
402 lines
16 KiB
C#
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);
|
|
}
|
|
}
|