增加车队轨迹控制核心与轮组自转模式

2026-08-21 17:33:29 +08:00
parent fea2265e2d
commit 0ab409cd2a
33 changed files with 1949 additions and 989 deletions
@@ -0,0 +1,64 @@
+using MyParking.Shared;
+
+namespace MultiWheelC.Fleet
+{
+    // 一次经过校验的虚拟车队原点状态快照。
+    public readonly struct FleetState
+    {
+        public FleetState(
+            double sampleTimestampSeconds,
+            Pose2D fleetPoseInWorld,
+            Twist2D twistAtFleetOriginInWorld,
+            bool hasValidVelocityEstimate)
+        {
+            NumericGuard.EnsureFiniteNonNegative(
+                sampleTimestampSeconds,
+                nameof(sampleTimestampSeconds));
+            NumericGuard.EnsureFinite(
+                fleetPoseInWorld,
+                nameof(fleetPoseInWorld));
+            NumericGuard.EnsureFinite(
+                twistAtFleetOriginInWorld,
+                nameof(twistAtFleetOriginInWorld));
+
+            SampleTimestampSeconds =
+                sampleTimestampSeconds;
+            FleetPoseInWorld = new Pose2D(
+                fleetPoseInWorld.XMeters,
+                fleetPoseInWorld.YMeters,
+                AngleMath.NormalizeRadians(
+                    fleetPoseInWorld.YawRadians));
+            HasValidVelocityEstimate =
+                hasValidVelocityEstimate;
+
+            // 位姿有效但速度尚未初始化时显式置零，避免控制器误用输入值。
+            TwistAtFleetOriginInWorld =
+                hasValidVelocityEstimate
+                    ? twistAtFleetOriginInWorld
+                    : Twist2D.Zero;
+
+            var worldPoseInFleet =
+                FrameTransform2D.Inverse(
+                    FleetPoseInWorld);
+            TwistAtFleetOriginInFleet =
+                FrameTransform2D.TransformTwistAtSamePoint(
+                    worldPoseInFleet,
+                    TwistAtFleetOriginInWorld);
+        }
+
+        // 状态源单调时钟中的采样时刻，单位为s。
+        public double SampleTimestampSeconds { get; }
+
+        // 车队坐标系原点在世界坐标系中的实际位姿。
+        public Pose2D FleetPoseInWorld { get; }
+
+        // 车队原点处的实际刚体速度，在世界坐标系中表达。
+        public Twist2D TwistAtFleetOriginInWorld { get; }
+
+        // 同一刚体速度在车队坐标系中表达，供车队控制器使用。
+        public Twist2D TwistAtFleetOriginInFleet { get; }
+
+        // 速度是否已经初始化并可用于闭环控制。
+        public bool HasValidVelocityEstimate { get; }
+    }
+}