62 lines
2.9 KiB
C#
62 lines
2.9 KiB
C#
using System;
|
||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||
|
||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||
|
||
/// <summary>
|
||
/// 在世界坐标与 Frenet 横向约定间转换，并始终以实际行驶方向确定 L 的正负。
|
||
/// 世界位置使用 X/Y（m）、航向使用 rad；倒车时车辆车身航向与行驶航向相差 π。
|
||
/// </summary>
|
||
public static class FrenetTransform
|
||
{
|
||
/// <summary>
|
||
/// 将参考点及其横向状态重建为世界车辆中心位姿。
|
||
/// 参数：lateralOffset 为 L（m），lateralDerivative 为 dL/dS（无量纲），minimumFrenetDenominator 为正的奇异性下界；参考点采用世界 X/Y（m）与 rad 航向。
|
||
/// 返回：当 1-κL 有限且不小于下界、重建位姿有限时返回 true；空参考点、非法输入或接近 Frenet 奇异点时返回 false。
|
||
/// </summary>
|
||
public static bool TryReconstruct(FrenetReferencePoint referencePoint, double lateralOffset, double lateralDerivative,
|
||
double minimumFrenetDenominator, out Pose2D pose)
|
||
{
|
||
pose = null;
|
||
if (referencePoint == null || !IsFinite(lateralOffset) || !IsFinite(lateralDerivative) ||
|
||
!IsFinite(minimumFrenetDenominator) || minimumFrenetDenominator <= 0d)
|
||
return false;
|
||
|
||
double denominator = 1d - referencePoint.GeometricCurvature * lateralOffset;
|
||
if (!IsFinite(denominator) || denominator < minimumFrenetDenominator)
|
||
return false;
|
||
|
||
double travelYaw = referencePoint.TravelYaw;
|
||
double x = referencePoint.X - lateralOffset * Math.Sin(travelYaw);
|
||
double y = referencePoint.Y + lateralOffset * Math.Cos(travelYaw);
|
||
double optimizedTravelYaw = travelYaw + Math.Atan2(lateralDerivative, denominator);
|
||
double vehicleYaw = referencePoint.Direction == TravelDirection.Forward
|
||
? AngleMath.NormalizeRadians(optimizedTravelYaw)
|
||
: AngleMath.NormalizeRadians(optimizedTravelYaw + Math.PI);
|
||
if (!IsFinite(x) || !IsFinite(y) || !IsFinite(vehicleYaw))
|
||
return false;
|
||
|
||
pose = new Pose2D(x, y, vehicleYaw);
|
||
return true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从车辆车身航向取得对应实际行驶的连续航向。
|
||
/// 参数：vehicleYaw 为 rad，direction 指定前进或倒车；返回：前进原样、倒车加 π，结果不归一化。
|
||
/// </summary>
|
||
internal static double GetTravelYaw(double vehicleYaw, TravelDirection direction)
|
||
{
|
||
return direction == TravelDirection.Forward ? vehicleYaw : vehicleYaw + Math.PI;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判定重建中间量是否为有限实数。
|
||
/// 参数：value 为任意标量；返回：NaN 或正负无穷时为 false。
|
||
/// </summary>
|
||
private static bool IsFinite(double value)
|
||
{
|
||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||
}
|
||
}
|