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

MultiWheelC/Control/Abstractions/LateralControlCommand.cs

@@ -3,37 +3,62 @@ using System;
 namespace MultiWheelC.Control.Abstractions
 {
     /// <summary>
-    /// 表示横向控制器生成的车体中心目标曲率命令。
+    /// 表示横向控制器生成的前、后GCP目标转角，单位为rad，逆时针为正。
     /// </summary>
     public readonly struct LateralControlCommand
     {
         /// <summary>
-        /// 创建统一使用SI单位和左转为正约定的横向控制命令。
+        /// 创建前、后GCP目标转角命令。
         /// </summary>
         public LateralControlCommand(
-            double targetCurvaturePerMeter)
+            double frontGcpAngleRadians,
+            double rearGcpAngleRadians)
         {
             EnsureFinite(
-                targetCurvaturePerMeter,
-                nameof(targetCurvaturePerMeter));
+                frontGcpAngleRadians,
+                nameof(frontGcpAngleRadians));
+            EnsureFinite(
+                rearGcpAngleRadians,
+                nameof(rearGcpAngleRadians));
 
-            TargetCurvaturePerMeter =
-                targetCurvaturePerMeter;
+            FrontGcpAngleRadians =
+                frontGcpAngleRadians;
+            RearGcpAngleRadians =
+                rearGcpAngleRadians;
         }
 
         /// <summary>
-        /// 获取车体中心目标轨迹曲率，单位为1/m，左转为正、右转为负。
+        /// 获取前GCP目标转角，单位为rad，逆时针为正。
         /// </summary>
-        public double TargetCurvaturePerMeter { get; }
+        public double FrontGcpAngleRadians { get; }
 
         /// <summary>
-        /// 创建保持直线行驶的零曲率命令。
+        /// 获取后GCP目标转角，单位为rad，逆时针为正。
+        /// </summary>
+        public double RearGcpAngleRadians { get; }
+
+        /// <summary>
+        /// 获取前后GCP的共同转角分量，主要用于横向平移修正。
+        /// </summary>
+        public double CommonAngleRadians =>
+            (FrontGcpAngleRadians +
+             RearGcpAngleRadians) / 2.0;
+
+        /// <summary>
+        /// 获取前后GCP的差动转角分量，主要用于曲率前馈和航向修正。
+        /// </summary>
+        public double DifferentialAngleRadians =>
+            (FrontGcpAngleRadians -
+             RearGcpAngleRadians) / 2.0;
+
+        /// <summary>
+        /// 创建前后GCP均保持车头方向的直线命令。
         /// </summary>
         public static LateralControlCommand Straight =>
-            new LateralControlCommand(0.0);
+            new LateralControlCommand(0.0, 0.0);
 
         /// <summary>
-        /// 检查横向曲率命令是否为有限值。
+        /// 检查GCP目标转角是否为有限值。
         /// </summary>
         private static void EnsureFinite(
             double value,
@@ -44,7 +69,7 @@ namespace MultiWheelC.Control.Abstractions
             {
                 throw new ArgumentOutOfRangeException(
                     parameterName,
-                    "横向控制目标曲率必须是有限值。");
+                    "GCP目标转角必须是有限值。");
             }
         }
     }