优化轨迹制动与轮速反馈并补充实验记录和中英文说明

2026-08-10 17:20:35 +08:00
parent 2f9e4285d3
commit a59499e638
25 changed files with 482 additions and 50 deletions
@@ -51,7 +51,8 @@ namespace MultiWheelC.Control.Execution
            double finishSpeedMetersPerSecond = 0.02,
            double finishHeadingToleranceRadians =
                3.0 * Math.PI / 180.0,
-            double maximumDistanceToTrajectoryMeters = 0.30)
+            double maximumDistanceToTrajectoryMeters = 0.30,
+            double terminalBrakingPreviewMeters = 0.02)
        {
            _stateProvider = stateProvider ??
                throw new ArgumentNullException(
@@ -81,6 +82,9 @@ namespace MultiWheelC.Control.Execution
            EnsureFinitePositive(
                maximumDistanceToTrajectoryMeters,
                nameof(maximumDistanceToTrajectoryMeters));
+            EnsureFiniteNonNegative(
+                terminalBrakingPreviewMeters,
+                nameof(terminalBrakingPreviewMeters));

            FinishDistanceMeters = finishDistanceMeters;
            FinishSpeedMetersPerSecond =
@@ -89,6 +93,8 @@ namespace MultiWheelC.Control.Execution
                finishHeadingToleranceRadians;
            MaximumDistanceToTrajectoryMeters =
                maximumDistanceToTrajectoryMeters;
+            TerminalBrakingPreviewMeters =
+                terminalBrakingPreviewMeters;
        }

        /// <summary>
@@ -111,6 +117,11 @@ namespace MultiWheelC.Control.Execution
        /// </summary>
        public double MaximumDistanceToTrajectoryMeters { get; }

+        /// <summary>
+        /// 获取沿轨迹提前读取更低制动参考速度的距离，单位为m。
+        /// </summary>
+        public double TerminalBrakingPreviewMeters { get; }
+
        /// <summary>
        /// 获取控制器当前是否持有并正在执行一条轨迹。
        /// </summary>
@@ -288,7 +299,7 @@ namespace MultiWheelC.Control.Execution
        }

        /// <summary>
-        /// 在轨迹起点零速固定点处读取前方速度，并限制为低速起步命令。
+        /// 在轨迹起步区域内保持最低释放速度，避免空间速度曲线零速固定点。
        /// </summary>
        private double ResolveReferenceSpeedForControl(
            TrajectoryProjection projection)
@@ -297,15 +308,18 @@ namespace MultiWheelC.Control.Execution
                projection.ReferencePoint
                    .ReferenceSpeedMetersPerSecond;

-            var requiresStartupRelease =
+            currentReferenceSpeed =
+                ApplyTerminalBrakingPreview(
+                    projection,
+                    currentReferenceSpeed);
+
+            var isInStartupRegion =
                projection.ArcLengthMeters <=
                    StartupRegionMeters &&
                projection.RemainingDistanceMeters >
-                    FinishDistanceMeters &&
-                Math.Abs(currentReferenceSpeed) <=
-                    ZeroReferenceSpeedToleranceMetersPerSecond;
+                    FinishDistanceMeters;

-            if (!requiresStartupRelease)
+            if (!isInStartupRegion)
            {
                return currentReferenceSpeed;
            }
@@ -314,6 +328,7 @@ namespace MultiWheelC.Control.Execution
                _trajectory.TotalLengthMeters,
                projection.ArcLengthMeters +
                StartupPreviewDistanceMeters);
