fix: accelerate EM trajectories from rest

2026-08-07 07:51:43 +08:00
parent 162f1a24e6
commit 0bba8d7e61
9 changed files with 273 additions and 20 deletions
@@ -84,7 +84,7 @@ public sealed class EmPlanningService : IEmPlanningService
            EmitDebug(request, "LS optimization and validation succeeded");

            EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(lateral.Path, segment.Direction,
-                initialProgressSpeed, horizon.TerminalType, configuration, out PathSpeedLimit speedLimit,
+                initialProgressSpeed, initialAcceleration, horizon.TerminalType, configuration, out PathSpeedLimit speedLimit,
                out string envelopeReason);
            if (envelopeStatus != EmPlanningStatus.Success)
                return Failure(envelopeStatus, request, envelopeReason);
@@ -167,9 +167,12 @@ public sealed class FullDirectionSegmentScheduleBuilder
        LongitudinalConfiguration configuration)
    {
        double adjustedTime = 0d;
+        double requestedPreviousTime = times[0];
        for (int index = 1; index < times.Count; index++)
        {
-            double requestedDuration = times[index] - times[index - 1];
+            double requestedTime = times[index];
+            double requestedDuration = requestedTime - requestedPreviousTime;
+            requestedPreviousTime = requestedTime;
            double speedChange = Math.Abs(referenceSpeeds[index] - referenceSpeeds[index - 1]);
            double accelerationLimit = referenceSpeeds[index] >= referenceSpeeds[index - 1]
                ? configuration.MaximumAccelerationMetersPerSecondSquared
@@ -191,7 +194,10 @@ public sealed class FullDirectionSegmentScheduleBuilder
        {
            double previousSlope = (speeds[index] - speeds[index - 1]) / (pathS[index] - pathS[index - 1]);
            double nextSlope = (speeds[index + 1] - speeds[index]) / (pathS[index + 1] - pathS[index]);
-            if (previousSlope * nextSlope < 0d)
+            bool changesDirection = previousSlope * nextSlope < 0d;
+            bool entersCruise = previousSlope > Tolerance && nextSlope <= Tolerance;
+            bool leavesCruise = previousSlope >= -Tolerance && nextSlope < -Tolerance;
+            if (changesDirection || entersCruise || leavesCruise)
                stations.Add(index);
        }
        stations.Add(pathS.Count - 1);
@@ -26,7 +26,10 @@ public sealed class LongitudinalObjectiveBuilder
        for (int index = 0; index < layout.KnotCount; index++)
        {
            double iteratePathS = Math.Max(0d, Math.Min(input.PathUpperBoundS, iterate.S[index]));
-            AddSquaredResidual(hessian, linearCost, layout.U(index), speedLimit.MaximumSpeedAt(iteratePathS),
+            double referenceSpeed = input.PlanningScope == EmPlanningScope.FullDirectionSegment
+                ? input.KnotSchedule.ReferenceSpeedMetersPerSecond[index]
+                : speedLimit.MaximumSpeedAt(iteratePathS);
+            AddSquaredResidual(hessian, linearCost, layout.U(index), referenceSpeed,
                weights.ReferenceSpeed, speedScale);
            AddSquaredResidual(hessian, linearCost, layout.A(index), 0d, weights.Acceleration, accelerationScale);
            if (index < layout.KnotCount - 1 && index < input.PreviousPathS.Count)
@@ -8,8 +8,15 @@ public sealed class LongitudinalSolutionValidator
 {
    public bool TryValidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate candidate,
        out LongitudinalCandidate validatedCandidate, out string failureReason)
+    {
+        return TryValidate(input, speedLimit, candidate, out validatedCandidate, out _, out failureReason);
+    }
+
+    public bool TryValidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate candidate,
+        out LongitudinalCandidate validatedCandidate, out EmPlanningStatus failureStatus, out string failureReason)
    {
        validatedCandidate = null;
+        failureStatus = EmPlanningStatus.LongitudinalInfeasible;
        failureReason = string.Empty;
        if (input == null || speedLimit == null || candidate == null)
        {
@@ -58,7 +65,8 @@ public sealed class LongitudinalSolutionValidator
                    progress > input.PathUpperBoundS + tolerance || speed < -tolerance ||
                    acceleration < -maximumDeceleration - tolerance || acceleration > maximumAcceleration + tolerance)
                {
-                    failureReason = "ST candidate violates physical bounds at knot " + index + ".";
+                    failureReason = "ST candidate violates physical bounds at knot " + index +
+                        " (S=" + progress + ", U=" + speed + ", A=" + acceleration + ").";
                    return false;
                }
                double speedLimitAtProgress = index == 0
@@ -84,6 +92,16 @@ public sealed class LongitudinalSolutionValidator
                    return false;
                }
            }
+            bool requiresProgress = input.PathUpperBoundS >
+                input.Configuration.Validation.TerminalPositionToleranceMeters;
+            double achievedProgress = candidate.S[candidate.S.Count - 1] - candidate.S[0];
+            if (input.PlanningScope == EmPlanningScope.FullDirectionSegment && requiresProgress &&
+                achievedProgress <= input.Configuration.Validation.SpatialToleranceMeters)
+            {
+                failureStatus = EmPlanningStatus.NoProgress;
+                failureReason = "NoProgress: a nonterminal full direction segment produced zero progress.";
+                return false;
+            }
            int stabilizationStart = candidate.S.Count;
            if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
            {
@@ -32,6 +32,15 @@ public sealed class PathSpeedLimitBuilder
            out speedLimit, out failureReason);
    }

+    public EmPlanningStatus Build(LateralPath path, TravelDirection direction,
+        double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
+        EmTerminalType terminalType, EmPlannerConfiguration configuration, out PathSpeedLimit speedLimit,
+        out string failureReason)
+    {
+        return BuildCore(path, direction, initialProgressSpeedMetersPerSecond,
+            initialAccelerationMetersPerSecondSquared, terminalType, configuration, out speedLimit, out failureReason);
+    }
+
    private EmPlanningStatus BuildCore(LateralPath path, TravelDirection direction,
        double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
        EmTerminalType terminalType, EmPlannerConfiguration configuration, out PathSpeedLimit speedLimit,
@@ -62,6 +71,7 @@ public sealed class PathSpeedLimitBuilder
        double maximumJerk = longitudinal.MaximumJerkMetersPerSecondCubed;
        double maximumLateralAcceleration = longitudinal.MaximumLateralAccelerationMetersPerSecondSquared;
        double maximumCurvatureRate = longitudinal.MaximumCurvatureRatePerMeterPerSecond;
+        double stoppingAcceleration = Math.Max(0d, initialAccelerationMetersPerSecondSquared);
        if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(maximumAcceleration) ||
            !IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) ||
            !IsPositiveFinite(maximumLateralAcceleration) || !IsPositiveFinite(maximumCurvatureRate))
