chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -0,0 +1,145 @@
|
||||
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,
|
||||
segment.StartVehicleCurvaturePerMeter);
|
||||
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,72 @@
|
||||
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 maximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
double rootMeanSquareVehicleCurvaturePerMeter,
|
||||
double totalAbsoluteCurvatureVariationPerMeter,
|
||||
double curvatureVariationEnergy,
|
||||
double minimumBodyClearanceMeters)
|
||||
{
|
||||
Path = CopyReadOnly(path);
|
||||
Segments = CopyReadOnly(segments);
|
||||
PathLengthMeters = pathLengthMeters;
|
||||
MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
|
||||
MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter = maximumAbsoluteVehicleCurvatureDerivativePerSquareMeter;
|
||||
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 MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter { get; }
|
||||
|
||||
/// <summary>车辆曲率均方根，单位 1/m。</summary>
|
||||
public double RootMeanSquareVehicleCurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>不跨换向点累计的绝对曲率变化，单位 1/m。</summary>
|
||||
public double TotalAbsoluteCurvatureVariationPerMeter { get; }
|
||||
|
||||
/// <summary>不跨换向点累计的曲率变化代价。</summary>
|
||||
public double CurvatureVariationEnergy { get; }
|
||||
|
||||
/// <summary>曲率变化代价的面向用户名称；保留 <see cref="CurvatureVariationEnergy"/> 以兼容既有调用方。</summary>
|
||||
public double CurvatureVariationCost => CurvatureVariationEnergy;
|
||||
|
||||
/// <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,490 @@
|
||||
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 maximumAbsoluteVehicleCurvatureDerivative = 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 segmentMaximumCurvatureDerivative,
|
||||
out double segmentCurvatureSquareSum,
|
||||
out int segmentCurvatureSampleCount,
|
||||
out double segmentVariation,
|
||||
out double segmentVariationEnergy,
|
||||
out double segmentMinimumClearance,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, segmentMaximumCurvature);
|
||||
maximumAbsoluteVehicleCurvatureDerivative = Math.Max(
|
||||
maximumAbsoluteVehicleCurvatureDerivative,
|
||||
segmentMaximumCurvatureDerivative);
|
||||
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,
|
||||
maximumAbsoluteVehicleCurvatureDerivative,
|
||||
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 maximumAbsoluteVehicleCurvatureDerivative,
|
||||
out double curvatureSquareSum,
|
||||
out int curvatureSampleCount,
|
||||
out double totalCurvatureVariation,
|
||||
out double curvatureVariationEnergy,
|
||||
out double minimumClearance,
|
||||
out string reason)
|
||||
{
|
||||
maximumAbsoluteVehicleCurvature = 0d;
|
||||
maximumAbsoluteVehicleCurvatureDerivative = 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];
|
||||
var vehicleCurvatures = new double[count];
|
||||
var curvatureDerivatives = 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 (index == 0 || index == count - 1)
|
||||
{
|
||||
// Coarse-path endpoints encode the vehicle pose at the exact integration anchor.
|
||||
// A chord across a finite integration step is not that pose's heading.
|
||||
travelHeading = segment.Direction == TravelDirection.Forward
|
||||
? samples[index].Heading
|
||||
: samples[index].Heading - Math.PI;
|
||||
}
|
||||
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
|
||||
{
|
||||
int leftIndex = index == 0 ? 0 : index - 1;
|
||||
int rightIndex = index == count - 1 ? count - 1 : index + 1;
|
||||
if (!TryEstimateGeometricCurvature(
|
||||
samples,
|
||||
unwrappedHeadings,
|
||||
leftIndex,
|
||||
rightIndex,
|
||||
out geometricCurvatures[index],
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
if (!NumericGuard.IsFinite(geometricCurvatures[index]))
|
||||
{
|
||||
reason = "候选路径曲率计算产生了非法数值。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
|
||||
vehicleCurvatures[index] = directionSign * geometricCurvatures[index];
|
||||
}
|
||||
|
||||
if (segment.StartVehicleCurvaturePerMeter.HasValue)
|
||||
{
|
||||
double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
|
||||
vehicleCurvatures[0] = segment.StartVehicleCurvaturePerMeter.Value;
|
||||
geometricCurvatures[0] = directionSign * vehicleCurvatures[0];
|
||||
}
|
||||
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
if (count == 1)
|
||||
{
|
||||
curvatureDerivatives[index] = 0d;
|
||||
}
|
||||
else if (index == 0)
|
||||
{
|
||||
curvatureDerivatives[index] =
|
||||
(vehicleCurvatures[1] - vehicleCurvatures[0]) /
|
||||
(localArcLengths[1] - localArcLengths[0]);
|
||||
}
|
||||
else if (index == count - 1)
|
||||
{
|
||||
curvatureDerivatives[index] =
|
||||
(vehicleCurvatures[index] - vehicleCurvatures[index - 1]) /
|
||||
(localArcLengths[index] - localArcLengths[index - 1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
curvatureDerivatives[index] =
|
||||
(vehicleCurvatures[index + 1] - vehicleCurvatures[index - 1]) /
|
||||
(localArcLengths[index + 1] - localArcLengths[index - 1]);
|
||||
}
|
||||
|
||||
if (!NumericGuard.IsFinite(curvatureDerivatives[index]))
|
||||
{
|
||||
reason = "候选路径曲率导数计算产生了非法数值。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
SmoothingPoint2D sample = samples[index];
|
||||
double vehicleCurvature = vehicleCurvatures[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,
|
||||
curvatureDerivatives[index],
|
||||
sample.BodyClearance,
|
||||
isGearSwitch,
|
||||
source));
|
||||
|
||||
maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(vehicleCurvature));
|
||||
maximumAbsoluteVehicleCurvatureDerivative = Math.Max(
|
||||
maximumAbsoluteVehicleCurvatureDerivative,
|
||||
Math.Abs(curvatureDerivatives[index]));
|
||||
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 TryEstimateGeometricCurvature(
|
||||
IReadOnlyList<SmoothingPoint2D> samples,
|
||||
IReadOnlyList<double> unwrappedHeadings,
|
||||
int leftIndex,
|
||||
int rightIndex,
|
||||
out double curvature,
|
||||
out string reason)
|
||||
{
|
||||
curvature = 0d;
|
||||
reason = string.Empty;
|
||||
double chordLength = Distance(samples[leftIndex], samples[rightIndex]);
|
||||
if (!NumericGuard.IsFinite(chordLength) || chordLength <= 0d)
|
||||
{
|
||||
reason = "候选路径曲率估计弦长无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double deltaHeading = unwrappedHeadings[rightIndex] - unwrappedHeadings[leftIndex];
|
||||
if (!NumericGuard.IsFinite(deltaHeading))
|
||||
{
|
||||
reason = "候选路径曲率估计航向差无效。";
|
||||
return false;
|
||||
}
|
||||
if (Math.Abs(deltaHeading) >= Math.PI)
|
||||
{
|
||||
reason = "候选路径曲率估计航向差存在π歧义。";
|
||||
return false;
|
||||
}
|
||||
|
||||
curvature = 2d * Math.Sin(deltaHeading / 2d) / chordLength;
|
||||
if (!NumericGuard.IsFinite(curvature))
|
||||
{
|
||||
reason = "候选路径曲率计算产生了非法数值。";
|
||||
return false;
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
+102
@@ -0,0 +1,102 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>按方向段局部弧长插值平滑算法的原始路径参考。</summary>
|
||||
internal static class PathReferenceInterpolator
|
||||
{
|
||||
internal static bool TryInterpolateByArcLength(
|
||||
IReadOnlyList<SmoothingPoint2D> points,
|
||||
double targetArcLength,
|
||||
out SmoothingPoint2D reference,
|
||||
out string reason)
|
||||
{
|
||||
reference = null;
|
||||
reason = string.Empty;
|
||||
if (points == null || points.Count == 0 || !NumericGuard.IsFinite(targetArcLength))
|
||||
{
|
||||
reason = "原始路径参考点或目标弧长无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int index = 0; index < points.Count; index++)
|
||||
{
|
||||
SmoothingPoint2D point = points[index];
|
||||
if (!IsValid(point) || (index > 0 && point.ArcLength < points[index - 1].ArcLength))
|
||||
{
|
||||
reason = "原始路径参考点包含非法数值或非递增弧长。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
SmoothingPoint2D first = points[0];
|
||||
SmoothingPoint2D last = points[points.Count - 1];
|
||||
if (targetArcLength < first.ArcLength || targetArcLength > last.ArcLength)
|
||||
{
|
||||
reason = "目标弧长不在原始方向段范围内。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (targetArcLength == first.ArcLength)
|
||||
{
|
||||
reference = first;
|
||||
return true;
|
||||
}
|
||||
if (targetArcLength == last.ArcLength)
|
||||
{
|
||||
reference = last;
|
||||
return true;
|
||||
}
|
||||
|
||||
for (int rightIndex = 1; rightIndex < points.Count; rightIndex++)
|
||||
{
|
||||
SmoothingPoint2D left = points[rightIndex - 1];
|
||||
SmoothingPoint2D right = points[rightIndex];
|
||||
if (targetArcLength > right.ArcLength) continue;
|
||||
if (targetArcLength == right.ArcLength)
|
||||
{
|
||||
reference = right;
|
||||
return true;
|
||||
}
|
||||
|
||||
double interval = right.ArcLength - left.ArcLength;
|
||||
if (!NumericGuard.IsPositiveFinite(interval))
|
||||
{
|
||||
reason = "原始路径参考点包含无法插值的重复弧长。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double ratio = (targetArcLength - left.ArcLength) / interval;
|
||||
reference = new SmoothingPoint2D(
|
||||
left.X + ratio * (right.X - left.X),
|
||||
left.Y + ratio * (right.Y - left.Y),
|
||||
targetArcLength,
|
||||
left.Heading + ratio * (right.Heading - left.Heading),
|
||||
left.UnwrappedHeading + ratio * (right.UnwrappedHeading - left.UnwrappedHeading),
|
||||
left.BodyClearance + ratio * (right.BodyClearance - left.BodyClearance),
|
||||
false,
|
||||
SmoothedPathPointSource.Interpolated);
|
||||
if (!IsValid(reference))
|
||||
{
|
||||
reference = null;
|
||||
reason = "原始路径参考插值产生非法数值。";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
reason = "原始路径参考无法定位目标弧长。";
|
||||
return false;
|
||||
}
|
||||
|
||||
private static bool IsValid(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.ArcLength >= 0d && point.BodyClearance >= 0d;
|
||||
}
|
||||
}
|
||||
+201
@@ -0,0 +1,201 @@
|
||||
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,
|
||||
request.CoarsePath[sourceSegment.StartIndex].VehicleCurvature);
|
||||
if (!_resampler.TryResample(unresampled, configuration.OutputSpacingMeters, out PreparedDirectionSegment resampled, out reason))
|
||||
return false;
|
||||
preparedSegments.Add(new PreparedDirectionSegment(
|
||||
resampled.SegmentIndex,
|
||||
resampled.Direction,
|
||||
resampled.Points,
|
||||
resampled.StartsAtGearSwitch,
|
||||
resampled.EndsAtGearSwitch,
|
||||
unresampled.StartVehicleCurvaturePerMeter));
|
||||
}
|
||||
|
||||
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,70 @@
|
||||
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)
|
||||
: this(segmentIndex, direction, points, startsAtGearSwitch, endsAtGearSwitch, null)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建带有真实起始车辆曲率边界状态的不可变方向段。</summary>
|
||||
public PreparedDirectionSegment(
|
||||
int segmentIndex,
|
||||
TravelDirection direction,
|
||||
IReadOnlyList<SmoothingPoint2D> points,
|
||||
bool startsAtGearSwitch,
|
||||
bool endsAtGearSwitch,
|
||||
double? startVehicleCurvaturePerMeter)
|
||||
{
|
||||
if (segmentIndex < 0) throw new ArgumentOutOfRangeException(nameof(segmentIndex));
|
||||
if (points == null || points.Count == 0) throw new ArgumentException("A prepared segment requires points.", nameof(points));
|
||||
if (startVehicleCurvaturePerMeter.HasValue && !IsFinite(startVehicleCurvaturePerMeter.Value))
|
||||
throw new ArgumentOutOfRangeException(nameof(startVehicleCurvaturePerMeter));
|
||||
|
||||
SegmentIndex = segmentIndex;
|
||||
Direction = direction;
|
||||
Points = CopyReadOnly(points);
|
||||
StartsAtGearSwitch = startsAtGearSwitch;
|
||||
EndsAtGearSwitch = endsAtGearSwitch;
|
||||
StartVehicleCurvaturePerMeter = startVehicleCurvaturePerMeter;
|
||||
}
|
||||
|
||||
/// <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; }
|
||||
|
||||
/// <summary>原始车辆在本段物理起点的曲率边界状态，单位 1/m。</summary>
|
||||
public double? StartVehicleCurvaturePerMeter { get; }
|
||||
|
||||
private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
|
||||
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,264 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>通过候选路径同一几何分析器和验证器构建安全、可公平比较的原始基线。</summary>
|
||||
internal static class RawPathBaselineBuilder
|
||||
{
|
||||
internal static bool TryCreate(
|
||||
PathSmoothingRequest request,
|
||||
PreparedPath preparedPath,
|
||||
PathGeometryAnalyzer analyzer,
|
||||
double outputSpacingMeters,
|
||||
SmoothedPathValidator validator,
|
||||
double maximumCollisionCheckStepMeters,
|
||||
out RawPathBaseline baseline,
|
||||
out string reason)
|
||||
{
|
||||
baseline = null;
|
||||
reason = string.Empty;
|
||||
if (request == null || preparedPath == null || analyzer == null || validator == null)
|
||||
{
|
||||
reason = "原始粗路径基线缺少请求、预处理路径、几何分析器或安全验证器。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!analyzer.TryAnalyze(preparedPath.Segments, outputSpacingMeters, out PathGeometryAnalysis analysis, out reason))
|
||||
return false;
|
||||
|
||||
if (validator.TryValidate(
|
||||
analysis.Path,
|
||||
analysis.Segments,
|
||||
preparedPath,
|
||||
request.Map,
|
||||
request.Vehicle,
|
||||
maximumCollisionCheckStepMeters,
|
||||
out IReadOnlyList<SmoothedPathPoint> safePath,
|
||||
out double minimumClearanceMeters,
|
||||
out reason))
|
||||
{
|
||||
baseline = new RawPathBaseline(safePath, analysis.Segments, CreateMetrics(analysis, minimumClearanceMeters));
|
||||
return true;
|
||||
}
|
||||
|
||||
if (reason != "平滑路径包含非法数值或超限车辆曲率。" ||
|
||||
!HasTrustedVehicleCurvaturesWithinLimit(request) ||
|
||||
!TryCreateTrustedRawAnalysis(request, out analysis, out reason) ||
|
||||
!validator.TryValidate(
|
||||
analysis.Path,
|
||||
analysis.Segments,
|
||||
preparedPath,
|
||||
request.Map,
|
||||
request.Vehicle,
|
||||
maximumCollisionCheckStepMeters,
|
||||
out safePath,
|
||||
out minimumClearanceMeters,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
baseline = new RawPathBaseline(safePath, analysis.Segments, CreateMetrics(analysis, minimumClearanceMeters));
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool HasTrustedVehicleCurvaturesWithinLimit(PathSmoothingRequest request)
|
||||
{
|
||||
if (request?.CoarsePath == null || request.CoarsePath.Count == 0 ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(
|
||||
request.Vehicle,
|
||||
out double maximumCurvaturePerMeter))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int index = 0; index < request.CoarsePath.Count; index++)
|
||||
{
|
||||
CoarsePathPoint point = request.CoarsePath[index];
|
||||
if (point == null || !IsFinite(point.VehicleCurvature) ||
|
||||
Math.Abs(point.VehicleCurvature) > maximumCurvaturePerMeter)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryCreateTrustedRawAnalysis(
|
||||
PathSmoothingRequest request,
|
||||
out PathGeometryAnalysis analysis,
|
||||
out string reason)
|
||||
{
|
||||
analysis = null;
|
||||
reason = string.Empty;
|
||||
if (request.CoarsePath == null || request.Segments == null ||
|
||||
request.CoarsePath.Count == 0 || request.Segments.Count == 0)
|
||||
{
|
||||
reason = "原始粗路径基线缺少可信路径或方向分段。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var path = new List<SmoothedPathPoint>(request.CoarsePath.Count);
|
||||
var segments = new List<SmoothedPathSegment>(request.Segments.Count);
|
||||
double maximumAbsoluteVehicleCurvature = 0d;
|
||||
double maximumAbsoluteVehicleCurvatureDerivative = 0d;
|
||||
double curvatureSquareSum = 0d;
|
||||
int curvatureSampleCount = 0;
|
||||
double totalCurvatureVariation = 0d;
|
||||
double curvatureVariationEnergy = 0d;
|
||||
double minimumClearance = double.PositiveInfinity;
|
||||
|
||||
for (int segmentIndex = 0; segmentIndex < request.Segments.Count; segmentIndex++)
|
||||
{
|
||||
PathSegment segment = request.Segments[segmentIndex];
|
||||
if (segment == null || segment.SegmentIndex != segmentIndex ||
|
||||
segment.StartIndex != path.Count || segment.EndIndex < segment.StartIndex ||
|
||||
segment.EndIndex >= request.CoarsePath.Count)
|
||||
{
|
||||
reason = "原始粗路径基线方向分段无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
|
||||
{
|
||||
CoarsePathPoint point = request.CoarsePath[pointIndex];
|
||||
if (point == null || point.Direction != segment.Direction ||
|
||||
!IsFinite(point.X) || !IsFinite(point.Y) || !IsFinite(point.Heading) ||
|
||||
!IsFinite(point.UnwrappedHeading) || !IsFinite(point.ArcLength) || point.ArcLength < 0d ||
|
||||
!IsFinite(point.VehicleCurvature) || !IsFinite(point.BodyClearance) || point.BodyClearance < 0d)
|
||||
{
|
||||
reason = "原始粗路径基线包含非法可信点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double derivative = EstimateVehicleCurvatureDerivative(request.CoarsePath, segment, pointIndex, out bool validDerivative);
|
||||
if (!validDerivative)
|
||||
{
|
||||
reason = "原始粗路径基线曲率导数弧长无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
|
||||
double geometricCurvature = directionSign * point.VehicleCurvature;
|
||||
SmoothedPathPointSource source = point.IsGearSwitchPoint
|
||||
? SmoothedPathPointSource.GearSwitch
|
||||
: SmoothedPathPointSource.CoarsePathFallback;
|
||||
path.Add(new SmoothedPathPoint(
|
||||
point.X,
|
||||
point.Y,
|
||||
point.Heading,
|
||||
point.UnwrappedHeading,
|
||||
point.ArcLength,
|
||||
point.Direction,
|
||||
geometricCurvature,
|
||||
point.VehicleCurvature,
|
||||
derivative,
|
||||
point.BodyClearance,
|
||||
point.IsGearSwitchPoint,
|
||||
source));
|
||||
|
||||
maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(point.VehicleCurvature));
|
||||
maximumAbsoluteVehicleCurvatureDerivative = Math.Max(
|
||||
maximumAbsoluteVehicleCurvatureDerivative,
|
||||
Math.Abs(derivative));
|
||||
curvatureSquareSum += point.VehicleCurvature * point.VehicleCurvature;
|
||||
curvatureSampleCount++;
|
||||
minimumClearance = Math.Min(minimumClearance, point.BodyClearance);
|
||||
if (pointIndex > segment.StartIndex)
|
||||
{
|
||||
CoarsePathPoint previous = request.CoarsePath[pointIndex - 1];
|
||||
double deltaArc = point.ArcLength - previous.ArcLength;
|
||||
double deltaCurvature = geometricCurvature -
|
||||
(directionSign * previous.VehicleCurvature);
|
||||
totalCurvatureVariation += Math.Abs(deltaCurvature);
|
||||
curvatureVariationEnergy +=
|
||||
(deltaCurvature / deltaArc) * (deltaCurvature / deltaArc) * deltaArc;
|
||||
}
|
||||
}
|
||||
|
||||
segments.Add(new SmoothedPathSegment(
|
||||
segment.SegmentIndex,
|
||||
segment.Direction,
|
||||
segment.StartIndex,
|
||||
segment.EndIndex,
|
||||
segment.StartsAtGearSwitch,
|
||||
segment.EndsAtGearSwitch));
|
||||
}
|
||||
|
||||
double rmsCurvature = curvatureSampleCount == 0 ? 0d : Math.Sqrt(curvatureSquareSum / curvatureSampleCount);
|
||||
analysis = new PathGeometryAnalysis(
|
||||
path,
|
||||
segments,
|
||||
request.CoarsePath[request.CoarsePath.Count - 1].ArcLength,
|
||||
maximumAbsoluteVehicleCurvature,
|
||||
maximumAbsoluteVehicleCurvatureDerivative,
|
||||
rmsCurvature,
|
||||
totalCurvatureVariation,
|
||||
curvatureVariationEnergy,
|
||||
minimumClearance);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static double EstimateVehicleCurvatureDerivative(
|
||||
IReadOnlyList<CoarsePathPoint> path,
|
||||
PathSegment segment,
|
||||
int pointIndex,
|
||||
out bool valid)
|
||||
{
|
||||
valid = true;
|
||||
if (segment.StartIndex == segment.EndIndex) return 0d;
|
||||
|
||||
int leftIndex = pointIndex == segment.StartIndex ? pointIndex : pointIndex - 1;
|
||||
int rightIndex = pointIndex == segment.EndIndex ? pointIndex : pointIndex + 1;
|
||||
double deltaArc = path[rightIndex].ArcLength - path[leftIndex].ArcLength;
|
||||
if (!IsFinite(deltaArc) || deltaArc <= 0d)
|
||||
{
|
||||
valid = false;
|
||||
return 0d;
|
||||
}
|
||||
return (path[rightIndex].VehicleCurvature - path[leftIndex].VehicleCurvature) / deltaArc;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>已通过完整车体复核的原始粗路径及其比较指标。</summary>
|
||||
internal sealed class RawPathBaseline
|
||||
{
|
||||
internal RawPathBaseline(
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathQualityMetrics metrics)
|
||||
{
|
||||
Path = path;
|
||||
Segments = segments;
|
||||
Metrics = metrics;
|
||||
}
|
||||
|
||||
internal IReadOnlyList<SmoothedPathPoint> Path { get; }
|
||||
internal IReadOnlyList<SmoothedPathSegment> Segments { get; }
|
||||
internal PathQualityMetrics Metrics { get; }
|
||||
}
|
||||
@@ -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