diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs
new file mode 100644
index 0000000..410a753
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs
@@ -0,0 +1,243 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
+
+/// 独立复核平滑候选的端点拓扑、车辆曲率和连续车体安全性。
+public sealed class SmoothedPathValidator
+{
+ private const double Tolerance = 1e-6d;
+ private readonly FootprintCollisionChecker _collisionChecker;
+
+ /// 创建使用默认连续车体碰撞检查器的平滑路径验证器。
+ public SmoothedPathValidator()
+ : this(new FootprintCollisionChecker())
+ {
+ }
+
+ /// 创建使用指定连续车体碰撞检查器的平滑路径验证器。
+ public SmoothedPathValidator(FootprintCollisionChecker collisionChecker)
+ {
+ _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
+ }
+
+ ///
+ /// 复核候选平滑路径。每个候选方向段必须保持原始段的端点和换向拓扑;
+ /// 输出中的净空均由本次实际车体检查重新计算,绝不沿用候选声明值。
+ ///
+ public bool TryValidate(
+ IReadOnlyList candidatePath,
+ IReadOnlyList candidateSegments,
+ PreparedPath originalPath,
+ PlanningGridMap map,
+ VehicleParameters vehicle,
+ double maximumCollisionCheckStepMeters,
+ out IReadOnlyList pathWithClearance,
+ out double minimumClearanceMeters,
+ out string reason)
+ {
+ pathWithClearance = EmptyPath();
+ minimumClearanceMeters = 0d;
+ reason = string.Empty;
+ if (candidatePath == null || candidateSegments == null || originalPath == null || map == null || vehicle == null ||
+ candidatePath.Count == 0 || candidateSegments.Count == 0 || !NumericGuard.IsPositiveFinite(maximumCollisionCheckStepMeters))
+ {
+ reason = "平滑候选、原始路径、地图、车辆或碰撞步长无效。";
+ return false;
+ }
+
+ if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvatureMeters))
+ {
+ reason = "车辆曲率约束无效。";
+ return false;
+ }
+
+ if (!TryValidateSegmentTopology(candidatePath, candidateSegments, originalPath, out reason)) return false;
+ if (Math.Abs(candidatePath[0].ArcLength) > Tolerance)
+ {
+ reason = "平滑路径首点的全局弧长必须为零。";
+ return false;
+ }
+
+ var checkedClearances = new double[candidatePath.Count];
+ double minimumClearance = double.PositiveInfinity;
+ for (int index = 0; index < candidatePath.Count; index++)
+ {
+ SmoothedPathPoint current = candidatePath[index];
+ if (!IsValidPoint(current) || Math.Abs(current.VehicleCurvature) > maximumCurvatureMeters + Tolerance)
+ {
+ reason = "平滑路径包含非法数值或超限车辆曲率。";
+ return false;
+ }
+
+ var currentPose = new Pose2D(current.X, current.Y, current.Heading);
+ if (!_collisionChecker.IsPoseCollisionFree(currentPose, map, vehicle, 0d, out double poseClearance))
+ {
+ reason = "平滑路径点未通过完整车体碰撞或边界复核。";
+ return false;
+ }
+
+ checkedClearances[index] = poseClearance;
+ minimumClearance = Math.Min(minimumClearance, poseClearance);
+ if (index == 0) continue;
+
+ SmoothedPathPoint previous = candidatePath[index - 1];
+ if (!IsUnwrappedHeadingContinuous(previous, current))
+ {
+ reason = "平滑路径展开航向不连续。";
+ return false;
+ }
+
+ if (IsDuplicatePoseAndArcLength(previous, current))
+ {
+ if (previous.Direction == current.Direction || !current.IsGearSwitchPoint)
+ {
+ reason = "相邻重复点不是合法换向对。";
+ return false;
+ }
+ continue;
+ }
+
+ if (current.IsGearSwitchPoint || current.ArcLength <= previous.ArcLength + Tolerance)
+ {
+ reason = "非换向点必须保持正弧长增量,换向点必须保留重复位姿。";
+ return false;
+ }
+
+ var previousPose = new Pose2D(previous.X, previous.Y, previous.Heading);
+ if (!_collisionChecker.IsSweptMotionCollisionFree(previousPose, currentPose, map, vehicle,
+ maximumCollisionCheckStepMeters, out double sweptClearance))
+ {
+ reason = "平滑路径相邻点之间的完整车体扫掠碰撞复核失败。";
+ return false;
+ }
+
+ checkedClearances[index - 1] = Math.Min(checkedClearances[index - 1], sweptClearance);
+ checkedClearances[index] = Math.Min(checkedClearances[index], sweptClearance);
+ minimumClearance = Math.Min(minimumClearance, sweptClearance);
+ }
+
+ var output = new List(candidatePath.Count);
+ for (int index = 0; index < candidatePath.Count; index++)
+ {
+ 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));
+ }
+
+ pathWithClearance = new ReadOnlyCollection(output);
+ minimumClearanceMeters = minimumClearance;
+ return true;
+ }
+
+ private static bool TryValidateSegmentTopology(
+ IReadOnlyList candidatePath,
+ IReadOnlyList candidateSegments,
+ PreparedPath originalPath,
+ out string reason)
+ {
+ reason = string.Empty;
+ if (originalPath.Segments == null || originalPath.Segments.Count != candidateSegments.Count)
+ {
+ reason = "平滑路径方向段数量必须保持原始换向拓扑。";
+ return false;
+ }
+
+ int expectedStartIndex = 0;
+ for (int segmentIndex = 0; segmentIndex < candidateSegments.Count; segmentIndex++)
+ {
+ SmoothedPathSegment candidateSegment = candidateSegments[segmentIndex];
+ PreparedDirectionSegment originalSegment = originalPath.Segments[segmentIndex];
+ if (candidateSegment == null || originalSegment == null || candidateSegment.SegmentIndex != segmentIndex ||
+ candidateSegment.StartIndex != expectedStartIndex || candidateSegment.StartIndex < 0 ||
+ candidateSegment.EndIndex < candidateSegment.StartIndex || candidateSegment.EndIndex >= candidatePath.Count ||
+ candidateSegment.Direction != originalSegment.Direction ||
+ candidateSegment.StartsAtGearSwitch != originalSegment.StartsAtGearSwitch ||
+ candidateSegment.EndsAtGearSwitch != originalSegment.EndsAtGearSwitch ||
+ originalSegment.Points == null || originalSegment.Points.Count == 0)
+ {
+ reason = "平滑路径方向段索引、方向或换向拓扑无效。";
+ return false;
+ }
+
+ SmoothedPathPoint candidateStart = candidatePath[candidateSegment.StartIndex];
+ SmoothedPathPoint candidateEnd = candidatePath[candidateSegment.EndIndex];
+ SmoothingPoint2D originalStart = originalSegment.Points[0];
+ SmoothingPoint2D originalEnd = originalSegment.Points[originalSegment.Points.Count - 1];
+ if (!SamePose(candidateStart, originalStart) || !SamePose(candidateEnd, originalEnd) ||
+ candidateStart.Direction != originalSegment.Direction || candidateEnd.Direction != originalSegment.Direction ||
+ candidateStart.IsGearSwitchPoint != originalStart.IsGearSwitchPoint ||
+ candidateEnd.IsGearSwitchPoint != originalEnd.IsGearSwitchPoint)
+ {
+ reason = "平滑路径改变了原始方向段端点或换向点。";
+ return false;
+ }
+
+ for (int pointIndex = candidateSegment.StartIndex; pointIndex <= candidateSegment.EndIndex; pointIndex++)
+ {
+ if (candidatePath[pointIndex] == null || candidatePath[pointIndex].Direction != originalSegment.Direction)
+ {
+ reason = "平滑路径方向段包含与段方向不一致的点。";
+ return false;
+ }
+ }
+
+ expectedStartIndex = candidateSegment.EndIndex + 1;
+ }
+
+ if (expectedStartIndex != candidatePath.Count)
+ {
+ reason = "平滑路径方向段未完整覆盖候选点。";
+ return false;
+ }
+ return true;
+ }
+
+ private static bool IsValidPoint(SmoothedPathPoint point)
+ {
+ return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
+ NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
+ NumericGuard.IsFinite(point.ArcLength) && point.ArcLength >= 0d &&
+ NumericGuard.IsFinite(point.GeometricCurvature) && NumericGuard.IsFinite(point.VehicleCurvature) &&
+ IsDirection(point.Direction) && Enum.IsDefined(typeof(SmoothedPathPointSource), point.Source) &&
+ Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= Tolerance;
+ }
+
+ private static bool SamePose(SmoothedPathPoint candidate, SmoothingPoint2D original)
+ {
+ return candidate != null && original != null && Math.Abs(candidate.X - original.X) <= Tolerance &&
+ Math.Abs(candidate.Y - original.Y) <= Tolerance &&
+ Math.Abs(AngleMath.ShortestSignedDifference(candidate.Heading, original.Heading)) <= Tolerance;
+ }
+
+ private static bool IsUnwrappedHeadingContinuous(SmoothedPathPoint previous, SmoothedPathPoint current)
+ {
+ double expectedDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
+ return NumericGuard.IsFinite(expectedDelta) &&
+ Math.Abs((current.UnwrappedHeading - previous.UnwrappedHeading) - expectedDelta) <= Tolerance;
+ }
+
+ private static bool IsDuplicatePoseAndArcLength(SmoothedPathPoint previous, SmoothedPathPoint current)
+ {
+ return Math.Abs(previous.X - current.X) <= Tolerance && Math.Abs(previous.Y - current.Y) <= Tolerance &&
+ Math.Abs(AngleMath.ShortestSignedDifference(previous.Heading, current.Heading)) <= Tolerance &&
+ Math.Abs(previous.ArcLength - current.ArcLength) <= Tolerance;
+ }
+
+ private static bool IsDirection(TravelDirection direction)
+ {
+ return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
+ }
+
+ private static IReadOnlyList EmptyPath()
+ {
+ return new ReadOnlyCollection(new List());
+ }
+}
diff --git a/ClumsyPilot/tests/verify_path_smoothing_validation.ps1 b/ClumsyPilot/tests/verify_path_smoothing_validation.ps1
new file mode 100644
index 0000000..06d923a
--- /dev/null
+++ b/ClumsyPilot/tests/verify_path_smoothing_validation.ps1
@@ -0,0 +1,168 @@
+param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
+
+$ErrorActionPreference = 'Stop'
+$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
+
+function Assert-True($Actual, [string]$Message) {
+ if (-not $Actual) { throw $Message }
+}
+
+function Assert-False($Actual, [string]$Message) {
+ if ($Actual) { throw $Message }
+}
+
+function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
+ if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
+ throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
+ }
+}
+
+function Get-RequiredType([string]$Name) {
+ return $assembly.GetType($Name, $true)
+}
+
+function New-Map([bool]$WithObstacle) {
+ $bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]4000, [single]0, [single]4000))
+ $request = [Activator]::CreateInstance($mapRequestType)
+ $request.Bounds = $bounds
+ $request.ResolutionMm = [single]50
+ if ($WithObstacle) {
+ $obstacle = [Activator]::CreateInstance($rectangleType, @([single]1900, [single]2100, [single]1800, [single]2200))
+ $obstacles = [Array]::CreateInstance($obstacleType, 1)
+ $obstacles.SetValue($obstacle, 0)
+ $source = [Activator]::CreateInstance($manualSourceType, @('validator-obstacle', [long]1, $true, $obstacles))
+ $sources = [Array]::CreateInstance($obstacleSourceType, 1)
+ $sources.SetValue($source, 0)
+ $request.ObstacleSources = $sources
+ } else {
+ $request.ObstacleSources = [Array]::CreateInstance($obstacleSourceType, 0)
+ $request.AllowExplicitEmptyMap = $true
+ }
+ $result = [Activator]::CreateInstance($mapFactoryType).Create($request)
+ Assert-True $result.Succeeded 'Validation test map must be built.'
+ Assert-True $result.Map.PlanningReady 'Validation test map must be ready.'
+ return $result.Map
+}
+
+function New-SmoothedPoint([double]$X, [double]$Y, [double]$ArcLength, $Direction,
+ [double]$VehicleCurvature = 0.0, [bool]$IsGearSwitch = $false, [double]$Clearance = 999.0) {
+ return [Activator]::CreateInstance($smoothedPointType, @(
+ $X, $Y, [double]0.0, [double]0.0, $ArcLength, $Direction,
+ $VehicleCurvature, $VehicleCurvature, $Clearance, $IsGearSwitch, $anchor))
+}
+
+function New-PreparedPoint([double]$X, [double]$Y, [double]$ArcLength, [bool]$IsGearSwitch = $false) {
+ return [Activator]::CreateInstance($smoothingPointType, @(
+ $X, $Y, $ArcLength, [double]0.0, [double]0.0, [double]999.0, $IsGearSwitch, $anchor))
+}
+
+function New-OneSegmentCase([double]$X0, [double]$Y0, [double]$X1, [double]$Y1, [double]$VehicleCurvature = 0.0,
+ [double]$StartArcLength = 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)
+ $candidateSegments = [Array]::CreateInstance($smoothedSegmentType, 1)
+ $candidateSegments.SetValue([Activator]::CreateInstance($smoothedSegmentType, @(0, $forward, 0, 1, $false, $false)), 0)
+ $preparedPoints = [Array]::CreateInstance($smoothingPointType, 2)
+ $preparedPoints.SetValue((New-PreparedPoint $X0 $Y0 0.0), 0)
+ $preparedPoints.SetValue((New-PreparedPoint $X1 $Y1 1.0), 1)
+ $preparedSegments = [Array]::CreateInstance($preparedSegmentType, 1)
+ $preparedSegments.SetValue([Activator]::CreateInstance($preparedSegmentType, @(0, $forward, $preparedPoints, $false, $false)), 0)
+ return [PSCustomObject]@{
+ CandidatePath = $candidatePath
+ CandidateSegments = $candidateSegments
+ Original = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$preparedSegments))
+ }
+}
+
+function Invoke-Validation($Case, $Map) {
+ $arguments = [object[]]@($Case.CandidatePath, $Case.CandidateSegments, $Case.Original, $Map, $vehicle,
+ [double]0.05, $null, [double]0.0, $null)
+ $accepted = $validateMethod.Invoke($validator, $arguments)
+ return [PSCustomObject]@{ Accepted = $accepted; Path = $arguments[6]; MinimumClearance = $arguments[7]; Reason = $arguments[8] }
+}
+
+function New-MovedGearSwitchCase {
+ $candidatePath = [Array]::CreateInstance($smoothedPointType, 4)
+ $candidatePath.SetValue((New-SmoothedPoint 0.5 0.5 0.0 $forward), 0)
+ $candidatePath.SetValue((New-SmoothedPoint 1.5 0.5 1.0 $forward), 1)
+ $candidatePath.SetValue((New-SmoothedPoint 1.6 0.5 1.0 $reverse 0.0 $true), 2)
+ $candidatePath.SetValue((New-SmoothedPoint 0.5 0.5 2.0 $reverse), 3)
+ $candidateSegments = [Array]::CreateInstance($smoothedSegmentType, 2)
+ $candidateSegments.SetValue([Activator]::CreateInstance($smoothedSegmentType, @(0, $forward, 0, 1, $false, $true)), 0)
+ $candidateSegments.SetValue([Activator]::CreateInstance($smoothedSegmentType, @(1, $reverse, 2, 3, $true, $false)), 1)
+ $preparedFirst = [Array]::CreateInstance($smoothingPointType, 2)
+ $preparedFirst.SetValue((New-PreparedPoint 0.5 0.5 0.0), 0)
+ $preparedFirst.SetValue((New-PreparedPoint 1.5 0.5 1.0), 1)
+ $preparedSecond = [Array]::CreateInstance($smoothingPointType, 2)
+ $preparedSecond.SetValue((New-PreparedPoint 1.5 0.5 0.0 $true), 0)
+ $preparedSecond.SetValue((New-PreparedPoint 0.5 0.5 1.0), 1)
+ $preparedSegments = [Array]::CreateInstance($preparedSegmentType, 2)
+ $preparedSegments.SetValue([Activator]::CreateInstance($preparedSegmentType, @(0, $forward, $preparedFirst, $false, $true)), 0)
+ $preparedSegments.SetValue([Activator]::CreateInstance($preparedSegmentType, @(1, $reverse, $preparedSecond, $true, $false)), 1)
+ return [PSCustomObject]@{
+ CandidatePath = $candidatePath
+ CandidateSegments = $candidateSegments
+ Original = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$preparedSegments))
+ }
+}
+
+$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
+$processing = $root + 'Processing.'
+$validation = $root + 'Validation.'
+$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
+$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
+
+$validatorType = Get-RequiredType ($validation + 'SmoothedPathValidator')
+$smoothedPointType = Get-RequiredType ($root + 'SmoothedPathPoint')
+$smoothedSegmentType = Get-RequiredType ($root + 'SmoothedPathSegment')
+$smoothingPointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
+$preparedSegmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
+$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
+$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
+$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
+$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
+$boundsType = Get-RequiredType ($mapping + 'MapBoundsMm')
+$obstacleType = Get-RequiredType ($mapping + 'IMapObstacle')
+$rectangleType = Get-RequiredType ($mapping + 'AxisAlignedRectangleObstacle')
+$obstacleSourceType = Get-RequiredType ($mapping + 'IMapObstacleSource')
+$manualSourceType = Get-RequiredType ($mapping + 'ManualObstacleSource')
+$mapRequestType = Get-RequiredType ($mapping + 'PlanningMapRequest')
+$mapFactoryType = Get-RequiredType ($mapping + 'PlanningMapFactory')
+
+$validator = [Activator]::CreateInstance($validatorType)
+$validateMethod = $validatorType.GetMethod('TryValidate')
+Assert-True ($null -ne $validateMethod) 'SmoothedPathValidator must expose TryValidate.'
+Assert-True ($validateMethod.GetParameters().Length -eq 9) 'TryValidate must accept path, segments, originals, map, vehicle, step, path, clearance, and reason.'
+$forward = [Enum]::Parse($directionType, 'Forward')
+$reverse = [Enum]::Parse($directionType, 'Reverse')
+$anchor = [Enum]::Parse($sourceType, 'Anchor')
+$vehicle = [Activator]::CreateInstance($vehicleType)
+$vehicle.LengthMeters = 0.20
+$vehicle.WidthMeters = 0.20
+$vehicle.SafetyMarginMeters = 0.0
+$vehicle.MaximumCurvaturePerMeter = 1.0
+$vehicle.MinimumTurningRadiusMeters = 1.0
+
+$emptyMap = New-Map $false
+$obstacleMap = New-Map $true
+$valid = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5) $obstacleMap
+Assert-True $valid.Accepted ('A valid straight candidate must pass. Reason=' + $valid.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.'
+
+$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.'
+$sweptCollision = Invoke-Validation (New-OneSegmentCase 1.5 2.0 2.5 2.0) $obstacleMap
+Assert-False $sweptCollision.Accepted 'A smoothing candidate whose sweep cuts through an obstacle must be rejected.'
+$outsideBounds = Invoke-Validation (New-OneSegmentCase 0.05 0.5 0.5 0.5) $emptyMap
+Assert-False $outsideBounds.Accepted 'A smoothing candidate outside map bounds must be rejected.'
+$overCurvature = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5 2.0) $emptyMap
+Assert-False $overCurvature.Accepted 'A smoothing candidate above vehicle maximum curvature must be rejected.'
+$nonzeroStartArc = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5 0.0 1.0) $emptyMap
+Assert-False $nonzeroStartArc.Accepted 'A smoothing candidate must start at global arc length zero.'
+$movedSwitch = Invoke-Validation (New-MovedGearSwitchCase) $emptyMap
+Assert-False $movedSwitch.Accepted 'A smoothing candidate with a moved gear-switch pose must be rejected.'
+
+Write-Output 'Path smoothing validation checks passed.'