221 lines
11 KiB
C#
221 lines
11 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||
|
||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||
|
||
/// <summary>
|
||
/// 在纵横向优化前选定的参考距离终端，保证不会跨越当前方向段。
|
||
/// 所有参考站量以当前段局部 S（m）表示；边界类型和纵向模式明确滚动延续、接近停车或精确停车语义。
|
||
/// </summary>
|
||
public sealed class PlanningHorizonSelection
|
||
{
|
||
/// <summary>
|
||
/// 创建仅供本程序集发布的窗口与停车边界选择。
|
||
/// 参数：两个参考站均为局部 S（m），boundary/terminal/mode 为已判定枚举；hasStopBoundary 表示终端是否受真实停车边界约束。
|
||
/// </summary>
|
||
internal PlanningHorizonSelection(double windowEndReferenceS, EmBoundaryType windowEndBoundaryType,
|
||
EmTerminalType terminalType, EmLongitudinalMode longitudinalMode,
|
||
double stopBoundaryReferenceS, bool hasStopBoundary)
|
||
{
|
||
WindowEndReferenceS = windowEndReferenceS;
|
||
WindowEndBoundaryType = windowEndBoundaryType;
|
||
TerminalType = terminalType;
|
||
LongitudinalMode = longitudinalMode;
|
||
StopBoundaryReferenceS = stopBoundaryReferenceS;
|
||
HasStopBoundary = hasStopBoundary;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 本次优化窗口末端的局部参考弧长 S，单位 m。
|
||
/// </summary>
|
||
public double WindowEndReferenceS { get; }
|
||
|
||
/// <summary>
|
||
/// 规划输出的终端参考弧长 S，当前与窗口末端相同，单位 m。
|
||
/// </summary>
|
||
public double TerminalReferenceS => WindowEndReferenceS;
|
||
|
||
/// <summary>
|
||
/// 窗口末端的边界语义；中途截断时为滚动安全停车边界。
|
||
/// </summary>
|
||
public EmBoundaryType WindowEndBoundaryType { get; }
|
||
|
||
/// <summary>
|
||
/// 供终端约束使用的目标、换向或滚动终端类型。
|
||
/// </summary>
|
||
public EmTerminalType TerminalType { get; }
|
||
|
||
/// <summary>
|
||
/// 纵向候选应滚动延续、接近停车边界还是在边界精确停车的模式。
|
||
/// </summary>
|
||
public EmLongitudinalMode LongitudinalMode { get; }
|
||
|
||
/// <summary>
|
||
/// 实际目标或换向停车边界的局部参考弧长 S，单位 m；无此边界时仍保存段末。
|
||
/// </summary>
|
||
public double StopBoundaryReferenceS { get; }
|
||
|
||
/// <summary>
|
||
/// 是否存在必须在当前方向段内处理的目标或换向停车边界。
|
||
/// </summary>
|
||
public bool HasStopBoundary { get; }
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据段边界、初始纵向状态和调度窗口选择 EM 优化地平线。
|
||
/// 输入速度为 m/s、加速度为 m/s²、距离为 m；无效输入、越界初态或不足以停车的段会以明确状态拒绝。
|
||
/// </summary>
|
||
public sealed class PlanningHorizonSelector
|
||
{
|
||
/// <summary>
|
||
/// 段终点、初态限值和停车距离比较使用的局部 S/距离容差，单位 m。
|
||
/// </summary>
|
||
private const double BoundaryTolerance = 1e-8d;
|
||
|
||
/// <summary>
|
||
/// 为当前方向段选择不跨界的参考距离窗口和纵向终端模式。
|
||
/// 参数：currentSegmentReferenceS 为 m，初速为 m/s，初加速度为 m/s²，configuration 提供 m、s 制限值；planningScope 必须为定义的范围枚举。
|
||
/// 返回：成功时给出 selection；配置/初态非法返回 InvalidInput，停车距离越段返回 StoppingDistanceInsufficient，非法全段终边界返回 InvalidReferencePath。
|
||
/// </summary>
|
||
public EmPlanningStatus Select(DirectionSegmentView segment, double currentSegmentReferenceS,
|
||
double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
|
||
EmPlanningScope planningScope, EmPlannerConfiguration configuration, out PlanningHorizonSelection selection,
|
||
out string failureReason)
|
||
{
|
||
selection = null;
|
||
failureReason = string.Empty;
|
||
if (segment == null || configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null ||
|
||
!IsFinite(currentSegmentReferenceS) || currentSegmentReferenceS < 0d ||
|
||
currentSegmentReferenceS > segment.LengthMeters + BoundaryTolerance ||
|
||
!IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d ||
|
||
!IsFinite(initialAccelerationMetersPerSecondSquared) ||
|
||
!Enum.IsDefined(typeof(EmPlanningScope), planningScope))
|
||
{
|
||
failureReason = "Planning horizon inputs are invalid.";
|
||
return EmPlanningStatus.InvalidInput;
|
||
}
|
||
|
||
LongitudinalConfiguration longitudinal = configuration.Longitudinal;
|
||
SchedulingConfiguration scheduling = configuration.Scheduling;
|
||
double directionMaximum = segment.Direction == TravelDirection.Forward
|
||
? longitudinal.MaximumForwardSpeedMetersPerSecond
|
||
: longitudinal.MaximumReverseSpeedMetersPerSecond;
|
||
if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(longitudinal.MaximumAccelerationMetersPerSecondSquared) ||
|
||
!IsPositiveFinite(longitudinal.MaximumDecelerationMetersPerSecondSquared) ||
|
||
!IsPositiveFinite(longitudinal.MaximumJerkMetersPerSecondCubed) ||
|
||
!IsPositiveFinite(longitudinal.ZeroSpeedHoldSeconds) || !IsPositiveFinite(scheduling.TimeHorizonSeconds) ||
|
||
!IsPositiveFinite(scheduling.DistanceHorizonMeters))
|
||
{
|
||
failureReason = "Planning horizon configuration is invalid.";
|
||
return EmPlanningStatus.InvalidInput;
|
||
}
|
||
if (initialProgressSpeedMetersPerSecond > directionMaximum + BoundaryTolerance ||
|
||
initialAccelerationMetersPerSecondSquared < -longitudinal.MaximumDecelerationMetersPerSecondSquared - BoundaryTolerance ||
|
||
initialAccelerationMetersPerSecondSquared > longitudinal.MaximumAccelerationMetersPerSecondSquared + BoundaryTolerance)
|
||
{
|
||
failureReason = "The initial state violates longitudinal bounds.";
|
||
return EmPlanningStatus.InvalidInput;
|
||
}
|
||
|
||
double remainingSegment = Math.Max(0d, segment.LengthMeters - currentSegmentReferenceS);
|
||
if (!JerkLimitedStoppingMath.TryCalculate(initialProgressSpeedMetersPerSecond,
|
||
initialAccelerationMetersPerSecondSquared, longitudinal.MaximumDecelerationMetersPerSecondSquared,
|
||
longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile initialStop,
|
||
out failureReason))
|
||
{
|
||
return EmPlanningStatus.InvalidInput;
|
||
}
|
||
if (initialStop.DistanceMeters + BoundaryTolerance > remainingSegment)
|
||
{
|
||
failureReason = "The current segment lacks the jerk-limited stopping distance.";
|
||
return EmPlanningStatus.StoppingDistanceInsufficient;
|
||
}
|
||
|
||
if (planningScope == EmPlanningScope.FullDirectionSegment)
|
||
{
|
||
EmBoundaryType boundary = segment.EndBoundary.BoundaryType;
|
||
if (!IsStopBoundary(boundary))
|
||
{
|
||
failureReason = "A full direction segment must end at Goal or GearSwitch.";
|
||
return EmPlanningStatus.InvalidReferencePath;
|
||
}
|
||
selection = new PlanningHorizonSelection(segment.LengthMeters, boundary,
|
||
ToTerminalType(boundary), EmLongitudinalMode.ExactStopAtBoundary,
|
||
segment.LengthMeters, true);
|
||
return EmPlanningStatus.Success;
|
||
}
|
||
|
||
double windowEnd = Math.Min(currentSegmentReferenceS + scheduling.DistanceHorizonMeters, segment.LengthMeters);
|
||
bool windowReachesSegmentEnd = windowEnd >= segment.LengthMeters - BoundaryTolerance;
|
||
bool hasStopBoundary = windowReachesSegmentEnd && IsStopBoundary(segment.EndBoundary.BoundaryType);
|
||
EmBoundaryType windowEndBoundaryType = windowReachesSegmentEnd
|
||
? segment.EndBoundary.BoundaryType
|
||
: EmBoundaryType.RollingSafetyStop;
|
||
if (!hasStopBoundary)
|
||
{
|
||
selection = new PlanningHorizonSelection(windowEnd, windowEndBoundaryType,
|
||
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation,
|
||
segment.LengthMeters, false);
|
||
return EmPlanningStatus.Success;
|
||
}
|
||
|
||
IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
|
||
scheduling.TimeHorizonSeconds, scheduling.OutputTimeStepSeconds);
|
||
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||
knotTimes, scheduling.OutputTimeStepSeconds);
|
||
double availableMotionTime = knotTimes[stabilizationStart];
|
||
double maximumStoppedDistance = JerkLimitedStoppingMath.CalculateMaximumStoppedDistance(
|
||
initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared, directionMaximum,
|
||
longitudinal.MaximumAccelerationMetersPerSecondSquared,
|
||
longitudinal.MaximumDecelerationMetersPerSecondSquared,
|
||
longitudinal.MaximumJerkMetersPerSecondCubed, availableMotionTime);
|
||
EmLongitudinalMode mode = remainingSegment <= maximumStoppedDistance + BoundaryTolerance
|
||
? EmLongitudinalMode.ExactStopAtBoundary
|
||
: EmLongitudinalMode.ApproachStopBoundary;
|
||
selection = new PlanningHorizonSelection(segment.LengthMeters, windowEndBoundaryType,
|
||
ToTerminalType(segment.EndBoundary.BoundaryType), mode, segment.LengthMeters, true);
|
||
return EmPlanningStatus.Success;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判断边界是否要求车辆在当前段内停止。
|
||
/// 参数：boundaryType 为边界枚举；返回：Goal 或 GearSwitchApproach 时为 true，其余边界允许滚动延续。
|
||
/// </summary>
|
||
private static bool IsStopBoundary(EmBoundaryType boundaryType)
|
||
{
|
||
return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将参考边界语义映射为纵向终端类型。
|
||
/// 参数：boundaryType 为边界枚举；返回：目标、任一换向边界分别映射到 Goal、GearSwitch，其余映射为 RollingSafetyStop。
|
||
/// </summary>
|
||
private static EmTerminalType ToTerminalType(EmBoundaryType boundaryType)
|
||
{
|
||
return boundaryType == EmBoundaryType.Goal
|
||
? EmTerminalType.Goal
|
||
: boundaryType == EmBoundaryType.GearSwitchApproach || boundaryType == EmBoundaryType.GearSwitchDeparture
|
||
? EmTerminalType.GearSwitch
|
||
: EmTerminalType.RollingSafetyStop;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判断配置中的正量是否可用于时间、距离或动力学计算。
|
||
/// 参数：value 为对应单位的标量；返回：仅有限且严格大于零时为 true。
|
||
/// </summary>
|
||
private static bool IsPositiveFinite(double value)
|
||
{
|
||
return IsFinite(value) && value > 0d;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判断规划输入是否为有限实数。
|
||
/// 参数：value 为任意标量；返回：NaN 与无穷均返回 false。
|
||
/// </summary>
|
||
private static bool IsFinite(double value)
|
||
{
|
||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||
}
|
||
}
|