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
@@ -109,7 +109,9 @@ internal static class EmPlanningServiceChecks
            CreateReferencePath(TravelDirection.Forward, false, 0.0075d), null,
            EmPlanningScope.FullDirectionSegment);
        ConfigureExactStopServiceScenario(request.Configuration);
-        EmPlanningResult result = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
+        double[] strictFullPrimal = CreateStrictFullScopePrimal(request.Configuration);
+        EmPlanningResult result = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success, null,
+            strictFullPrimal)).Plan(
            request, CancellationToken.None);
        VerifySuccess(result, request, EmTerminalType.Goal, "full scope publication");
        Verification.Equal(EmPlanningScope.FullDirectionSegment, result.Trajectory.Metadata.PlanningScope,
@@ -127,9 +129,9 @@ internal static class EmPlanningServiceChecks
        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.2d;
    }

-    private static void ConfigureExactStopServiceScenario(EmPlannerConfiguration configuration)
-    {
-        configuration.Scheduling.TimeHorizonSeconds = 0.40d;
+        private static void ConfigureExactStopServiceScenario(EmPlannerConfiguration configuration)
+        {
+            configuration.Scheduling.TimeHorizonSeconds = 0.40d;
        configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
        configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
@@ -137,8 +139,171 @@ internal static class EmPlanningServiceChecks
        configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
        configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d;
        configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
-        configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
-    }
+            configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
+        }
+
+        private static double[] CreateStrictFullScopePrimal(EmPlannerConfiguration configuration)
+        {
+            var path = new LateralPath(new[]
+            {
+                new LateralPathPoint(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d),
+                new LateralPathPoint(0.0075d, 0.0075d, 0d, 0d, 0d, 0d, 0.0075d, 0d, 0d, 0d, 0d, 0d),
+            }, true);
+            EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.05d,
+                EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
+            Verification.Equal(EmPlanningStatus.Success, status, "full scope test envelope: " + failureReason);
+            status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.05d, 0d,
+                configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
+                out LongitudinalKnotSchedule schedule, out failureReason);
+            Verification.Equal(EmPlanningStatus.Success, status, "full scope test schedule: " + failureReason);
+            var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.05d, 0d,
+                EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+                EmPlanningScope.FullDirectionSegment, schedule, Array.Empty<double>(), Array.Empty<double>());
+            int motionIntervalCount = schedule.TerminalHoldStartIndex;
+            Verification.True(motionIntervalCount >= 3, "full scope test schedule has three motion intervals");
+            int terminalFirstInterval = motionIntervalCount - 3;
+            var terminalTimes = new double[4];
+            for (int index = 1; index < terminalTimes.Length; index++)
+                terminalTimes[index] = terminalTimes[index - 1] +
+                    schedule.KnotTimes[terminalFirstInterval + index] -
+                    schedule.KnotTimes[terminalFirstInterval + index - 1];
+            var motionTimes = new double[motionIntervalCount + 1];
+            for (int index = 0; index < motionTimes.Length; index++)
+                motionTimes[index] = schedule.KnotTimes[index];
+            var influence = new double[3, 3];
+            for (int interval = 0; interval < 3; interval++)
+            {
+                var basis = new double[3];
+                basis[interval] = 1d;
+                LongitudinalCandidate response = LongitudinalCandidate.Integrate(terminalTimes, 0d, 0d, 0d, basis);
+                int terminalIndex = response.S.Count - 1;
+                influence[0, interval] = response.A[terminalIndex];
+                influence[1, interval] = response.U[terminalIndex];
+                influence[2, interval] = response.S[terminalIndex];
+            }
+            var validator = new LongitudinalSolutionValidator();
+            for (int firstJerkStep = -20; firstJerkStep <= 0; firstJerkStep++)
+            {
+                for (int secondJerkStep = terminalFirstInterval >= 2 ? -20 : 0;
+                    secondJerkStep <= (terminalFirstInterval >= 2 ? 20 : 0); secondJerkStep++)
+                {
+                    for (int thirdJerkStep = terminalFirstInterval >= 3 ? -20 : 0;
+                        thirdJerkStep <= (terminalFirstInterval >= 3 ? 20 : 0); thirdJerkStep++)
+                    {
+                        var jerk = new double[motionIntervalCount];
+                        jerk[0] = firstJerkStep;
+                        if (terminalFirstInterval >= 2)
+                            jerk[1] = secondJerkStep;
+                        if (terminalFirstInterval >= 3)
+                            jerk[2] = thirdJerkStep;
+                        LongitudinalCandidate baseline = LongitudinalCandidate.Integrate(motionTimes, 0d, 0.05d,
+                            0d, jerk);
+                        double[] target =
+                        {
+                            -baseline.A[baseline.A.Count - 1],
+                            -baseline.U[baseline.U.Count - 1],
+                            0.0075d - baseline.S[baseline.S.Count - 1],
+                        };
+                        if (!TrySolveThreeByThree(influence, target, out double[] terminalJerk))
+                            throw new InvalidOperationException("Full scope strict candidate terminal system is singular.");
+                        for (int interval = 0; interval < 3; interval++)
+                            jerk[terminalFirstInterval + interval] = terminalJerk[interval];
+                        LongitudinalCandidate motion = LongitudinalCandidate.Integrate(motionTimes, 0d, 0.05d, 0d,
+                            jerk);
+                        LongitudinalCandidate candidate = AppendFullStopTail(schedule.KnotTimes, motionIntervalCount,
+                            motion);
+                        if (!validator.TryValidate(input, speedLimit, candidate, out LongitudinalCandidate strict, out _))
+                            continue;
+                        return ToPrimal(strict);
+                    }
+                }
+            }
+            throw new InvalidOperationException("Unable to construct a strict full-scope test candidate: hold=" +
+                motionIntervalCount + ";times=" + string.Join(",", schedule.KnotTimes));
+        }
+
+        private static LongitudinalCandidate AppendFullStopTail(IReadOnlyList<double> times, int motionIntervalCount,
+            LongitudinalCandidate motion)
+        {
+            var pathS = new double[times.Count];
+            var speed = new double[times.Count];
+            var acceleration = new double[times.Count];
+            var jerk = new double[times.Count - 1];
+            for (int index = 0; index <= motionIntervalCount; index++)
+            {
+                pathS[index] = index == motionIntervalCount ? 0.0075d : motion.S[index];
+                speed[index] = index == motionIntervalCount ? 0d : motion.U[index];
+                acceleration[index] = index == motionIntervalCount ? 0d : motion.A[index];
+            }
+            for (int index = motionIntervalCount + 1; index < times.Count; index++)
+                pathS[index] = 0.0075d;
+            for (int index = 0; index < motion.J.Count; index++)
+                jerk[index] = motion.J[index];
+            return new LongitudinalCandidate(times, pathS, speed, acceleration, jerk);
+        }
+
+        private static double[] ToPrimal(LongitudinalCandidate candidate)
+        {
+            var layout = new LongitudinalVariableLayout(candidate.S.Count);
+            var primal = new double[layout.VariableCount];
+            for (int index = 0; index < candidate.S.Count; index++)
+            {
+                primal[layout.S(index)] = candidate.S[index];
+                primal[layout.U(index)] = candidate.U[index];
+                primal[layout.A(index)] = candidate.A[index];
+            }
+            for (int index = 0; index < candidate.J.Count; index++)
+                primal[layout.J(index)] = candidate.J[index];
+            return primal;
+        }
+
+        private static bool TrySolveThreeByThree(double[,] matrix, IReadOnlyList<double> rightHandSide,
+            out double[] solution)
+        {
+            var augmented = new double[3, 4];
+            for (int row = 0; row < 3; row++)
+            {
+                for (int column = 0; column < 3; column++)
+                    augmented[row, column] = matrix[row, column];
+                augmented[row, 3] = rightHandSide[row];
+            }
+            for (int column = 0; column < 3; column++)
+            {
+                int pivot = column;
+                for (int row = column + 1; row < 3; row++)
+                {
+                    if (Math.Abs(augmented[row, column]) > Math.Abs(augmented[pivot, column]))
+                        pivot = row;
+                }
+                if (Math.Abs(augmented[pivot, column]) < 1e-12d)
+                {
+                    solution = Array.Empty<double>();
+                    return false;
+                }
+                if (pivot != column)
+                {
+                    for (int index = column; index < 4; index++)
+                    {
+                        double temporary = augmented[column, index];
+                        augmented[column, index] = augmented[pivot, index];
+                        augmented[pivot, index] = temporary;
+                    }
+                }
+                double divisor = augmented[column, column];
+                for (int index = column; index < 4; index++)
+                    augmented[column, index] /= divisor;
+                for (int row = 0; row < 3; row++)
+                {
+                    if (row == column)
+                        continue;
+                    double factor = augmented[row, column];
+                    for (int index = column; index < 4; index++)
+                        augmented[row, index] -= factor * augmented[column, index];
+                }
+            }
+            solution = new[] { augmented[0, 3], augmented[1, 3], augmented[2, 3] };
+            return true;
+        }

    private static void AssertExactStopStabilization(EmTrajectory trajectory, EmBoundaryType boundaryType, string name)
    {
@@ -452,20 +617,23 @@ internal static class EmPlanningServiceChecks
    {
        private readonly PipelineSolverMode mode;
        private readonly PlanningGridMap? mapToCorrupt;
+        private readonly IReadOnlyList<double>? strictFullPrimal;
        private int longitudinalCallCount;

        public QuadraticProgram? LastLongitudinalProblem { get; private set; }

-        public ScriptedPipelineSolver(PipelineSolverMode mode, PlanningGridMap? mapToCorrupt = null)
+        public ScriptedPipelineSolver(PipelineSolverMode mode, PlanningGridMap? mapToCorrupt = null,
+            IReadOnlyList<double>? strictFullPrimal = null)
        {
            this.mode = mode;
            this.mapToCorrupt = mapToCorrupt;
+            this.strictFullPrimal = strictFullPrimal;
        }

        public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
            CancellationToken cancellationToken)
        {
-            bool longitudinal = problem.VariableCount > 100;
+            bool longitudinal = IsLongitudinalProblem(problem);
            if (mode == PipelineSolverMode.SolverUnavailable)
                return Result(QpSolveStatus.SolverUnavailable, Array.Empty<double>());
            if (!longitudinal)
@@ -479,12 +647,18 @@ internal static class EmPlanningServiceChecks
            LastLongitudinalProblem = problem;
            if (mode == PipelineSolverMode.LongitudinalInfeasible)
                return Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>());
+            if (strictFullPrimal != null && strictFullPrimal.Count == problem.VariableCount)
+            {
+                longitudinalCallCount++;
+                return Result(QpSolveStatus.Solved, strictFullPrimal);
+            }
            if (mode == PipelineSolverMode.PublicationValidationFailure && longitudinalCallCount == 0)
                CorruptMapAtOrigin(mapToCorrupt);
            if (mode == PipelineSolverMode.TimeoutWithFallback && ++longitudinalCallCount > 1)
                return Result(QpSolveStatus.TimeLimit, Array.Empty<double>());
            longitudinalCallCount++;
-            return Result(QpSolveStatus.Solved, warmStart);
+            return Result(QpSolveStatus.Solved,
+                TryCreateStrictExactStopPrimal(problem, out double[] strictPrimal) ? strictPrimal : warmStart);
        }

        private static void CorruptMapAtOrigin(PlanningGridMap? map)
