First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix: unify path smoothing geometry metrics

Browse Source
This commit is contained in:
梁薄云
2026-07-30 16:39:08 +08:00
parent d7ceb761b7
commit d6b34f88ce
16 changed files with 259 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.superpowers/sdd/task-3-report.md
+70
View File
@@ -0,0 +1,70 @@
# Task 3：统一几何导数与公平原始基线
## 完成内容
- `PathGeometryAnalyzer` 在每个独立方向段内一次性计算车辆曲率数组、`dκ/ds` 数组和导数峰值；换挡重复点绝不参与同一次差分。
- `PathGeometryAnalysis` 提供导数峰值和 `CurvatureVariationCost` 兼容别名；每个分析输出点携带有限导数。
- `PreparedDirectionSegment` 可携带经验证的物理起始车辆曲率；预处理、重采样和现有平滑候选均保持该边界状态。
- 原始路径基线改由与候选相同的几何分析器、输出间距和安全验证器构建，并从分析结果生成全部指标（含峰值导数）。
- 净空复核要求导数有限，且重建输出时原样保留导数；粗路径回退也保留导数。
## TDD 记录
### RED
```powershell
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_geometry.ps1
```
构建成功（2 个既有 obsolete 警告）；geometry 脚本按预期失败：
```text
PathGeometryAnalysis must expose peak d-kappa/d-s.
```
```powershell
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_validation.ps1
```
按预期失败：
```text
Clearance recomputation must preserve the first point curvature derivative. Expected=0.25 Actual=0
```
### GREEN / 回归
```powershell
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_geometry.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_validation.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_service.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_comparison.ps1
```
关键输出：
```text
已成功生成。（2 个既有 obsolete 警告，0 个错误）
Path smoothing geometry checks passed.
Path smoothing validation checks passed.
Path smoothing service checks passed.
Path smoothing comparison checks passed.
```
## 文件
- 计划列出的 geometry、preprocessor、raw-baseline、validator、facade 和两份验证脚本。
- 为保证物理起点状态可从预处理一路传至候选分析，额外更新 `ArcLengthResampler`、三种现有平滑器和 `SmoothingAlgorithmRunner`；这些改动仅传递新增边界状态或导数指标，不改变算法几何。
## 自审
- 单点段导数固定为 `0`；重复/零距离段继续沿既有拒绝路径失败；非有限起始边界、导数和计算结果均被拒绝。
- 导数只以本方向段局部弧长差分；换挡的相同弧长重复点位于不同段，未用于分母。
- 原始基线和候选均通过 `PathGeometryAnalyzer` 再通过 `SmoothedPathValidator`。
- `CurvatureVariationEnergy` 未删除；新增的 `CurvatureVariationCost` 是只读兼容别名。
## 风险
- 公平基线会改变原始路径的重采样密度及因此产生的比较百分比；当前 comparison 回归脚本已通过。构建仍报告项目既有的两个 obsolete API 警告，未由本任务引入。
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/CubicBSplineSmoother.cs
+2 -1
View File
@@ -59,7 +59,8 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
sourceSegment.Direction,
points,
sourceSegment.StartsAtGearSwitch,
sourceSegment.EndsAtGearSwitch));
sourceSegment.EndsAtGearSwitch,
sourceSegment.StartVehicleCurvaturePerMeter));
}
return SmoothingCandidate.Success(candidateSegments);
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/LocalCubicBezierSmoother.cs
+2 -1
View File
@@ -55,7 +55,8 @@ internal sealed class LocalCubicBezierSmoother : IPathSmoother
sourceSegment.Direction,
points,
sourceSegment.StartsAtGearSwitch,
sourceSegment.EndsAtGearSwitch));
sourceSegment.EndsAtGearSwitch,
sourceSegment.StartVehicleCurvaturePerMeter));
}
return SmoothingCandidate.Success(candidateSegments);
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/PiecewiseQuinticSmoother.cs
+2 -1
View File
@@ -60,7 +60,8 @@ internal sealed class PiecewiseQuinticSmoother : IPathSmoother
sourceSegment.Direction,
points,
sourceSegment.StartsAtGearSwitch,
sourceSegment.EndsAtGearSwitch));
sourceSegment.EndsAtGearSwitch,
sourceSegment.StartVehicleCurvaturePerMeter));
}
return SmoothingCandidate.Success(candidateSegments);
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingAlgorithmRunner.cs
+1
View File
@@ -99,6 +99,7 @@ internal sealed class SmoothingAlgorithmRunner
true,
analysis.PathLengthMeters,
analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
analysis.RootMeanSquareVehicleCurvaturePerMeter,
analysis.TotalAbsoluteCurvatureVariationPerMeter,
analysis.CurvatureVariationEnergy,
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs
+4
View File
@@ -13,6 +13,7 @@ public sealed class PathSmoothingComparisonService
{
private readonly PathSmoothingService _smoothingService = new PathSmoothingService();
private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
/// <summary>比较所有请求方法；一个方法的失败不会阻止其他方法，取消会停止后续启动。</summary>
@@ -139,6 +140,8 @@ public sealed class PathSmoothingComparisonService
if (!RawPathBaselineBuilder.TryCreate(
smoothingRequest,
preparedPath,
_analyzer,
configuration.OutputSpacingMeters,
_validator,
configuration.MaximumCollisionCheckStepMeters,
out RawPathBaseline rawPath,
@@ -201,6 +204,7 @@ public sealed class PathSmoothingComparisonService
true,
candidate.PathLengthMeters,
candidate.MaximumAbsoluteVehicleCurvaturePerMeter,
candidate.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
candidate.RootMeanSquareVehicleCurvaturePerMeter,
candidate.TotalAbsoluteCurvatureVariationPerMeter,
candidate.CurvatureVariationEnergy,
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs
+6
View File
@@ -15,6 +15,7 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
public sealed class PathSmoothingService
{
private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
private readonly SmoothingAlgorithmRunner _runner = new SmoothingAlgorithmRunner();
private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
private readonly IPathSmoother _bSpline = new CubicBSplineSmoother();
@@ -39,6 +40,8 @@ public sealed class PathSmoothingService
if (!RawPathBaselineBuilder.TryCreate(
request,
preparedPath,
_analyzer,
configuration.OutputSpacingMeters,
_validator,
configuration.MaximumCollisionCheckStepMeters,
out _,
@@ -134,6 +137,8 @@ public sealed class PathSmoothingService
if (!RawPathBaselineBuilder.TryCreate(
request,
revalidatedPath,
_analyzer,
configuration.OutputSpacingMeters,
_validator,
configuration.MaximumCollisionCheckStepMeters,
out RawPathBaseline rawPath,
@@ -234,6 +239,7 @@ public sealed class PathSmoothingService
point.Direction,
point.GeometricCurvature,
point.VehicleCurvature,
point.VehicleCurvatureDerivative,
point.BodyClearance,
point.IsGearSwitchPoint,
SmoothedPathPointSource.CoarsePathFallback));
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/ArcLengthResampler.cs
+2 -1
View File
@@ -111,7 +111,8 @@ public sealed class ArcLengthResampler
segment.Direction,
points,
segment.StartsAtGearSwitch,
segment.EndsAtGearSwitch);
segment.EndsAtGearSwitch,
segment.StartVehicleCurvaturePerMeter);
return true;
}
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalysis.cs
+9 -1
View File
@@ -13,6 +13,7 @@ public sealed class PathGeometryAnalysis
IReadOnlyList<SmoothedPathSegment> segments,
double pathLengthMeters,
double maximumAbsoluteVehicleCurvaturePerMeter,
double maximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
double rootMeanSquareVehicleCurvaturePerMeter,
double totalAbsoluteCurvatureVariationPerMeter,
double curvatureVariationEnergy,
@@ -22,6 +23,7 @@ public sealed class PathGeometryAnalysis
Segments = CopyReadOnly(segments);
PathLengthMeters = pathLengthMeters;
MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter = maximumAbsoluteVehicleCurvatureDerivativePerSquareMeter;
RootMeanSquareVehicleCurvaturePerMeter = rootMeanSquareVehicleCurvaturePerMeter;
TotalAbsoluteCurvatureVariationPerMeter = totalAbsoluteCurvatureVariationPerMeter;
CurvatureVariationEnergy = curvatureVariationEnergy;
@@ -40,15 +42,21 @@ public sealed class PathGeometryAnalysis
/// <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>
/// <summary>不跨换向点累计的曲率变化代价。</summary>
public double CurvatureVariationEnergy { get; }
/// <summary>曲率变化代价的面向用户名称；保留 <see cref="CurvatureVariationEnergy"/> 以兼容既有调用方。</summary>
public double CurvatureVariationCost => CurvatureVariationEnergy;
/// <summary>输入点携带的最小保守净空，单位 m。</summary>
public double MinimumBodyClearanceMeters { get; }
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs
+60 -2
View File
@@ -37,6 +37,7 @@ public sealed class PathGeometryAnalyzer
double previousOutputUnwrappedHeading = 0d;
bool hasPreviousOutputHeading = false;
double maximumAbsoluteVehicleCurvature = 0d;
double maximumAbsoluteVehicleCurvatureDerivative = 0d;
double curvatureSquareSum = 0d;
int curvatureSampleCount = 0;
double totalCurvatureVariation = 0d;
@@ -61,6 +62,7 @@ public sealed class PathGeometryAnalyzer
ref hasPreviousOutputHeading,
outputPath,
out double segmentMaximumCurvature,
out double segmentMaximumCurvatureDerivative,
out double segmentCurvatureSquareSum,
out int segmentCurvatureSampleCount,
out double segmentVariation,
@@ -72,6 +74,9 @@ public sealed class PathGeometryAnalyzer
}
maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, segmentMaximumCurvature);
maximumAbsoluteVehicleCurvatureDerivative = Math.Max(
maximumAbsoluteVehicleCurvatureDerivative,
segmentMaximumCurvatureDerivative);
curvatureSquareSum += segmentCurvatureSquareSum;
curvatureSampleCount += segmentCurvatureSampleCount;
totalCurvatureVariation += segmentVariation;
@@ -94,6 +99,7 @@ public sealed class PathGeometryAnalyzer
outputSegments,
cumulativeArcLength,
maximumAbsoluteVehicleCurvature,
maximumAbsoluteVehicleCurvatureDerivative,
rmsCurvature,
totalCurvatureVariation,
curvatureVariationEnergy,
@@ -110,6 +116,7 @@ public sealed class PathGeometryAnalyzer
ref bool hasPreviousOutputHeading,
List<SmoothedPathPoint> output,
out double maximumAbsoluteVehicleCurvature,
out double maximumAbsoluteVehicleCurvatureDerivative,
out double curvatureSquareSum,
out int curvatureSampleCount,
out double totalCurvatureVariation,
@@ -118,6 +125,7 @@ public sealed class PathGeometryAnalyzer
out string reason)
{
maximumAbsoluteVehicleCurvature = 0d;
maximumAbsoluteVehicleCurvatureDerivative = 0d;
curvatureSquareSum = 0d;
curvatureSampleCount = 0;
totalCurvatureVariation = 0d;
@@ -129,6 +137,8 @@ public sealed class PathGeometryAnalyzer
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++)
{
@@ -214,9 +224,53 @@ public sealed class PathGeometryAnalyzer
for (int index = 0; index < count; index++)
{
SmoothingPoint2D sample = samples[index];
double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
double vehicleCurvature = directionSign * geometricCurvatures[index];
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;
@@ -229,11 +283,15 @@ public sealed class PathGeometryAnalyzer
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);
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathSmoothingPreprocessor.cs
+9 -2
View File
@@ -75,10 +75,17 @@ public sealed class PathSmoothingPreprocessor
sourceSegment.Direction,
segmentPoints,
sourceSegment.StartsAtGearSwitch,
sourceSegment.EndsAtGearSwitch);
sourceSegment.EndsAtGearSwitch,
request.CoarsePath[sourceSegment.StartIndex].VehicleCurvature);
if (!_resampler.TryResample(unresampled, configuration.OutputSpacingMeters, out PreparedDirectionSegment resampled, out reason))
return false;
preparedSegments.Add(resampled);
preparedSegments.Add(new PreparedDirectionSegment(
resampled.SegmentIndex,
resampled.Direction,
resampled.Points,
resampled.StartsAtGearSwitch,
resampled.EndsAtGearSwitch,
unresampled.StartVehicleCurvaturePerMeter));
}
preparedPath = new PreparedPath(preparedSegments);
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PreparedDirectionSegment.cs
+20
View File
@@ -15,15 +15,30 @@ public sealed class PreparedDirectionSegment
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>
@@ -41,6 +56,11 @@ public sealed class PreparedDirectionSegment
/// <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);
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs
+18 -100
View File
@@ -1,19 +1,16 @@
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
/// <summary>将已验证的粗路径转换为不改变任何锚点位姿或车辆曲率的安全比较基线。</summary>
/// <summary>通过候选路径同一几何分析器和验证器构建安全、可公平比较的原始基线。</summary>
internal static class RawPathBaselineBuilder
{
private const double MinimumArcDeltaMeters = 1e-12d;
internal static bool TryCreate(
PathSmoothingRequest request,
PreparedPath preparedPath,
PathGeometryAnalyzer analyzer,
double outputSpacingMeters,
SmoothedPathValidator validator,
double maximumCollisionCheckStepMeters,
out RawPathBaseline baseline,
@@ -21,17 +18,18 @@ internal static class RawPathBaselineBuilder
{
baseline = null;
reason = string.Empty;
if (request == null || preparedPath == null || validator == null)
if (request == null || preparedPath == null || analyzer == null || validator == null)
{
reason = "原始粗路径基线缺少请求、预处理路径或安全验证器。";
reason = "原始粗路径基线缺少请求、预处理路径、几何分析器或安全验证器。";
return false;
}
IReadOnlyList<SmoothedPathPoint> candidatePath = CreatePoints(request.CoarsePath);
IReadOnlyList<SmoothedPathSegment> candidateSegments = CreateSegments(request.Segments);
if (!analyzer.TryAnalyze(preparedPath.Segments, outputSpacingMeters, out PathGeometryAnalysis analysis, out reason))
return false;
if (!validator.TryValidate(
candidatePath,
candidateSegments,
analysis.Path,
analysis.Segments,
preparedPath,
request.Map,
request.Vehicle,
@@ -43,102 +41,22 @@ internal static class RawPathBaselineBuilder
return false;
}
baseline = new RawPathBaseline(safePath, candidateSegments, CreateMetrics(safePath, candidateSegments, minimumClearanceMeters));
baseline = new RawPathBaseline(safePath, analysis.Segments, CreateMetrics(analysis, minimumClearanceMeters));
return true;
}
private static IReadOnlyList<SmoothedPathPoint> CreatePoints(IReadOnlyList<CoarsePathPoint> coarsePath)
{
var points = new List<SmoothedPathPoint>(coarsePath == null ? 0 : coarsePath.Count);
if (coarsePath != null)
{
for (int index = 0; index < coarsePath.Count; index++)
{
CoarsePathPoint point = coarsePath[index];
points.Add(new SmoothedPathPoint(
point.X,
point.Y,
point.Heading,
point.UnwrappedHeading,
point.ArcLength,
point.Direction,
point.VehicleCurvature,
point.VehicleCurvature,
point.BodyClearance,
point.IsGearSwitchPoint,
point.IsGearSwitchPoint ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor));
}
}
return new ReadOnlyCollection<SmoothedPathPoint>(points);
}
private static IReadOnlyList<SmoothedPathSegment> CreateSegments(IReadOnlyList<PathSegment> coarseSegments)
{
var segments = new List<SmoothedPathSegment>(coarseSegments == null ? 0 : coarseSegments.Count);
if (coarseSegments != null)
{
for (int index = 0; index < coarseSegments.Count; index++)
{
PathSegment segment = coarseSegments[index];
segments.Add(new SmoothedPathSegment(
segment.SegmentIndex,
segment.Direction,
segment.StartIndex,
segment.EndIndex,
segment.StartsAtGearSwitch,
segment.EndsAtGearSwitch));
}
}
return new ReadOnlyCollection<SmoothedPathSegment>(segments);
}
private static PathQualityMetrics CreateMetrics(
IReadOnlyList<SmoothedPathPoint> path,
IReadOnlyList<SmoothedPathSegment> segments,
PathGeometryAnalysis analysis,
double minimumClearanceMeters)
{
double maximumAbsoluteVehicleCurvature = 0d;
double curvatureSquareSum = 0d;
int curvatureSampleCount = 0;
double totalAbsoluteCurvatureVariation = 0d;
double curvatureVariationEnergy = 0d;
for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
{
SmoothedPathSegment segment = segments[segmentIndex];
SmoothedPathPoint previous = null;
for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
{
SmoothedPathPoint current = path[pointIndex];
double curvature = current.VehicleCurvature;
maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(curvature));
curvatureSquareSum += curvature * curvature;
curvatureSampleCount++;
if (previous != null)
{
double arcDelta = current.ArcLength - previous.ArcLength;
if (arcDelta > MinimumArcDeltaMeters)
{
double curvatureDelta = curvature - previous.VehicleCurvature;
totalAbsoluteCurvatureVariation += Math.Abs(curvatureDelta);
curvatureVariationEnergy += curvatureDelta * curvatureDelta / arcDelta;
}
}
previous = current;
}
}
double rootMeanSquareVehicleCurvature = curvatureSampleCount == 0
? 0d
: Math.Sqrt(curvatureSquareSum / curvatureSampleCount);
double pathLengthMeters = path.Count == 0 ? 0d : path[path.Count - 1].ArcLength;
return new PathQualityMetrics(
true,
pathLengthMeters,
maximumAbsoluteVehicleCurvature,
rootMeanSquareVehicleCurvature,
totalAbsoluteCurvatureVariation,
curvatureVariationEnergy,
analysis.PathLengthMeters,
analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
analysis.RootMeanSquareVehicleCurvaturePerMeter,
analysis.TotalAbsoluteCurvatureVariationPerMeter,
analysis.CurvatureVariationEnergy,
minimumClearanceMeters,
0d,
0d,
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs
+3 -1
View File
@@ -129,7 +129,8 @@ public sealed class SmoothedPathValidator
SmoothedPathPoint point = candidatePath[index];
output.Add(new SmoothedPathPoint(
point.X, point.Y, point.Heading, point.UnwrappedHeading, point.ArcLength, point.Direction,
point.GeometricCurvature, point.VehicleCurvature, checkedClearances[index], point.IsGearSwitchPoint, point.Source));
point.GeometricCurvature, point.VehicleCurvature, point.VehicleCurvatureDerivative,
checkedClearances[index], point.IsGearSwitchPoint, point.Source));
}
pathWithClearance = new ReadOnlyCollection<SmoothedPathPoint>(output);
@@ -206,6 +207,7 @@ public sealed class SmoothedPathValidator
NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
NumericGuard.IsFinite(point.ArcLength) && point.ArcLength >= 0d &&
NumericGuard.IsFinite(point.GeometricCurvature) && NumericGuard.IsFinite(point.VehicleCurvature) &&
NumericGuard.IsFinite(point.VehicleCurvatureDerivative) &&
IsDirection(point.Direction) && Enum.IsDefined(typeof(SmoothedPathPointSource), point.Source) &&
Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= Tolerance;
}
ClumsyPilot/tests/verify_path_smoothing_geometry.ps1
+39
View File
@@ -62,6 +62,22 @@ function New-DirectionSegment(
$Index, $Direction, $typedPoints, $StartsAtGearSwitch, $EndsAtGearSwitch))
}
function New-DirectionSegmentWithStartCurvature(
[int]$Index,
$Direction,
[double]$StartVehicleCurvature,
[object[]]$Points,
[bool]$StartsAtGearSwitch = $false,
[bool]$EndsAtGearSwitch = $false) {
$typedPoints = [Array]::CreateInstance($pointType, $Points.Count)
for ($pointIndex = 0; $pointIndex -lt $Points.Count; $pointIndex++) {
$typedPoints.SetValue($Points[$pointIndex], $pointIndex)
}
return [Activator]::CreateInstance($segmentType, @(
$Index, $Direction, $typedPoints, $StartsAtGearSwitch, $EndsAtGearSwitch, $StartVehicleCurvature))
}
function New-CoarsePoint(
[double]$X,
[double]$Y,
@@ -167,6 +183,8 @@ $mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.Plann
Assert-True ($null -ne $preparedPathType) 'PreparedPath must be discoverable for smoothing algorithms.'
Assert-True ($null -ne $preprocessorType) 'PathSmoothingPreprocessor must be discoverable for request preparation.'
Assert-True ($null -ne $resamplerType) 'ArcLengthResampler must be discoverable for deterministic resampling.'
Assert-True ($null -ne $analysisType.GetProperty('MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter')) `
'PathGeometryAnalysis must expose peak d-kappa/d-s.'
$analyzer = [Activator]::CreateInstance($analyzerType)
$analyzeMethod = $analyzerType.GetMethod('TryAnalyze')
@@ -184,6 +202,12 @@ $straight = New-DirectionSegment 0 $forward @(
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0))
$straightAnalysis = Invoke-Analysis @($straight)
Assert-Equal 21 $straightAnalysis.Path.Count 'A one-metre straight must produce twenty 0.05 m intervals plus the initial point.'
Assert-Near 0.0 $straightAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter 0.000000001 `
'A straight must have zero peak d-kappa/d-s.'
foreach ($point in $straightAnalysis.Path) {
Assert-Near 0.0 $point.VehicleCurvatureDerivative 0.000000001 `
'A straight point must carry zero d-kappa/d-s.'
}
for ($index = 1; $index -lt $straightAnalysis.Path.Count; $index++) {
$left = $straightAnalysis.Path[$index - 1]
$right = $straightAnalysis.Path[$index]
@@ -246,6 +270,10 @@ Assert-Equal 'Forward' $switchLeft.Direction.ToString() 'The first gear-switch p
Assert-Equal 'Reverse' $switchRight.Direction.ToString() 'The second gear-switch pose must retain its reverse segment direction.'
Assert-True (-not [double]::IsNaN($switchLeft.GeometricCurvature)) 'No derivative may cross the gear-switch duplicate point.'
Assert-True (-not [double]::IsNaN($switchRight.GeometricCurvature)) 'No reverse derivative may cross the gear-switch duplicate point.'
Assert-True (-not [double]::IsNaN($switchLeft.VehicleCurvatureDerivative)) `
'The forward side of a gear switch must have a finite one-sided derivative.'
Assert-True (-not [double]::IsNaN($switchRight.VehicleCurvatureDerivative)) `
'The reverse side of a gear switch must have a finite one-sided derivative.'
# Curvature at a curved segment's end and the following straight reverse segment's start must remain independently differentiated.
$forwardArcToSwitch = New-DirectionSegment 0 $forward $forwardArcPoints.ToArray() $false $true
@@ -255,6 +283,17 @@ $reverseStraightAfterArc = New-DirectionSegment 1 $reverse @(
$curveSwitchAnalysis = Invoke-Analysis @($forwardArcToSwitch, $reverseStraightAfterArc)
$reverseStraightStart = $curveSwitchAnalysis.Path[$curveSwitchAnalysis.Segments[1].StartIndex]
Assert-Near 0.0 $reverseStraightStart.GeometricCurvature 0.000000001 'A gear-switch must not use the preceding curve to differentiate a reverse straight segment.'
Assert-Near 0.0 $reverseStraightStart.VehicleCurvatureDerivative 0.000000001 `
'No curvature derivative may cross from the preceding forward arc into a reverse straight.'
$physicalStart = New-DirectionSegmentWithStartCurvature 0 $forward 0.20 @(
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0))
$physicalStartAnalysis = Invoke-Analysis @($physicalStart)
Assert-Near 0.20 $physicalStartAnalysis.Path[0].VehicleCurvature 0.000000001 `
'The unified analyzer must retain a real start steering-curvature boundary state.'
Assert-True ($physicalStartAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter -gt 0.0) `
'A real start-curvature mismatch must remain visible to the quality analyzer.'
# A boundary that claims a gear switch must be a duplicated pose with opposite direction; discontinuities are rejected.
$invalidSwitch = New-DirectionSegment 1 $reverse @(
ClumsyPilot/tests/verify_path_smoothing_validation.ps1
+12 -5
View File
@@ -45,10 +45,11 @@ function New-Map([bool]$WithObstacle) {
}
function New-SmoothedPoint([double]$X, [double]$Y, [double]$ArcLength, $Direction,
[double]$VehicleCurvature = 0.0, [bool]$IsGearSwitch = $false, [double]$Clearance = 999.0) {
[double]$VehicleCurvature = 0.0, [bool]$IsGearSwitch = $false, [double]$Clearance = 999.0,
[double]$VehicleCurvatureDerivative = 0.0) {
return [Activator]::CreateInstance($smoothedPointType, @(
$X, $Y, [double]0.0, [double]0.0, $ArcLength, $Direction,
$VehicleCurvature, $VehicleCurvature, $Clearance, $IsGearSwitch, $anchor))
$VehicleCurvature, $VehicleCurvature, $VehicleCurvatureDerivative, $Clearance, $IsGearSwitch, $anchor))
}
function New-PreparedPoint([double]$X, [double]$Y, [double]$ArcLength, [bool]$IsGearSwitch = $false) {
@@ -57,10 +58,10 @@ function New-PreparedPoint([double]$X, [double]$Y, [double]$ArcLength, [bool]$Is
}
function New-OneSegmentCase([double]$X0, [double]$Y0, [double]$X1, [double]$Y1, [double]$VehicleCurvature = 0.0,
[double]$StartArcLength = 0.0) {
[double]$StartArcLength = 0.0, [double]$VehicleCurvatureDerivative = 0.0) {
$candidatePath = [Array]::CreateInstance($smoothedPointType, 2)
$candidatePath.SetValue((New-SmoothedPoint $X0 $Y0 $StartArcLength $forward), 0)
$candidatePath.SetValue((New-SmoothedPoint $X1 $Y1 ($StartArcLength + 1.0) $forward $VehicleCurvature), 1)
$candidatePath.SetValue((New-SmoothedPoint $X0 $Y0 $StartArcLength $forward 0.0 $false 999.0 $VehicleCurvatureDerivative), 0)
$candidatePath.SetValue((New-SmoothedPoint $X1 $Y1 ($StartArcLength + 1.0) $forward $VehicleCurvature $false 999.0 $VehicleCurvatureDerivative), 1)
$candidateSegments = [Array]::CreateInstance($smoothedSegmentType, 1)
$candidateSegments.SetValue([Activator]::CreateInstance($smoothedSegmentType, @(0, $forward, 0, 1, $false, $false)), 0)
$preparedPoints = [Array]::CreateInstance($smoothingPointType, 2)
@@ -151,6 +152,12 @@ Assert-True $valid.Accepted ('A valid straight candidate must pass. Reason=' + $
Assert-True ($null -ne $valid.Path) 'A valid candidate must return clearance-recomputed points.'
Assert-True ($valid.Path[0].BodyClearance -lt 999.0) 'Validated output must replace an overclaimed candidate clearance.'
Assert-True ($valid.MinimumClearance -ge 0.0) 'A valid candidate must report non-negative conservative clearance.'
$derivativePreserved = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5 0.0 0.0 0.25) $obstacleMap
Assert-True $derivativePreserved.Accepted ('A valid derivative-bearing candidate must pass. Reason=' + $derivativePreserved.Reason)
Assert-Near 0.25 $derivativePreserved.Path[0].VehicleCurvatureDerivative 0.000000001 `
'Clearance recomputation must preserve the first point curvature derivative.'
Assert-Near 0.25 $derivativePreserved.Path[1].VehicleCurvatureDerivative 0.000000001 `
'Clearance recomputation must preserve the final point curvature derivative.'
$pointCollision = Invoke-Validation (New-OneSegmentCase 2.0 2.0 2.5 2.0) $obstacleMap
Assert-False $pointCollision.Accepted 'A smoothing candidate that touches an obstacle must be rejected.'