完善状态估计、车队协调与舵轮辨识日志

2026-08-24 18:39:41 +08:00
parent 0ab409cd2a
commit 95c0b19a26
40 changed files with 5983 additions and 95 deletions
@@ -0,0 +1,181 @@
+using System;
+using System.Collections.Generic;
+using MyParking.Shared;
+// 夹紧后只执行一次 → 建立固定布局
+namespace MultiWheelC.Fleet
+{
+    // 建立编队时使用的一辆成员车世界位姿快照。
+    public readonly struct FleetMemberPose
+    {
+        public FleetMemberPose(
+            int vehicleId,
+            Pose2D poseInWorld)
+        {
+            if (vehicleId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(vehicleId),
+                    "编队成员车号必须大于零。");
+            }
+
+            NumericGuard.EnsureFinite(
+                poseInWorld,
+                nameof(poseInWorld));
+
+            VehicleId = vehicleId;
+            PoseInWorld = new Pose2D(
+                poseInWorld.XMeters,
+                poseInWorld.YMeters,
+                AngleMath.NormalizeRadians(
+                    poseInWorld.YawRadians));
+        }
+
+        public int VehicleId { get; }
+
+        public Pose2D PoseInWorld { get; }
+    }
+
+    // 保存布局建立时的车队世界位姿和固定成员布局。
+    public readonly struct FleetLayoutCaptureResult
+    {
+        public FleetLayoutCaptureResult(
+            Pose2D fleetPoseInWorld,
+            FleetLayout layout)
+        {
+            NumericGuard.EnsureFinite(
+                fleetPoseInWorld,
+                nameof(fleetPoseInWorld));
+
+            FleetPoseInWorld = new Pose2D(
+                fleetPoseInWorld.XMeters,
+                fleetPoseInWorld.YMeters,
+                AngleMath.NormalizeRadians(
+                    fleetPoseInWorld.YawRadians));
+            Layout = layout ??
+                throw new ArgumentNullException(
+                    nameof(layout));
+        }
+
+        public Pose2D FleetPoseInWorld { get; }
+
+        public FleetLayout Layout { get; }
+    }
+
+    // 根据同一世界坐标系中的成员位姿建立车队几何中心和固定布局。
+    public static class FleetLayoutCapture
+    {
+        public static FleetLayoutCaptureResult Capture(
+            IReadOnlyList<FleetMemberPose> memberPoses,
+            int leaderVehicleId)
+        {
+            if (memberPoses == null)
+            {
+                throw new ArgumentNullException(
+                    nameof(memberPoses));
+            }
+
+            if (memberPoses.Count == 0)
+            {
+                throw new ArgumentException(
+                    "建立编队布局至少需要一辆成员车。",
+                    nameof(memberPoses));
+            }
+
+            if (leaderVehicleId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(leaderVehicleId),
+                    "主车车号必须大于零。");
+            }
+
+            var vehicleIds = new HashSet<int>();
+            var centerXMeters = 0.0;
+            var centerYMeters = 0.0;
+            var leaderFound = false;
+            var leaderYawRadians = 0.0;
+
+            for (var index = 0;
+                 index < memberPoses.Count;
+                 index++)
+            {
+                var memberPose = memberPoses[index];
+                if (memberPose.VehicleId <= 0)
+                {
+                    throw new ArgumentOutOfRangeException(
+                        nameof(memberPoses),
+                        $"第{index}辆成员车的车号必须大于零。");
+                }
+
+                NumericGuard.EnsureFinite(
+                    memberPose.PoseInWorld,
+                    $"{nameof(memberPoses)}[{index}]." +
+                    nameof(FleetMemberPose.PoseInWorld));
+
+                if (!vehicleIds.Add(memberPose.VehicleId))
+                {
+                    throw new ArgumentException(
+                        $"成员位姿包含重复车号{memberPose.VehicleId}。",
+                        nameof(memberPoses));
+                }
+
+                centerXMeters +=
+                    memberPose.PoseInWorld.XMeters;
+                centerYMeters +=
+                    memberPose.PoseInWorld.YMeters;
+
+                if (memberPose.VehicleId == leaderVehicleId)
+                {
+                    leaderFound = true;
+                    leaderYawRadians =
+                        memberPose.PoseInWorld.YawRadians;
+                }
+            }
+
+            if (!leaderFound)
+            {
+                throw new ArgumentException(
+                    $"成员位姿中不存在主车{leaderVehicleId}。",
+                    nameof(leaderVehicleId));
+            }
+
+            centerXMeters /= memberPoses.Count;
+            centerYMeters /= memberPoses.Count;
+            NumericGuard.EnsureFinite(
+                centerXMeters,
+                nameof(centerXMeters));
+            NumericGuard.EnsureFinite(
+                centerYMeters,
+                nameof(centerYMeters));
+
+            var fleetPoseInWorld = new Pose2D(
+                centerXMeters,
+                centerYMeters,
+                AngleMath.NormalizeRadians(
+                    leaderYawRadians));
+            var worldPoseInFleet =
+                FrameTransform2D.Inverse(
+                    fleetPoseInWorld);
+            var vehicleLayouts =
+                new VehicleLayout[memberPoses.Count];
+
+            for (var index = 0;
+                 index < memberPoses.Count;
+                 index++)
+            {
+                var memberPose = memberPoses[index];
+                var poseInFleet =
+                    FrameTransform2D.Compose(
+                        worldPoseInFleet,
+                        memberPose.PoseInWorld);
+                vehicleLayouts[index] =
+                    new VehicleLayout(
+                        memberPose.VehicleId,
+                        poseInFleet);
+            }
+
+            return new FleetLayoutCaptureResult(
+                fleetPoseInWorld,
+                new FleetLayout(vehicleLayouts));
+        }
+    }
+}