打通新版Stanley轨迹跟踪闭环并补充实验测试与数据分析脚本

Co-authored-by: Cursor <cursoragent@cursor.com>

MultiWheelC/Control/Allocation/AckermannGcpAllocator.cs

@@ -1 +1,133 @@
-// 车体中心命令曲率转换成前后GCP方向
+using System;
+using MultiWheelC.Control.Abstractions;
+
+namespace MultiWheelC.Control.Allocation
+{
+    /// <summary>
+    /// 将车体中心目标曲率按对称前后转向策略转换为旧版底盘的前后GCP方向。
+    /// </summary>
+    public sealed class AckermannGcpAllocator
+    {
+        /// <summary>
+        /// 创建使用指定GCP半间距和最大GCP转角的对称转向分配器。
+        /// </summary>
+        public AckermannGcpAllocator(
+            double controlPointRadiusMeters,
+            double maximumGcpAngleRadians)
+        {
+            EnsureFinitePositive(
+                controlPointRadiusMeters,
+                nameof(controlPointRadiusMeters));
+            EnsureFinitePositive(
+                maximumGcpAngleRadians,
+                nameof(maximumGcpAngleRadians));
+
+            if (maximumGcpAngleRadians >=
+                Math.PI / 2.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(maximumGcpAngleRadians),
+                    "最大GCP转角必须小于π/2，避免曲率换算出现奇异值。");
+            }
+
+            ControlPointRadiusMeters =
+                controlPointRadiusMeters;
+            MaximumGcpAngleRadians =
+                maximumGcpAngleRadians;
+        }
+
+        /// <summary>
+        /// 获取车体中心到前、后GCP的距离，单位为m。
+        /// </summary>
+        public double ControlPointRadiusMeters { get; }
+
+        /// <summary>
+        /// 获取前后GCP允许的最大转角绝对值，单位为rad。
+        /// </summary>
+        public double MaximumGcpAngleRadians { get; }
+
+        /// <summary>
+        /// 获取当前GCP几何和转角限制允许的最大车体中心曲率，单位为1/m。
+        /// </summary>
+        public double MaximumCurvaturePerMeter =>
+            Math.Tan(MaximumGcpAngleRadians) /
+            ControlPointRadiusMeters;
+
+        /// <summary>
+        /// 将纵向命令速度和车体中心目标曲率分配为前后GCP运动命令。
+        /// </summary>
+        public GcpMotionCommand Allocate(
+            double speedMetersPerSecond,
+            LateralControlCommand lateralCommand)
+        {
+            EnsureFinite(
+                speedMetersPerSecond,
+                nameof(speedMetersPerSecond));
+
+            var limitedCurvaturePerMeter =
+                Clamp(
+                    lateralCommand
+                        .TargetCurvaturePerMeter,
+                    -MaximumCurvaturePerMeter,
+                    MaximumCurvaturePerMeter);
+
+            var frontAngleRadians =
+                Math.Atan(
+                    limitedCurvaturePerMeter *
+                    ControlPointRadiusMeters);
+            var rearAngleRadians =
+                -frontAngleRadians;
+
+            return new GcpMotionCommand(
+                speedMetersPerSecond,
+                frontAngleRadians,
+                rearAngleRadians);
+        }
+
+        /// <summary>
+        /// 将数值限制在指定闭区间内。
+        /// </summary>
+        private static double Clamp(
+            double value,
+            double minimum,
+            double maximum)
+        {
+            return Math.Max(
+                minimum,
+                Math.Min(maximum, value));
+        }
+
+        /// <summary>
+        /// 检查参数是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "GCP分配参数必须是正有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查参数或命令是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "GCP分配参数和命令必须是有限值。");
+            }
+        }
+    }
+}

MultiWheelC/Control/Allocation/GcpMotionCommand.cs

@@ -1,9 +1,67 @@
-// 表示发送给底盘前的中间命令
-// public readonly struct GcpMotionCommand
-// {
-//     public double SpeedMetersPerSecond { get; }
+using System;
 
-//     public double FrontAngleRadians { get; }
+namespace MultiWheelC.Control.Allocation
+{
+    /// <summary>
+    /// 表示发送给旧版多舵轮四轮解算前的有符号速度和前后GCP角度命令。
+    /// </summary>
+    public readonly struct GcpMotionCommand
+    {
+        /// <summary>
+        /// 创建统一使用m/s和rad的前后几何控制点运动命令。
+        /// </summary>
+        public GcpMotionCommand(
+            double speedMetersPerSecond,
+            double frontAngleRadians,
+            double rearAngleRadians)
+        {
+            EnsureFinite(
+                speedMetersPerSecond,
+                nameof(speedMetersPerSecond));
+            EnsureFinite(
+                frontAngleRadians,
+                nameof(frontAngleRadians));
+            EnsureFinite(
+                rearAngleRadians,
+                nameof(rearAngleRadians));
 
-//     public double RearAngleRadians { get; }
-// }
+            SpeedMetersPerSecond =
+                speedMetersPerSecond;
+            FrontAngleRadians =
+                frontAngleRadians;
+            RearAngleRadians =
+                rearAngleRadians;
+        }
+
+        /// <summary>
+        /// 获取准备交给底盘的有符号纵向速度，单位为m/s，正值表示前进。
+        /// </summary>
+        public double SpeedMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取前几何控制点相对车体X轴的目标方向，单位为rad，逆时针为正。
+        /// </summary>
+        public double FrontAngleRadians { get; }
+
+        /// <summary>
+        /// 获取后几何控制点相对车体X轴的目标方向，单位为rad，逆时针为正。
+        /// </summary>
+        public double RearAngleRadians { get; }
+
+        /// <summary>
+        /// 检查底盘中间命令是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "GCP运动命令必须由有限值组成。");
+            }
+        }
+    }
+}