@@ -502,41 +676,234 @@ internal static class EmPlanningServiceChecks
            distances[index] = 0d;
        }

-        private static double[] CreateStrictLongitudinalPrimal(QuadraticProgram problem)
+        private static bool TryCreateStrictExactStopPrimal(QuadraticProgram problem, out double[] primal)
        {
+            primal = Array.Empty<double>();
            int variableCount = problem.VariableCount;
            int knotCount = (variableCount + 1) / 4;
            var layout = new LongitudinalVariableLayout(knotCount);
-            var jerk = new double[knotCount - 1];
-            const int rampIntervals = 5;
-            for (int index = 0; index < rampIntervals; index++) jerk[index] = 1d;
-            for (int index = rampIntervals; index < 3 * rampIntervals; index++) jerk[index] = -1d;
-            for (int index = 3 * rampIntervals; index < 4 * rampIntervals; index++) jerk[index] = 1d;
+            int stabilizationStart = FindExactStopTailStart(problem, layout);
+            if (stabilizationStart < 3)
+                return false;
            var times = new double[knotCount];
-            for (int index = 0; index < times.Length; index++) times[index] = index * 0.05d;
-            LongitudinalCandidate baseCandidate = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d, jerk);
-            double terminalPathS = ReadFixedVariable(problem, layout.S(knotCount - 1));
-            double scale = terminalPathS / baseCandidate.S[baseCandidate.S.Count - 1];
-            for (int index = 0; index < jerk.Length; index++) jerk[index] *= scale;
-            LongitudinalCandidate candidate = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d, jerk);
+            for (int index = 0; index < knotCount - 1; index++)
+            {
+                if (!TryReadDynamicsDuration(problem, layout, index, out double duration))
+                    return false;
+                times[index + 1] = times[index] + duration;
+            }
+
+            var motionTimes = new double[stabilizationStart + 1];
+            Array.Copy(times, motionTimes, motionTimes.Length);
+            double initialPathS = ReadFixedVariable(problem, layout.S(0));
+            double initialSpeed = ReadFixedVariable(problem, layout.U(0));
+            double initialAcceleration = ReadFixedVariable(problem, layout.A(0));
+            double terminalPathS = ReadFixedVariable(problem, layout.S(stabilizationStart));
+            var preferredJerk = new double[stabilizationStart];
+            LongitudinalCandidate baseline = LongitudinalCandidate.Integrate(motionTimes, initialPathS, initialSpeed,
+                initialAcceleration, preferredJerk);
+            var influence = new double[3, stabilizationStart];
+            for (int interval = 0; interval < stabilizationStart; interval++)
+            {
+                var basis = new double[stabilizationStart];
+                basis[interval] = 1d;
+                LongitudinalCandidate response = LongitudinalCandidate.Integrate(motionTimes, 0d, 0d, 0d, basis);
+                int terminalIndex = response.S.Count - 1;
+                influence[0, interval] = response.A[terminalIndex];
+                influence[1, interval] = response.U[terminalIndex];
+                influence[2, interval] = response.S[terminalIndex];
+            }
+            double[] target =
+            {
+                -baseline.A[baseline.A.Count - 1],
+                -baseline.U[baseline.U.Count - 1],
+                terminalPathS - baseline.S[baseline.S.Count - 1],
+            };
+            var jerk = new double[knotCount - 1];
+            if (stabilizationStart >= 4)
+            {
+                int terminalFirstInterval = stabilizationStart - 3;
+                var terminalInfluence = new double[3, 3];
+                for (int row = 0; row < 3; row++)
+                {
+                    for (int column = 0; column < 3; column++)
+                        terminalInfluence[row, column] = influence[row, terminalFirstInterval + column];
+                }
+                if (!TrySolveThreeByThree(terminalInfluence, target, out double[] terminalJerk))
+                    return false;
+                for (int interval = 0; interval < 3; interval++)
+                    jerk[terminalFirstInterval + interval] = terminalJerk[interval];
+            }
+            else
+            {
+                var gram = new double[3, 3];
+                for (int row = 0; row < 3; row++)
+                {
+                    for (int column = 0; column < 3; column++)
+                    {
+                        for (int interval = 0; interval < stabilizationStart; interval++)
+                            gram[row, column] += influence[row, interval] * influence[column, interval];
+                    }
+                }
+                if (!TrySolveThreeByThree(gram, target, out double[] multipliers))
+                    return false;
+                for (int interval = 0; interval < stabilizationStart; interval++)
+                {
+                    jerk[interval] = preferredJerk[interval];
+                    for (int row = 0; row < 3; row++)
+                        jerk[interval] += influence[row, interval] * multipliers[row];
+                }
+            }
+            var motionJerk = new double[stabilizationStart];
+            Array.Copy(jerk, motionJerk, motionJerk.Length);
+            LongitudinalCandidate candidate = LongitudinalCandidate.Integrate(motionTimes, initialPathS, initialSpeed,
+                initialAcceleration, motionJerk);
+            primal = CreateExactStopPrimal(layout, knotCount, stabilizationStart, terminalPathS, candidate);
+            return true;
+        }
+
+        private static double[] CreateExactStopPrimal(LongitudinalVariableLayout layout, int knotCount,
+            int stabilizationStart, double terminalPathS, LongitudinalCandidate candidate)
+        {
            var primal = new double[layout.VariableCount];
            for (int index = 0; index < knotCount; index++)
            {
-                primal[layout.S(index)] = candidate.S[index];
-                primal[layout.U(index)] = candidate.U[index];
-                primal[layout.A(index)] = candidate.A[index];
-            }
-            for (int index = 0; index < jerk.Length; index++) primal[layout.J(index)] = candidate.J[index];
-            for (int index = 4 * rampIntervals; index < knotCount; index++)
-            {
-                primal[layout.S(index)] = terminalPathS;
-                primal[layout.U(index)] = 0d;
-                primal[layout.A(index)] = 0d;
+                bool isTerminalTail = index >= stabilizationStart;
+                primal[layout.S(index)] = isTerminalTail ? terminalPathS : candidate.S[index];
+                primal[layout.U(index)] = isTerminalTail ? 0d : candidate.U[index];
+                primal[layout.A(index)] = isTerminalTail ? 0d : candidate.A[index];
            }
+            for (int index = 0; index < candidate.J.Count; index++)
+                primal[layout.J(index)] = candidate.J[index];
            return primal;
        }

