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,306 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MultiWheelC.Control.Allocation;
+using MultiWheelC.Control.Execution;
+using MultiWheelC.Control.Lateral;
+using MultiWheelC.Control.Longitudinal;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 使用新版横纵向控制器持续跟踪一条世界坐标系二维轨迹。
+    /// </summary>
+    public sealed class TrajectoryTrackingMovement
+        : MovementDefinition
+    {
+        /// <summary>
+        /// 获取或设置本次动作需要跟踪的世界坐标系轨迹。
+        /// </summary>
+        public Trajectory2D Trajectory;
+
+        /// <summary>
+        /// 获取或设置本次动作使用的车辆状态源；为空时自动创建Detour状态源。
+        /// </summary>
+        public IVehicleStateProvider StateProvider;
+
+        /// <summary>
+        /// 获取或设置每个有效控制周期结束后的诊断数据观察回调。
+        /// </summary>
+        public Action<ParkingGeometricController> CycleObserver;
+
+        /// <summary>
+        /// Stanley横向误差增益，单位为1/s。
+        /// </summary>
+        public double StanleyCrossTrackGainPerSecond = 0.4;
+
+        /// <summary>
+        /// Stanley航向误差增益。
+        /// </summary>
+        public double StanleyHeadingErrorGain = 1.0;
+
+        /// <summary>
+        /// Stanley低速分母保护速度，单位为m/s。
+        /// </summary>
+        public double StanleyMinimumSpeedMetersPerSecond = 0.15;
+
+        /// <summary>
+        /// 获取或设置Stanley是否优先使用当前状态源提供的实际纵向速度。
+        /// </summary>
+        public bool StanleyUsesActualSpeed = true;
+
+        /// <summary>
+        /// Stanley横向误差共同转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumCrossTrackCorrectionRadians =
+            AngleMath.DegreesToRadians(10.0);
+
+        /// <summary>
+        /// Stanley航向误差差动转角分量的最大绝对值，单位为rad。
+        /// </summary>
+        public double MaximumHeadingCorrectionRadians =
+            AngleMath.DegreesToRadians(10.0);
+
+        /// <summary>
+        /// 纵向速度外环比例增益。
+        /// </summary>
+        public double LongitudinalKp = 0.5;
+
+        /// <summary>
+        /// 纵向速度外环积分增益，单位为1/s。
+        /// </summary>
+        public double LongitudinalKiPerSecond;
+
+        /// <summary>
+        /// 纵向速度外环微分增益，单位为s。
+        /// </summary>
+        public double LongitudinalKdSeconds;
+
+        /// <summary>
+        /// 纵向积分项允许产生的最大速度修正绝对值，单位为m/s。
+        /// </summary>
+        public double MaximumIntegralCorrectionMetersPerSecond = 0.05;
+
+        /// <summary>
+        /// 纵向PID不进行反馈修正的速度误差死区，单位为m/s。
+        /// </summary>
+        public double LongitudinalSpeedErrorDeadbandMetersPerSecond =
+            0.025;
+
+        /// <summary>
+        /// 底盘纵向命令速度绝对值上限，单位为m/s。
+        /// </summary>
+        public double MaximumCommandSpeedMetersPerSecond = 0.50;
+
+        /// <summary>
+        /// 前后GCP允许的最大转角绝对值，单位为rad。
+        /// </summary>
+        public double MaximumGcpAngleRadians =
+            AngleMath.DegreesToRadians(45.0);
+
+        /// <summary>
+        /// 前后GCP目标转角最大变化率，单位为rad/s。
+        /// </summary>
+        public double MaximumGcpAngleRateRadiansPerSecond =
+            AngleMath.DegreesToRadians(15.0);
+
+        /// <summary>
+        /// 终点位置和剩余弧长的完成容差，单位为m。
+        /// </summary>
+        public double FinishDistanceMeters = 0.03;
+
+        /// <summary>
+        /// 终点停稳判定允许的实际线速度，单位为m/s。
+        /// </summary>
+        public double FinishSpeedMetersPerSecond = 0.02;
+
+        /// <summary>
+        /// 终点航向完成容差，单位为rad。
+        /// </summary>
+        public double FinishHeadingToleranceRadians =
+            AngleMath.DegreesToRadians(3.0);
+
+        /// <summary>
+        /// 车辆允许偏离参考轨迹的最大欧氏距离，单位为m。
+        /// </summary>
+        public double MaximumDistanceToTrajectoryMeters = 0.30;
+
+        /// <summary>
+        /// 单次轨迹动作允许的最长执行时间，单位为s。
+        /// </summary>
+        public double ExecutionTimeoutSeconds = 120.0;
+
+        /// <summary>
+        /// 获取本次动作创建的控制器，尚未开始时为空。
+        /// </summary>
+        public ParkingGeometricController Controller { get; private set; }
+
+        /// <summary>
+        /// 创建控制器并持续执行控制周期，直到轨迹完成、失败或动作被取消。
+        /// </summary>
+        public override IEnumerable<bool> Get()
+        {
+            ValidateParameters();
+
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                throw new InvalidOperationException(
+                    "当前底盘不是MultiWheelChassis，无法执行新版轨迹跟踪动作。");
+            }
+
+            var adapter = new MultiWheelChassisAdapter(
+                chassis,
+                PilotDefinition.Self.CarNum);
+
+            // 新版GCP控制统一以真实车头为车体X正方向，避免继承上一次蟹行偏置。
+            adapter.ResetToBodyFrame();
+
+            var stateProvider =
+                StateProvider ??
+                new DetourVehicleStateProvider();
+            var controlPointRadiusMeters =
+                chassis.ControlPointRadius / 1000.0;
+
+            var lateralController =
+                new StanleyLateralController(
+                    controlPointRadiusMeters,
+                    StanleyCrossTrackGainPerSecond,
+                    StanleyHeadingErrorGain,
+                    StanleyMinimumSpeedMetersPerSecond,
+                    StanleyUsesActualSpeed,
+                    MaximumCrossTrackCorrectionRadians,
+                    MaximumHeadingCorrectionRadians);
+            var longitudinalController =
+                new PidLongitudinalController(
+                    LongitudinalKp,
+                    LongitudinalKiPerSecond,
+                    LongitudinalKdSeconds,
+                    MaximumIntegralCorrectionMetersPerSecond,
+                    MaximumCommandSpeedMetersPerSecond,
+                    LongitudinalSpeedErrorDeadbandMetersPerSecond);
+            var gcpAllocator =
+                new GcpCommandAllocator(
+                    MaximumGcpAngleRadians);
+            var commandExecutor =
+                new GcpCommandExecutor(
+                    adapter,
+                    MaximumGcpAngleRateRadiansPerSecond);
+
+            Controller = new ParkingGeometricController(
+                stateProvider,
+                lateralController,
+                longitudinalController,
+                gcpAllocator,
+                commandExecutor,
+                FinishDistanceMeters,
+                FinishSpeedMetersPerSecond,
+                FinishHeadingToleranceRadians,
+                MaximumDistanceToTrajectoryMeters);
+
+            var clock = Stopwatch.StartNew();
+            var previousCycleSeconds =
+                clock.Elapsed.TotalSeconds;
+            Controller.Start(Trajectory);
+
+            try
+            {
+                while (true)
+                {
+                    if (clock.Elapsed.TotalSeconds >
+                        ExecutionTimeoutSeconds)
+                    {
+                        throw new TimeoutException(
+                            $"新版轨迹跟踪超过{ExecutionTimeoutSeconds:F1}s仍未完成。");
+                    }
+
+                    var currentCycleSeconds =
+                        clock.Elapsed.TotalSeconds;
+                    var deltaTimeSeconds =
+                        currentCycleSeconds -
+                        previousCycleSeconds;
+                    previousCycleSeconds =
+                        currentCycleSeconds;
+
+                    // 极短首周期不参与PID和GCP角速度限制，等待调度器进入下一周期。
+                    if (deltaTimeSeconds <= 1e-6)
+                    {
+                        yield return true;
+                        continue;
+                    }
+
+                    var result =
+                        Controller.ExecuteCycle(
+                            deltaTimeSeconds);
+
+                    if (Controller.LastVehicleState.HasValue)
+                    {
+                        CycleObserver?.Invoke(Controller);
+                    }
+
+                    if (result ==
+                        ParkingControlCycleResult.Completed)
+                    {
+                        break;
+                    }
+
+                    if (result ==
+                        ParkingControlCycleResult.Faulted)
+                    {
+                        throw new InvalidOperationException(
+                            string.IsNullOrWhiteSpace(
+                                Controller.LastFailureReason)
+                                ? "新版轨迹跟踪控制器发生未知故障。"
+                                : Controller.LastFailureReason,
+                            Controller.LastException);
+                    }
+
+                    if (result ==
+                        ParkingControlCycleResult.Inactive)
+                    {
+                        throw new InvalidOperationException(
+                            "新版轨迹跟踪控制器在轨迹完成前意外停止活动。");
+                    }
+
+                    // CommandSent和短暂StateUnavailable均继续下一控制周期；
+                    // 后者已经由控制器主动停车，等待Detour恢复。
+                    yield return true;
+                }
+            }
+            finally
+            {
+                Controller.Cancel();
+            }
+
+            yield return false;
+        }
+
+        /// <summary>
+        /// 在接管实际底盘前检查动作自身无法由子控制器检查的参数。
+        /// </summary>
+        private void ValidateParameters()
+        {
+            if (Trajectory == null)
+            {
+                throw new InvalidOperationException(
+                    "新版轨迹跟踪动作没有设置Trajectory。");
+            }
+
+            if (double.IsNaN(ExecutionTimeoutSeconds) ||
+                double.IsInfinity(ExecutionTimeoutSeconds) ||
+                ExecutionTimeoutSeconds <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(ExecutionTimeoutSeconds),
+                    "轨迹跟踪超时时间必须是正有限值。");
+            }
+        }
+    }
+}