全向车适配案例

概述 / Overview

全向车（mecanum 麦克纳姆轮或 4 独立舵轮）能 任意方向平移 + 独立旋转，无非完整约束，适合狭窄场景。MDCS 中典型全向车是大重载（10 T+）平板搬运车，常出现在双车联动场景。

Omni vehicles (mecanum-wheel or 4-independent-steered-wheel) can translate in any direction + rotate independently — no non-holonomic constraints, ideal for tight spaces. The typical MDCS omni is a 10 t+ heavy-load flat-bed for twin-car operations.

参考：浙江联核（`D:\src\cookbook\adaption-reference\浙江联核\new\`）、卓一全向叉车（`D:\src\cookbook\adaption-reference\卓一\zhuoyiomniforklift\`）。 References: Zhejiang Lianhe (`adaption-reference\浙江联核\new\`), Zhuoyi omni-forklift.

硬件清单 / Hardware

部件 / Part	麦克纳姆 / Mecanum	四舵轮 / 4-steered
驱动轮数 / Drive wheels	4 (左前 / 右前 / 左后 / 右后全独立)	4 独立驱动 + 4 独立转向
编码器 / Encoders	每轮一个 / 1 per wheel	每轮 2 个（驱 + 转）
IMU	必备 / required	必备 / required
控制板 / Controller	单板可控 4 路 BLDC	通常每对驱 / 转一对一块板
重载承重 / Payload	0.3–3 T	5–20 T
适用场景 / Use case	中等重量、清洁机器人	重载、长货物

1. Medulla 适配 / Medulla side

全向车的关键是 速度合成：把 `(vx, vy, omega)` 拆成 4 路轮速 / 转向角，下发到电机。这通常在 LadderLogic 内做：

The crux is velocity synthesis: decompose `(vx, vy, omega)` into 4-wheel speeds / steer angles. Do it inside the LadderLogic loop:

public class OmniCart : CartDefinition
{
    [AsInitParam] public string canDevice = "USBCAN0";
    [AsInitParam] public float  wheelBase = 1600; // mm (front-rear distance)
    [AsInitParam] public float  trackWidth = 1200; // mm
    [AsInitParam] public float  wheelRadius = 100; // mm

    // 上位 IO — 三轴速度
    [AsUpperIO] public float vxCmd;        // 前向 / forward
    [AsUpperIO] public float vyCmd;        // 横向 / lateral (left positive)
    [AsUpperIO] public float omegaCmd;     // 转向角速度 / yaw rate

    // 下位 IO — 4 路状态
    [AsLowerIO] public float vEstW1, vEstW2, vEstW3, vEstW4;
    [AsLowerIO] public bool  eStop;

    [UseLadderLogic(IntervalMs = 50)]
    public class Loop : LadderLogic<OmniCart>
    {
        public override void Run(OmniCart s)
        {
            // 麦克纳姆轮 inverse kinematics（标准 4 轮，X 型布局）
            var L = (s.wheelBase + s.trackWidth) / 2f;
            var w1 = s.vxCmd - s.vyCmd - L * s.omegaCmd;   // 左前
            var w2 = s.vxCmd + s.vyCmd + L * s.omegaCmd;   // 右前
            var w3 = s.vxCmd + s.vyCmd - L * s.omegaCmd;   // 左后
            var w4 = s.vxCmd - s.vyCmd + L * s.omegaCmd;   // 右后

            // 转换到 RPM 并下发
            s.can.SendWheelRPM(
                w1 / s.wheelRadius,
                w2 / s.wheelRadius,
                w3 / s.wheelRadius,
                w4 / s.wheelRadius);

            // 读回 4 路速度
            var status = s.can.ReadStatus();
            s.vEstW1 = status.w1; s.vEstW2 = status.w2;
            s.vEstW3 = status.w3; s.vEstW4 = status.w4;
            s.eStop  = status.eStop;
        }
    }
}

四舵轮变体在上述基础上再加 4 路 steer angle，并把 (vx, vy, omega) → (4 个轮速 + 4 个转向角) 的解算改为 每个轮的方向指向行驶切线 + 每个轮的轮速 等于切线速率。

The 4-steered variant adds 4 steer-angle outputs and solves so each wheel's direction points tangent to its instantaneous motion arc.

2. Clumsy 自动驾驶适配 / Clumsy side

全向车需要 全向版本的 Movement —— 不能直接用 `SteeringLineFollowing`（它假设阿克曼 / 差速）。建议家族：

Omni vehicles need omni variants of the Movements — `SteeringLineFollowing` assumes Ackermann / differential. The recommended family:

`OmniLineFollowing` — 直线，可指定 移动朝向与 车头朝向独立
`OmniArcFollowing` — 圆弧
`OmniInPlaceRotate` — 不动平移、原地旋转
`OmniSlotFollowing` — 替代 `SlotFollowing` 用于料架跟随

差异点：全向车可以 横向接近 工位（车头不变向，车身横向移动到工位），常用于狭窄取料场景。

The signature feature: omni can side-approach a station (car heading fixed, body slides sideways onto target), useful in narrow pick aisles.

public void SideApproachShelf(double sx, double sy, double tx, double ty)
{
    Queue(async () =>
    {
        DriveTask.WaitDriveTask(new OmniLineFollowing
        {
            srcX = sx, srcY = sy, dstX = tx, dstY = ty,
            holdHeading = currentTh,   // 保持车头方向
            basespeed = 300
        }.Follow());
    });
}

3. SimpleComposer 端 / SimpleComposer side

全向车的包络是 圆盘形（旋转无差异），调度需要的可达性比阿克曼车型多很多。建议在 SimpleComposer 中给全向车型设置：

较小的包络半径（无前后偏置）
更宽松的可达性（无需考虑转向半径）

The omni envelope is a disk (rotation symmetric); reachability is much more permissive than Ackermann. Configure: small disk envelope, no turning-radius constraint in pathfinding.

4. 标定与调试 / Calibration & tuning

轮间距与轮径 / Track + wheel radius —— 每轮独立测，麦克纳姆轮的 滚轴角度（45° 或 -45°）必须配对正确。
轮间打滑 / Slip: 同向运行时四轮速差应 < 2%。
横向移动直线度 / Sideway straightness: 横移 5 m，偏离 < 50 mm。否则有一轮打滑或方向错。
带载性能 / Loaded behaviour: 满载时麦克纳姆轮容易"挤压"地面，横向速度损耗严重，要建立载重-补偿表。

Per-wheel track + radius — for mecanum the roller angle (±45°) sign must be correct per corner.
Slip: in straight-fwd motion, the 4-wheel speed spread < 2%.
Side-shift straightness: 5 m of pure lateral; lateral deviation < 50 mm.
Loaded behaviour: heavy loads cause mecanum rollers to scrub; build a load → speed-compensation table.

5. 常见问题 / Common pitfalls

横向移动不直 / Side motion drifts: 通常是滚轴角度对应错（左前 / 右前 / 左后 / 右后 4 个角的滚轴角应为 -45° / +45° / +45° / -45° X 型布局）。
重载时减速过快 / Decelerates too fast loaded: 载重 → 加速度上限映射需重做。
原地旋转有平移 / In-place rotation drifts: 轮径 / 轮间距标定不准。
麦克纳姆滚轴磨损快 / Mecanum rollers wear quickly: 改 4 舵轮设计或限制带载横移用例。

6. 在双车联动中作为成员 / In twin-car coordination

重载双车联动默认全向车作为主从两台。全向车的 V 形特征槽通常装在车体侧面（联动时两车横向并排，槽侧对槽侧）。

The default twin-car coordination configuration uses two omni AGVs side-by-side, with V-grooves on the lateral faces (side-to-side, not tail-to-tail) so each can observe the other while moving.