添加单车底盘仿真平台并完善运动控制与夹臂功能

Shared/ChassisCommand.cs

@@ -0,0 +1,183 @@
+// 纯数据层：只描述坐标、速度和命令
+// 定义二维坐标、位姿、速度、车队布局和单车底盘命令。
+// Shared层统一使用SI单位：位置m、线速度m/s、角度rad、角速度rad/s。
+// 车体坐标系采用右手系：X向前、Y向左、逆时针角度和角速度为正。
+// 命名约定：XxxInYyy表示Xxx在Yyy坐标系中的表达。
+
+namespace MyParking.Shared
+{
+    /// <summary>
+    /// 二维坐标点，X、Y单位均为米。
+    /// </summary>
+    public readonly struct Point2D
+    {
+        public Point2D(double xMeters, double yMeters)
+        {
+            XMeters = xMeters;
+            YMeters = yMeters;
+        }
+
+        public double XMeters { get; }
+
+        public double YMeters { get; }
+
+        public static Point2D Zero => new Point2D(0.0, 0.0);
+    }
+
+    /// <summary>
+    /// 二维局部坐标系在父坐标系中的位姿。
+    /// 位置单位为米，朝向单位为弧度，逆时针为正。
+    /// 具体父子关系由变量名称说明，例如RadarPoseInBody。
+    /// </summary>
+    public readonly struct Pose2D
+    {
+        public Pose2D(
+            double xMeters,
+            double yMeters,
+            double yawRadians)
+        {
+            XMeters = xMeters;
+            YMeters = yMeters;
+            YawRadians = yawRadians;
+        }
+
+        public double XMeters { get; }
+
+        public double YMeters { get; }
+
+        public double YawRadians { get; }
+
+        public Point2D Position =>
+            new Point2D(XMeters, YMeters);
+
+        public static Pose2D Identity =>
+            new Pose2D(0.0, 0.0, 0.0);
+    }
+
+    /// <summary>
+    /// 二维刚体速度。
+    /// 线速度单位为m/s，角速度单位为rad/s。
+    /// 速度所属坐标系由持有该Twist2D的外层类型或变量名称确定。
+    /// </summary>
+    public readonly struct Twist2D
+    {
+        public Twist2D(
+            double vxMetersPerSecond,
+            double vyMetersPerSecond,
+            double omegaRadiansPerSecond)
+        {
+            VxMetersPerSecond = vxMetersPerSecond;
+            VyMetersPerSecond = vyMetersPerSecond;
+            OmegaRadiansPerSecond = omegaRadiansPerSecond;
+        }
+
+        public double VxMetersPerSecond { get; }
+
+        public double VyMetersPerSecond { get; }
+
+        public double OmegaRadiansPerSecond { get; }
+
+        public static Twist2D Zero =>
+            new Twist2D(0.0, 0.0, 0.0);
+    }
+
+    /// <summary>
+    /// 发送给单辆车的车体坐标系速度命令。
+    /// </summary>
+    public readonly struct ChassisCommand
+    {
+        public ChassisCommand(
+            int vehicleId,
+            Twist2D bodyTwist)
+        {
+            VehicleId = vehicleId;
+            BodyTwist = bodyTwist;
+        }
+
+        public int VehicleId { get; }
+
+        /// <summary>
+        /// 单车车体坐标系速度：X向前、Y向左、逆时针旋转为正。
+        /// </summary>
+        public Twist2D BodyTwist { get; }
+
+        /// <summary>
+        /// 创建指定车辆的停止命令。
+        /// </summary>
+        public static ChassisCommand Stop(int vehicleId)
+        {
+            return new ChassisCommand(
+                vehicleId,
+                Twist2D.Zero);
+        }
+    }
+
+    /// <summary>
+    /// 单辆车的车体坐标系在车队坐标系中的位姿。
+    /// </summary>
+    public readonly struct VehicleLayout
+    {
+        public VehicleLayout(
+            int vehicleId,
+            Pose2D poseInFleet)
+        {
+            VehicleId = vehicleId;
+            PoseInFleet = poseInFleet;
+        }
+
+        public int VehicleId { get; }
+
+        public Pose2D PoseInFleet { get; }
+    }
+
+    /// <summary>
+    /// 车队整体运动命令，速度分量均在车队坐标系中表达。
+    /// </summary>
+    public readonly struct FleetMotionCommand
+    {
+        public FleetMotionCommand(
+            Point2D referencePointInFleet,
+            Twist2D twistAtReferencePoint)
+        {
+            ReferencePointInFleet = referencePointInFleet;
+            TwistAtReferencePoint = twistAtReferencePoint;
+        }
+
+        /// <summary>
+        /// 速度命令对应的参考点，也可作为自定义旋转中心。
+        /// </summary>
+        public Point2D ReferencePointInFleet { get; }
+
+        /// <summary>
+        /// 参考点处的车队速度。
+        /// </summary>
+        public Twist2D TwistAtReferencePoint { get; }
+
+        /// <summary>
+        /// 创建绕指定中心原地旋转的车队命令。
+        /// </summary>
+        public static FleetMotionCommand RotateAround(
+            Point2D rotationCenterInFleet,
+            double omegaRadiansPerSecond)
+        {
+            return new FleetMotionCommand(
+                rotationCenterInFleet,
+                new Twist2D(
+                    0.0,
+                    0.0,
+                    omegaRadiansPerSecond));
+        }
+
+        /// <summary>
+        /// 创建车队停止命令。
+        /// </summary>
+        public static FleetMotionCommand Stop()
+        {
+            return new FleetMotionCommand(
+                Point2D.Zero,
+                Twist2D.Zero);
+        }
+    }
+
+
+}

