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,275 @@
+using System;
+using MyParking.Shared;
+
+namespace MultiWheelC.StateEstimation
+{
+    /// <summary>
+    /// 根据连续有效的Detour世界位姿和真实时间差估算车辆二维速度。
+    /// </summary>
+    public sealed class VelocityEstimator2D
+    {
+        public const double DefaultLinearFilterTimeConstantSeconds =
+            0.15;
+        public const double DefaultAngularFilterTimeConstantSeconds =
+            0.20;
+
+        private readonly FirstOrderLowPassFilter
+            _worldVelocityXFilter;
+        private readonly FirstOrderLowPassFilter
+            _worldVelocityYFilter;
+        private readonly FirstOrderLowPassFilter
+            _angularVelocityFilter;
+
+        private bool _hasPreviousSample;
+        private Pose2D _previousPoseInWorld;
+        private double _previousTimestampSeconds;
+
+        /// <summary>
+        /// 创建使用默认0.15s线速度和0.20s角速度时间常数的估计器。
+        /// </summary>
+        public VelocityEstimator2D()
+            : this(
+                DefaultLinearFilterTimeConstantSeconds,
+                DefaultAngularFilterTimeConstantSeconds)
+        {
+        }
+
+        /// <summary>
+        /// 创建使用指定线速度和角速度滤波时间常数的估计器。
+        /// </summary>
+        public VelocityEstimator2D(
+            double linearFilterTimeConstantSeconds,
+            double angularFilterTimeConstantSeconds)
+        {
+            _worldVelocityXFilter =
+                new FirstOrderLowPassFilter(
+                    linearFilterTimeConstantSeconds);
+            _worldVelocityYFilter =
+                new FirstOrderLowPassFilter(
+                    linearFilterTimeConstantSeconds);
+            _angularVelocityFilter =
+                new FirstOrderLowPassFilter(
+                    angularFilterTimeConstantSeconds);
+        }
+
+        /// <summary>
+        /// 获取是否已经保存了可用于下一次差分的位姿基准。
+        /// </summary>
+        public bool HasPreviousSample =>
+            _hasPreviousSample;
+
+        /// <summary>
+        /// 使用一个新的有效定位样本更新并返回车辆状态。
+        /// </summary>
+        public VehicleState Update(
+            Pose2D poseInWorld,
+            double sampleTimestampSeconds)
+        {
+            EnsureFinitePose(
+                poseInWorld,
+                nameof(poseInWorld));
+            EnsureFiniteNonNegative(
+                sampleTimestampSeconds,
+                nameof(sampleTimestampSeconds));
+
+            var normalizedPoseInWorld =
+                new Pose2D(
+                    poseInWorld.XMeters,
+                    poseInWorld.YMeters,
+                    AngleMath.NormalizeRadians(
+                        poseInWorld.YawRadians));
+
+            if (!_hasPreviousSample)
+            {
+                return Reset(
+                    normalizedPoseInWorld,
+                    sampleTimestampSeconds);
+            }
+
+            var deltaTimeSeconds =
+                sampleTimestampSeconds -
+                _previousTimestampSeconds;
+
+            if (deltaTimeSeconds <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(sampleTimestampSeconds),
+                    "新定位样本的单调时间戳必须严格大于上一帧。");
+            }
+
+            var rawVelocityXInWorld =
+                (normalizedPoseInWorld.XMeters -
+                 _previousPoseInWorld.XMeters) /
+                deltaTimeSeconds;
+            var rawVelocityYInWorld =
+                (normalizedPoseInWorld.YMeters -
+                 _previousPoseInWorld.YMeters) /
+                deltaTimeSeconds;
+            var rawAngularVelocity =
+                AngleMath.ShortestDifferenceRadians(
+                    normalizedPoseInWorld.YawRadians,
+                    _previousPoseInWorld.YawRadians) /
+                deltaTimeSeconds;
+
+            var filteredTwistInWorld =
+                new Twist2D(
+                    _worldVelocityXFilter.Update(
+                        rawVelocityXInWorld,
+                        deltaTimeSeconds),
+                    _worldVelocityYFilter.Update(
+                        rawVelocityYInWorld,
+                        deltaTimeSeconds),
+                    _angularVelocityFilter.Update(
+                        rawAngularVelocity,
+                        deltaTimeSeconds));
+
+            _previousPoseInWorld =
+                normalizedPoseInWorld;
+            _previousTimestampSeconds =
+                sampleTimestampSeconds;
+
+            return new VehicleState(
+                sampleTimestampSeconds,
+                normalizedPoseInWorld,
+                filteredTwistInWorld,
+                true);
+        }
+
+        /// <summary>
+        /// 更新位姿差分基准但保留当前滤波速度，避免定位跳变形成虚假速度尖峰。
+        /// </summary>
+        public VehicleState RebasePreservingVelocity(
+            Pose2D poseInWorld,
+            double sampleTimestampSeconds)
+        {
+            EnsureFinitePose(
+                poseInWorld,
+                nameof(poseInWorld));
+            EnsureFiniteNonNegative(
+                sampleTimestampSeconds,
+                nameof(sampleTimestampSeconds));
+
+            var normalizedPoseInWorld =
+                new Pose2D(
+                    poseInWorld.XMeters,
+                    poseInWorld.YMeters,
+                    AngleMath.NormalizeRadians(
+                        poseInWorld.YawRadians));
+
+            _previousPoseInWorld =
+                normalizedPoseInWorld;
+            _previousTimestampSeconds =
+                sampleTimestampSeconds;
+            _hasPreviousSample = true;
+
+            var hasValidVelocityEstimate =
+                _worldVelocityXFilter.IsInitialized &&
+                _worldVelocityYFilter.IsInitialized &&
+                _angularVelocityFilter.IsInitialized;
+
+            var retainedTwistInWorld =
+                hasValidVelocityEstimate
+                    ? new Twist2D(
+                        _worldVelocityXFilter.Value,
+                        _worldVelocityYFilter.Value,
+                        _angularVelocityFilter.Value)
+                    : Twist2D.Zero;
+
+            return new VehicleState(
+                sampleTimestampSeconds,
+                normalizedPoseInWorld,
+                retainedTwistInWorld,
+                hasValidVelocityEstimate);
+        }
+
+        /// <summary>
+        /// 使用当前定位重新建立差分基准，并返回速度无效的零速状态。
+        /// </summary>
+        public VehicleState Reset(
+            Pose2D poseInWorld,
+            double sampleTimestampSeconds)
+        {
+            EnsureFinitePose(
+                poseInWorld,
+                nameof(poseInWorld));
+            EnsureFiniteNonNegative(
+                sampleTimestampSeconds,
+                nameof(sampleTimestampSeconds));
+
+            _previousPoseInWorld =
+                new Pose2D(
+                    poseInWorld.XMeters,
+                    poseInWorld.YMeters,
+                    AngleMath.NormalizeRadians(
+                        poseInWorld.YawRadians));
+            _previousTimestampSeconds =
+                sampleTimestampSeconds;
+            _hasPreviousSample = true;
+
+            _worldVelocityXFilter.Reset();
+            _worldVelocityYFilter.Reset();
+            _angularVelocityFilter.Reset();
+
+            return new VehicleState(
+                sampleTimestampSeconds,
+                _previousPoseInWorld,
+                Twist2D.Zero,
+                false);
+        }
+
+        /// <summary>
+        /// 清除差分基准和全部滤波历史，使下一帧重新初始化估计器。
+        /// </summary>
+        public void Reset()
+        {
+            _hasPreviousSample = false;
+            _previousPoseInWorld = Pose2D.Identity;
+            _previousTimestampSeconds = 0.0;
+
+            _worldVelocityXFilter.Reset();
+            _worldVelocityYFilter.Reset();
+            _angularVelocityFilter.Reset();
+        }
+
+        /// <summary>
+        /// 检查位姿是否由有限数值组成。
+        /// </summary>
+        private static void EnsureFinitePose(
+            Pose2D pose,
+            string parameterName)
+        {
+            if (!IsFinite(pose.XMeters) ||
+                !IsFinite(pose.YMeters) ||
+                !IsFinite(pose.YawRadians))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "速度估计使用的车辆位姿必须由有限数值组成。");
+            }
+        }
+
+        /// <summary>
+        /// 检查数值是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            if (!IsFinite(value) || value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "速度估计使用的采样时刻必须是非负有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 判断数值是否可用于速度估计。
+        /// </summary>
+        private static bool IsFinite(double value)
+        {
+            return !double.IsNaN(value) &&
+                   !double.IsInfinity(value);
+        }
+    }
+}