feat: derive adaptive full-segment ST schedule

2026-08-06 23:54:34 +08:00
parent dad4ff4b04
commit 3269d556b6
14 changed files with 1642 additions and 108 deletions
@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;

 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

@@ -11,67 +12,112 @@ public sealed class PathSpeedLimitBuilder
    private const double StationMergeToleranceMeters = 1e-12d;

    public EmPlanningStatus Build(LongitudinalPlanningInput input, out PathSpeedLimit speedLimit, out string failureReason)
+    {
+        if (input == null)
+        {
+            speedLimit = null;
+            failureReason = "Longitudinal planning input is required.";
+            return EmPlanningStatus.InvalidInput;
+        }
+        return BuildCore(input.Path, input.Direction, input.InitialProgressSpeedMetersPerSecond,
+            input.InitialAccelerationMetersPerSecondSquared, input.TerminalType, input.Configuration,
+            out speedLimit, out failureReason);
+    }
+
+    public EmPlanningStatus Build(LateralPath path, TravelDirection direction,
+        double initialProgressSpeedMetersPerSecond, EmTerminalType terminalType,
+        EmPlannerConfiguration configuration, out PathSpeedLimit speedLimit, out string failureReason)
+    {
+        return BuildCore(path, direction, initialProgressSpeedMetersPerSecond, 0d, terminalType, configuration,
+            out speedLimit, out failureReason);
+    }
+
+    private EmPlanningStatus BuildCore(LateralPath path, TravelDirection direction,
+        double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
+        EmTerminalType terminalType, EmPlannerConfiguration configuration, out PathSpeedLimit speedLimit,
+        out string failureReason)
    {
        speedLimit = null;
        failureReason = string.Empty;
-        if (input == null)
+        if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2 ||
+            !Enum.IsDefined(typeof(TravelDirection), direction) || !Enum.IsDefined(typeof(EmTerminalType), terminalType) ||
+            configuration == null || !IsFinite(initialProgressSpeedMetersPerSecond) ||
+            initialProgressSpeedMetersPerSecond < 0d || !IsFinite(initialAccelerationMetersPerSecondSquared))
        {
            failureReason = "Longitudinal planning input is required.";
            return EmPlanningStatus.InvalidInput;
        }

-        if (!TryGetLimits(input, out double directionMaximum, out double maximumAcceleration, out double maximumDeceleration,
-                out double maximumJerk, out double maximumLateralAcceleration, out double maximumCurvatureRate,
-                out failureReason))
+        if (configuration.Longitudinal == null)
        {
+            failureReason = "Longitudinal configuration is required.";
            return EmPlanningStatus.InvalidInput;
        }
-        if (input.InitialProgressSpeedMetersPerSecond > directionMaximum + StopDistanceToleranceMeters ||
-            input.InitialAccelerationMetersPerSecondSquared < -maximumDeceleration - StopDistanceToleranceMeters ||
-            input.InitialAccelerationMetersPerSecondSquared > maximumAcceleration + StopDistanceToleranceMeters)
+        LongitudinalConfiguration longitudinal = configuration.Longitudinal;
+        double directionMaximum = direction == TravelDirection.Forward
+            ? longitudinal.MaximumForwardSpeedMetersPerSecond
+            : longitudinal.MaximumReverseSpeedMetersPerSecond;
+        double maximumAcceleration = longitudinal.MaximumAccelerationMetersPerSecondSquared;
+        double maximumDeceleration = longitudinal.MaximumDecelerationMetersPerSecondSquared;
+        double maximumJerk = longitudinal.MaximumJerkMetersPerSecondCubed;
+        double maximumLateralAcceleration = longitudinal.MaximumLateralAccelerationMetersPerSecondSquared;
+        double maximumCurvatureRate = longitudinal.MaximumCurvatureRatePerMeterPerSecond;
+        if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(maximumAcceleration) ||
+            !IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) ||
+            !IsPositiveFinite(maximumLateralAcceleration) || !IsPositiveFinite(maximumCurvatureRate))
+        {
+            failureReason = "Longitudinal limits must be positive and finite.";
+            return EmPlanningStatus.InvalidInput;
+        }
+        if (initialProgressSpeedMetersPerSecond > directionMaximum + StopDistanceToleranceMeters ||
+            initialAccelerationMetersPerSecondSquared < -maximumDeceleration - StopDistanceToleranceMeters ||
+            initialAccelerationMetersPerSecondSquared > maximumAcceleration + StopDistanceToleranceMeters)
        {
            failureReason = "The initial longitudinal state violates the configured hard bounds.";
            return EmPlanningStatus.InvalidInput;
        }