Shared/FleetKinematics.cs

@@ -0,0 +1 @@
+// 把车队整体速度分解为每辆车的局部速度

Shared/FrameTransform2D.cs

@@ -0,0 +1,177 @@
+// 车体、运动、车队坐标系之间的转换
+using System;
+
+namespace MyParking.Shared
+{
+    /// <summary>
+    /// 提供二维刚体坐标系之间的点、向量、位姿和速度变换。
+    /// 坐标系采用X向前、Y向左、逆时针为正的右手系。
+    /// </summary>
+    public static class FrameTransform2D
+    {
+        private const double TwoPi = 2.0 * Math.PI;
+        /// <summary>
+        /// 将角度归一化到[-π, π)范围。
+        /// </summary>
+        public static double NormalizeAngle(double angleRadians)
+        {
+
+            if (double.IsNaN(angleRadians) || double.IsInfinity(angleRadians))
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(angleRadians),
+                    "角度必须是有限数值。");
+            }
+            angleRadians %= TwoPi;
+            if (angleRadians >= Math.PI)
+                angleRadians -= TwoPi;
+            if (angleRadians < -Math.PI)
+                angleRadians += TwoPi;
+            return angleRadians;
+
+        }
+
+        /// <summary>
+        /// 计算从current到target的最短角度差。
+        /// 返回正值表示逆时针旋转。
+        /// </summary>
+        public static double ShortestAngleDifference(
+            double targetRadians,
+            double currentRadians)
+        {
+            return NormalizeAngle(targetRadians - currentRadians);
+        }
+
+        /// <summary>
+        /// 将源坐标系中的点变换到目标坐标系。
+        /// sourcePoseInTarget表示源坐标系在目标坐标系中的位姿。
+        /// </summary>
+        public static Point2D TransformPoint(
+            Pose2D sourcePoseInTarget,
+            Point2D pointInSource)
+        {
+            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
+            var sin = Math.Sin(sourcePoseInTarget.YawRadians);
+
+            return new Point2D(
+                sourcePoseInTarget.XMeters +
+                cos * pointInSource.XMeters -
+                sin * pointInSource.YMeters,
+
+                sourcePoseInTarget.YMeters +
+                sin * pointInSource.XMeters +
+                cos * pointInSource.YMeters);
+        }
+
+        /// <summary>
+        /// 将目标坐标系中的点反向变换到源坐标系。
+        /// </summary>
+        public static Point2D InverseTransformPoint(
+            Pose2D sourcePoseInTarget,
+            Point2D pointInTarget)
+        {
+            var dx = pointInTarget.XMeters - sourcePoseInTarget.XMeters;
+
+            var dy = pointInTarget.YMeters - sourcePoseInTarget.YMeters;
+
+            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
+            var sin = Math.Sin(sourcePoseInTarget.YawRadians);
+
+            return new Point2D(
+                cos * dx + sin * dy,
+                -sin * dx + cos * dy);
+        }
+
+        /// <summary>
+        /// 将源坐标系中的向量旋转到目标坐标系。
+        /// 向量没有位置，因此不叠加平移量。
+        /// </summary>
+        public static Point2D TransformVector(
+            Pose2D sourcePoseInTarget,
+            Point2D vectorInSource)
+        {
+            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
+            var sin = Math.Sin(sourcePoseInTarget.YawRadians);
+
+            return new Point2D(
+                cos * vectorInSource.XMeters -
+                sin * vectorInSource.YMeters,
+
+                sin * vectorInSource.XMeters +
+                cos * vectorInSource.YMeters);
+        }
+
+        /// <summary>
+        /// 组合两级坐标变换。
+        /// parentFromMiddle表示middle在parent中的位姿；
+        /// middleFromChild表示child在middle中的位姿；
+        /// 返回child在parent中的位姿。
+        /// </summary>
+        public static Pose2D Compose(
+            Pose2D parentFromMiddle,
+            Pose2D middleFromChild)
+        {
+            var childPositionInParent = TransformPoint(
+                parentFromMiddle,
+                middleFromChild.Position);
+
+            return new Pose2D(
+                childPositionInParent.XMeters,
+                childPositionInParent.YMeters,
+                NormalizeAngle(
+                    parentFromMiddle.YawRadians +
+                    middleFromChild.YawRadians));
+        }
+
+        /// <summary>
+        /// 对坐标变换求逆。
+        /// 输入child在parent中的位姿，返回parent在child中的位姿。
+        /// </summary>
+        public static Pose2D Inverse(Pose2D childPoseInParent)
+        {
+            var cos = Math.Cos(childPoseInParent.YawRadians);
+            var sin = Math.Sin(childPoseInParent.YawRadians);
+
+            return new Pose2D(
+                -cos * childPoseInParent.XMeters -
+                sin * childPoseInParent.YMeters,
+
+                sin * childPoseInParent.XMeters -
+                cos * childPoseInParent.YMeters,
+
+                NormalizeAngle(
+                    -childPoseInParent.YawRadians));
+        }
+
+
+        /// <summary>
+        /// 将源坐标系中的位姿变换到目标坐标系。
+        /// </summary>
+        public static Pose2D TransformPose(
+            Pose2D sourcePoseInTarget,
+            Pose2D poseInSource)
+        {
+            return Compose(sourcePoseInTarget, poseInSource);
+        }
+
+        /// <summary>
+        /// 转换同一物理参考点处的速度表达坐标系。
+        /// 只旋转线速度，角速度保持不变。
+        /// </summary>
+        public static Twist2D TransformTwistAtSamePoint(
+            Pose2D sourcePoseInTarget,
+            Twist2D twistInSource)
+        {
+            var linearVelocityInTarget = TransformVector(
+                sourcePoseInTarget,
+                new Point2D(
+                    twistInSource.VxMetersPerSecond,
+                    twistInSource.VyMetersPerSecond));
+
+            return new Twist2D(
+                linearVelocityInTarget.XMeters,
+                linearVelocityInTarget.YMeters,
+                twistInSource.OmegaRadiansPerSecond);
+        }
+    }
+}

