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

fix: preserve trusted raw path curvature

Browse Source
This commit is contained in:
梁薄云
2026-07-31 15:28:49 +08:00
parent 9173f9bfb5
commit 272a847b2a
3 changed files with 362 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs
+50 -12
View File
@@ -199,20 +199,20 @@ public sealed class PathGeometryAnalyzer
{
geometricCurvatures[index] = 0d;
}
else if (index == 0)
{
geometricCurvatures[index] = (unwrappedHeadings[1] - unwrappedHeadings[0]) /
(localArcLengths[1] - localArcLengths[0]);
}
else if (index == count - 1)
{
geometricCurvatures[index] = (unwrappedHeadings[index] - unwrappedHeadings[index - 1]) /
(localArcLengths[index] - localArcLengths[index - 1]);
}
else
{
geometricCurvatures[index] = (unwrappedHeadings[index + 1] - unwrappedHeadings[index - 1]) /
(localArcLengths[index + 1] - localArcLengths[index - 1]);
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]))
@@ -311,6 +311,44 @@ public sealed class PathGeometryAnalyzer
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,
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs
+157 -1
View File
@@ -1,4 +1,6 @@
using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
@@ -27,7 +29,7 @@ internal static class RawPathBaselineBuilder
if (!analyzer.TryAnalyze(preparedPath.Segments, outputSpacingMeters, out PathGeometryAnalysis analysis, out reason))
return false;
if (!validator.TryValidate(
if (validator.TryValidate(
analysis.Path,
analysis.Segments,
preparedPath,
@@ -37,6 +39,23 @@ internal static class RawPathBaselineBuilder
out IReadOnlyList<SmoothedPathPoint> safePath,
out double minimumClearanceMeters,
out reason))
{
baseline = new RawPathBaseline(safePath, analysis.Segments, CreateMetrics(analysis, minimumClearanceMeters));
return true;
}
if (reason != "平滑路径包含非法数值或超限车辆曲率。" ||
!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;
}
@@ -45,6 +64,143 @@ internal static class RawPathBaselineBuilder
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)
ClumsyPilot/tests/verify_path_smoothing_geometry.ps1
+155
View File
@@ -150,6 +150,110 @@ function New-EmptyGeometryMap {
return $map
}
function Invoke-GeometryValidation($Analysis, [object[]]$Segments, $Vehicle) {
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
for ($index = 0; $index -lt $Segments.Count; $index++) {
$typedSegments.SetValue($Segments[$index], $index)
}
$preparedPath = [Activator]::CreateInstance($preparedPathType, [object[]](, $typedSegments))
$arguments = [object[]]@(
$Analysis.Path, $Analysis.Segments, $preparedPath, (New-EmptyGeometryMap), $Vehicle, [double]0.05,
$null, [double]0.0, $null)
$accepted = $validateMethod.Invoke($validator, $arguments)
return [pscustomobject]@{ Accepted = $accepted; Reason = $arguments[8] }
}
function New-CircularDirectionSegment(
$Direction,
[double]$Curvature,
[int]$Intervals,
[double]$ChordLength) {
$radius = 1.0 / $Curvature
$headingStep = 2.0 * [Math]::Asin($Curvature * $ChordLength / 2.0)
$points = New-Object System.Collections.Generic.List[object]
for ($index = 0; $index -le $Intervals; $index++) {
$theta = $headingStep * $index
$heading = if ($Direction.ToString() -eq 'Forward') { $theta } else { $theta + [Math]::PI }
[void]$points.Add((New-GeometryPoint `
(1.0 + $radius * [Math]::Sin($theta)) `
(1.0 + $radius * (1.0 - [Math]::Cos($theta))) `
($index * $ChordLength) $heading $heading))
}
return New-DirectionSegment 0 $Direction $points.ToArray()
}
function Get-OldPolylineCurvatureMaximum($Analysis) {
$maximum = 0.0
$path = $Analysis.Path
for ($index = 0; $index -lt $path.Count; $index++) {
if ($path.Count -eq 1) {
$curvature = 0.0
}
elseif ($index -eq 0) {
$curvature = ($path[1].UnwrappedHeading - $path[0].UnwrappedHeading) /
($path[1].ArcLength - $path[0].ArcLength)
}
elseif ($index -eq $path.Count - 1) {
$curvature = ($path[$index].UnwrappedHeading - $path[$index - 1].UnwrappedHeading) /
($path[$index].ArcLength - $path[$index - 1].ArcLength)
}
else {
$curvature = ($path[$index + 1].UnwrappedHeading - $path[$index - 1].UnwrappedHeading) /
($path[$index + 1].ArcLength - $path[$index - 1].ArcLength)
}
$maximum = [Math]::Max($maximum, [Math]::Abs($curvature))
}
return $maximum
}
function Get-QuinticAnalyticCurvatureMaximum(
[double]$C2,
[double]$C3,
[double]$C4,
[double]$C5,
[int]$ReferenceSamples = 20000) {
$maximum = 0.0
for ($index = 0; $index -lt $ReferenceSamples; $index++) {
$t = $index / [double]($ReferenceSamples - 1)
$firstDerivative = 2.0 * $C2 * $t + 3.0 * $C3 * $t * $t +
4.0 * $C4 * $t * $t * $t + 5.0 * $C5 * $t * $t * $t * $t
$secondDerivative = 2.0 * $C2 + 6.0 * $C3 * $t +
12.0 * $C4 * $t * $t + 20.0 * $C5 * $t * $t * $t
$curvature = [Math]::Abs($secondDerivative / [Math]::Pow(1.0 + $firstDerivative * $firstDerivative, 1.5))
$maximum = [Math]::Max($maximum, $curvature)
}
return $maximum
}
function New-QuinticDirectionSegment(
[double]$C2,
[double]$C3,
[double]$C4,
[double]$C5,
[double]$DistributionPower,
[int]$Samples = 400) {
$points = New-Object System.Collections.Generic.List[object]
$arcLength = 0.0
$previousX = 0.0
$previousY = 0.0
for ($index = 0; $index -lt $Samples; $index++) {
$t = [Math]::Pow($index / [double]($Samples - 1), $DistributionPower)
$x = 1.0 + $t
$y = $C2 * $t * $t + $C3 * $t * $t * $t + $C4 * $t * $t * $t * $t + $C5 * $t * $t * $t * $t * $t
if ($index -gt 0) {
$arcLength += [Math]::Sqrt(($x - $previousX) * ($x - $previousX) + ($y - $previousY) * ($y - $previousY))
}
$heading = [Math]::Atan2(
2.0 * $C2 * $t + 3.0 * $C3 * $t * $t + 4.0 * $C4 * $t * $t * $t + 5.0 * $C5 * $t * $t * $t * $t,
1.0)
[void]$points.Add((New-GeometryPoint $x $y $arcLength $heading $heading))
$previousX = $x
$previousY = $y
}
return New-DirectionSegment 0 $forward $points.ToArray()
}
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
$processing = $root + 'Processing.'
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
@@ -179,6 +283,7 @@ $vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
$boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm'
$mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest'
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'
$validatorType = Get-RequiredType ($root + 'Validation.SmoothedPathValidator')
Assert-True ($null -ne $preparedPathType) 'PreparedPath must be discoverable for smoothing algorithms.'
Assert-True ($null -ne $preprocessorType) 'PathSmoothingPreprocessor must be discoverable for request preparation.'
@@ -190,12 +295,62 @@ $analyzer = [Activator]::CreateInstance($analyzerType)
$analyzeMethod = $analyzerType.GetMethod('TryAnalyze')
Assert-True ($null -ne $analyzeMethod) 'PathGeometryAnalyzer must expose TryAnalyze.'
Assert-Equal 4 $analyzeMethod.GetParameters().Length 'TryAnalyze must accept segments, spacing, analysis, and reason.'
$validator = [Activator]::CreateInstance($validatorType)
$validateMethod = $validatorType.GetMethod('TryValidate')
Assert-True ($null -ne $validateMethod) 'SmoothedPathValidator must expose TryValidate.'
Assert-Equal 9 $validateMethod.GetParameters().Length 'SmoothedPathValidator.TryValidate must retain its public contract.'
$forward = [Enum]::Parse($directionType, 'Forward')
$reverse = [Enum]::Parse($directionType, 'Reverse')
$anchor = [Enum]::Parse($sourceType, 'Anchor')
$coarseAnchor = [Enum]::Parse($coarseSourceType, 'Start')
# The chord-corrected estimator must retain an exact circular curvature limit for both travel directions.
$maximumAllowedCurvature = 5.0 / 6.0
$circleVehicle = [Activator]::CreateInstance($vehicleType)
$circleVehicle.LengthMeters = 0.20
$circleVehicle.WidthMeters = 0.20
$circleVehicle.SafetyMarginMeters = 0.0
$circleVehicle.MaximumCurvaturePerMeter = $maximumAllowedCurvature
foreach ($direction in @($forward, $reverse)) {
$exactLimitCircle = New-CircularDirectionSegment $direction $maximumAllowedCurvature 20 0.05
$exactLimitAnalysis = Invoke-Analysis @($exactLimitCircle)
foreach ($point in $exactLimitAnalysis.Path) {
Assert-True ([Math]::Abs($point.VehicleCurvature) -le $maximumAllowedCurvature + 1.0e-9) `
('An exact-limit ' + $direction + ' circle must not exceed the curvature limit.')
}
$validation = Invoke-GeometryValidation $exactLimitAnalysis @($exactLimitCircle) $circleVehicle
Assert-True $validation.Accepted ('The validator must accept an analyzed exact-limit ' + $direction + ' circle. Reason=' + $validation.Reason)
}
# An analyzed over-limit circle must remain detectable by the unchanged validator threshold.
$overLimitCircle = New-CircularDirectionSegment $forward ($maximumAllowedCurvature + 0.01) 20 0.05
$overLimitAnalysis = Invoke-Analysis @($overLimitCircle)
Assert-True ($overLimitAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter -gt $maximumAllowedCurvature + 1.0e-6) `
'An over-limit circle must exceed the vehicle curvature limit by more than the validator tolerance.'
$overLimitValidation = Invoke-GeometryValidation $overLimitAnalysis @($overLimitCircle) $circleVehicle
Assert-False $overLimitValidation.Accepted 'The validator must reject an analyzed over-limit circle.'
# The chord-corrected estimator must not under-estimate these smooth references more than the former polyline estimator.
foreach ($quinticCase in @(
[pscustomobject]@{ Name = 'SBend'; C2 = 0.0; C3 = 0.30; C4 = -0.45; C5 = 0.18; Power = 1.0 },
[pscustomobject]@{ Name = 'EndpointPeak'; C2 = 0.18; C3 = -0.12; C4 = 0.0; C5 = 0.0; Power = 1.0 },
[pscustomobject]@{ Name = 'NonUniformFinalInterval'; C2 = -0.12; C3 = 0.36; C4 = -0.30; C5 = 0.08; Power = 1.7 })) {
$quintic = New-QuinticDirectionSegment $quinticCase.C2 $quinticCase.C3 $quinticCase.C4 $quinticCase.C5 $quinticCase.Power
$quinticAnalysis = Invoke-Analysis @($quintic)
$analyticMaximum = Get-QuinticAnalyticCurvatureMaximum $quinticCase.C2 $quinticCase.C3 $quinticCase.C4 $quinticCase.C5
$newDeficit = [Math]::Max(0.0, $analyticMaximum - $quinticAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter)
$oldDeficit = [Math]::Max(0.0, $analyticMaximum - (Get-OldPolylineCurvatureMaximum $quinticAnalysis))
Assert-True ($newDeficit -le $oldDeficit + 1.0e-6) `
($quinticCase.Name + ' must not have greater one-sided curvature under-estimation than the old polyline estimator.')
}
# A half-turn over a single chord has no unambiguous geometric curvature estimate.
$ambiguousTurn = New-DirectionSegment 0 $forward @(
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
(New-GeometryPoint 1.0 0.0 1.0 ([Math]::PI) ([Math]::PI)))
Assert-AnalysisRejected @($ambiguousTurn) 'A two-point heading turn of π must be rejected as ambiguous.'
# Forward straight: resampling is exactly 0.05 m, preserves the exact endpoint, and has zero curvature.
$straight = New-DirectionSegment 0 $forward @(
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),