潜伏顶升车(KIVA类小车)适配案例

概述 / Overview

潜伏顶升车（也称 KIVA 类小车、料架搬运车）是从料架下方钻入、顶升后整体搬运料架到目的地的低矮 AGV。结构最简，但对 Detour 定位精度与 自动对准料架 算法要求最高。

The lift-AGV (also "KIVA-class" after Amazon's KIVA) is a low-profile AGV that slides under a rack, jacks it up, and carries it. Mechanically simplest, but most demanding on Detour precision and the rack-alignment algorithm.

参考：浙江迈睿 KIVA 适配（`D:\src\cookbook\adaption-reference\浙江迈睿\CartAdapters\kiva\`）以及苏州艾吉威的对应 ClumsyPilot 实现。 References: Zhejiang Mairui KIVA adapter (`D:\src\cookbook\adaption-reference\浙江迈睿\CartAdapters\kiva\`) plus Suzhou Eswei's ClumsyPilot.

硬件清单 / Hardware

部件 / Part	典型规格 / Typical spec
底盘 / Chassis	双驱动差速（差速旋转） / Two-wheel differential
驱动电机 / Drive motor	BLDC 200–600 W × 2
顶升机构 / Jack	电动丝杆 / liquid jack；行程 30–100 mm
雷达 / Lidar	顶面双向 2D（前后扫描）或单向 + 360° 反光板
IMU / IMU	6-DoF，必备
接近开关 / Proximity	料架腿检测辅助 / aux for leg detection
通讯 / Comm	工控 PC 本体 + Wi-Fi 5

不需要叉齿 / 倾角 / 复杂 PLC，硬件 IO 极少；MDCS 适配的核心难度在 钻入精度与 带架原地旋转。

No fork / tilt / complex PLC; minimal hardware IO. The MDCS adaptation difficulty is in slide-under accuracy and in-place rotation while carrying.

1. Medulla 适配 / Medulla side

最小 IO 表 / Minimal IO surface:

public class KivaCart : CartDefinition
{
    [AsInitParam] public string motorPort = "COM3";
    [AsInitParam] public float  wheelBase  = 460;      // mm, between drive wheels
    [AsInitParam] public float  jackStroke = 80;       // mm

    [AsUpperIO] public float vLeft, vRight;   // 双驱差速指令 / per-wheel cmd
    [AsUpperIO] public float jackTarget;       // 0 = down, jackStroke = up
    [AsUpperIO] public bool  led;              // 顶部状态灯 / status LED

    [AsLowerIO] public float vLeftEst, vRightEst;
    [AsLowerIO] public float jackHeight;
    [AsLowerIO] public bool  jackAtTarget;
    [AsLowerIO] public bool  eStop;
    [AsLowerIO] public int   batteryPercent;
    [AsLowerIO] public bool  legProx;          // 接近开关：腿在我下面 / leg under me
}

ladder logic 同样 50 ms 周期；KIVA 一般通过厂商 USB-CAN 或 RS485 与轮控板通讯（具体协议见厂商手册）。

The ladder logic runs at 50 ms; talk to the wheel controllers over the vendor's USB-CAN or RS485 channel.

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

关键 Movement / Key movements:

`SteeringLineFollowing` — 巡线到 料架前 anchor
`AutoShelfFetching_ManyLegs` — 自动钻入并对准料架（见使用自动对准料架功能）
`JackUp(targetHeight)` — 顶升包封，等待 `jackAtTarget` 为 true
`InPlaceRotate(angle)` — 带架原地差速旋转（必须在 jack-up 后启用 带载包络）

public class AGV : SimpleAgvInterface
{
    public void PickRack(double anchorX, double anchorY, double rackX, double rackY)
    {
        Queue(
          async () =>
          {
              DriveTask.WaitDriveTask(new SteeringLineFollowing
              {
                  srcX = currentX, srcY = currentY,
                  dstX = anchorX,  dstY = anchorY,
                  basespeed = 800
              }.Follow());
          },
          async () =>
          {
              // Slide under + auto-align (从 anchor 切到 rack centre)
              DriveTask.WaitDriveTask(new AutoShelfFetching_ManyLegs
              {
                  aX = anchorX, aY = anchorY,
                  bX = rackX,   bY = rackY,
                  initSpeed = 600, inShelfSpeed = 200,
                  stopDist = 30, width = Configuration.conf.rackLegSpan,
                  startingSlot = 10
              }.Get());
          },
          async () =>
          {
              // Jack up
              SetUpperIO("jackTarget", Configuration.conf.jackStroke);
              await WaitForLowerIO("jackAtTarget", 4000);
              carryingRack = true;
          });
    }

    public void DropRack(double targetX, double targetY)
    {
        Queue(
          async () =>
          {
              DriveTask.WaitDriveTask(new SteeringLineFollowing
              {
                  srcX = currentX, srcY = currentY,
                  dstX = targetX, dstY = targetY,
                  basespeed = 500    // 带载减速 / slow when loaded
              }.Follow());
          },
          async () =>
          {
              SetUpperIO("jackTarget", 0);
              await WaitForLowerIO("jackAtTarget", 4000);
              carryingRack = false;
          },
          async () =>
          {
              // 后退 200 mm 退出料架 / withdraw 200 mm
              DriveTask.WaitDriveTask(new SteeringLineFollowingReverse
              {
                  srcX = targetX, srcY = targetY,
                  dstX = targetX - 0.2 * Math.Cos(currentTh),
                  dstY = targetY - 0.2 * Math.Sin(currentTh),
                  basespeed = 200
              }.ReverseFollow());
          });
    }
}

3. SimpleComposer 端 / SimpleComposer side

顶升车 ClumsyCar 子类需要 携带料架时的扩展包络（料架长宽通常大于车体）：

The ClumsyCar subclass needs an alternate carried-rack envelope (rack is wider/longer than the AGV):

[CarType(Name = "KivaCart", Title = "KIVA 顶升车 / KIVA lift-AGV")]
public class KivaCar : ClumsyCar
{
    public bool carryingRack => /* 从车上心跳读 / from heartbeat */;

    public override Envelope GetActiveEnvelope()
    {
        return carryingRack ? carriedRackEnvelope : base.GetActiveEnvelope();
    }
}

4. 标定与调试 / Calibration & tuning

轮距与轮径标定 / Track + wheel diameter: 做正反 5 m 直线，比较 Detour 估的位移与实际，差异 > 2% 必须改 wheel radius 配置。
差速旋转误差 / Rotation error: 原地转 360° × 10，累计偏差 < 5°。误差大 → 轮距错。
料架腿检测先验 / Leg priors: 真测料架腿距，写入 `width`。
顶升时长 / Jack timing: `inshelfMillis` 比顶升时长长 1 s，避免还没顶完就开走。
反光板 / SLAM 切换 / Reflector / SLAM switch: 反光板辅助定位时务必检查 KIVA 顶面雷达视野，料架钻入后是否仍能看到反光板。

Wheel-track + diameter cal: drive 5 m fwd/back; if Detour pose differs > 2%, fix the radius.
Rotation error: spin 360° ×10 in place; cumulative drift < 5° is acceptable.
Leg priors: write real measured leg span into the `width` field.
Jack timing: `inshelfMillis` should be 1 s longer than the jack-up time.
SLAM vs reflector: confirm the top-mounted lidar still sees reflectors after the rack is on top.

5. 常见问题 / Common pitfalls

钻入时撞腿 / Hits leg on entry: 检测器 `blobDist` 太大或 anchor 站点离料架太近。
顶升后偏摆 / Sway after jack: 料架重心偏，或 jack 行程未完全到位；查 `jackAtTarget` 信号。
带架转弯失败 / Rotation fails with rack: 带载包络未启用，调度允许了无法通过的窄通道。
返回原料架放回失败 / Re-dock to same rack fails: anchor 与 rack 站点坐标在 SLAM 漂移后不一致；定期重标定。

6. 通讯与节拍 / Comm & throughput

控制周期：50 ms（Medulla LadderLogic）/ Control cycle: 50 ms.
心跳：100 ms（向 SimpleComposer 报位置 + jackHeight）/ Heartbeat: 100 ms.
取放节拍参考：单次（取-搬-放）约 35–50 s（含 anchor 站到 anchor 站直线 8 m）。