Shared/MultiWheelChassisAdapter.cs

@@ -0,0 +1,289 @@
+// 将统一命令转换为原 Chassis API 调用
+using System;
+using CommonUsage.Chassis;
+
+namespace MyParking.Shared
+{
+    /// <summary>
+    /// 将统一的单车车体速度命令转换为旧版MultiWheelChassis调用。
+    /// 车体坐标系固定为X向前、Y向左、逆时针为正。
+    /// </summary>
+    public sealed class MultiWheelChassisAdapter
+    {
+        #region 辅助内容
+        private const double RadiansToDegrees = 180.0 / Math.PI;
+        private const float BiasTolerance = 0.001f;
+        private readonly MultiWheelChassis _chassis;
+        /// <summary>
+        /// 当前适配器对应的车辆编号。
+        /// </summary>
+        public int VehicleId { get; }
+
+        /// <summary>
+        /// 检查旧底盘是否仍处于无偏置的真实车体坐标系。
+        /// </summary>
+        private void EnsureBodyFrameIsActive()
+        {
+            var bias = _chassis.GetOriginBias();
+
+            if (Math.Abs(bias.X) <= BiasTolerance &&
+                Math.Abs(bias.Y) <= BiasTolerance &&
+                Math.Abs(bias.Z) <= BiasTolerance)
+            {
+                return;
+            }
+
+            throw new InvalidOperationException(
+                "MultiWheelChassis的坐标偏置在适配器创建后被修改。" +
+                $"当前偏置为X={bias.X}, Y={bias.Y}, Th={bias.Z}°。" +
+                "请不要再调用DirectionAngle或SetOriginBias控制蟹行。");
+        }
+        /// <summary>
+        /// 检查底盘命令是否包含无效数值。
+        /// </summary>
+        private static void ValidateTwist(Twist2D twist)
+        {
+            ValidateFinite(
+                twist.VxMetersPerSecond,
+                nameof(twist.VxMetersPerSecond));
+
+            ValidateFinite(
+                twist.VyMetersPerSecond,
+                nameof(twist.VyMetersPerSecond));
+
+            ValidateFinite(
+                twist.OmegaRadiansPerSecond,
+                nameof(twist.OmegaRadiansPerSecond));
+        }
+        /// <summary>
+        /// 检查数值是否为有限值。
+        /// </summary>
+        private static void ValidateFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "底盘速度命令不能是NaN或无穷大。");
+            }
+
+            if (value > float.MaxValue ||
+                value < -float.MaxValue)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "底盘速度命令超过float可表示范围。");
+            }
+        }
+
+        /// <summary>
+        /// 获取最近一次底盘运动分解失败原因。
+        /// </summary>
+        public string LastFailureReason =>
+            _chassis.LastMotionDecomposeFailureReason;
+        #endregion
+
+        /// <summary>
+        /// 将旧底盘的原点偏置恢复为真实单车车体坐标系。
+        /// </summary>
+        public void ResetToBodyFrame()
+        {
+            _chassis.SetOriginBias(
+                x: 0.0f,
+                y: 0.0f,
+                th: 0.0f);
+        }
+        public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId)
+        {
+            _chassis = chassis ?? throw new ArgumentNullException(nameof(chassis));
+            if (vehicleId <= 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(vehicleId),
+                    "车辆编号必须大于零。");
+            }
+            VehicleId = vehicleId;
+#pragma warning disable CS0612, CS0618
+            var wheels = _chassis.GetSteerWheels();
+#pragma warning restore CS0612, CS0618
+
+            if (wheels.Count == 0)
+            {
+                throw new InvalidOperationException(
+                    "MultiWheelChassis尚未完成舵轮初始化，" +
+                    "不能创建底盘适配器。");
+            }
+            // 禁用旧版DirectionAngle/ZeroDirection坐标偏置，
+            // 保证SendXYThSpeed直接使用真实车体坐标系。
+            ResetToBodyFrame();
+        }
+
+
+        /// <summary>
+        /// 将车体坐标系速度命令发送给多舵轮底盘。
+        /// </summary>
+        public bool Send(ChassisCommand command, TimeSpan? interval = null)
+        {
+            if (command.VehicleId != VehicleId)
+            {
+                throw new InvalidOperationException(
+                    $"命令车辆编号{command.VehicleId}与适配器车辆编号" +
+                    $"{VehicleId}不一致。");
+            }
+            ValidateTwist(command.BodyTwist);
+            // 防止其他旧逻辑再次调用DirectionAngle或
+            // SetOriginBias改变底盘坐标语义。
+            EnsureBodyFrameIsActive();
+            var vxMetersPerSecond =
+                            (float)command.BodyTwist.VxMetersPerSecond;
+            var vyMetersPerSecond =
+                (float)command.BodyTwist.VyMetersPerSecond;
+            var omegaDegreesPerSecond =
+                (float)(
+                    command.BodyTwist.OmegaRadiansPerSecond *
+                    RadiansToDegrees);
+            var success = _chassis.SendXYThSpeed(
+                                vxMetersPerSecond,
+                                vyMetersPerSecond,
+                                omegaDegreesPerSecond,
+                                interval);
+            if (!success)
+            {
+                // 防止分解失败后继续执行上一条运动命令。
+                _chassis.PredefinedDriveStop();
+            }
+            return success;
+        }
+        /// <summary>
+        /// 按底盘减速度配置平滑停车，需要在控制周期中持续调用。
+        /// </summary>
+        public void RampStop(TimeSpan? interval = null)
+        {
+            _chassis.RampStop(interval);
+        }
+        /// <summary>
+        /// 立即将所有驱动轮速度下发为零。
+        /// </summary>
+        public void StopImmediately()
+        {
+            _chassis.PredefinedDriveStop();
+        }
+
+        /// <summary>
+        /// 停车并将所有舵轮转到指定的车体角度。
+        /// 只调整舵轮角度，不产生车辆线速度。
+        /// </summary>
+        public bool PrepareParallelDirection(
+            double directionRadians)
+        {
+            EnsureBodyFrameIsActive();
+            var targetDegrees = (float)(FrameTransform2D.NormalizeAngle(directionRadians) *
+                                    RadiansToDegrees);
+
+#pragma warning disable CS0612, CS0618
+            var wheels = _chassis.GetSteerWheels();
+#pragma warning restore CS0612, CS0618
+
+            // 没有舵轮时不能认为预对齐成功。
+            if (wheels.Count == 0)
+            {
+                return false;
+            }
+
+            // 先检查所有舵轮能否到达目标机械角度。
+            foreach (var wheel in wheels)
+            {
+                if (targetDegrees < wheel.AngleLowerLimit ||
+                    targetDegrees > wheel.AngleUpperLimit)
+                {
+                    return false;
+                }
+            }
+
+            // 模式切换前立即停止驱动轮。
+            _chassis.PredefinedDriveStop();
+            // 检查完成后再统一下发，避免只转动一部分舵轮。
+            foreach (var wheel in wheels)
+            {
+                wheel.WriteAngle(targetDegrees);
+            }
+
+            return true;
+        }
+
+        /// <summary>
+        /// 检查所有舵轮是否已经对准给定方向。
+        /// </summary>
+        public bool AreParallelWheelsAligned(
+            double directionRadians,
+            double toleranceRadians)
+        {
+            if (double.IsNaN(toleranceRadians) ||
+        double.IsInfinity(toleranceRadians) ||
+        toleranceRadians < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(toleranceRadians),
+                    "舵轮到位容差必须是非负有限值。");
+            }
+
+            EnsureBodyFrameIsActive();
+            var targetDegrees = (float)(
+                FrameTransform2D.NormalizeAngle(directionRadians) *
+                180.0 / Math.PI);
+
+            var toleranceDegrees = (float)(
+                Math.Abs(toleranceRadians) *
+                180.0 / Math.PI);
+
+#pragma warning disable CS0612, CS0618
+            var wheels = _chassis.GetSteerWheels();
+#pragma warning restore CS0612, CS0618
+
+            foreach (var wheel in wheels)
+            {
+                var angleErrorDegrees = targetDegrees - wheel.ReadAngle();
+
+                if (Math.Abs(angleErrorDegrees) >
+                    toleranceDegrees)
+                {
+                    return false;
+                }
+            }
+
+            return true;
+        }
+
+        /// <summary>
+        /// 停车并将舵轮预对齐到原地自转方向。
+        /// 返回是否成功生成舵轮目标。
+        /// </summary>
+        public bool PrepareSpin(
+            TimeSpan? interval = null)
+        {
+            EnsureBodyFrameIsActive();
+            _chassis.PredefinedDriveStop();
+
+            var success =
+                _chassis.SendRotateMotion(
+                    0.0f,
+                    interval);
+
+            if (!success)
+            {
+                _chassis.PredefinedDriveStop();
+            }
+            return success;
+        }
+        /// <summary>
+        /// 所有舵轮是否已对齐到原地自转方向。
+        /// </summary>
+        public bool AreSpinWheelsAligned => _chassis.LastRotateAligned;
+
+
+
+    }
+}