@@ -118,7 +128,7 @@ public sealed class PathSpeedLimitBuilder
            if (hasStopBoundary)
            {
                AddJerkLimitedStoppingStations(lowerPoint.PathS, upperPoint.PathS, stopBoundaryPathS,
-                    directionMaximum, maximumAcceleration, maximumDeceleration, maximumJerk,
+                    directionMaximum, stoppingAcceleration, maximumDeceleration, maximumJerk,
                    segmentIndex == 0, segmentStations);
            }
            segmentStations.Sort();
@@ -134,7 +144,7 @@ public sealed class PathSpeedLimitBuilder
                double curvatureDerivative = Interpolate(lowerPoint.VehicleCurvatureDerivative,
                    upperPoint.VehicleCurvatureDerivative, fraction);
                AddLimitSample(samplePathS, curvature, curvatureDerivative, hasStopBoundary,
-                    stopBoundaryPathS, directionMaximum, maximumAcceleration, maximumDeceleration,
+                    stopBoundaryPathS, directionMaximum, stoppingAcceleration, maximumDeceleration,
                    maximumJerk, maximumLateralAcceleration, maximumCurvatureRate, pathS, maximum, lateral,
                    curvatureRate, stopping);
            }
@@ -188,14 +198,14 @@ public sealed class PathSpeedLimitBuilder
    }

    private static void AddJerkLimitedStoppingStations(double lowerPathS, double upperPathS,
-        double stopBoundaryPathS, double directionMaximum, double maximumAcceleration, double maximumDeceleration,
+        double stopBoundaryPathS, double directionMaximum, double stoppingAcceleration, double maximumDeceleration,
        double maximumJerk, bool includeLower, IList<double> stations)
    {
        const int stoppingSpeedSampleCount = 64;
        for (int step = 0; step < stoppingSpeedSampleCount; step++)
        {
            double speed = directionMaximum * step / stoppingSpeedSampleCount;
-            if (!JerkLimitedStoppingMath.TryCalculate(speed, maximumAcceleration,
+            if (!JerkLimitedStoppingMath.TryCalculate(speed, stoppingAcceleration,
                    maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop, out _))
            {
                throw new ArgumentException("The configured jerk-limited stop envelope cannot be sampled.");
@@ -210,7 +220,7 @@ public sealed class PathSpeedLimitBuilder
    }

    private static void AddLimitSample(double samplePathS, double curvature, double curvatureDerivative,
-        bool hasStopBoundary, double stopBoundaryPathS, double directionMaximum, double maximumAcceleration,
+        bool hasStopBoundary, double stopBoundaryPathS, double directionMaximum, double stoppingAcceleration,
        double maximumDeceleration, double maximumJerk, double maximumLateralAcceleration,
        double maximumCurvatureRate, IList<double> pathS, IList<double> maximum, IList<double> lateral,
        IList<double> curvatureRate, IList<double> stopping)
@@ -220,7 +230,7 @@ public sealed class PathSpeedLimitBuilder
        double curvatureRateLimit = maximumCurvatureRate / Math.Max(Math.Abs(curvatureDerivative), CurvatureEpsilon);
        double stoppingLimit = hasStopBoundary
            ? JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
-                Math.Max(0d, stopBoundaryPathS - samplePathS), maximumAcceleration,
+                Math.Max(0d, stopBoundaryPathS - samplePathS), stoppingAcceleration,
                maximumDeceleration, maximumJerk, directionMaximum)
            : directionMaximum;
        double lateralValue = ClampFinite(lateralLimit, directionMaximum);
@@ -2,6 +2,7 @@ using System;
 using System.Collections.Generic;
 using System.Diagnostics;
 using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;

 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

@@ -222,6 +223,14 @@ public sealed class SequentialLongitudinalOptimizer
        projectionSolveCount = 0;
        failureStatus = EmPlanningStatus.LongitudinalInfeasible;
        failureReason = string.Empty;
+        if (input.InitialProgressSpeedMetersPerSecond <= input.Configuration.Validation.SpatialToleranceMeters &&
+            Math.Abs(input.InitialAccelerationMetersPerSecondSquared) <=
+                input.Configuration.Validation.KinematicTolerance &&
+            TryCreateStaticStartSeed(input, speedLimit, out LongitudinalCandidate staticStartSeed))
+        {
+            candidate = staticStartSeed;
+            return true;
+        }
        LongitudinalCandidate linearizationIterate = CreateScheduleReferenceIterate(input);
        string lastRejection = string.Empty;
        for (int iteration = 0; iteration < iterationLimit; iteration++)
@@ -302,11 +311,17 @@ public sealed class SequentialLongitudinalOptimizer
            if (solved.Status == QpSolveStatus.Solved || HasStrictResiduals(solved, convergenceTolerance))
            {
                if (_solutionValidator.TryValidate(input, speedLimit, projected, out LongitudinalCandidate strict,
-                        out string validationFailure))
+                        out EmPlanningStatus validationStatus, out string validationFailure))
                {
                    candidate = strict;
                    return true;
                }
+                if (validationStatus == EmPlanningStatus.NoProgress)
+                {
+                    failureStatus = validationStatus;
+                    failureReason = validationFailure;
+                    return false;
+                }
                lastRejection = validationFailure;
            }

@@ -377,6 +392,14 @@ public sealed class SequentialLongitudinalOptimizer
            double dt = times[index + 1] - times[index];
            double speedLimitAtS = speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, s)));
            double targetSpeed = Math.Min(input.InitialProgressSpeedMetersPerSecond, speedLimitAtS);
