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

2026-08-07 18:13:55 +08:00
parent bc37e71ad0
commit 88f651e0c2
25 changed files with 310 additions and 105 deletions
@@ -0,0 +1,104 @@
+using System;
+using MultiWheelC.Control.Abstractions;
+
+namespace MultiWheelC.Control.Allocation
+{
+    /// <summary>
+    /// 独立限制前后GCP目标转角并与纵向速度组合成底盘运动命令。
+    /// </summary>
+    public sealed class GcpCommandAllocator
+    {
+        /// <summary>
+        /// 创建使用指定前后GCP最大转角的命令分配器。
+        /// </summary>
+        public GcpCommandAllocator(double maximumGcpAngleRadians)
+        {
+            EnsureFinitePositive(
+                maximumGcpAngleRadians,
+                nameof(maximumGcpAngleRadians));
+
+            if (maximumGcpAngleRadians >= Math.PI / 2.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(maximumGcpAngleRadians),
+                    "最大GCP转角必须小于π/2。");
+            }
+
+            MaximumGcpAngleRadians = maximumGcpAngleRadians;
+        }
+
+        /// <summary>
+        /// 获取前后GCP允许的最大转角绝对值，单位为rad。
+        /// </summary>
+        public double MaximumGcpAngleRadians { get; }
+
+        /// <summary>
+        /// 将纵向速度和前后GCP转角组合为底盘运动命令。
+        /// </summary>
+        public GcpMotionCommand Allocate(
+            double speedMetersPerSecond,
+            LateralControlCommand lateralCommand)
+        {
+            EnsureFinite(
+                speedMetersPerSecond,
+                nameof(speedMetersPerSecond));
+
+            var frontAngleRadians = ClampSymmetric(
+                lateralCommand.FrontGcpAngleRadians,
+                MaximumGcpAngleRadians);
+            var rearAngleRadians = ClampSymmetric(
+                lateralCommand.RearGcpAngleRadians,
+                MaximumGcpAngleRadians);
+
+            return new GcpMotionCommand(
+                speedMetersPerSecond,
+                frontAngleRadians,
+                rearAngleRadians);
+        }
+
+        /// <summary>
+        /// 将数值按正负对称方式限制在指定绝对值内。
+        /// </summary>
+        private static double ClampSymmetric(
+            double value,
+            double maximumAbsoluteValue)
+        {
+            return Math.Max(
+                -maximumAbsoluteValue,
+                Math.Min(maximumAbsoluteValue, 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分配参数和命令必须是有限值。");
+            }
+        }
+    }
+}