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,19 @@
+namespace MultiWheelC.Control.Abstractions
+{
+    /// <summary>
+    /// 定义Stanley、LQR和MPC等车体中心横向控制器的统一接口。
+    /// </summary>
+    public interface ILateralController
+    {
+        /// <summary>
+        /// 根据本周期车辆状态和轨迹误差计算车体中心目标曲率。
+        /// </summary>
+        LateralControlCommand Compute(
+            PathTrackingContext context);
+
+        /// <summary>
+        /// 清除控制器跨周期状态，以便开始新轨迹或异常恢复后重新运行。
+        /// </summary>
+        void Reset();
+    }
+}
@@ -0,0 +1,19 @@
+namespace MultiWheelC.Control.Abstractions
+{
+    /// <summary>
+    /// 定义根据参考速度和实际纵向速度生成底盘命令速度的统一接口。
+    /// </summary>
+    public interface ILongitudinalController
+    {
+        /// <summary>
+        /// 根据本周期速度目标、速度反馈和时间间隔计算有符号底盘命令速度。
+        /// </summary>
+        double ComputeSpeedMetersPerSecond(
+            PathTrackingContext context);
+
+        /// <summary>
+        /// 清除积分、历史误差和其他跨周期状态，以便安全开始新的控制过程。
+        /// </summary>
+        void Reset();
+    }
+}
@@ -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目标转角必须是有限值。");
+            }
+        }
+    }
+}
@@ -0,0 +1,126 @@
+using System;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+
+namespace MultiWheelC.Control.Abstractions
+{
+    /// <summary>
+    /// 保存一次轨迹跟踪控制周期使用的车辆状态、轨迹投影和真实时间间隔。
+    /// </summary>
+    public readonly struct PathTrackingContext
+    {
+        /// <summary>
+        /// 创建横向和纵向控制器共享的只读控制输入快照。
+        /// </summary>
+        public PathTrackingContext(
+            VehicleState vehicleState,
+            TrajectoryProjection projection,
+            double referenceSpeedMetersPerSecond,
+            double deltaTimeSeconds)
+        {
+            EnsureFinite(
+                referenceSpeedMetersPerSecond,
+                nameof(referenceSpeedMetersPerSecond));
+            EnsureFinitePositive(
+                deltaTimeSeconds,
+                nameof(deltaTimeSeconds));
+
+            VehicleState = vehicleState;
+            Projection = projection;
+            ReferenceSpeedMetersPerSecond =
+                referenceSpeedMetersPerSecond;
+            DeltaTimeSeconds = deltaTimeSeconds;
+        }
+
+        /// <summary>
+        /// 获取本周期经过校验的实际车辆位姿和速度状态。
+        /// </summary>
+        public VehicleState VehicleState { get; }
+
+        /// <summary>
+        /// 获取实际车体中心投影到参考轨迹后得到的参考状态和跟踪误差。
+        /// </summary>
+        public TrajectoryProjection Projection { get; }
+
+        /// <summary>
+        /// 获取本次控制计算距离上次计算的真实时间间隔，单位为s。
+        /// </summary>
+        public double DeltaTimeSeconds { get; }
+
+        /// <summary>
+        /// 获取轨迹投影点要求的有符号参考速度，单位为m/s。
+        /// </summary>
+        public double ReferenceSpeedMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取车辆在车体X轴方向上的实际纵向速度，单位为m/s。
+        /// </summary>
+        public double ActualLongitudinalSpeedMetersPerSecond =>
+            VehicleState.TwistInBody
+                .VxMetersPerSecond;
+
+        /// <summary>
+        /// 获取轨迹投影点的参考曲率，单位为1/m，左转为正。
+        /// </summary>
+        public double ReferenceCurvaturePerMeter =>
+            Projection.ReferencePoint
+                .CurvaturePerMeter;
+
+        /// <summary>
+        /// 获取参考轨迹相对车辆的有符号横向误差，单位为m，轨迹在车辆左侧时为正。
+        /// </summary>
+        public double LateralErrorMeters =>
+            Projection.LateralErrorMeters;
+
+        /// <summary>
+        /// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。
+        /// </summary>
+        public double HeadingErrorRadians =>
+            Projection.HeadingErrorRadians;
+
+        /// <summary>
+        /// 获取当前投影位置沿参考轨迹到终点的剩余距离，单位为m。
+        /// </summary>
+        public double RemainingDistanceMeters =>
+            Projection.RemainingDistanceMeters;
+
+        /// <summary>
+        /// 获取实际速度是否已经由至少两个连续有效定位样本估算得到。
+        /// </summary>
+        public bool HasValidVelocityEstimate =>
+            VehicleState.HasValidVelocityEstimate;
+
+        /// <summary>
+        /// 检查控制周期是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                value <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "轨迹跟踪控制周期必须是正有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查控制参考速度是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "轨迹跟踪参考速度必须是有限值。");
+            }
+        }
+    }
+}