+            if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
+            {
+                double desiredSpeed = input.Direction == TravelDirection.Forward
+                    ? configuration.DesiredForwardSpeedMetersPerSecond
+                    : configuration.DesiredReverseSpeedMetersPerSecond;
+                double scheduleSpeed = input.KnotSchedule.ReferenceSpeedMetersPerSecond[index];
+                targetSpeed = Math.Min(desiredSpeed, Math.Min(scheduleSpeed, speedLimitAtS));
+            }
            double lowerJerk = Math.Max(-configuration.MaximumJerkMetersPerSecondCubed,
                (-configuration.MaximumDecelerationMetersPerSecondSquared - a) / dt);
            lowerJerk = Math.Max(lowerJerk, -2d * (u + a * dt) / (dt * dt));
@@ -442,6 +465,44 @@ public sealed class SequentialLongitudinalOptimizer
        return CreateApproachSeed(input, times, speedLimit);
    }

+    private bool TryCreateStaticStartSeed(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
+        out LongitudinalCandidate candidate)
+    {
+        candidate = null;
+        int stabilizationStart = input.KnotSchedule.TerminalHoldStartIndex;
+        if (stabilizationStart < 5)
+            return false;
+
+        IReadOnlyList<double> times = input.KnotSchedule.KnotTimes;
+        if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, out candidate))
+            return true;
+        double firstDuration = times[1] - times[0];
+        double secondDuration = times[2] - times[1];
+        LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
+        double maximumFirstJerk = Math.Min(configuration.MaximumJerkMetersPerSecondCubed,
+            Math.Min(configuration.MaximumAccelerationMetersPerSecondSquared / firstDuration,
+                configuration.MaximumJerkMetersPerSecondCubed * secondDuration / firstDuration));
+        for (int sample = 1; sample <= 256; sample++)
+        {
+            double firstJerk = maximumFirstJerk * sample / 256d;
+            var jerk = new double[stabilizationStart];
+            jerk[0] = firstJerk;
+            jerk[1] = -firstJerk * firstDuration / secondDuration;
+            if (!TryCloseExactStopEndpoint(input, times, stabilizationStart, jerk,
+                    out LongitudinalCandidate probe))
+            {
+                continue;
+            }
+            if (_solutionValidator.TryValidate(input, speedLimit, probe,
+                    out LongitudinalCandidate strict, out _))
+            {
+                candidate = strict;
+                return true;
+            }
+        }
+        return false;
+    }
+
    private static bool TryCreateCruiseThenBrakeSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
        double stopBoundaryPathS, out LongitudinalCandidate candidate)
    {
@@ -609,6 +670,40 @@ public sealed class SequentialLongitudinalOptimizer
        return AppendExactStopTail(times, stabilizationStart, input.StopBoundaryPathS, motion);
    }

