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
@@ -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)
    {