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

2026-08-10 17:20:35 +08:00
parent 2f9e4285d3
commit a59499e638
25 changed files with 482 additions and 50 deletions
@@ -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,
+                    "车辆状态输入必须是有限值。");
+            }
+        }
+    }
+}