feat: 发布 EM 轨迹规划首个版本

2026-08-11 20:35:59 +08:00
parent 569de5f13c
commit 1903e71fc1
522 changed files with 4188 additions and 119188 deletions
@@ -2,9 +2,17 @@ using System;

 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

-/// <summary>World pose projected onto one bounded direction segment.</summary>
+/// <summary>
+/// 世界车辆位姿投影到单一受限方向段后的 Frenet 描述。
+/// S 与 L 单位均为 m，航向误差为 rad，距离以 m² 保存；对象不代表跨越换向边界的投影。
+/// </summary>
 public sealed class FrenetProjection
 {
+    /// <summary>
+    /// 从参考点和相对量创建不可变投影结果。
+    /// 参数：lateralOffset 为沿行驶方向左法线的 L（m），headingError 为行驶航向差（rad），squaredDistanceMeters 为非负 m²。
+    /// 返回：携带参考 S 的投影；空参考点、非有限值或负平方距离会被拒绝。
+    /// </summary>
    public FrenetProjection(FrenetReferencePoint referencePoint, double lateralOffset, double headingError,
        double squaredDistanceMeters)
    {
@@ -20,12 +28,35 @@ public sealed class FrenetProjection
        SquaredDistanceMeters = squaredDistanceMeters;
    }

+    /// <summary>
+    /// 投影命中的插值参考点，其坐标为世界 X/Y（m）。
+    /// </summary>
    public FrenetReferencePoint ReferencePoint { get; }
+
+    /// <summary>
+    /// 命中点在当前方向段的局部参考弧长 S，单位 m。
+    /// </summary>
    public double ReferenceS { get; }
+
+    /// <summary>
+    /// 世界位姿相对参考行驶方向左法线的横向偏移 L，单位 m。
+    /// </summary>
    public double LateralOffset { get; }
+
+    /// <summary>
+    /// 车辆行驶航向相对参考行驶航向的归一化误差，单位 rad。
+    /// </summary>
    public double HeadingError { get; }
+
+    /// <summary>
+    /// 世界位置与命中参考位置的欧氏距离平方，单位 m²。
+    /// </summary>
    public double SquaredDistanceMeters { get; }

+    /// <summary>
+    /// 判定投影标量是否有限。
+    /// 参数：value 为任意实数；返回：NaN 和正负无穷均返回 false。
+    /// </summary>
    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
@@ -4,11 +4,21 @@ using MultiWheelC.TrajectoryPlanning.Utils;

 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

-/// <summary>Deterministically projects a world pose only inside the supplied direction segment and S window.</summary>
+/// <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)
    {
@@ -16,6 +26,10 @@ public sealed class FrenetProjector
            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)
    {
@@ -63,6 +77,10 @@ public sealed class FrenetProjector
        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)
    {
@@ -84,12 +102,20 @@ public sealed class FrenetProjector
        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)
    {
@@ -101,13 +127,25 @@ public sealed class FrenetProjector
            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;
@@ -115,10 +153,25 @@ public sealed class FrenetProjector
            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;
@@ -129,3 +182,7 @@ public sealed class FrenetProjector
        }
    }
 }
+    /// <summary>
+    /// 在窗口内投影世界位姿，按距离、种子 S 距离及较小 S 的顺序稳定选择候选。
+    /// 参数：worldPose 使用世界 X/Y（m）和车身航向（rad），S 窗口、距离阈值和种子均为 m；返回：窗口非法、无候选或候选超阈值时返回 false。
+    /// </summary>
@@ -4,9 +4,17 @@ using MultiWheelC.TrajectoryPlanning.Utils;

 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

-/// <summary>Immutable interpolated reference sample in a single direction segment.</summary>
+/// <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)
@@ -33,23 +41,69 @@ public sealed class FrenetReferencePoint
        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>Unwrapped direction of travel, used internally for geometry.</summary>
+    /// <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))
@@ -4,9 +4,17 @@ using MultiWheelC.TrajectoryPlanning.Utils;

 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

-/// <summary>Coordinate conversion that keeps Frenet lateral sign relative to travel direction.</summary>
+/// <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)
    {
@@ -33,11 +41,19 @@ public static class FrenetTransform
        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);
@@ -4,11 +4,22 @@ using MultiWheelC.TrajectoryPlanning.Utils;

 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

-/// <summary>Interpolates reference geometry inside exactly one direction segment.</summary>
+/// <summary>
+/// 在恰好一个方向段内按局部参考弧长插值参考几何。
+/// 输入和输出 S、X/Y 与净空均为 m，航向为 rad，曲率为 1/m；插值不会越过方向段边界。
+/// </summary>
 public static class ReferencePathInterpolator
 {
+    /// <summary>
+    /// 接受端点轻微 S 超差并判断精确节点的数值容差，单位 m。
+    /// </summary>
    private const double Epsilon = 1e-12d;

+    /// <summary>
+    /// 为给定局部 S 生成方向一致的 Frenet 参考样本。
+    /// 参数：segment 不可为空，referenceS 为 m；[-1e-12, Length+1e-12] 内的端点超差会夹紧到边界。
+    /// 返回：精确点直接转换，区间内对位置、展开航向和几何量线性插值；超界或退化跨度会引发异常。
+    /// </summary>
    public static FrenetReferencePoint Interpolate(DirectionSegmentView segment, double referenceS)
    {
        if (segment == null)
@@ -45,6 +56,10 @@ public static class ReferencePathInterpolator
        return FromPoint(segment.Points[segment.Points.Count - 1]);
    }

+    /// <summary>
+    /// 将原始平滑路径点转换为同一局部 S 的 Frenet 参考样本。
+    /// 参数：point 已携带世界 X/Y（m）、航向（rad）和曲率量；返回：保持其方向及所有几何量的不可变副本。
+    /// </summary>
    private static FrenetReferencePoint FromPoint(SmoothedPathPoint point)
    {
        return new FrenetReferencePoint(point.ArcLength, point.X, point.Y, point.Heading, point.UnwrappedHeading,
@@ -52,11 +67,19 @@ public static class ReferencePathInterpolator
            point.BodyClearance);
    }

+    /// <summary>
+    /// 在线性标量区间内计算插值值。
+    /// 参数：lower、upper 为同单位端点，fraction 为无单位比例；返回：同单位的未夹紧线性结果。
+    /// </summary>
    private static double Lerp(double lower, double upper, double fraction)
    {
        return lower + (upper - lower) * fraction;
    }

+    /// <summary>
+    /// 判定插值输入是否为有限实数。
+    /// 参数：value 为任意标量；返回：NaN 或无穷时为 false。
+    /// </summary>
    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);