feat: integrate trajectory tracking controller runtime

2026-08-10 13:26:56 +08:00
parent f923affc8f
commit 54ef239231
29 changed files with 5135 additions and 0 deletions
@@ -0,0 +1,76 @@
+using System;
+
+namespace MultiWheelC.Control.Abstractions
+{
+    /// <summary>
+    /// 表示横向控制器生成的前、后GCP目标转角，单位为rad，逆时针为正。
+    /// </summary>
+    public readonly struct LateralControlCommand
+    {
+        /// <summary>
+        /// 创建前、后GCP目标转角命令。
+        /// </summary>
+        public LateralControlCommand(
+            double frontGcpAngleRadians,
+            double rearGcpAngleRadians)
+        {
+            EnsureFinite(
+                frontGcpAngleRadians,
+                nameof(frontGcpAngleRadians));
+            EnsureFinite(
+                rearGcpAngleRadians,
+                nameof(rearGcpAngleRadians));
+
+            FrontGcpAngleRadians =
+                frontGcpAngleRadians;
+            RearGcpAngleRadians =
+                rearGcpAngleRadians;
+        }
+
+        /// <summary>
+        /// 获取前GCP目标转角，单位为rad，逆时针为正。
+        /// </summary>
+        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, 0.0);
+
+        /// <summary>
+        /// 检查GCP目标转角是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "GCP目标转角必须是有限值。");
+            }
+        }
+    }
+}