189 lines
9.0 KiB
C#
189 lines
9.0 KiB
C#
using System;
|
||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||
|
||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||
|
||
/// <summary>
|
||
/// 在指定方向段及局部 S 窗口内确定性地投影世界位姿。
|
||
/// 投影不跨越换向边界；世界 X/Y 与距离为 m，航向为 rad，并以种子 S 消除等距候选的拓扑歧义。
|
||
/// </summary>
|
||
public sealed class FrenetProjector
|
||
{
|
||
/// <summary>
|
||
/// 等距候选比较和零长度线段识别使用的 S/距离平方数值容差 1e-14。
|
||
/// </summary>
|
||
private const double TieTolerance = 1e-14d;
|
||
|
||
/// <summary>
|
||
/// 在窗口内投影世界位姿，并以窗口起点作为等距候选的种子 S。
|
||
/// 参数：窗口与 maximumDistanceMeters 均为当前段局部 m 制距离；返回：命中距离不超过阈值时返回 true，否则 projection 为 null。
|
||
/// </summary>
|
||
public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
|
||
double maximumReferenceS, double maximumDistanceMeters, out FrenetProjection projection)
|
||
{
|
||
return TryProject(worldPose, segment, minimumReferenceS, maximumReferenceS, maximumDistanceMeters,
|
||
minimumReferenceS, out projection);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在窗口内投影世界位姿，按距离、种子 S 距离及较小 S 的顺序稳定选择候选。
|
||
/// 参数：worldPose 使用世界 X/Y（m）和车身航向（rad），S 窗口、距离阈值和种子均为 m；返回：窗口非法、无候选或候选超阈值时返回 false。
|
||
/// </summary>
|
||
public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
|
||
double maximumReferenceS, double maximumDistanceMeters, double seedReferenceS, out FrenetProjection projection)
|
||
{
|
||
projection = null;
|
||
if (worldPose == null || segment == null || !IsFinite(worldPose.X) || !IsFinite(worldPose.Y) ||
|
||
!IsFinite(worldPose.Heading) || !IsFinite(minimumReferenceS) || !IsFinite(maximumReferenceS) ||
|
||
!IsFinite(maximumDistanceMeters) || !IsFinite(seedReferenceS) || maximumDistanceMeters < 0d)
|
||
return false;
|
||
|
||
double lowerBound = Math.Max(0d, minimumReferenceS);
|
||
double upperBound = Math.Min(segment.LengthMeters, maximumReferenceS);
|
||
if (lowerBound > upperBound)
|
||
return false;
|
||
|
||
Candidate best = null;
|
||
for (int index = 0; index + 1 < segment.Points.Count; index++)
|
||
{
|
||
double startS = Math.Max(lowerBound, segment.Points[index].ArcLength);
|
||
double endS = Math.Min(upperBound, segment.Points[index + 1].ArcLength);
|
||
if (startS > endS)
|
||
continue;
|
||
|
||
FrenetReferencePoint start = ReferencePathInterpolator.Interpolate(segment, startS);
|
||
FrenetReferencePoint end = ReferencePathInterpolator.Interpolate(segment, endS);
|
||
ConsiderLine(worldPose, start, end, seedReferenceS, ref best);
|
||
}
|
||
|
||
if (best == null && Math.Abs(lowerBound - upperBound) <= TieTolerance)
|
||
{
|
||
FrenetReferencePoint point = ReferencePathInterpolator.Interpolate(segment, lowerBound);
|
||
ConsiderPoint(worldPose, point, seedReferenceS, ref best);
|
||
}
|
||
|
||
if (best == null || best.SquaredDistanceMeters > maximumDistanceMeters * maximumDistanceMeters)
|
||
return false;
|
||
|
||
FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(segment, best.ReferenceS);
|
||
double dx = worldPose.X - reference.X;
|
||
double dy = worldPose.Y - reference.Y;
|
||
double travelYaw = reference.TravelYaw;
|
||
double lateralOffset = -dx * Math.Sin(travelYaw) + dy * Math.Cos(travelYaw);
|
||
double egoTravelYaw = FrenetTransform.GetTravelYaw(worldPose.Heading, segment.Direction);
|
||
double headingError = AngleMath.NormalizeRadians(egoTravelYaw - travelYaw);
|
||
projection = new FrenetProjection(reference, lateralOffset, headingError, best.SquaredDistanceMeters);
|
||
return true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 考察被 S 窗口裁剪后的参考折线边，并将世界点的正交投影加入最优候选比较。
|
||
/// 参数：端点为世界 X/Y（m）和局部 S（m），seedReferenceS 为 m；零长度（不超过 1e-14）边退化为两个端点比较。
|
||
/// </summary>
|
||
private static void ConsiderLine(Pose2D worldPose, FrenetReferencePoint start, FrenetReferencePoint end,
|
||
double seedReferenceS, ref Candidate best)
|
||
{
|
||
double dx = end.X - start.X;
|
||
double dy = end.Y - start.Y;
|
||
double lengthSquared = dx * dx + dy * dy;
|
||
if (lengthSquared <= TieTolerance)
|
||
{
|
||
ConsiderPoint(worldPose, start, seedReferenceS, ref best);
|
||
ConsiderPoint(worldPose, end, seedReferenceS, ref best);
|
||
return;
|
||
}
|
||
|
||
double fraction = ((worldPose.X - start.X) * dx + (worldPose.Y - start.Y) * dy) / lengthSquared;
|
||
fraction = Math.Max(0d, Math.Min(1d, fraction));
|
||
double referenceS = start.ReferenceS + (end.ReferenceS - start.ReferenceS) * fraction;
|
||
double x = start.X + dx * fraction;
|
||
double y = start.Y + dy * fraction;
|
||
Consider(worldPose, referenceS, x, y, seedReferenceS, ref best);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将一个离散参考点作为投影候选参与比较。
|
||
/// 参数：point 的位置为世界 m 制坐标，seedReferenceS 为局部 m 制种子；结果通过 best 原位更新。
|
||
/// </summary>
|
||
private static void ConsiderPoint(Pose2D worldPose, FrenetReferencePoint point, double seedReferenceS,
|
||
ref Candidate best)
|
||
{
|
||
Consider(worldPose, point.ReferenceS, point.X, point.Y, seedReferenceS, ref best);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 依据世界平面平方距离登记候选，并按确定性优先级替换当前最佳值。
|
||
/// 参数：referenceS、x、y 与 seedReferenceS 分别为局部 S（m）和世界坐标（m）；平方距离由内部计算，单位 m²。
|
||
/// </summary>
|
||
private static void Consider(Pose2D worldPose, double referenceS, double x, double y, double seedReferenceS,
|
||
ref Candidate best)
|
||
{
|
||
double dx = worldPose.X - x;
|
||
double dy = worldPose.Y - y;
|
||
double squaredDistance = dx * dx + dy * dy;
|
||
var candidate = new Candidate(referenceS, squaredDistance, Math.Abs(referenceS - seedReferenceS));
|
||
if (best == null || candidate.IsPreferredTo(best))
|
||
best = candidate;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判定投影计算的标量是否有限。
|
||
/// 参数：value 为任意实数；返回：NaN 与无穷均返回 false。
|
||
/// </summary>
|
||
private static bool IsFinite(double value)
|
||
{
|
||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 保存一个待比较的折线投影候选。
|
||
/// ReferenceS 和 SeedDistance 使用 m，SquaredDistanceMeters 使用 m²，优先级由距离、种子距离及较小 S 依次决定。
|
||
/// </summary>
|
||
private sealed class Candidate
|
||
{
|
||
/// <summary>
|
||
/// 创建投影候选。
|
||
/// 参数：referenceS 与 seedDistance 为 m，squaredDistanceMeters 为 m²；调用方仅传入有限的已计算值。
|
||
/// </summary>
|
||
public Candidate(double referenceS, double squaredDistanceMeters, double seedDistance)
|
||
{
|
||
ReferenceS = referenceS;
|
||
SquaredDistanceMeters = squaredDistanceMeters;
|
||
SeedDistance = seedDistance;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 候选在当前方向段的局部参考弧长 S，单位 m。
|
||
/// </summary>
|
||
public double ReferenceS { get; }
|
||
|
||
/// <summary>
|
||
/// 候选世界位置到车辆位置的距离平方，单位 m²。
|
||
/// </summary>
|
||
public double SquaredDistanceMeters { get; }
|
||
|
||
/// <summary>
|
||
/// 候选 S 与调用方种子 S 的绝对距离，单位 m。
|
||
/// </summary>
|
||
public double SeedDistance { get; }
|
||
|
||
/// <summary>
|
||
/// 比较两个候选的稳定优先级。
|
||
/// 参数：other 为非空候选；返回：平方距离差超过 1e-14 时取较小者，随后取较近种子，仍相等时取较小 S。
|
||
/// </summary>
|
||
public bool IsPreferredTo(Candidate other)
|
||
{
|
||
if (SquaredDistanceMeters < other.SquaredDistanceMeters - TieTolerance) return true;
|
||
if (SquaredDistanceMeters > other.SquaredDistanceMeters + TieTolerance) return false;
|
||
if (SeedDistance < other.SeedDistance - TieTolerance) return true;
|
||
if (SeedDistance > other.SeedDistance + TieTolerance) return false;
|
||
return ReferenceS < other.ReferenceS;
|
||
}
|
||
}
|
||
}
|
||
/// <summary>
|
||
/// 在窗口内投影世界位姿，按距离、种子 S 距离及较小 S 的顺序稳定选择候选。
|
||
/// 参数：worldPose 使用世界 X/Y（m）和车身航向（rad），S 窗口、距离阈值和种子均为 m；返回：窗口非法、无候选或候选超阈值时返回 false。
|
||
/// </summary>
|