完善原地自转控制逻辑并加入纵向速度死区与实验绘图改进

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-07 13:00:56 +08:00
parent f8881bc243
commit 14ca1150e4
12 changed files with 971 additions and 70 deletions
@@ -7,6 +7,7 @@ using ClumsyCore.Pilot;
 using CommonUsage.Chassis;
 using MDCSToolBox.Commons.Controllers;
 using MyParking.Shared;
+using MultiWheelC.StateEstimation;

 namespace MultiWheelC
 {
@@ -17,7 +18,11 @@ namespace MultiWheelC
        /// </summary>
        public float AngleTarget;

-        public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
+        // 留作标定或单元测试时显式替换；为空时使用经过校验的Detour状态源。
+        public Func<float> ThetaReader;
+
+        public IVehicleStateProvider StateProvider =
+            new DetourVehicleStateProvider();

        public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;

@@ -37,6 +42,12 @@ namespace MultiWheelC
        // 自转舵轮准备超时时间，单位s。
        public float WheelAlignmentTimeoutSeconds = 10f;

+        // 航向尚未到位时允许下发的最小有效角速度，单位deg/s。
+        public float MinimumAngularSpeedDegreesPerSecond = 1f;
+
+        // 舵轮到位后执行航向闭环允许的最长时间，单位s。
+        public float RotationTimeoutSeconds = 15f;
+
        // 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
        public override IEnumerable<bool> Get()
        {
@@ -44,6 +55,8 @@ namespace MultiWheelC
                throw new InvalidOperationException(
                    "当前底盘不是MultiWheelChassis，无法执行原地自转。");

+            ValidateParameters();
+
            var adapter = new MultiWheelChassisAdapter(
                Chassis,
                PilotDefinition.Self.CarNum);
@@ -55,7 +68,9 @@ namespace MultiWheelC
                DateTime? alignedSince = null;
                while (true)
                {
-                    if (!adapter.PrepareSpin())
+                    if (!adapter.PrepareSpin(
+                            alignmentToleranceDegrees:
+                                WheelAlignmentToleranceDegrees))
                        throw new InvalidOperationException(
                            "无法生成原地自转舵轮目标：" +
                            adapter.LastFailureReason);
@@ -84,18 +99,86 @@ namespace MultiWheelC
                    yield return true;
                }

+                var alignmentToleranceRadians =
+                    AngleMath.DegreesToRadians(
+                        WheelAlignmentToleranceDegrees);
+                if (!adapter.AdoptPreparedSpinForXYTh(
+                        alignmentToleranceRadians))
+                {
+                    throw new InvalidOperationException(
+                        "无法将已到位的自转舵角交接给XYTh：" +
+                        adapter.LastFailureReason);
+                }
+
                var targetAngle =
                    (float)AngleMath.NormalizeDegrees(AngleTarget);
                var p = PidparamsRead();
-                thPid = new PIDController(ThetaReader, p.Kp);
+                var currentAngle = ReadCurrentAngleDegrees();
+                var cachedCurrentAngle = currentAngle;
+                thPid = new PIDController(
+                    () => cachedCurrentAngle,
+                    p.Kp);
                thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
                    p.OutputUpperThreshold, p.SpeedAccPerSec);
                var lastCommandTime = DateTime.Now;
+                var rotationStarted = DateTime.Now;

                while (true)
                {
+                    if ((DateTime.Now - rotationStarted)
+                            .TotalSeconds >
+                        RotationTimeoutSeconds)
+                    {
+                        throw new TimeoutException(
+                            $"原地自转超过{RotationTimeoutSeconds:F1}s仍未到位。");
+                    }
+
+                    currentAngle = ReadCurrentAngleDegrees();
+                    cachedCurrentAngle = currentAngle;
                    var s = thPid.GetResponse(targetAngle, true);
-                    Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
+                    var angleErrorDegrees =
+                        (float)AngleMath
+                            .ShortestDifferenceDegrees(
+                                targetAngle,
+                                currentAngle);
+
+                    // PID进入到位死区后等待其0.3s稳定确认；等待期间
+                    // 只清零驱动速度，不清除已经准备好的自转舵角状态。
+                    if (Math.Abs(angleErrorDegrees) <=
+                        p.DeadZone)
+                    {
+                        CommandAngularSpeedObserver?.Invoke(0f);
+                        adapter
+                            .StopXYThDrivePreserveSteeringState();
+
+                        if (thPid.IsArrived())
+                            break;
+
+                        yield return true;
+                        continue;
+                    }
+
+                    // PID输出低于底盘有效轮速范围时提高到最小可执行值，
+                    // 避免接近目标时反复出现微小命令但车辆实际不动。
+                    if (Math.Abs(s) > 1e-6f &&
+                        Math.Abs(s) <
+                            MinimumAngularSpeedDegreesPerSecond)
+                    {
+                        s = Math.Sign(angleErrorDegrees) *
+                            MinimumAngularSpeedDegreesPerSecond;
+                    }
+
+                    // PID加速限制在首周期可能暂时输出零；此时保留
+                    // 已交接的自转状态，等待下一周期产生有效角速度。
+                    if (Math.Abs(s) <= 1e-6f)
+                    {
+                        CommandAngularSpeedObserver?.Invoke(0f);
+                        adapter
+                            .StopXYThDrivePreserveSteeringState();
+                        yield return true;
+                        continue;
+                    }
+
                    CommandAngularSpeedObserver?.Invoke(s);
                    var now = DateTime.Now;
                    var interval = now - lastCommandTime;
@@ -118,7 +201,6 @@ namespace MultiWheelC
                            "安全XYTh原地旋转底盘解算失败：" +
                            adapter.LastFailureReason);
                    }
