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

2026-08-24 18:39:41 +08:00
parent 0ab409cd2a
commit 95c0b19a26
40 changed files with 5983 additions and 95 deletions
@@ -37,14 +37,21 @@ namespace MultiWheelC.Fleet
            double terminalApproachGainPerSecond = 0.8,
            double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
            double curvaturePreviewSeconds = 0.20,
-            double maximumCurvaturePreviewMeters = 0.12)
+            double maximumCurvaturePreviewMeters = 0.12,
+            double motionDirectionInFleetRadians = 0.0)
        {
            NumericGuard.EnsureFinitePositive(
                virtualControlPointRadiusMeters,
                nameof(virtualControlPointRadiusMeters));
+            NumericGuard.EnsureFinite(
+                motionDirectionInFleetRadians,
+                nameof(motionDirectionInFleetRadians));

            VirtualControlPointRadiusMeters =
                virtualControlPointRadiusMeters;
+            MotionDirectionInFleetRadians =
+                AngleMath.NormalizeRadians(
+                    motionDirectionInFleetRadians);
            _trackingCore = new PathTrackingCore(
                lateralController,
                longitudinalController,
@@ -58,12 +65,16 @@ namespace MultiWheelC.Fleet
                terminalApproachGainPerSecond,
                maximumTerminalApproachSpeedMetersPerSecond,
                curvaturePreviewSeconds,
-                maximumCurvaturePreviewMeters);
+                maximumCurvaturePreviewMeters,
+                MotionDirectionInFleetRadians);
        }

        // 虚拟车队中心到前、后GCP的距离，单位为m。
        public double VirtualControlPointRadiusMeters { get; }

+        // 当前运动坐标系+X轴相对车队坐标系+X轴的方向，单位为rad。
+        public double MotionDirectionInFleetRadians { get; }
+
        public double FinishDistanceMeters =>
            _trackingCore.FinishDistanceMeters;

@@ -169,13 +180,20 @@ namespace MultiWheelC.Fleet

            try
            {
-                var twistAtFleetOrigin =
+                var twistAtFleetOriginInMotionFrame =
                    GcpKinematics.ToBodyTwist(
                        output.Command.Value,
                        VirtualControlPointRadiusMeters);
+                var twistAtFleetOriginInFleet =
+                    FrameTransform2D.TransformTwistAtSamePoint(
+                        new Pose2D(
+                            0.0,
+                            0.0,
+                            MotionDirectionInFleetRadians),
+                        twistAtFleetOriginInMotionFrame);
                command = new FleetMotionCommand(
                    Point2D.Zero,
-                    twistAtFleetOrigin);
+                    twistAtFleetOriginInFleet);
                LastCommand = command;
                return FleetControlCycleResult.CommandGenerated;
            }