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
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.'