-                    if (thPid.IsArrived()) break;
                    yield return true;
                }

@@ -130,5 +212,109 @@ namespace MultiWheelC
                adapter.StopImmediately();
            }
        }
+
+        /// <summary>
+        /// 检查原地自转的舵轮准备、最小速度和超时参数是否可执行。
+        /// </summary>
+        private void ValidateParameters()
+        {
+            EnsureFinitePositive(
+                WheelAlignmentToleranceDegrees,
+                nameof(WheelAlignmentToleranceDegrees),
+                allowZero: true);
+            EnsureFinitePositive(
+                WheelAlignmentStableSeconds,
+                nameof(WheelAlignmentStableSeconds),
+                allowZero: true);
+            EnsureFinitePositive(
+                WheelAlignmentTimeoutSeconds,
+                nameof(WheelAlignmentTimeoutSeconds));
+            EnsureFinitePositive(
+                MinimumAngularSpeedDegreesPerSecond,
+                nameof(MinimumAngularSpeedDegreesPerSecond));
+            EnsureFinitePositive(
+                RotationTimeoutSeconds,
+                nameof(RotationTimeoutSeconds));
+
+            var pidParameters = PidparamsRead();
+            if (pidParameters == null)
+            {
+                throw new InvalidOperationException(
+                    "原地自转PID参数读取结果为空。");
+            }
+
+            EnsureFinitePositive(
+                pidParameters.DeadZone,
+                "PidparamsRead.DeadZone");
+            EnsureFinitePositive(
+                pidParameters.OutputUpperThreshold,
+                "PidparamsRead.OutputUpperThreshold");
+            EnsureFinitePositive(
+                pidParameters.SpeedAccPerSec,
+                "PidparamsRead.SpeedAccPerSec");
+            EnsureFinitePositive(
+                pidParameters.Kp,
+                "PidparamsRead.Kp");
+
+            if (MinimumAngularSpeedDegreesPerSecond >
+                pidParameters.OutputUpperThreshold)
+            {
+                throw new InvalidOperationException(
+                    "原地自转最小有效角速度不能大于最大角速度。");
+            }
+        }
+
+        /// <summary>
+        /// 读取经过状态源校验的世界航向，显式设置ThetaReader时优先使用替代读数。
+        /// </summary>
+        private float ReadCurrentAngleDegrees()
+        {
+            if (ThetaReader != null)
+            {
+                var angleDegrees = ThetaReader();
+                if (float.IsNaN(angleDegrees) ||
+                    float.IsInfinity(angleDegrees))
+                {
+                    throw new InvalidOperationException(
+                        "自定义航向读取结果不是有效角度。");
+                }
+
+                return (float)AngleMath.NormalizeDegrees(
+                    angleDegrees);
+            }
+
+            if (StateProvider == null ||
+                !StateProvider.TryGetState(out var state))
+            {
+                throw new InvalidOperationException(
+                    "无法从Detour状态源读取有效车辆航向。" +
+                    (StateProvider is DetourVehicleStateProvider provider
+                        ? provider.LastFailureReason
+                        : ""));
+            }
+
+            return (float)AngleMath.RadiansToDegrees(
+                state.PoseInWorld.YawRadians);
+        }
+
+        /// <summary>
+        /// 检查原地自转参数是否为正有限值，部分时间和容差参数允许为零。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            float value,
+            string parameterName,
+            bool allowZero = false)
+        {
+            if (float.IsNaN(value) ||
+                float.IsInfinity(value) ||
+                (allowZero
+                    ? value < 0f
+                    : value <= 0f))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "原地自转参数必须是有效的正数。");
+            }
+        }
    }
 }
@@ -75,6 +75,12 @@ namespace MultiWheelC
        /// </summary>
        public double MaximumIntegralCorrectionMetersPerSecond = 0.05;

+        /// <summary>
+        /// 纵向PID不进行反馈修正的速度误差死区，单位为m/s。
+        /// </summary>
+        public double LongitudinalSpeedErrorDeadbandMetersPerSecond =
+            0.025;
+
        /// <summary>
        /// 底盘纵向命令速度绝对值上限，单位为m/s。
        /// </summary>
@@ -90,7 +96,7 @@ namespace MultiWheelC
        /// 前后GCP目标转角最大变化率，单位为rad/s。
        /// </summary>
        public double MaximumGcpAngleRateRadiansPerSecond =
-            AngleMath.DegreesToRadians(10.0);
+            AngleMath.DegreesToRadians(15.0);

        /// <summary>
        /// 终点位置和剩余弧长的完成容差，单位为m。
@@ -164,7 +170,8 @@ namespace MultiWheelC
                    LongitudinalKiPerSecond,
                    LongitudinalKdSeconds,
                    MaximumIntegralCorrectionMetersPerSecond,
-                    MaximumCommandSpeedMetersPerSecond);
+                    MaximumCommandSpeedMetersPerSecond,
+                    LongitudinalSpeedErrorDeadbandMetersPerSecond);
            var gcpAllocator =
                new AckermannGcpAllocator(
                    controlPointRadiusMeters,