+
            var previewReferenceSpeed =
                _trajectory
                    .SampleAtArcLength(
@@ -323,13 +338,67 @@ namespace MultiWheelC.Control.Execution
            if (Math.Abs(previewReferenceSpeed) <=
                ZeroReferenceSpeedToleranceMetersPerSecond)
            {
-                return 0.0;
+                return currentReferenceSpeed;
            }

-            return Math.Sign(previewReferenceSpeed) *
-                   Math.Min(
-                       Math.Abs(previewReferenceSpeed),
-                       MaximumStartupSpeedMetersPerSecond);
+            var startupReleaseSpeed =
+                Math.Sign(previewReferenceSpeed) *
+                Math.Min(
+                    Math.Abs(previewReferenceSpeed),
+                    MaximumStartupSpeedMetersPerSecond);
+
+            // 当前空间速度已经高于起步释放速度时，
+            // 正常交还给原始速度曲线。
+            if (Math.Sign(currentReferenceSpeed) ==
+                    Math.Sign(startupReleaseSpeed) &&
+                Math.Abs(currentReferenceSpeed) >=
+                    Math.Abs(startupReleaseSpeed))
+            {
+                return currentReferenceSpeed;
+            }
+
+            return startupReleaseSpeed;
+        }
+
+        /// <summary>
+        /// 采用前方更低的同方向参考速度，使车辆在终点减速段提前制动。
+        /// </summary>
+        private double ApplyTerminalBrakingPreview(
+            TrajectoryProjection projection,
+            double currentReferenceSpeed)
+        {
+            if (TerminalBrakingPreviewMeters <= 0.0)
+            {
+                return currentReferenceSpeed;
+            }
+
+            var previewArcLengthMeters = Math.Min(
+                _trajectory.TotalLengthMeters,
+                projection.ArcLengthMeters +
+                TerminalBrakingPreviewMeters);
+            var previewReferenceSpeed =
+                _trajectory
+                    .SampleAtArcLength(
+                        previewArcLengthMeters)
+                    .ReferenceSpeedMetersPerSecond;
+
+            var previewIsStop =
+                Math.Abs(previewReferenceSpeed) <=
+                ZeroReferenceSpeedToleranceMetersPerSecond;
+            var hasSameDirection =
+                Math.Sign(previewReferenceSpeed) ==
+                Math.Sign(currentReferenceSpeed);
+            var previewIsSlower =
+                Math.Abs(previewReferenceSpeed) <
+                Math.Abs(currentReferenceSpeed);
+
+            if (previewIsSlower &&
+                (previewIsStop || hasSameDirection))
+            {
+                return previewReferenceSpeed;
+            }
+
+            return currentReferenceSpeed;
        }

        /// <summary>
@@ -37,7 +37,7 @@ namespace MultiWheelC
        public double StraightMaximumSpeedMetersPerSecond = 0.40; // 直线限速。
        public double CurveMaximumSpeedMetersPerSecond = 0.30; // 转弯和过渡段限速。
        public double AccelerationMetersPerSecondSquared = 0.20; // 参考加速度。
-        public double DecelerationMetersPerSecondSquared = 0.12; // 参考减速度。
+        public double DecelerationMetersPerSecondSquared = 0.08; // 参考减速度。
        public double PointSpacingMeters = 0.02; // 离散轨迹点间距。

        /// <summary>
@@ -124,7 +124,7 @@ namespace MultiWheelC
        /// <summary>
        /// 获取或设置参考速度减速度，单位为m/s²。
        /// </summary>
-        public double DecelerationMetersPerSecondSquared = 0.10;
+        public double DecelerationMetersPerSecondSquared = 0.08;

        /// <summary>
        /// 获取或设置离散轨迹点间距，单位为m。
@@ -485,7 +485,7 @@ namespace MultiWheelC
        /// <summary>
        /// 获取或设置参考速度减速度，单位为m/s²。
        /// </summary>
-        public double DecelerationMetersPerSecondSquared = 0.12;
+        public double DecelerationMetersPerSecondSquared = 0.08;

        /// <summary>
        /// 获取或设置离散轨迹点间距，单位为m。
@@ -126,6 +126,11 @@ namespace MultiWheelC
        public double FinishHeadingToleranceRadians =
            AngleMath.DegreesToRadians(3.0);

+        /// <summary>
+        /// 终点减速阶段提前读取参考速度的距离，单位为m。
+        /// </summary>
+        public double TerminalBrakingPreviewMeters = 0.02;
+
        /// <summary>
        /// 车辆允许偏离参考轨迹的最大欧氏距离，单位为m。
        /// </summary>
@@ -203,7 +208,8 @@ namespace MultiWheelC
                FinishDistanceMeters,
                FinishSpeedMetersPerSecond,
                FinishHeadingToleranceRadians,
-                MaximumDistanceToTrajectoryMeters);
+                MaximumDistanceToTrajectoryMeters,
+                TerminalBrakingPreviewMeters);

            var clock = Stopwatch.StartNew();
            var previousCycleSeconds =
