feat: add lateral optimization model

2026-08-04 00:50:10 +08:00
parent 113d0c9d9b
commit c225b17d37
8 changed files with 625 additions and 2 deletions
@@ -0,0 +1,114 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>One discrete lateral iterate expressed against exact reference-S stations.</summary>
+public sealed class LateralCandidate
+{
+    public LateralCandidate(IReadOnlyList<double> referenceStations, IReadOnlyList<double> l, IReadOnlyList<double> dl,
+        IReadOnlyList<double> ddl, IReadOnlyList<double> dddl)
+    {
+        ReferenceStations = CopyStations(referenceStations);
+        int stationCount = ReferenceStations.Count;
+        L = CopyValues(l, stationCount, nameof(l));
+        DL = CopyValues(dl, stationCount, nameof(dl));
+        DDL = CopyValues(ddl, stationCount, nameof(ddl));
+        DDDL = CopyValues(dddl, stationCount - 1, nameof(dddl));
+    }
+
+    public IReadOnlyList<double> ReferenceStations { get; }
+
+    public IReadOnlyList<double> L { get; }
+
+    public IReadOnlyList<double> DL { get; }
+
+    public IReadOnlyList<double> DDL { get; }
+
+    public IReadOnlyList<double> DDDL { get; }
+
+    public static LateralCandidate Integrate(IReadOnlyList<double> referenceStations, double initialL, double initialDL,
+        double initialDDL, IReadOnlyList<double> dddl)
+    {
+        IReadOnlyList<double> stations = CopyStations(referenceStations);
+        if (!IsFinite(initialL) || !IsFinite(initialDL) || !IsFinite(initialDDL))
+            throw new ArgumentOutOfRangeException(nameof(initialL));
+        IReadOnlyList<double> copiedJerk = CopyValues(dddl, stations.Count - 1, nameof(dddl));
+
+        var l = new double[stations.Count];
+        var dl = new double[stations.Count];
+        var ddl = new double[stations.Count];
+        l[0] = initialL;
+        dl[0] = initialDL;
+        ddl[0] = initialDDL;
+        for (int index = 0; index < copiedJerk.Count; index++)
+        {
+            double ds = stations[index + 1] - stations[index];
+            double jerk = copiedJerk[index];
+            ddl[index + 1] = ddl[index] + ds * jerk;
+            dl[index + 1] = dl[index] + ds * ddl[index] + 0.5d * ds * ds * jerk;
+            l[index + 1] = l[index] + ds * dl[index] + 0.5d * ds * ds * ddl[index] +
+                ds * ds * ds * jerk / 6d;
+        }
+        return new LateralCandidate(stations, l, dl, ddl, copiedJerk);
+    }
+
+    public bool SatisfiesExactDiscreteDynamics(double tolerance)
+    {
+        if (!IsFinite(tolerance) || tolerance < 0d)
+            throw new ArgumentOutOfRangeException(nameof(tolerance));
+
+        for (int index = 0; index < DDDL.Count; index++)
+        {
+            double ds = ReferenceStations[index + 1] - ReferenceStations[index];
+            double jerk = DDDL[index];
+            if (Math.Abs(DDL[index + 1] - (DDL[index] + ds * jerk)) > tolerance ||
+                Math.Abs(DL[index + 1] - (DL[index] + ds * DDL[index] + 0.5d * ds * ds * jerk)) > tolerance ||
+                Math.Abs(L[index + 1] - (L[index] + ds * DL[index] + 0.5d * ds * ds * DDL[index] +
+                    ds * ds * ds * jerk / 6d)) > tolerance)
+            {
+                return false;
+            }
+        }
+        return true;
+    }
+
+    private static IReadOnlyList<double> CopyStations(IReadOnlyList<double> values)
+    {
+        if (values == null || values.Count < 2)
+            throw new ArgumentException("At least two reference-S stations are required.", nameof(values));
+
+        var copy = new List<double>(values.Count);
+        double previous = double.NegativeInfinity;
+        for (int index = 0; index < values.Count; index++)
+        {
+            double value = values[index];
+            if (!IsFinite(value) || value <= previous)
+                throw new ArgumentException("Reference-S stations must be finite and strictly increasing.", nameof(values));
+            copy.Add(value);
+            previous = value;
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static IReadOnlyList<double> CopyValues(IReadOnlyList<double> values, int expectedCount, string parameterName)
+    {
+        if (values == null || values.Count != expectedCount)
+            throw new ArgumentException("Lateral value count does not match the station layout.", parameterName);
+
+        var copy = new List<double>(values.Count);
+        for (int index = 0; index < values.Count; index++)
+        {
+            if (!IsFinite(values[index]))
+                throw new ArgumentOutOfRangeException(parameterName);
+            copy.Add(values[index]);
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}
@@ -0,0 +1,29 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable reconstructed lateral path, marked only after independent validation.</summary>
+public sealed class LateralPath
+{
+    public LateralPath(IReadOnlyList<LateralPathPoint> points, bool independentlyValidated)
+    {
+        if (points == null)
+            throw new ArgumentNullException(nameof(points));
+
+        var copy = new List<LateralPathPoint>(points.Count);
+        for (int index = 0; index < points.Count; index++)
+        {
+            if (points[index] == null)
+                throw new ArgumentException("Lateral path points cannot contain null values.", nameof(points));
+            copy.Add(points[index]);
+        }
+        Points = new ReadOnlyCollection<LateralPathPoint>(copy);
+        IsIndependentlyValidated = independentlyValidated;
+    }
+
+    public IReadOnlyList<LateralPathPoint> Points { get; }
+
+    public bool IsIndependentlyValidated { get; }
+}
@@ -0,0 +1,61 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable world-space sample reconstructed from one lateral candidate.</summary>
+public sealed class LateralPathPoint
+{
+    public LateralPathPoint(double referenceS, double pathS, double l, double dl, double ddl, double dddl,
+        double x, double y, double vehicleYaw, double geometricCurvature, double vehicleCurvature,
+        double vehicleCurvatureDerivative)
+    {
+        RequireFinite(referenceS, nameof(referenceS));
+        RequireFinite(pathS, nameof(pathS));
+        RequireFinite(l, nameof(l));
+        RequireFinite(dl, nameof(dl));
+        RequireFinite(ddl, nameof(ddl));
+        RequireFinite(dddl, nameof(dddl));
+        RequireFinite(x, nameof(x));
+        RequireFinite(y, nameof(y));
+        RequireFinite(vehicleYaw, nameof(vehicleYaw));
+        RequireFinite(geometricCurvature, nameof(geometricCurvature));
+        RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
+        RequireFinite(vehicleCurvatureDerivative, nameof(vehicleCurvatureDerivative));
+        if (referenceS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(referenceS));
+        if (pathS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(pathS));
+
+        ReferenceS = referenceS;
+        PathS = pathS;
+        L = l;
+        DL = dl;
+        DDL = ddl;
+        DDDL = dddl;
+        X = x;
+        Y = y;
+        VehicleYaw = vehicleYaw;
+        GeometricCurvature = geometricCurvature;
+        VehicleCurvature = vehicleCurvature;
+        VehicleCurvatureDerivative = vehicleCurvatureDerivative;
+    }
+
+    public double ReferenceS { get; }
+    public double PathS { get; }
+    public double L { get; }
+    public double DL { get; }
+    public double DDL { get; }
+    public double DDDL { get; }
+    public double X { get; }
+    public double Y { get; }
+    public double VehicleYaw { get; }
+    public double GeometricCurvature { get; }
+    public double VehicleCurvature { get; }
+    public double VehicleCurvatureDerivative { get; }
+
+    private static void RequireFinite(double value, string parameterName)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentOutOfRangeException(parameterName);
+    }
+}
@@ -0,0 +1,140 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable inputs for one lateral solve over a single direction segment.</summary>
+public sealed class LateralPlanningInput
+{
+    private const double Epsilon = 1e-12d;
+
+    public LateralPlanningInput(DirectionSegmentView referenceSegment, StaticCorridor corridor,
+        FrenetProjection startProjection, EmTerminalType terminalType, VehicleParameters vehicle,
+        EmPlannerConfiguration configuration, IReadOnlyList<FrenetProjection> previousTrajectorySeed)
+    {
+        if (referenceSegment == null)
+            throw new ArgumentNullException(nameof(referenceSegment));
+        if (corridor == null)
+            throw new ArgumentNullException(nameof(corridor));
+        if (startProjection == null)
+            throw new ArgumentNullException(nameof(startProjection));
+        if (vehicle == null)
+            throw new ArgumentNullException(nameof(vehicle));
+        if (configuration == null)
+            throw new ArgumentNullException(nameof(configuration));
+        if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
+            throw new ArgumentOutOfRangeException(nameof(terminalType));
+
+        ReferenceSegment = referenceSegment;
+        Corridor = CopyCorridor(corridor);
+        ReferenceStations = CopyStationValues(Corridor.Stations);
+        ValidateCorridorMatchesInput(referenceSegment, Corridor.Stations, startProjection);
+        StartProjection = startProjection;
+        TerminalType = terminalType;
+        Vehicle = CopyVehicle(vehicle);
+        Configuration = configuration.Copy();
+        PreviousTrajectorySeed = CopySeed(previousTrajectorySeed, referenceSegment);
+    }
+
+    public DirectionSegmentView ReferenceSegment { get; }
+
+    public StaticCorridor Corridor { get; }
+
+    public IReadOnlyList<double> ReferenceStations { get; }
+
+    public FrenetProjection StartProjection { get; }
+
+    public EmTerminalType TerminalType { get; }
+
+    public VehicleParameters Vehicle { get; }
+
+    public EmPlannerConfiguration Configuration { get; }
+
+    public IReadOnlyList<FrenetProjection> PreviousTrajectorySeed { get; }
+
+    private static StaticCorridor CopyCorridor(StaticCorridor source)
+    {
+        var stations = new List<LateralInterval>(source.Stations.Count);
+        for (int index = 0; index < source.Stations.Count; index++)
+        {
+            LateralInterval station = source.Stations[index];
+            if (station == null)
+                throw new ArgumentException("Corridor stations cannot be null.", nameof(source));
+            stations.Add(new LateralInterval(station.ReferenceS, station.MinimumL, station.MaximumL, station.SeedL));
+        }
+        return new StaticCorridor(stations);
+    }
+
+    private static IReadOnlyList<double> CopyStationValues(IReadOnlyList<LateralInterval> stations)
+    {
+        if (stations.Count < 2)
+            throw new ArgumentException("A lateral planning input requires at least two corridor stations.", nameof(stations));
+
+        var copy = new List<double>(stations.Count);
+        double previous = double.NegativeInfinity;
+        for (int index = 0; index < stations.Count; index++)
+        {
+            double referenceS = stations[index].ReferenceS;
+            if (referenceS <= previous)
+                throw new ArgumentException("Corridor reference-S stations must be strictly increasing.", nameof(stations));
+            copy.Add(referenceS);
+            previous = referenceS;
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static void ValidateCorridorMatchesInput(DirectionSegmentView segment, IReadOnlyList<LateralInterval> stations,
+        FrenetProjection start)
+    {
+        if (start.ReferencePoint == null || start.ReferencePoint.Direction != segment.Direction ||
+            start.ReferenceS < -Epsilon || start.ReferenceS > segment.LengthMeters + Epsilon)
+        {
+            throw new ArgumentException("Start projection must belong to the selected direction segment.", nameof(start));
+        }
+        if (Math.Abs(stations[0].ReferenceS - start.ReferenceS) > Epsilon)
+            throw new ArgumentException("The first corridor station must match the start projection reference S.", nameof(stations));
+        if (start.LateralOffset < stations[0].MinimumL - Epsilon || start.LateralOffset > stations[0].MaximumL + Epsilon)
+            throw new ArgumentException("Start projection is outside the first hard corridor interval.", nameof(start));
+
+        for (int index = 0; index < stations.Count; index++)
+        {
+            if (stations[index].ReferenceS < -Epsilon || stations[index].ReferenceS > segment.LengthMeters + Epsilon)
+                throw new ArgumentException("Corridor stations must lie inside the selected direction segment.", nameof(stations));
+        }
+    }
+
+    private static IReadOnlyList<FrenetProjection> CopySeed(IReadOnlyList<FrenetProjection> seed,
+        DirectionSegmentView segment)
+    {
+        var copy = new List<FrenetProjection>(seed == null ? 0 : seed.Count);
+        if (seed != null)
+        {
+            for (int index = 0; index < seed.Count; index++)
+            {
+                FrenetProjection projection = seed[index];
+                if (projection == null || projection.ReferencePoint.Direction != segment.Direction ||
+                    projection.ReferenceS < -Epsilon || projection.ReferenceS > segment.LengthMeters + Epsilon)
+                {
+                    throw new ArgumentException("Previous lateral seed must belong to the selected direction segment.", nameof(seed));
+                }
+                copy.Add(projection);
+            }
+        }
+        return new ReadOnlyCollection<FrenetProjection>(copy);
+    }
+
+    private static VehicleParameters CopyVehicle(VehicleParameters vehicle)
+    {
+        return new VehicleParameters
+        {
+            LengthMeters = vehicle.LengthMeters,
+            WidthMeters = vehicle.WidthMeters,
+            SafetyMarginMeters = vehicle.SafetyMarginMeters,
+            MaximumCurvaturePerMeter = vehicle.MaximumCurvaturePerMeter,
+            MinimumTurningRadiusMeters = vehicle.MinimumTurningRadiusMeters,
+        };
+    }
+}
@@ -0,0 +1,29 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Result of the lateral stage; only independently validated paths may be successful.</summary>
+public sealed class LateralPlanningResult
+{
+    public LateralPlanningResult(EmPlanningStatus status, LateralPath path, string failureReason)
+    {
+        if (!Enum.IsDefined(typeof(EmPlanningStatus), status))
+            throw new ArgumentOutOfRangeException(nameof(status));
+
+        bool successful = status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;
+        if (successful && (path == null || path.Points.Count == 0 || !path.IsIndependentlyValidated))
+            throw new ArgumentException("Successful lateral results require a non-empty independently validated path.", nameof(path));
+        if (!successful && path != null)
+            throw new ArgumentException("Failed lateral results cannot contain a path.", nameof(path));
+
+        Status = status;
+        Path = path;
+        FailureReason = failureReason ?? string.Empty;
+    }
+
+    public EmPlanningStatus Status { get; }
+
+    public LateralPath Path { get; }
+
+    public string FailureReason { get; }
+}
@@ -0,0 +1,63 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Deterministic variable ranges for one lateral QP discretized on N stations.</summary>
+public sealed class LateralVariableLayout
+{
+    public LateralVariableLayout(int stationCount)
+    {
+        if (stationCount < 2)
+            throw new ArgumentOutOfRangeException(nameof(stationCount), "At least two reference-S stations are required.");
+
+        StationCount = stationCount;
+        LStart = 0;
+        DLStart = stationCount;
+        DDLStart = 2 * stationCount;
+        DDDLStart = 3 * stationCount;
+        VariableCount = 4 * stationCount - 1;
+    }
+
+    public int StationCount { get; }
+
+    public int LStart { get; }
+
+    public int DLStart { get; }
+
+    public int DDLStart { get; }
+
+    public int DDDLStart { get; }
+
+    public int VariableCount { get; }
+
+    public int L(int stationIndex)
+    {
+        RequireStationIndex(stationIndex);
+        return LStart + stationIndex;
+    }
+
+    public int DL(int stationIndex)
+    {
+        RequireStationIndex(stationIndex);
+        return DLStart + stationIndex;
+    }
+
+    public int DDL(int stationIndex)
+    {
+        RequireStationIndex(stationIndex);
+        return DDLStart + stationIndex;
+    }
+
+    public int DDDL(int intervalIndex)
+    {
+        if (intervalIndex < 0 || intervalIndex >= StationCount - 1)
+            throw new ArgumentOutOfRangeException(nameof(intervalIndex));
+        return DDDLStart + intervalIndex;
+    }
+
+    private void RequireStationIndex(int stationIndex)
+    {
+        if (stationIndex < 0 || stationIndex >= StationCount)
+            throw new ArgumentOutOfRangeException(nameof(stationIndex));
+    }
+}