拆分MultiWheelC并新增轨迹投影、Detour状态估计与Stanley跟踪控制

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-05 18:07:01 +08:00
parent 7447317812
commit 47973cc94b
38 changed files with 3982 additions and 976 deletions
@@ -0,0 +1,138 @@
+using System;
+using MyParking.Shared;
+
+namespace MultiWheelC.StateEstimation
+{
+    /// <summary>
+    /// 保存一次经过校验的车辆位姿和速度估计快照，统一使用SI单位。
+    /// </summary>
+    public readonly struct VehicleState
+    {
+        /// <summary>
+        /// 创建车辆状态，并将世界坐标速度同步转换到车体坐标系。
+        /// </summary>
+        public VehicleState(
+            double sampleTimestampSeconds,
+            Pose2D poseInWorld,
+            Twist2D twistInWorld,
+            bool hasValidVelocityEstimate)
+        {
+            EnsureFiniteNonNegative(
+                sampleTimestampSeconds,
+                nameof(sampleTimestampSeconds));
+            EnsureFinitePose(
+                poseInWorld,
+                nameof(poseInWorld));
+            EnsureFiniteTwist(
+                twistInWorld,
+                nameof(twistInWorld));
+
+            SampleTimestampSeconds =
+                sampleTimestampSeconds;
+            PoseInWorld = new Pose2D(
+                poseInWorld.XMeters,
+                poseInWorld.YMeters,
+                AngleMath.NormalizeRadians(
+                    poseInWorld.YawRadians));
+            HasValidVelocityEstimate =
+                hasValidVelocityEstimate;
+
+            // 第一帧或定位重置后的速度不可用于闭环控制，
+            // 此时显式置零，避免调用方误用残留速度。
+            TwistInWorld = hasValidVelocityEstimate
+                ? twistInWorld
+                : Twist2D.Zero;
+
+            var worldPoseInBody =
+                FrameTransform2D.Inverse(
+                    PoseInWorld);
+            TwistInBody =
+                FrameTransform2D.TransformTwistAtSamePoint(
+                    worldPoseInBody,
+                    TwistInWorld);
+        }
+
+        /// <summary>
+        /// 获取状态源单调时钟中的采样时刻，单位为s。
+        /// </summary>
+        public double SampleTimestampSeconds { get; }
+
+        /// <summary>
+        /// 获取车体中心在Detour世界坐标系中的位姿，单位为m和rad。
+        /// </summary>
+        public Pose2D PoseInWorld { get; }
+
+        /// <summary>
+        /// 获取在世界坐标系中表达的车辆速度，单位为m/s和rad/s。
+        /// </summary>
+        public Twist2D TwistInWorld { get; }
+
+        /// <summary>
+        /// 获取在车体坐标系中表达的车辆速度，X向前、Y向左、逆时针为正。
+        /// </summary>
+        public Twist2D TwistInBody { get; }
+
+        /// <summary>
+        /// 获取当前速度是否已由至少两个连续有效定位样本估算得到。
+        /// </summary>
+        public bool HasValidVelocityEstimate { get; }
+
+        /// <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 EnsureFiniteTwist(
+            Twist2D twist,
+            string parameterName)
+        {
+            if (!IsFinite(twist.VxMetersPerSecond) ||
+                !IsFinite(twist.VyMetersPerSecond) ||
+                !IsFinite(twist.OmegaRadiansPerSecond))
+            {
+                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);
+        }
+    }
+}