chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -0,0 +1,8 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>三次 B 样条平滑参数。</summary>
|
||||
public sealed class CubicBSplineOptions
|
||||
{
|
||||
/// <summary>端点切向控制柄相对于相邻弦长的比例。</summary>
|
||||
public double EndpointTangentScale { get; set; } = 1d / 3d;
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>局部三次 Bézier 平滑参数。</summary>
|
||||
public sealed class LocalCubicBezierOptions
|
||||
{
|
||||
/// <summary>判定为明显转角的最小航向变化，单位 rad。</summary>
|
||||
public double CornerHeadingThresholdRadians { get; set; } = Math.PI / 18d;
|
||||
|
||||
/// <summary>单个局部平滑窗口的最大弧长，单位 m。</summary>
|
||||
public double MaximumWindowLengthMeters { get; set; } = 0.60d;
|
||||
|
||||
/// <summary>控制柄相对于窗口局部弦长的比例。</summary>
|
||||
public double HandleLengthRatio { get; set; } = 1d / 3d;
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>局部 G2 五次过渡的可配置阈值。</summary>
|
||||
public sealed class LocalG2QuinticOptions
|
||||
{
|
||||
public double MinimumWindowLengthMeters { get; set; } = 0.20d;
|
||||
public double PreferredWindowLengthMeters { get; set; } = 0.50d;
|
||||
public double MaximumWindowLengthMeters { get; set; } = 0.80d;
|
||||
public double MaximumDeviationMeters { get; set; } = 0.10d;
|
||||
public double AbsoluteCurvatureJumpFloorPerMeter { get; set; } = 0.001d;
|
||||
public double CurvatureJumpRatioOfMaximum { get; set; } = 0.05d;
|
||||
public double MinimumPeakGradientImprovementRatio { get; set; } = 0.20d;
|
||||
public double MaximumVariationCostRegressionRatio { get; set; } = 0.02d;
|
||||
public int MaximumCandidatesPerRegion { get; set; } = 12;
|
||||
}
|
||||
@@ -0,0 +1,108 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>一条原始或平滑路径的不可变质量指标。</summary>
|
||||
public sealed class PathQualityMetrics
|
||||
{
|
||||
/// <summary>创建全零、不可行的质量指标。</summary>
|
||||
public PathQualityMetrics()
|
||||
: this(false, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建完整的质量指标快照。</summary>
|
||||
public PathQualityMetrics(
|
||||
bool isFeasible,
|
||||
double pathLengthMeters,
|
||||
double maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
double rootMeanSquareVehicleCurvaturePerMeter,
|
||||
double totalAbsoluteCurvatureVariationPerMeter,
|
||||
double curvatureVariationEnergy,
|
||||
double minimumBodyClearanceMeters,
|
||||
double lengthChangePercent,
|
||||
double peakCurvatureChangePercent,
|
||||
double curvatureVariationChangePercent,
|
||||
double minimumClearanceChangeMeters)
|
||||
: this(
|
||||
isFeasible,
|
||||
pathLengthMeters,
|
||||
maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
0d,
|
||||
rootMeanSquareVehicleCurvaturePerMeter,
|
||||
totalAbsoluteCurvatureVariationPerMeter,
|
||||
curvatureVariationEnergy,
|
||||
minimumBodyClearanceMeters,
|
||||
lengthChangePercent,
|
||||
peakCurvatureChangePercent,
|
||||
curvatureVariationChangePercent,
|
||||
minimumClearanceChangeMeters)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建带有曲率导数峰值的完整质量指标快照。</summary>
|
||||
public PathQualityMetrics(
|
||||
bool isFeasible,
|
||||
double pathLengthMeters,
|
||||
double maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
double maximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
double rootMeanSquareVehicleCurvaturePerMeter,
|
||||
double totalAbsoluteCurvatureVariationPerMeter,
|
||||
double curvatureVariationEnergy,
|
||||
double minimumBodyClearanceMeters,
|
||||
double lengthChangePercent,
|
||||
double peakCurvatureChangePercent,
|
||||
double curvatureVariationChangePercent,
|
||||
double minimumClearanceChangeMeters)
|
||||
{
|
||||
IsFeasible = isFeasible;
|
||||
PathLengthMeters = pathLengthMeters;
|
||||
MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
|
||||
MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter = maximumAbsoluteVehicleCurvatureDerivativePerSquareMeter;
|
||||
RootMeanSquareVehicleCurvaturePerMeter = rootMeanSquareVehicleCurvaturePerMeter;
|
||||
TotalAbsoluteCurvatureVariationPerMeter = totalAbsoluteCurvatureVariationPerMeter;
|
||||
CurvatureVariationEnergy = curvatureVariationEnergy;
|
||||
MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
|
||||
LengthChangePercent = lengthChangePercent;
|
||||
PeakCurvatureChangePercent = peakCurvatureChangePercent;
|
||||
CurvatureVariationChangePercent = curvatureVariationChangePercent;
|
||||
MinimumClearanceChangeMeters = minimumClearanceChangeMeters;
|
||||
}
|
||||
|
||||
/// <summary>该路径是否通过完整安全和运动学复核。</summary>
|
||||
public bool IsFeasible { 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>曲率变化代价的兼容名称。</summary>
|
||||
public double CurvatureVariationCost => CurvatureVariationEnergy;
|
||||
|
||||
/// <summary>完整扩大车体的最小保守净空，单位 m。</summary>
|
||||
public double MinimumBodyClearanceMeters { get; }
|
||||
|
||||
/// <summary>相对原始粗路径的长度变化百分比。</summary>
|
||||
public double LengthChangePercent { get; }
|
||||
|
||||
/// <summary>相对原始粗路径的峰值曲率变化百分比。</summary>
|
||||
public double PeakCurvatureChangePercent { get; }
|
||||
|
||||
/// <summary>相对原始粗路径的曲率变化百分比。</summary>
|
||||
public double CurvatureVariationChangePercent { get; }
|
||||
|
||||
/// <summary>相对原始粗路径的最小净空变化，单位 m。</summary>
|
||||
public double MinimumClearanceChangeMeters { get; }
|
||||
}
|
||||
@@ -0,0 +1,53 @@
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>路径平滑的公共配置；所有距离使用 m。</summary>
|
||||
public sealed class PathSmoothingConfiguration
|
||||
{
|
||||
/// <summary>创建带有安全默认值的平滑配置。</summary>
|
||||
public PathSmoothingConfiguration()
|
||||
{
|
||||
OutputSpacingMeters = 0.025d;
|
||||
MaximumCollisionCheckStepMeters = 0.025d;
|
||||
MinimumClearanceReserveMeters = 0.02d;
|
||||
SmoothingStrength = 1d;
|
||||
AllowFallbackToCoarsePath = true;
|
||||
RetryStrengthScales = new ReadOnlyCollection<double>(
|
||||
new List<double> { 1d, 0.75d, 0.50d, 0.25d });
|
||||
}
|
||||
|
||||
/// <summary>正式单算法入口使用的方法。</summary>
|
||||
public SmoothingMethod Method { get; set; }
|
||||
|
||||
/// <summary>输出路径的目标弧长采样间距，单位 m。</summary>
|
||||
public double OutputSpacingMeters { get; set; }
|
||||
|
||||
/// <summary>扫掠碰撞检查的最大步长，单位 m。</summary>
|
||||
public double MaximumCollisionCheckStepMeters { get; set; }
|
||||
|
||||
/// <summary>平滑候选必须在最小净空之外保留的额外余量，单位 m。</summary>
|
||||
public double MinimumClearanceReserveMeters { get; set; }
|
||||
|
||||
/// <summary>算法初始平滑强度。</summary>
|
||||
public double SmoothingStrength { get; set; }
|
||||
|
||||
/// <summary>所有平滑尝试失败时是否允许发布经过复核的原粗路径。</summary>
|
||||
public bool AllowFallbackToCoarsePath { get; set; }
|
||||
|
||||
/// <summary>有限且严格递减的平滑强度重试比例。</summary>
|
||||
public IReadOnlyList<double> RetryStrengthScales { get; }
|
||||
|
||||
/// <summary>三次 B 样条专用参数。</summary>
|
||||
public CubicBSplineOptions CubicBSpline { get; } = new CubicBSplineOptions();
|
||||
|
||||
/// <summary>局部三次 Bézier 专用参数。</summary>
|
||||
public LocalCubicBezierOptions LocalCubicBezier { get; } = new LocalCubicBezierOptions();
|
||||
|
||||
/// <summary>分段五次多项式专用参数。</summary>
|
||||
public PiecewiseQuinticOptions PiecewiseQuintic { get; } = new PiecewiseQuinticOptions();
|
||||
|
||||
/// <summary>局部 G2 五次过渡专用参数。</summary>
|
||||
public LocalG2QuinticOptions LocalG2Quintic { get; } = new LocalG2QuinticOptions();
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>一次平滑尝试的不可变诊断信息。</summary>
|
||||
public sealed class PathSmoothingDiagnostics
|
||||
{
|
||||
/// <summary>创建不含路径指标的默认诊断信息。</summary>
|
||||
public PathSmoothingDiagnostics()
|
||||
: this(new PathQualityMetrics(), TimeSpan.Zero, 0, 0d, string.Empty)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建完整的平滑诊断快照。</summary>
|
||||
public PathSmoothingDiagnostics(
|
||||
PathQualityMetrics metrics,
|
||||
TimeSpan elapsed,
|
||||
int retryCount,
|
||||
double acceptedStrength,
|
||||
string terminationReason = null)
|
||||
{
|
||||
Metrics = metrics ?? new PathQualityMetrics();
|
||||
Elapsed = elapsed;
|
||||
RetryCount = retryCount;
|
||||
AcceptedStrength = acceptedStrength;
|
||||
TerminationReason = terminationReason ?? string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>使用所有测量量创建平滑诊断快照。</summary>
|
||||
public PathSmoothingDiagnostics(
|
||||
bool isFeasible,
|
||||
double pathLengthMeters,
|
||||
double maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
double rootMeanSquareVehicleCurvaturePerMeter,
|
||||
double totalAbsoluteCurvatureVariationPerMeter,
|
||||
double curvatureVariationEnergy,
|
||||
double minimumBodyClearanceMeters,
|
||||
double lengthChangePercent,
|
||||
double peakCurvatureChangePercent,
|
||||
double curvatureVariationChangePercent,
|
||||
double minimumClearanceChangeMeters,
|
||||
TimeSpan elapsed,
|
||||
int retryCount,
|
||||
double acceptedStrength,
|
||||
string terminationReason = null)
|
||||
: this(
|
||||
new PathQualityMetrics(
|
||||
isFeasible,
|
||||
pathLengthMeters,
|
||||
maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
rootMeanSquareVehicleCurvaturePerMeter,
|
||||
totalAbsoluteCurvatureVariationPerMeter,
|
||||
curvatureVariationEnergy,
|
||||
minimumBodyClearanceMeters,
|
||||
lengthChangePercent,
|
||||
peakCurvatureChangePercent,
|
||||
curvatureVariationChangePercent,
|
||||
minimumClearanceChangeMeters),
|
||||
elapsed,
|
||||
retryCount,
|
||||
acceptedStrength,
|
||||
terminationReason)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>路径质量指标；始终非空。</summary>
|
||||
public PathQualityMetrics Metrics { get; }
|
||||
|
||||
/// <summary>从算法入口到返回诊断的耗时。</summary>
|
||||
public TimeSpan Elapsed { get; }
|
||||
|
||||
/// <summary>已执行的安全强度重试次数。</summary>
|
||||
public int RetryCount { get; }
|
||||
|
||||
/// <summary>通过复核的平滑强度；未接受候选时为零。</summary>
|
||||
public double AcceptedStrength { get; }
|
||||
|
||||
/// <summary>面向调用方的稳定终止说明。</summary>
|
||||
public string TerminationReason { get; }
|
||||
}
|
||||
+17
@@ -0,0 +1,17 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>局部 G2 区域未替换原始路径的稳定原因。</summary>
|
||||
public enum PathSmoothingRegionFailureReason
|
||||
{
|
||||
None,
|
||||
WindowUnavailable,
|
||||
CandidateGenerationFailed,
|
||||
Collision,
|
||||
InsufficientClearance,
|
||||
CurvatureExceeded,
|
||||
CurvatureOvershoot,
|
||||
DeviationExceeded,
|
||||
InsufficientImprovement,
|
||||
VariationCostRegression,
|
||||
GlobalValidationRollback,
|
||||
}
|
||||
@@ -0,0 +1,83 @@
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>单个局部 G2 平滑区域的不可变发布报告。</summary>
|
||||
public sealed class PathSmoothingRegionReport
|
||||
{
|
||||
public PathSmoothingRegionReport(
|
||||
int segmentIndex,
|
||||
double startArcLengthMeters,
|
||||
double endArcLengthMeters,
|
||||
IReadOnlyList<double> curvatureJumpsPerMeter,
|
||||
double plannedWindowLengthMeters,
|
||||
double actualWindowLengthMeters,
|
||||
double leftWindowLengthMeters,
|
||||
double rightWindowLengthMeters,
|
||||
int candidateCount,
|
||||
int selectedCandidateIndex,
|
||||
PathSmoothingRegionStatus status,
|
||||
PathSmoothingRegionFailureReason failureReason,
|
||||
double rawPeakCurvatureDerivativePerSquareMeter,
|
||||
double resultPeakCurvatureDerivativePerSquareMeter,
|
||||
double rawCurvatureVariationCost,
|
||||
double resultCurvatureVariationCost,
|
||||
double maximumDeviationMeters,
|
||||
double minimumBodyClearanceMeters,
|
||||
double maximumAbsoluteVehicleCurvaturePerMeter)
|
||||
{
|
||||
SegmentIndex = segmentIndex;
|
||||
StartArcLengthMeters = startArcLengthMeters;
|
||||
EndArcLengthMeters = endArcLengthMeters;
|
||||
CurvatureJumpsPerMeter = CopyReadOnly(curvatureJumpsPerMeter);
|
||||
PlannedWindowLengthMeters = plannedWindowLengthMeters;
|
||||
ActualWindowLengthMeters = actualWindowLengthMeters;
|
||||
LeftWindowLengthMeters = leftWindowLengthMeters;
|
||||
RightWindowLengthMeters = rightWindowLengthMeters;
|
||||
CandidateCount = candidateCount;
|
||||
SelectedCandidateIndex = status == PathSmoothingRegionStatus.Improved
|
||||
? selectedCandidateIndex
|
||||
: -1;
|
||||
Status = status;
|
||||
FailureReason = failureReason;
|
||||
RawPeakCurvatureDerivativePerSquareMeter = rawPeakCurvatureDerivativePerSquareMeter;
|
||||
ResultPeakCurvatureDerivativePerSquareMeter = resultPeakCurvatureDerivativePerSquareMeter;
|
||||
RawCurvatureVariationCost = rawCurvatureVariationCost;
|
||||
ResultCurvatureVariationCost = resultCurvatureVariationCost;
|
||||
MaximumDeviationMeters = maximumDeviationMeters;
|
||||
MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
|
||||
MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
|
||||
}
|
||||
|
||||
public int SegmentIndex { get; }
|
||||
public double StartArcLengthMeters { get; }
|
||||
public double EndArcLengthMeters { get; }
|
||||
public IReadOnlyList<double> CurvatureJumpsPerMeter { get; }
|
||||
public double PlannedWindowLengthMeters { get; }
|
||||
public double ActualWindowLengthMeters { get; }
|
||||
public double LeftWindowLengthMeters { get; }
|
||||
public double RightWindowLengthMeters { get; }
|
||||
public int CandidateCount { get; }
|
||||
public int SelectedCandidateIndex { get; }
|
||||
public PathSmoothingRegionStatus Status { get; }
|
||||
public PathSmoothingRegionFailureReason FailureReason { get; }
|
||||
public double RawPeakCurvatureDerivativePerSquareMeter { get; }
|
||||
public double ResultPeakCurvatureDerivativePerSquareMeter { get; }
|
||||
public double RawCurvatureVariationCost { get; }
|
||||
public double ResultCurvatureVariationCost { get; }
|
||||
public double MaximumDeviationMeters { get; }
|
||||
public double MinimumBodyClearanceMeters { get; }
|
||||
public double MaximumAbsoluteVehicleCurvaturePerMeter { get; }
|
||||
|
||||
private static IReadOnlyList<double> CopyReadOnly(IReadOnlyList<double> source)
|
||||
{
|
||||
var copy = new List<double>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<double>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>单个局部 G2 平滑区域的处理结果。</summary>
|
||||
public enum PathSmoothingRegionStatus
|
||||
{
|
||||
Improved,
|
||||
RetainedOriginal,
|
||||
}
|
||||
@@ -0,0 +1,97 @@
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>路径平滑所需的原始粗路径、复核上下文和配置。</summary>
|
||||
public sealed class PathSmoothingRequest
|
||||
{
|
||||
private readonly VehicleParameters _vehicle;
|
||||
private readonly PathSmoothingConfiguration _configuration;
|
||||
|
||||
/// <summary>创建路径平滑请求，并复制粗路径和方向分段集合。</summary>
|
||||
public PathSmoothingRequest(
|
||||
IReadOnlyList<CoarsePathPoint> coarsePath,
|
||||
IReadOnlyList<PathSegment> segments,
|
||||
PlanningGridMap map,
|
||||
VehicleParameters vehicle,
|
||||
PathSmoothingConfiguration configuration)
|
||||
{
|
||||
CoarsePath = CopyReadOnly(coarsePath);
|
||||
Segments = CopyReadOnly(segments);
|
||||
Map = map;
|
||||
_vehicle = CopyVehicle(vehicle);
|
||||
_configuration = CopyConfiguration(configuration);
|
||||
}
|
||||
|
||||
/// <summary>原始粗路径的不可变快照。</summary>
|
||||
public IReadOnlyList<CoarsePathPoint> CoarsePath { get; }
|
||||
|
||||
/// <summary>原始粗路径方向分段的不可变快照。</summary>
|
||||
public IReadOnlyList<PathSegment> Segments { get; }
|
||||
|
||||
/// <summary>用于平滑后完整车体复核的规划栅格地图。</summary>
|
||||
public PlanningGridMap Map { get; }
|
||||
|
||||
/// <summary>车辆几何与最大曲率约束的不可变快照副本。</summary>
|
||||
public VehicleParameters Vehicle => CopyVehicle(_vehicle);
|
||||
|
||||
/// <summary>本次平滑配置的不可变快照副本。</summary>
|
||||
public PathSmoothingConfiguration Configuration => CopyConfiguration(_configuration);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
private static VehicleParameters CopyVehicle(VehicleParameters source)
|
||||
{
|
||||
if (source == null) return null;
|
||||
return new VehicleParameters
|
||||
{
|
||||
LengthMeters = source.LengthMeters,
|
||||
WidthMeters = source.WidthMeters,
|
||||
SafetyMarginMeters = source.SafetyMarginMeters,
|
||||
MaximumCurvaturePerMeter = source.MaximumCurvaturePerMeter,
|
||||
MinimumTurningRadiusMeters = source.MinimumTurningRadiusMeters,
|
||||
};
|
||||
}
|
||||
|
||||
private static PathSmoothingConfiguration CopyConfiguration(PathSmoothingConfiguration source)
|
||||
{
|
||||
if (source == null) return null;
|
||||
var copy = new PathSmoothingConfiguration
|
||||
{
|
||||
Method = source.Method,
|
||||
OutputSpacingMeters = source.OutputSpacingMeters,
|
||||
MaximumCollisionCheckStepMeters = source.MaximumCollisionCheckStepMeters,
|
||||
MinimumClearanceReserveMeters = source.MinimumClearanceReserveMeters,
|
||||
SmoothingStrength = source.SmoothingStrength,
|
||||
AllowFallbackToCoarsePath = source.AllowFallbackToCoarsePath,
|
||||
};
|
||||
copy.CubicBSpline.EndpointTangentScale = source.CubicBSpline.EndpointTangentScale;
|
||||
copy.LocalCubicBezier.CornerHeadingThresholdRadians = source.LocalCubicBezier.CornerHeadingThresholdRadians;
|
||||
copy.LocalCubicBezier.MaximumWindowLengthMeters = source.LocalCubicBezier.MaximumWindowLengthMeters;
|
||||
copy.LocalCubicBezier.HandleLengthRatio = source.LocalCubicBezier.HandleLengthRatio;
|
||||
copy.PiecewiseQuintic.KnotSpacingMeters = source.PiecewiseQuintic.KnotSpacingMeters;
|
||||
copy.PiecewiseQuintic.MinimumKnotSpacingMeters = source.PiecewiseQuintic.MinimumKnotSpacingMeters;
|
||||
copy.LocalG2Quintic.MinimumWindowLengthMeters = source.LocalG2Quintic.MinimumWindowLengthMeters;
|
||||
copy.LocalG2Quintic.PreferredWindowLengthMeters = source.LocalG2Quintic.PreferredWindowLengthMeters;
|
||||
copy.LocalG2Quintic.MaximumWindowLengthMeters = source.LocalG2Quintic.MaximumWindowLengthMeters;
|
||||
copy.LocalG2Quintic.MaximumDeviationMeters = source.LocalG2Quintic.MaximumDeviationMeters;
|
||||
copy.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter = source.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter;
|
||||
copy.LocalG2Quintic.CurvatureJumpRatioOfMaximum = source.LocalG2Quintic.CurvatureJumpRatioOfMaximum;
|
||||
copy.LocalG2Quintic.MinimumPeakGradientImprovementRatio = source.LocalG2Quintic.MinimumPeakGradientImprovementRatio;
|
||||
copy.LocalG2Quintic.MaximumVariationCostRegressionRatio = source.LocalG2Quintic.MaximumVariationCostRegressionRatio;
|
||||
copy.LocalG2Quintic.MaximumCandidatesPerRegion = source.LocalG2Quintic.MaximumCandidatesPerRegion;
|
||||
return copy;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,154 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>路径平滑的最终不可变结果。</summary>
|
||||
public sealed class PathSmoothingResult
|
||||
{
|
||||
private static readonly IReadOnlyList<SmoothedPathPoint> EmptyPath =
|
||||
new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());
|
||||
private static readonly IReadOnlyList<SmoothedPathSegment> EmptySegments =
|
||||
new ReadOnlyCollection<SmoothedPathSegment>(new List<SmoothedPathSegment>());
|
||||
private static readonly IReadOnlyList<PathSmoothingRegionReport> EmptyRegionReports =
|
||||
new ReadOnlyCollection<PathSmoothingRegionReport>(new List<PathSmoothingRegionReport>());
|
||||
|
||||
private PathSmoothingResult(
|
||||
PathSmoothingStatus status,
|
||||
SmoothingMethod? method,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathSmoothingDiagnostics diagnostics,
|
||||
IReadOnlyList<PathSmoothingRegionReport> regionReports)
|
||||
{
|
||||
Status = status;
|
||||
Method = method;
|
||||
Path = path;
|
||||
Segments = segments;
|
||||
RegionReports = regionReports;
|
||||
Diagnostics = diagnostics ?? new PathSmoothingDiagnostics(
|
||||
new PathQualityMetrics(),
|
||||
TimeSpan.Zero,
|
||||
0,
|
||||
0d,
|
||||
"No smoothing diagnostics were supplied.");
|
||||
}
|
||||
|
||||
/// <summary>最终发布状态。</summary>
|
||||
public PathSmoothingStatus Status { get; }
|
||||
|
||||
/// <summary>成功或回退时的实际（或尝试）平滑方法；失败时为空。</summary>
|
||||
public SmoothingMethod? Method { get; }
|
||||
|
||||
/// <summary>成功或经过复核的回退路径；其他状态始终为空且不可变。</summary>
|
||||
public IReadOnlyList<SmoothedPathPoint> Path { get; }
|
||||
|
||||
/// <summary>覆盖 <see cref="Path"/> 的方向分段；其他状态始终为空且不可变。</summary>
|
||||
public IReadOnlyList<SmoothedPathSegment> Segments { get; }
|
||||
|
||||
/// <summary>局部 G2 各检测区域的不可变报告；传统算法结果为空。</summary>
|
||||
public IReadOnlyList<PathSmoothingRegionReport> RegionReports { get; }
|
||||
|
||||
/// <summary>本次平滑的质量和终止诊断；始终非空。</summary>
|
||||
public PathSmoothingDiagnostics Diagnostics { get; }
|
||||
|
||||
/// <summary>创建已通过所有复核的平滑结果。</summary>
|
||||
public static PathSmoothingResult Success(
|
||||
SmoothingMethod method,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathSmoothingDiagnostics diagnostics)
|
||||
{
|
||||
ValidatePublishedResult(method, path, segments, diagnostics);
|
||||
return new PathSmoothingResult(
|
||||
PathSmoothingStatus.Success,
|
||||
method,
|
||||
CopyReadOnly(path),
|
||||
CopyReadOnly(segments),
|
||||
diagnostics,
|
||||
EmptyRegionReports);
|
||||
}
|
||||
|
||||
/// <summary>创建经过完整复核的原始粗路径回退结果。</summary>
|
||||
public static PathSmoothingResult Fallback(
|
||||
SmoothingMethod attemptedMethod,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathSmoothingDiagnostics diagnostics)
|
||||
{
|
||||
ValidatePublishedResult(attemptedMethod, path, segments, diagnostics);
|
||||
return new PathSmoothingResult(
|
||||
PathSmoothingStatus.FallbackToCoarsePath,
|
||||
attemptedMethod,
|
||||
CopyReadOnly(path),
|
||||
CopyReadOnly(segments),
|
||||
diagnostics,
|
||||
EmptyRegionReports);
|
||||
}
|
||||
|
||||
/// <summary>发布经过完整复核的局部 G2 预平滑结果。</summary>
|
||||
public static PathSmoothingResult PublishLocalG2(
|
||||
PathSmoothingStatus status,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathSmoothingDiagnostics diagnostics,
|
||||
IReadOnlyList<PathSmoothingRegionReport> regionReports)
|
||||
{
|
||||
if (status != PathSmoothingStatus.Complete &&
|
||||
status != PathSmoothingStatus.PartialImprovement &&
|
||||
status != PathSmoothingStatus.NotNeeded &&
|
||||
status != PathSmoothingStatus.Unchanged)
|
||||
throw new ArgumentException("Use a Local G2 publication status.", nameof(status));
|
||||
if (regionReports == null)
|
||||
throw new ArgumentNullException(nameof(regionReports));
|
||||
|
||||
ValidatePublishedResult(SmoothingMethod.LocalG2Quintic, path, segments, diagnostics);
|
||||
return new PathSmoothingResult(
|
||||
status,
|
||||
SmoothingMethod.LocalG2Quintic,
|
||||
CopyReadOnly(path),
|
||||
CopyReadOnly(segments),
|
||||
diagnostics,
|
||||
CopyReadOnly(regionReports));
|
||||
}
|
||||
|
||||
/// <summary>创建不发布路径的失败、不可行、取消或输入无效结果。</summary>
|
||||
public static PathSmoothingResult Failure(PathSmoothingStatus status, PathSmoothingDiagnostics diagnostics)
|
||||
{
|
||||
if (status == PathSmoothingStatus.Success ||
|
||||
status == PathSmoothingStatus.FallbackToCoarsePath ||
|
||||
status == PathSmoothingStatus.Complete ||
|
||||
status == PathSmoothingStatus.PartialImprovement ||
|
||||
status == PathSmoothingStatus.NotNeeded ||
|
||||
status == PathSmoothingStatus.Unchanged)
|
||||
throw new ArgumentException("Use Success or Fallback to publish a path.", nameof(status));
|
||||
if (!Enum.IsDefined(typeof(PathSmoothingStatus), status))
|
||||
throw new ArgumentOutOfRangeException(nameof(status));
|
||||
return new PathSmoothingResult(status, null, EmptyPath, EmptySegments, diagnostics, EmptyRegionReports);
|
||||
}
|
||||
|
||||
private static void ValidatePublishedResult(
|
||||
SmoothingMethod method,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathSmoothingDiagnostics diagnostics)
|
||||
{
|
||||
if (!Enum.IsDefined(typeof(SmoothingMethod), method))
|
||||
throw new ArgumentOutOfRangeException(nameof(method));
|
||||
if (path == null || path.Count == 0)
|
||||
throw new ArgumentException("Published smoothing results require a non-empty path.", nameof(path));
|
||||
if (segments == null || segments.Count == 0)
|
||||
throw new ArgumentException("Published smoothing results require non-empty segments.", nameof(segments));
|
||||
if (diagnostics == null || diagnostics.Metrics == null || !diagnostics.Metrics.IsFeasible)
|
||||
throw new ArgumentException("Published smoothing results require feasible diagnostics.", nameof(diagnostics));
|
||||
}
|
||||
|
||||
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,16 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>路径平滑的最终发布状态。</summary>
|
||||
public enum PathSmoothingStatus
|
||||
{
|
||||
Success,
|
||||
FallbackToCoarsePath,
|
||||
InvalidInput,
|
||||
Infeasible,
|
||||
Failed,
|
||||
Cancelled,
|
||||
Complete,
|
||||
PartialImprovement,
|
||||
NotNeeded,
|
||||
Unchanged,
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>分段五次 Hermite 平滑参数。</summary>
|
||||
public sealed class PiecewiseQuinticOptions
|
||||
{
|
||||
/// <summary>相邻内部结点的目标距离，单位 m。</summary>
|
||||
public double KnotSpacingMeters { get; set; } = 0.50d;
|
||||
|
||||
/// <summary>允许创建内部结点的最小间距，单位 m。</summary>
|
||||
public double MinimumKnotSpacingMeters { get; set; } = 0.10d;
|
||||
}
|
||||
@@ -0,0 +1,100 @@
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>平滑空间路径上的不可变采样点；位置和长度单位为 m，航向为 rad，曲率为 1/m。</summary>
|
||||
public sealed class SmoothedPathPoint
|
||||
{
|
||||
public SmoothedPathPoint(
|
||||
double xMeters,
|
||||
double yMeters,
|
||||
double headingRadians,
|
||||
double unwrappedHeadingRadians,
|
||||
double arcLengthMeters,
|
||||
TravelDirection direction,
|
||||
double geometricCurvaturePerMeter,
|
||||
double vehicleCurvaturePerMeter,
|
||||
double bodyClearanceMeters,
|
||||
bool isGearSwitchPoint,
|
||||
SmoothedPathPointSource source)
|
||||
: this(
|
||||
xMeters,
|
||||
yMeters,
|
||||
headingRadians,
|
||||
unwrappedHeadingRadians,
|
||||
arcLengthMeters,
|
||||
direction,
|
||||
geometricCurvaturePerMeter,
|
||||
vehicleCurvaturePerMeter,
|
||||
0d,
|
||||
bodyClearanceMeters,
|
||||
isGearSwitchPoint,
|
||||
source)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建带有车辆曲率对弧长导数的不可变采样点。</summary>
|
||||
public SmoothedPathPoint(
|
||||
double xMeters,
|
||||
double yMeters,
|
||||
double headingRadians,
|
||||
double unwrappedHeadingRadians,
|
||||
double arcLengthMeters,
|
||||
TravelDirection direction,
|
||||
double geometricCurvaturePerMeter,
|
||||
double vehicleCurvaturePerMeter,
|
||||
double vehicleCurvatureDerivativePerSquareMeter,
|
||||
double bodyClearanceMeters,
|
||||
bool isGearSwitchPoint,
|
||||
SmoothedPathPointSource source)
|
||||
{
|
||||
X = xMeters;
|
||||
Y = yMeters;
|
||||
Heading = headingRadians;
|
||||
UnwrappedHeading = unwrappedHeadingRadians;
|
||||
ArcLength = arcLengthMeters;
|
||||
Direction = direction;
|
||||
GeometricCurvature = geometricCurvaturePerMeter;
|
||||
VehicleCurvature = vehicleCurvaturePerMeter;
|
||||
VehicleCurvatureDerivative = vehicleCurvatureDerivativePerSquareMeter;
|
||||
BodyClearance = bodyClearanceMeters;
|
||||
IsGearSwitchPoint = isGearSwitchPoint;
|
||||
Source = source;
|
||||
}
|
||||
|
||||
/// <summary>世界 X 坐标，单位 m。</summary>
|
||||
public double X { get; }
|
||||
|
||||
/// <summary>世界 Y 坐标，单位 m。</summary>
|
||||
public double Y { get; }
|
||||
|
||||
/// <summary>归一化的车辆航向，单位 rad。</summary>
|
||||
public double Heading { get; }
|
||||
|
||||
/// <summary>连续展开的车辆航向，单位 rad。</summary>
|
||||
public double UnwrappedHeading { get; }
|
||||
|
||||
/// <summary>从完整路径起点累计的弧长，单位 m。</summary>
|
||||
public double ArcLength { get; }
|
||||
|
||||
/// <summary>该点所在连续方向段的行驶方向。</summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>按几何弧长计算的有符号路径曲率，单位 1/m。</summary>
|
||||
public double GeometricCurvature { get; }
|
||||
|
||||
/// <summary>车辆模型使用的有符号曲率，单位 1/m。</summary>
|
||||
public double VehicleCurvature { get; }
|
||||
|
||||
/// <summary>车辆曲率对弧长的导数 dκ/ds，单位 1/m²。</summary>
|
||||
public double VehicleCurvatureDerivative { get; }
|
||||
|
||||
/// <summary>扩大车体后的保守净空下界，单位 m。</summary>
|
||||
public double BodyClearance { get; }
|
||||
|
||||
/// <summary>该点是否为新方向段开始的换向点。</summary>
|
||||
public bool IsGearSwitchPoint { get; }
|
||||
|
||||
/// <summary>该点在平滑流程中的来源。</summary>
|
||||
public SmoothedPathPointSource Source { get; }
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>平滑路径点的来源。</summary>
|
||||
public enum SmoothedPathPointSource
|
||||
{
|
||||
Anchor,
|
||||
Interpolated,
|
||||
GearSwitch,
|
||||
CoarsePathFallback,
|
||||
LocalG2Transition,
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>平滑路径中方向一致的连续点范围；起止索引均包含在内。</summary>
|
||||
public sealed class SmoothedPathSegment
|
||||
{
|
||||
public SmoothedPathSegment(
|
||||
int segmentIndex,
|
||||
TravelDirection direction,
|
||||
int startIndex,
|
||||
int endIndex,
|
||||
bool startsAtGearSwitch,
|
||||
bool endsAtGearSwitch)
|
||||
{
|
||||
SegmentIndex = segmentIndex;
|
||||
Direction = direction;
|
||||
StartIndex = startIndex;
|
||||
EndIndex = endIndex;
|
||||
StartsAtGearSwitch = startsAtGearSwitch;
|
||||
EndsAtGearSwitch = endsAtGearSwitch;
|
||||
}
|
||||
|
||||
/// <summary>从零开始的分段序号。</summary>
|
||||
public int SegmentIndex { get; }
|
||||
|
||||
/// <summary>本段行驶方向。</summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>本段在平滑路径中的起始包含式索引。</summary>
|
||||
public int StartIndex { get; }
|
||||
|
||||
/// <summary>本段在平滑路径中的结束包含式索引。</summary>
|
||||
public int EndIndex { get; }
|
||||
|
||||
/// <summary>本段首点是否为换向后保留的新方向点。</summary>
|
||||
public bool StartsAtGearSwitch { get; }
|
||||
|
||||
/// <summary>本段末点是否紧邻下一方向段的换向对。</summary>
|
||||
public bool EndsAtGearSwitch { get; }
|
||||
}
|
||||
@@ -0,0 +1,10 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>支持的粗路径平滑方法。</summary>
|
||||
public enum SmoothingMethod
|
||||
{
|
||||
CubicBSpline,
|
||||
LocalCubicBezier,
|
||||
PiecewiseQuintic,
|
||||
LocalG2Quintic,
|
||||
}
|
||||
Reference in New Issue
Block a user