设备跟随联动

概述 / Overview

"设备跟随联动"指 AGV 跟随 非 AGV 设备（手推叉车、人工拖车、卷料车、料箱等）协同移动的功能。与双车 / 多车联动的区别：本场景中 领头是设备 / 人，不属于 MDCS 调度网络，没有可控信号；跟随车只能依靠 视觉 + 激光观测领头物体并跟随。

"Device-following coordination" lets an AGV shadow a non-AGV device (manual trolley, hand-pushed forklift, coil cart, kit-box, …). Compared to twin-car coordination, the leader here is a piece of equipment or a person — it's outside the MDCS scheduling network and has no controllable signal. The follower can only observe the leader via vision / lidar and respond.

应用场景 / Scenarios

人工卸车 + AGV 接驳: 工人在卡车上把货物推到边沿，AGV 在地面跟随接货位。
卷料车跟随 / Coil-cart shadowing: 操作工推卷料车从一道工序到下一道，AGV 跟随承担载货。
跨工序流转 / Inter-process transfer: 装配线上的料箱由工艺人员沿不规则路径推动，AGV 自动跟随至工位。
拖车协同 / Trailer-tow assist: 牵引车前牵，AGV 在后段加推支撑（适用于长拖车）。

与已有联动方案的差异 / What makes it different

项 / Item	双车联动 / Twin-car	设备跟随联动 / Device-follow
领头 / Leader	同样是 MDCS AGV / Another MDCS AGV	任意非 AGV 设备 / 人 / Any device / human
通讯 / Comms	AP Wi-Fi 心跳 / AP heartbeat	无通讯，纯视觉 + 激光 / None — vision / lidar only
任务下发 / Task dispatch	调度直接派单 / Scheduler dispatches both	领头无法接收任务；调度只能管跟随车
路径 / Path	调度规划 / Scheduler-planned	领头自由路径 / Leader free-form
速度 / Speed	调度约束 / Scheduler-bounded	跟随者跟领头实时速度（带上限）/ Follower matches leader (with cap)
失联处理 / Lost-leader	联动解体 / Decouple	跟随者就地停 / Stop in place

实现方案 / Implementation

感知 / Perception

跟随车需要在领头设备上识别一个稳定的 视觉锚点 — 几种可选： The follower needs a stable visual anchor on the leader. Options:

V 槽 / V-groove — 在设备背面装 1 个 V 槽（同双车联动）。2D 激光检测，最便宜可靠。
AprilTag / ArUco — 在设备背面贴一个易识别 fiducial；前向相机检测。
3D 模板 — 用 3D 点云模板匹配（领头形状已知，如标准卷料车）。
腿对检测 / Leg-pair — 领头有两条立柱（如标准托盘车），用 `LidarDetectTray` 检测。

推荐组合：V 槽 + AprilTag 冗余；任一可识别即继续，两者全丢 ≥ 2 s 则停车。

Recommended combination: V-groove + AprilTag redundancy. If either is found, continue. If both lose lock for ≥ 2 s, stop.

跟随控制器 / Follow controller

跟随车的运动控制本质是一个 常距点对点跟踪器，目标点 = 领头锚点 - (固定偏移)。 The follower's motion control is essentially a constant-distance point tracker, with the target = leader anchor − fixed offset.

public class DeviceFollow : MovementDefinition
{
    public float standoff = 1500;        // 距领头多远 / standoff (mm)
    public float maxSpeed = 600;
    public float ahesionGain = 0.8f;     // 横向 / 朝向校正强度

    public override IEnumerable<bool> Get()
    {
        var detector = new LidarDetectTray
        {
            blobDist = 100,
            BlobPtCount = 3
        };

        var lostFrames = 0;
        while (true)
        {
            var lead = detector.DetectLeader();   // 领头锚点
            if (lead == null)
            {
                if (++lostFrames > 50)   // 50 ticks ≈ 2 s
                {
                    DriveStop();
                    yield break;
                }
                yield return true;
                continue;
            }
            lostFrames = 0;

            var dist = lead.Range;
            var lateral = lead.Lateral;
            var theta  = lead.Heading;

            // 控制律 / control law
            var fwd = MathF.Min(maxSpeed, (dist - standoff) * 1.2f);
            var turn = -(lateral * ahesionGain + theta * 0.5f);

            SetDriveCommand(fwd, turn);
            yield return true;
        }
    }
}

安全与边界 / Safety guards

跟随距离上下限 / Standoff bounds: 太近会撞领头，太远会丢锁；推荐 1.0–2.5 m。
加速度上限 / Accel bound: 领头突然提速时跟随车不能瞬时跟上；加上 0.5 m/s² 上限。
转向角速度上限 / Angular vel bound: 领头急转 → 跟随车保持不超过 0.5 rad/s。
与人接触 / Contact with humans: 跟随车的紧急停止线必须独立于跟随逻辑；激光雷达 1 m 内有任何障碍物立即触发 e-stop。

调度集成 / Scheduling integration

SimpleComposer 把"跟随设备" 状态视为一个特殊 mission：调度不规划跟随车的路径，只是 把跟随车锁定在跟随状态。
`Car.CurrentMission = Following("DeviceXX")`；调度看到这个状态时不会派其它任务给该 AGV。
跟随过程中跟随车汇报自己的实时位姿；调度可视化展示 领头-跟随 链路。

SimpleComposer treats "follow-device" as a special mission state. The scheduler does not plan a path; it just locks the AGV into following mode and won't dispatch other tasks until the follow ends.

启动 / 结束 / Start & end

启动 / Start: 通过 SimpleComposer 上的"启动设备跟随"按钮；选定要跟随的设备类型；车开始尝试识别。
确认 / Confirm: 识别到锚点后车体调整姿态并停在 standoff 距离上；车灯绿色提示。
结束 / End: 操作工通过手柄按"结束跟随"，或跟随车到达预设终点站点（参数化）。

调试要点 / Tuning

跟随抖动 / Jitter: 调低 `ahesionGain`，或在检测端加 EWMA 滤波。
领头突然消失 / Sudden lost-lock: 缩短 `lostFrames` 阈值（默认 50 ticks ≈ 2 s）。
拐弯时甩出 / Slips on turn: 把 `maxSpeed` 降到 400 mm/s，或在领头加 AprilTag 增强检测稳定性。