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