+        private static int FindExactStopTailStart(QuadraticProgram problem, LongitudinalVariableLayout layout)
+        {
+            for (int index = 1; index < layout.KnotCount; index++)
+            {
+                if (TryReadFixedVariable(problem, layout.S(index), out _) &&
+                    TryReadFixedVariable(problem, layout.U(index), out _) &&
+                    TryReadFixedVariable(problem, layout.A(index), out _))
+                {
+                    return index;
+                }
+            }
+            return -1;
+        }
+
+        private static bool TryReadDynamicsDuration(QuadraticProgram problem, LongitudinalVariableLayout layout,
+            int interval, out double duration)
+        {
+            duration = 0d;
+            for (int row = 0; row < problem.ConstraintCount; row++)
+            {
+                if (Math.Abs(problem.LowerBounds[row]) > 1e-12d || Math.Abs(problem.UpperBounds[row]) > 1e-12d ||
+                    CountRowEntries(problem, row) != 3 ||
+                    Math.Abs(ReadCoefficient(problem, row, layout.A(interval + 1)) - 1d) > 1e-12d ||
+                    Math.Abs(ReadCoefficient(problem, row, layout.A(interval)) + 1d) > 1e-12d)
+                {
+                    continue;
+                }
+                double jerkCoefficient = ReadCoefficient(problem, row, layout.J(interval));
+                if (jerkCoefficient >= -1e-12d)
+                    continue;
+                duration = -jerkCoefficient;
+                return true;
+            }
+            return false;
+        }
+
+        private static int CountRowEntries(QuadraticProgram problem, int row)
+        {
+            int count = 0;
+            for (int column = 0; column < problem.ConstraintMatrix.ColumnCount; column++)
+            {
+                for (int index = problem.ConstraintMatrix.ColumnPointers[column];
+                    index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
+                {
+                    if (problem.ConstraintMatrix.RowIndices[index] == row)
+                        count++;
+                }
+            }
+            return count;
+        }
+
+        private static double ReadCoefficient(QuadraticProgram problem, int row, int column)
+        {
+            for (int index = problem.ConstraintMatrix.ColumnPointers[column];
+                index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
+            {
+                if (problem.ConstraintMatrix.RowIndices[index] == row)
+                    return problem.ConstraintMatrix.Values[index];
+            }
+            return 0d;
+        }
+
+        private static bool TrySolveThreeByThree(double[,] matrix, IReadOnlyList<double> rightHandSide,
+            out double[] solution)
+        {
+            var augmented = new double[3, 4];
+            for (int row = 0; row < 3; row++)
+            {
+                for (int column = 0; column < 3; column++)
+                    augmented[row, column] = matrix[row, column];
+                augmented[row, 3] = rightHandSide[row];
+            }
+            for (int column = 0; column < 3; column++)
+            {
+                int pivot = column;
+                for (int row = column + 1; row < 3; row++)
+                {
+                    if (Math.Abs(augmented[row, column]) > Math.Abs(augmented[pivot, column]))
+                        pivot = row;
+                }
+                if (Math.Abs(augmented[pivot, column]) < 1e-12d)
+                {
+                    solution = Array.Empty<double>();
+                    return false;
+                }
+                if (pivot != column)
+                {
+                    for (int index = column; index < 4; index++)
+                    {
+                        double temporary = augmented[column, index];
+                        augmented[column, index] = augmented[pivot, index];
+                        augmented[pivot, index] = temporary;
+                    }
+                }
+                double divisor = augmented[column, column];
+                for (int index = column; index < 4; index++)
+                    augmented[column, index] /= divisor;
+                for (int row = 0; row < 3; row++)
+                {
+                    if (row == column)
+                        continue;
+                    double factor = augmented[row, column];
+                    for (int index = column; index < 4; index++)
+                        augmented[row, index] -= factor * augmented[column, index];
+                }
+            }
+            solution = new[] { augmented[0, 3], augmented[1, 3], augmented[2, 3] };
+            return true;
+        }
+
+        private static bool IsLongitudinalProblem(QuadraticProgram problem)
+        {
+            if (problem.VariableCount < 7 || (problem.VariableCount + 1) % 4 != 0)
+                return false;
+            int knotCount = (problem.VariableCount + 1) / 4;
+            return problem.ConstraintCount >= 8 * knotCount - 2;
+        }
+
        private static double ReadFixedVariable(QuadraticProgram problem, int variable)
+        {
+            if (TryReadFixedVariable(problem, variable, out double value))
+                return value;
+            throw new InvalidOperationException("Expected a fixed ST variable constraint.");
+        }
+
+        private static bool TryReadFixedVariable(QuadraticProgram problem, int variable, out double value)
        {
            for (int row = 0; row < problem.ConstraintCount; row++)
            {
@@ -557,10 +924,12 @@ internal static class EmPlanningServiceChecks
                if (entryCount == 1 && Math.Abs(coefficient) > 1e-12d &&
                    Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) <= 1e-12d)
                {
-                    return problem.LowerBounds[row] / coefficient;
+                    value = problem.LowerBounds[row] / coefficient;
+                    return true;
                }
            }
-            throw new InvalidOperationException("Expected a fixed ST variable constraint.");
+            value = 0d;
+            return false;
        }

        private static QpSolveResult Result(QpSolveStatus status, IReadOnlyList<double> primal)