-        if (input.HasStopBoundary)
+        bool hasStopBoundary = terminalType != EmTerminalType.RollingSafetyStop;
+        double stopBoundaryPathS = path.Points[path.Points.Count - 1].PathS;
+        if (hasStopBoundary)
        {
-            if (!JerkLimitedStoppingMath.TryCalculate(input.InitialProgressSpeedMetersPerSecond,
-                    input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk,
+            if (!JerkLimitedStoppingMath.TryCalculate(initialProgressSpeedMetersPerSecond,
+                    initialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk,
                    out JerkLimitedStoppingProfile stopProfile, out failureReason))
            {
                return EmPlanningStatus.InvalidInput;
            }
-            if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.StopBoundaryPathS)
+            if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > stopBoundaryPathS)
            {
                failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop.";
                return EmPlanningStatus.StoppingDistanceInsufficient;
            }
        }
-        if (input.Configuration.Scheduling == null || !IsPositiveFinite(input.Configuration.Scheduling.OutputTimeStepSeconds))
+        if (configuration.Scheduling == null ||
+            !IsPositiveFinite(configuration.Scheduling.MaximumOptimizationSpatialStepMeters))
        {
-            failureReason = "The output time step required to refine the PathS speed envelope is invalid.";
+            failureReason = "The optimization spatial step required to refine the PathS speed envelope is invalid.";
            return EmPlanningStatus.InvalidInput;
        }

-        double maximumStationSpacing = directionMaximum * input.Configuration.Scheduling.OutputTimeStepSeconds;
+        double maximumStationSpacing = configuration.Scheduling.MaximumOptimizationSpatialStepMeters;
        var pathS = new List<double>();
        var maximum = new List<double>();
        var lateral = new List<double>();
        var curvatureRate = new List<double>();
        var stopping = new List<double>();
-        for (int segmentIndex = 0; segmentIndex < input.Path.Points.Count - 1; segmentIndex++)
+        for (int segmentIndex = 0; segmentIndex < path.Points.Count - 1; segmentIndex++)
        {
-            LateralPathPoint lowerPoint = input.Path.Points[segmentIndex];
-            LateralPathPoint upperPoint = input.Path.Points[segmentIndex + 1];
+            LateralPathPoint lowerPoint = path.Points[segmentIndex];
+            LateralPathPoint upperPoint = path.Points[segmentIndex + 1];
            double span = upperPoint.PathS - lowerPoint.PathS;
            int subdivisions = Math.Max(1, checked((int)Math.Ceiling(span / maximumStationSpacing)));
            var segmentStations = new List<double>(subdivisions + 16);
            for (int subdivision = segmentIndex == 0 ? 0 : 1; subdivision <= subdivisions; subdivision++)
                segmentStations.Add(Interpolate(lowerPoint.PathS, upperPoint.PathS, (double)subdivision / subdivisions));
-            if (input.HasStopBoundary)
+            if (hasStopBoundary)
            {
-                AddJerkLimitedStoppingStations(lowerPoint.PathS, upperPoint.PathS, input.StopBoundaryPathS,
+                AddJerkLimitedStoppingStations(lowerPoint.PathS, upperPoint.PathS, stopBoundaryPathS,
                    directionMaximum, maximumAcceleration, maximumDeceleration, maximumJerk,
                    segmentIndex == 0, segmentStations);
            }
@@ -87,8 +133,8 @@ public sealed class PathSpeedLimitBuilder
                double curvature = Interpolate(lowerPoint.VehicleCurvature, upperPoint.VehicleCurvature, fraction);
                double curvatureDerivative = Interpolate(lowerPoint.VehicleCurvatureDerivative,
                    upperPoint.VehicleCurvatureDerivative, fraction);
-                AddLimitSample(samplePathS, curvature, curvatureDerivative, input.HasStopBoundary,
-                    input.StopBoundaryPathS, directionMaximum, maximumAcceleration, maximumDeceleration,
+                AddLimitSample(samplePathS, curvature, curvatureDerivative, hasStopBoundary,
+                    stopBoundaryPathS, directionMaximum, maximumAcceleration, maximumDeceleration,
                    maximumJerk, maximumLateralAcceleration, maximumCurvatureRate, pathS, maximum, lateral,
                    curvatureRate, stopping);
            }
@@ -97,7 +143,7 @@ public sealed class PathSpeedLimitBuilder
        try
        {
            speedLimit = new PathSpeedLimit(pathS, maximum, lateral, curvatureRate, stopping, directionMaximum,
-                input.HasStopBoundary);
+                hasStopBoundary);
            return EmPlanningStatus.Success;
        }
        catch (ArgumentException exception)