265 lines
11 KiB
C#
265 lines
11 KiB
C#
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; }
|
|
}
|