重构横向控制器输出前后GCP目标转角并简化命令分配器

MultiWheelC/Control/Allocation/GcpCommandAllocator.cs

@@ -4,57 +4,36 @@ using MultiWheelC.Control.Abstractions;
 namespace MultiWheelC.Control.Allocation
 {
     /// <summary>
-    /// 将车体中心目标曲率按对称前后转向策略转换为旧版底盘的前后GCP方向。
+    /// 独立限制前后GCP目标转角并与纵向速度组合成底盘运动命令。
     /// </summary>
-    public sealed class AckermannGcpAllocator
+    public sealed class GcpCommandAllocator
     {
         /// <summary>
-        /// 创建使用指定GCP半间距和最大GCP转角的对称转向分配器。
+        /// 创建使用指定前后GCP最大转角的命令分配器。
         /// </summary>
-        public AckermannGcpAllocator(
-            double controlPointRadiusMeters,
-            double maximumGcpAngleRadians)
+        public GcpCommandAllocator(double maximumGcpAngleRadians)
         {
-            EnsureFinitePositive(
-                controlPointRadiusMeters,
-                nameof(controlPointRadiusMeters));
             EnsureFinitePositive(
                 maximumGcpAngleRadians,
                 nameof(maximumGcpAngleRadians));
 
-            if (maximumGcpAngleRadians >=
-                Math.PI / 2.0)
+            if (maximumGcpAngleRadians >= Math.PI / 2.0)
             {
                 throw new ArgumentOutOfRangeException(
                     nameof(maximumGcpAngleRadians),
-                    "最大GCP转角必须小于π/2，避免曲率换算出现奇异值。");
+                    "最大GCP转角必须小于π/2。");
             }
 
-            ControlPointRadiusMeters =
-                controlPointRadiusMeters;
-            MaximumGcpAngleRadians =
-                maximumGcpAngleRadians;
+            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运动命令。
+        /// 将纵向速度和前后GCP转角组合为底盘运动命令。
         /// </summary>
         public GcpMotionCommand Allocate(
             double speedMetersPerSecond,
@@ -64,19 +43,12 @@ namespace MultiWheelC.Control.Allocation
                 speedMetersPerSecond,
                 nameof(speedMetersPerSecond));
 
-            var limitedCurvaturePerMeter =
-                Clamp(
-                    lateralCommand
-                        .TargetCurvaturePerMeter,
-                    -MaximumCurvaturePerMeter,
-                    MaximumCurvaturePerMeter);
-
-            var frontAngleRadians =
-                Math.Atan(
-                    limitedCurvaturePerMeter *
-                    ControlPointRadiusMeters);
-            var rearAngleRadians =
-                -frontAngleRadians;
+            var frontAngleRadians = ClampSymmetric(
+                lateralCommand.FrontGcpAngleRadians,
+                MaximumGcpAngleRadians);
+            var rearAngleRadians = ClampSymmetric(
+                lateralCommand.RearGcpAngleRadians,
+                MaximumGcpAngleRadians);
 
             return new GcpMotionCommand(
                 speedMetersPerSecond,
@@ -85,16 +57,15 @@ namespace MultiWheelC.Control.Allocation
         }
 
         /// <summary>
-        /// 将数值限制在指定闭区间内。
+        /// 将数值按正负对称方式限制在指定绝对值内。
         /// </summary>
-        private static double Clamp(
+        private static double ClampSymmetric(
             double value,
-            double minimum,
-            double maximum)
+            double maximumAbsoluteValue)
         {
             return Math.Max(
-                minimum,
-                Math.Min(maximum, value));
+                -maximumAbsoluteValue,
+                Math.Min(maximumAbsoluteValue, value));
         }
 
         /// <summary>