113 lines
4.2 KiB
C#
113 lines
4.2 KiB
C#
using System;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>
|
|
/// 单一方向段内的不可变插值参考样本，连接世界几何与 Frenet 坐标。
|
|
/// X/Y 与参考 S 为 m，航向为 rad，曲率为 1/m、曲率导数为 1/m²；倒车段仍以车辆航向保存几何。
|
|
/// </summary>
|
|
public sealed class FrenetReferencePoint
|
|
{
|
|
/// <summary>
|
|
/// 创建已验证的方向段参考样本。
|
|
/// 参数：位置采用世界 X/Y（m），referenceS 为段局部弧长（m），航向为 rad，曲率量遵循 1/m 与 1/m²；所有数值必须有限。
|
|
/// 返回：车辆航向会规范到 [-π, π]；任一非有限输入会引发 <see cref="ArgumentOutOfRangeException"/>。
|
|
/// </summary>
|
|
public FrenetReferencePoint(double referenceS, double x, double y, double vehicleYaw, double unwrappedVehicleYaw,
|
|
TravelDirection direction, double geometricCurvature, double vehicleCurvature,
|
|
double vehicleCurvatureDerivative, double bodyClearance)
|
|
{
|
|
RequireFinite(referenceS, nameof(referenceS));
|
|
RequireFinite(x, nameof(x));
|
|
RequireFinite(y, nameof(y));
|
|
RequireFinite(vehicleYaw, nameof(vehicleYaw));
|
|
RequireFinite(unwrappedVehicleYaw, nameof(unwrappedVehicleYaw));
|
|
RequireFinite(geometricCurvature, nameof(geometricCurvature));
|
|
RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
|
|
RequireFinite(vehicleCurvatureDerivative, nameof(vehicleCurvatureDerivative));
|
|
RequireFinite(bodyClearance, nameof(bodyClearance));
|
|
|
|
ReferenceS = referenceS;
|
|
X = x;
|
|
Y = y;
|
|
VehicleYaw = AngleMath.NormalizeRadians(vehicleYaw);
|
|
UnwrappedVehicleYaw = unwrappedVehicleYaw;
|
|
Direction = direction;
|
|
GeometricCurvature = geometricCurvature;
|
|
VehicleCurvature = vehicleCurvature;
|
|
VehicleCurvatureDerivative = vehicleCurvatureDerivative;
|
|
BodyClearance = bodyClearance;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 样本在当前方向段的局部参考弧长 S，单位 m。
|
|
/// </summary>
|
|
public double ReferenceS { get; }
|
|
|
|
/// <summary>
|
|
/// 参考中心线点的世界 X 坐标，单位 m。
|
|
/// </summary>
|
|
public double X { get; }
|
|
|
|
/// <summary>
|
|
/// 参考中心线点的世界 Y 坐标，单位 m。
|
|
/// </summary>
|
|
public double Y { get; }
|
|
|
|
/// <summary>
|
|
/// 归一化后的车辆车身航向，单位 rad。
|
|
/// </summary>
|
|
public double VehicleYaw { get; }
|
|
|
|
/// <summary>
|
|
/// 连续展开的车辆车身航向，单位 rad，供跨点几何插值使用。
|
|
/// </summary>
|
|
public double UnwrappedVehicleYaw { get; }
|
|
|
|
/// <summary>
|
|
/// 该参考样本的行驶方向，决定 Frenet 横向正负号和行驶航向。
|
|
/// </summary>
|
|
public TravelDirection Direction { get; }
|
|
|
|
/// <summary>
|
|
/// 中心线几何曲率，单位 1/m。
|
|
/// </summary>
|
|
public double GeometricCurvature { get; }
|
|
|
|
/// <summary>
|
|
/// 满足车辆模型后的车辆曲率，单位 1/m。
|
|
/// </summary>
|
|
public double VehicleCurvature { get; }
|
|
|
|
/// <summary>
|
|
/// 车辆曲率相对弧长的导数，单位 1/m²。
|
|
/// </summary>
|
|
public double VehicleCurvatureDerivative { get; }
|
|
|
|
/// <summary>
|
|
/// 参考点记录的车体净空或可用裕度，单位 m。
|
|
/// </summary>
|
|
public double BodyClearance { get; }
|
|
|
|
/// <summary>
|
|
/// 获取与当前行驶方向一致的连续航向。
|
|
/// 返回：前进时为车辆展开航向，倒车时加 π；单位 rad，仅供 Frenet 几何计算，不重新归一化。
|
|
/// </summary>
|
|
public double TravelYaw
|
|
{
|
|
get { return Direction == TravelDirection.Forward ? UnwrappedVehicleYaw : UnwrappedVehicleYaw + Math.PI; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// 拒绝不能安全保存为参考几何的数值。
|
|
/// 参数：value 是待验证的任意单位标量，name 是异常参数名；NaN 或无穷会引发 <see cref="ArgumentOutOfRangeException"/>。
|
|
/// </summary>
|
|
private static void RequireFinite(double value, string name)
|
|
{
|
|
if (double.IsNaN(value) || double.IsInfinity(value))
|
|
throw new ArgumentOutOfRangeException(name);
|
|
}
|
|
}
|