Shared/SteeringConstraintManager.cs

@@ -0,0 +1,225 @@
+// 然后因为我这个车的舵轮是有正负120°限制的，所以有这种情况，比如我给了+90°让它进入蟹行模式，然后我在蟹行模式我去追一个相对于车左侧方向+40°的轨迹，那这时候就超过限制了，那我这是要做一个什么措施吗，比如我能想到的就是达到这个限位要么就停车打印说明到达限位，安全措施；要么就是我在追这个轨迹的过程中知道我大概可能超过限位，然后我提前停下，自转，调整为车头朝向为轨迹的方向，也就是自转调整到我车当前模式的初始姿态方向这种；你有什么好的建议吗，其他的建议吗，或者有什么可行的方法，因为实际的过程还是需要考虑这个限位的问题的
+
+
+
+
+
+
+// 9:49
+
+
+
+
+
+
+// 这个问题必须考虑，但“目标轮角超过 +120°”不一定意味着命令无法执行。对于舵轮，首先要利用“轮角反向、轮速反向”的等价关系，然后才考虑停车和车身重定向。
+// 1. +130° 不一定超过限位后就无法运动
+// 你的例子是：
+// 蟹行基准方向：+90°
+// 轨迹又向该方向增加：+40°
+// 期望运动方向：+130°
+// 舵轮限制：[-120°, +120°]
+// 直接使用：
+// 轮角：+130°
+// 轮速：+0.3 m/s
+// 确实超过了 +120°。
+// 但它有一个完全等价的表达：
+// 轮角：-50°
+// 轮速：-0.3 m/s
+// 因为：
+// (θ, v) ≡ (θ - 180°, -v)
+// 即：
+// (+130°, +0.3) ≡ (-50°, -0.3)
+// 虽然轮子的机械朝向和转速符号变了，但轮胎与地面的速度向量完全相同。
+// 所以第一层措施应当是：
+// 先尝试期望角度
+// 如果超过限位
+//     尝试角度±180°
+//     同时将轮速取反
+// 如果等价角度满足限位
+//     使用等价解
+// 否则才判定无法执行
+// 你当前的 CommonUsage.dll 中已经有类似的舵角反解逻辑，SendXYThSpeed() 返回 bool 也是为了告诉上层运动分解是否成功。
+// 2. 你的±120°结构在静态上覆盖了所有运动方向
+// 因为：
+// 可转范围总宽度 = 240°
+// 大于 180°。
+// 只要驱动轮允许正反转，那么对于任意二维速度方向，通常都能在下面两种表达中找到一个合法值：
+// (θ, +v)
+// (θ ± 180°, -v)
+// 因此单纯从“某一时刻目标速度方向”看，+130° 可以用 -50° 表达，不一定需要旋转车身。
+// 真正困难的是动态过程：
+// 当前轮角：+90°
+// 新目标等价轮角：-50°
+// 需要转动：-140°
+// 虽然最终角度合法，但舵轮需要转过很大的角度。在舵轮没有到位前，不能继续以正常速度驱动车辆，否则实际运动方向会与轨迹命令不一致。
+// 所以核心问题不是“有没有合法终态”，而是：
+// 从当前轮角切换到合法目标轮角的过程是否安全、连续。
+
+// 3. 建议采用四层处理策略
+// 第一层：等价舵角选择
+// 为每一个舵轮计算速度向量：
+// Vix = Vx - ω·yi
+// Viy = Vy + ω·xi
+// 然后：
+// θi = atan2(Viy, Vix)
+// si = sqrt(Vix² + Viy²)
+// 尝试候选解：
+// 候选1：(θi, si)
+// 候选2：(θi + 180°, -si)
+// 候选3：(θi - 180°, -si)
+// 过滤掉超过 [-120°, +120°] 的候选，再选择相对当前机械轮角转动最小的候选。
+// 这里计算机械转角距离时，不要简单把差值归一化成最短圆周角，因为舵轮不能无限旋转。对于有限机械区间，应当直接比较：
+// Math.Abs(candidateAngle - currentMechanicalAngle)
+// 第二层：软限位
+// 不要把 ±120° 当作正常工作的边界，建议设置软限位，例如：
+// 物理硬限位：[-120°, +120°]
+// 软件工作限位：[-105°, +105°]
+// 预警区域：[105°, 120°]
+// 具体余量需要实车标定，初期可以保留 10°～20°。
+// 状态可以分成：
+// |θ| < 100°       正常
+// 100°～105°       接近限位，开始规划
+// 105°～120°       限速或停止切换
+// >120°            禁止下发并报警
+// 软限位的意义是给控制器留出制动、舵轮响应和误差空间。
+// 第三层：舵轮转向期间限制驱动速度
+// 如果新的合法舵角与当前舵角相差很大，例如：
+// 当前：+90°
+// 目标：-50°
+// 差值：140°
+// 不应当马上正常行驶，应进入：
+// 减速
+//   ↓
+// 驱动速度降为0
+//   ↓
+// 舵轮转到-50°
+//   ↓
+// 确认所有舵轮到位
+//   ↓
+// 轮速使用负值重新起步
+// 可以设置阈值：
+// 舵角误差 < 2°：允许正常起步
+// 舵角误差 2°～10°：低速或保持
+// 舵角误差 > 10°：驱动速度为0
+// 实际阈值根据舵向响应速度标定。
+// 需要注意：把所有 Vx、Vy、Omega 同比例减小，只能降低运动速度，通常不会改变目标舵轮角度。因此“减速”能让切换更安全，但不一定能让超限角度重新变得合法。
+// 4. 车身重定向是更平滑的高级方案
+// 你提出的：
+// 提前停车，然后自转，调整车头朝向，再继续跟踪。
+
+// 这是正确而且很实用的方案，尤其适合连续轨迹跟踪。
+// 但不一定要把车头完全转到轨迹方向，应当根据当前驾驶模式选择车身朝向。
+// 假设轨迹方向相对当前车体为：
+// +130°
+// 如果希望继续保持蟹行模式，舵轮的优选方向是 +90°，那么只需要让车体逆时针自转：
+// 130° - 90° = 40°
+// 车体旋转 +40° 后：
+// 世界轨迹方向不变
+// 轨迹在新车体坐标系中的方向：+90°
+// 舵轮回到标准蟹行角度：+90°
+// 不需要让车头旋转完整的 130°。
+// 如果准备切换成正常前进模式，优选舵角是 0°，才需要让车头基本对准轨迹：
+// 130° - 0° = 130°
+// 因此可以使用下面的通用公式：
+// 期望车身朝向 =
+// 世界轨迹切线方向 - 当前模式的优选舵轮方向
+// 其中：
+// 正常模式优选角：0°
+// 左蟹行优选角：+90°
+// 右蟹行优选角：-90°
+// 对于蟹行轨迹，车身朝向参考值为：
+// yawReference =
+//     pathTangentWorldRadians -
+//     Math.PI / 2.0;
+// 这样车体会逐渐调整自身朝向，使轨迹方向始终落在舵轮舒适区，而不是一直顶着 ±120° 工作。
+// 5. 不建议到达硬限位才突然自转
+// 更好的方案是向前预测。
+// 轨迹跟踪本身通常有前视点，可以提前计算未来一段轨迹对应的目标舵角：
+// 当前时刻目标轮角
+// 未来0.2秒目标轮角
+// 未来0.5秒目标轮角
+// 未来1.0秒目标轮角
+// 一旦预测到未来会进入软限位，就提前触发车身重定向：
+// 正常跟踪
+//   ↓
+// 预测舵角接近软限位
+//   ↓
+// 逐渐降低线速度
+//   ↓
+// 停车
+//   ↓
+// 车身原地旋转
+//   ↓
+// 舵轮调整到模式初始方向
+//   ↓
+// 重新捕获轨迹
+//   ↓
+// 继续跟踪
+// 可以定义状态机：
+// internal enum TrackingConstraintState
+// {
+//     Tracking,
+//     ApproachingSteeringLimit,
+//     Stopping,
+//     ReorientingBody,
+//     AligningWheels,
+//     Resuming,
+//     Fault
+// }
+// 不要在一个控制周期内同时完成停车、车身旋转和恢复跟踪。
+// 6. LQR和限位处理应该分层
+// 以后写 LQR 时，不建议让基础 LQR 直接承担全部舵轮限位逻辑。
+// 推荐结构：
+// 轨迹参考
+//    ↓
+// LQR轨迹控制器
+// 输出期望 Vx、Vy、Omega
+//    ↓
+// 舵轮约束管理器
+// 等价角选择、软限位、预测、车身重定向
+//    ↓
+// ChassisCommand
+//    ↓
+// MultiWheelChassisAdapter
+//    ↓
+// SendXYThSpeed
+// 可以以后增加一个文件：
+// Shared/SteeringConstraintManager.cs
+// 它负责：
+// 根据 Vx、Vy、Omega 预测每个舵轮目标角。
+// 选择“舵角反向、轮速反向”的等价解。
+// 检查硬限位和软限位。
+// 判断是否需要停车对齐。
+// 给出是否需要重新规划车身朝向。
+// 记录哪个舵轮导致不可执行。
+// 而 FrameTransform2D、ChassisCommand 不应该包含这些车辆机械约束。
+// 7. 当前阶段建议先做到什么
+// 现在先实现最小安全版本：
+// 使用 SendXYThSpeed()，让底层优先选择等价舵角。
+// 检查 adapter.Send(command) 的返回值。
+// 返回 false 时立即停车，不能继续沿用上一条速度。
+// 打印每个舵轮的当前角度、目标角度和上下限。
+// 模式切换时先停车、预转舵轮、确认到位，再开放摇杆。
+// 先用较低速度测试 +90° → +110° → +120° → +130° 的过程。
+// 最低限度的发送逻辑应当是：
+// var success = adapter.Send(command, interval);
+
+// if (!success)
+// {
+//     adapter.Stop(interval);
+
+//     Console.WriteLine(
+//         "底盘命令分解失败，已停车：" +
+//         "可能存在舵角限制或舵轮无法完成方向切换。");
+// }
+// 然后再实现预测重定向。
+// 推荐的最终策略
+// 对于你的停车机器人，我建议采用：
+// 优先级1：使用θ±180°、轮速反向的等价解
+// 优先级2：选择机械转角变化较小的等价解
+// 优先级3：接近软限位时降低速度
+// 优先级4：提前规划车身朝向，保持舵轮在舒适角度
+// 优先级5：无法连续切换时停车、自转、重新对齐
+// 优先级6：仍不可行时保持停车并报警
+// 所以你的 +130° 示例不应该一开始就判故障。它可以先使用 -50° + 反向轮速。如果从当前 +90° 切换到 -50° 的动态过程不安全，再停车调整。若轨迹允许改变车身姿态，更理想的办法是让车身提前旋转 +40°，从而继续以标准 +90° 蟹行姿态跟踪，而不是一直逼近机械硬限位。