diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs index 2ffa77b..6153af8 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs @@ -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 samples, + IReadOnlyList 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 input, double spacingMeters, diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs index 488d3d5..decf6f3 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs @@ -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 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(request.CoarsePath.Count); + var segments = new List(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 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) diff --git a/ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 b/ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 index dcef756..42bfbde 100644 --- a/ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 +++ b/ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 @@ -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),