@@ -0,0 +1,274 @@
+using System;
+using CommonUsage.Chassis;
+using MyParking.Shared;
+using System.Diagnostics;
+
+namespace MultiWheelC.StateEstimation
+{
+    /// <summary>
+    /// 保留外部状态源的Detour位姿，并以舵轮电机反馈解算的车体纵向速度替换Detour差分纵向速度。
+    /// </summary>
+    public sealed class WheelFeedbackVehicleStateProvider
+        : IVehicleStateProvider
+    {
+        private readonly Stopwatch _wheelSpeedClock = Stopwatch.StartNew();
+        public const double DefaultVelocityFilterTimeConstantSeconds =
+            0.10;
+
+        private readonly object _syncRoot = new object();
+        private readonly IVehicleStateProvider _poseProvider;
+        private readonly MultiWheelChassis _chassis;
+        private readonly FirstOrderLowPassFilter _longitudinalSpeedFilter;
+
+        private bool _hasPreviousTimestamp;
+        private double _previousTimestampSeconds;
+        private bool _hasVelocityDiagnostics;
+        private double _latestDetourBodyVxMetersPerSecond;
+        private bool _latestDetourVelocityValid;
+        private double _latestRawWheelBodyVxMetersPerSecond;
+        private double _latestFilteredWheelBodyVxMetersPerSecond;
+        private bool _latestWheelVelocityValid;
+
+        /// <summary>
+        /// 创建使用默认0.10s低通时间常数的电机反馈纵向速度状态源。
+        /// </summary>
+        public WheelFeedbackVehicleStateProvider(
+            IVehicleStateProvider poseProvider,
+            MultiWheelChassis chassis)
+            : this(
+                poseProvider,
+                chassis,
+                DefaultVelocityFilterTimeConstantSeconds)
+        {
+        }
+
+        /// <summary>
+        /// 创建使用指定低通时间常数的电机反馈纵向速度状态源。
+        /// </summary>
+        public WheelFeedbackVehicleStateProvider(
+            IVehicleStateProvider poseProvider,
+            MultiWheelChassis chassis,
+            double velocityFilterTimeConstantSeconds)
+        {
+            _poseProvider = poseProvider ??
+                throw new ArgumentNullException(
+                    nameof(poseProvider));
+            _chassis = chassis ??
+                throw new ArgumentNullException(
+                    nameof(chassis));
+            _longitudinalSpeedFilter =
+                new FirstOrderLowPassFilter(
+                    velocityFilterTimeConstantSeconds);
+        }
+
+        /// <summary>
+        /// 获取最近一次读取失败的原因，正常时为空字符串。
+        /// </summary>
+        public string LastFailureReason { get; private set; } =
+            string.Empty;
+
+        /// <summary>
+        /// 读取Detour位姿和电机反馈速度，并组合成统一车辆状态。
+        /// </summary>
+        public bool TryGetState(out VehicleState state)
+        {
+            lock (_syncRoot)
+            {
+                if (!_poseProvider.TryGetState(
+                        out var poseState))
+                {
+                    state = default;
+                    LastFailureReason =
+                        "基础位姿状态源暂时不可用。";
+                    return false;
+                }
+
+                try
+                {
+                    var actualCarSpeed =
+                        _chassis.GetCarSpeed(true);
+                    var rawLongitudinalSpeedMetersPerSecond =
+                        (double)actualCarSpeed.Vx;
+
+                    EnsureFinite(
+                        rawLongitudinalSpeedMetersPerSecond,
+                        "电机反馈车体纵向速度");
+
+                    var wheelSpeedTimestampSeconds =
+                        _wheelSpeedClock.Elapsed.TotalSeconds;
+
+                    var filteredLongitudinalSpeedMetersPerSecond =
+                        UpdateLongitudinalSpeedFilter(
+                            rawLongitudinalSpeedMetersPerSecond,
+                            wheelSpeedTimestampSeconds,
+                            out var hasValidWheelSpeedEstimate);
+
+                    _latestDetourBodyVxMetersPerSecond =
+                        poseState.TwistInBody.VxMetersPerSecond;
+                    _latestDetourVelocityValid =
+                        poseState.HasValidVelocityEstimate;
+                    _latestRawWheelBodyVxMetersPerSecond =
+                        rawLongitudinalSpeedMetersPerSecond;
+                    _latestFilteredWheelBodyVxMetersPerSecond =
+                        filteredLongitudinalSpeedMetersPerSecond;
+                    _latestWheelVelocityValid =
+                        hasValidWheelSpeedEstimate;
+                    _hasVelocityDiagnostics = true;
+
+                    // 第一阶段只替换控制器使用的车体纵向速度；横向速度和角速度
+                    // 继续使用Detour估计，避免轮速差和舵角误差放大Vy与Omega噪声。
+                    var twistInBody = new Twist2D(
+                        filteredLongitudinalSpeedMetersPerSecond,
+                        poseState.TwistInBody
+                            .VyMetersPerSecond,
+                        poseState.TwistInBody
+                            .OmegaRadiansPerSecond);
+
+                    var twistInWorld =
+                        FrameTransform2D
+                            .TransformTwistAtSamePoint(
+                                poseState.PoseInWorld,
+                                twistInBody);
+
+                    state = new VehicleState(
+                        poseState.SampleTimestampSeconds,
+                        poseState.PoseInWorld,
+                        twistInWorld,
+                        hasValidWheelSpeedEstimate);
+
+                    LastFailureReason = string.Empty;
+                    return true;
+                }
+                catch (Exception exception)
+                {
+                    state = default;
+                    LastFailureReason =
+                        "舵轮电机反馈车体速度解算失败：" +
+                        exception.Message;
+                    return false;
+                }
+            }
+        }
+
+        /// <summary>
+        /// 读取最近一帧Detour纵向速度和轮速解算纵向速度，供实验记录使用。
+        /// </summary>
+        public bool TryGetLatestVelocityDiagnostics(
+            out double detourBodyVxMetersPerSecond,
+            out bool detourVelocityValid,
+            out double rawWheelBodyVxMetersPerSecond,
+            out double filteredWheelBodyVxMetersPerSecond,
+            out bool wheelVelocityValid)
+        {
+            lock (_syncRoot)
+            {
+                detourBodyVxMetersPerSecond =
+                    _latestDetourBodyVxMetersPerSecond;
+                detourVelocityValid =
+                    _latestDetourVelocityValid;
+                rawWheelBodyVxMetersPerSecond =
+                    _latestRawWheelBodyVxMetersPerSecond;
+                filteredWheelBodyVxMetersPerSecond =
+                    _latestFilteredWheelBodyVxMetersPerSecond;
+                wheelVelocityValid =
+                    _latestWheelVelocityValid;
+                return _hasVelocityDiagnostics;
+            }
+        }
+
+        /// <summary>
+        /// 清除电机反馈速度的时间基准和低通滤波历史。
+        /// </summary>
+        public void Reset()
+        {
+            lock (_syncRoot)
+            {
+                _longitudinalSpeedFilter.Reset();
+                _wheelSpeedClock.Restart();
+                _hasPreviousTimestamp = false;
+                _previousTimestampSeconds = 0.0;
+                _hasVelocityDiagnostics = false;
+                _latestDetourBodyVxMetersPerSecond = 0.0;
+                _latestDetourVelocityValid = false;
+                _latestRawWheelBodyVxMetersPerSecond = 0.0;
+                _latestFilteredWheelBodyVxMetersPerSecond = 0.0;
+                _latestWheelVelocityValid = false;
+                LastFailureReason = string.Empty;
+            }
+        }
+
+        /// <summary>
+        /// 使用真实状态时间间隔更新纵向速度低通滤波，并在首帧建立基准。
+        /// </summary>
+        private double UpdateLongitudinalSpeedFilter(
+            double rawLongitudinalSpeedMetersPerSecond,
+            double timestampSeconds,
+            out bool hasValidWheelSpeedEstimate)
+        {
+            EnsureFiniteNonNegative(
+                timestampSeconds,
+                nameof(timestampSeconds));
+
+            if (!_hasPreviousTimestamp)
+            {
+                _longitudinalSpeedFilter.Reset(
+                    rawLongitudinalSpeedMetersPerSecond);
+                _previousTimestampSeconds = timestampSeconds;
+                _hasPreviousTimestamp = true;
+                hasValidWheelSpeedEstimate = false;
+                return rawLongitudinalSpeedMetersPerSecond;
+            }
+
+            var deltaTimeSeconds =
+                timestampSeconds -
+                _previousTimestampSeconds;
+            _previousTimestampSeconds = timestampSeconds;
+
+            if (deltaTimeSeconds <= 0.0)
+            {
+                _longitudinalSpeedFilter.Reset(
+                    rawLongitudinalSpeedMetersPerSecond);
+                hasValidWheelSpeedEstimate = false;
+                return rawLongitudinalSpeedMetersPerSecond;
+            }
+
+            hasValidWheelSpeedEstimate = true;
+            return _longitudinalSpeedFilter.Update(
+                rawLongitudinalSpeedMetersPerSecond,
+                deltaTimeSeconds);
+        }
+
+        /// <summary>
+        /// 检查采样时刻是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (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,
+                    "车辆状态输入必须是有限值。");
+            }
+        }
+    }
+}