// 然后因为我这个车的舵轮是有正负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° 蟹行姿态跟踪，而不是一直逼近机械硬限位。