+    private static bool TryCloseExactStopEndpoint(LongitudinalPlanningInput input, IReadOnlyList<double> times,
+        int stabilizationStart, double[] jerk, out LongitudinalCandidate candidate)
+    {
+        candidate = null;
+        var motionTimes = new double[stabilizationStart + 1];
+        for (int index = 0; index < motionTimes.Length; index++)
+            motionTimes[index] = times[index];
+        LongitudinalCandidate motion = LongitudinalCandidate.Integrate(motionTimes, 0d,
+            input.InitialProgressSpeedMetersPerSecond, input.InitialAccelerationMetersPerSecondSquared, jerk);
+        int terminalIndex = motion.S.Count - 1;
+        double[] correction =
+        {
+            -motion.A[terminalIndex],
+            -motion.U[terminalIndex],
+            input.StopBoundaryPathS - motion.S[terminalIndex],
+        };
+        var influence = new double[3, 3];
+        for (int basisIndex = 0; basisIndex < 3; basisIndex++)
+        {
+            var basis = new double[stabilizationStart];
+            basis[stabilizationStart - 3 + basisIndex] = 1d;
+            LongitudinalCandidate response = LongitudinalCandidate.Integrate(motionTimes, 0d, 0d, 0d, basis);
+            influence[0, basisIndex] = response.A[terminalIndex];
+            influence[1, basisIndex] = response.U[terminalIndex];
+            influence[2, basisIndex] = response.S[terminalIndex];
+        }
+        if (!TrySolveThreeByThree(influence, correction, out double[] adjustment))
+            return false;
+        for (int index = 0; index < adjustment.Length; index++)
+            jerk[stabilizationStart - 3 + index] += adjustment[index];
+        candidate = CreateExactCandidate(input, times, stabilizationStart, jerk);
+        return true;
+    }
+
    private static LongitudinalCandidate CreateScheduleReferenceSeed(LongitudinalPlanningInput input,
        IReadOnlyList<double> times, PathSpeedLimit speedLimit)
    {