自动识别工位并取放货

概述 / Overview

"自动识别工位并取放货"指 AGV 行驶到工位附近后，使用激光雷达自动识别工位（料架、托盘、卷料、料框、机台口等）的精确位姿，然后调整车体姿态并完成取放货动作。整个过程不依赖工位上的固定二维码 / 反光板 / 磁条，而是直接从工位本身的几何特征反推位姿，因此对工位精度、地面平整度、初始停靠精度的容忍度大幅提升。

"Auto station-aware pick & place" lets an AGV detect the exact pose of a workstation (rack, pallet, coil, kit-bin, machine port, …) from its 2D-lidar geometry once the car is roughly in front of it, then auto-trim its own pose and complete the pick/place motion. No fiducial (QR / reflector / magnetic strip) is required, so tolerance to placement error and floor irregularity is far better than dead-reckoning approaches.

适用场景 / Scenarios

潜伏顶升车（KIVA 类）从料架下方钻入，自动对齐货架四脚 / Lift-AGV slides under a 4-legged rack and auto-aligns.
叉车 / 拖车取放料架、托盘 / Forklift or tractor handling racks and pallets.
重载平板车在工位口对接（如机台进出料口）/ Heavy flat-bed AGV docking at a machine's I/O port.
异型工位（如卷料 V 型托架、料筐侧立位）/ Irregular stations such as V-cradles for coils or upright kit-bins.

工作原理 / Principle

工位识别共用一套 检测 → 跟踪 → 终止 的流水线，差异在于检测器（detector）和跟踪器（tracker）的选型。MDCS 在 Clumsy 中以 `MovementDefinition` 的形式封装：

The station-identification pipeline is uniform: detect → track → stop. Only the detector and tracker components differ between station types. In Clumsy each variant is packaged as a `MovementDefinition` subclass.

阶段 / Stage	任务 / Task	关键类 / Key class
检测 / Detect	从激光点云中找出工位特征点 / pull station features from the cloud	`LidarDetectTray`, `Lidar2dDetect3LegTray`, `LidarBlobPairDetector`, `LidarDetectPallet`
跟踪 / Track	用工位特征点驱动差动 / 全向轮，逼近期望停靠位姿 / drive wheels toward the target docking pose using the detector output	`SlotFollowing`, `SteeringLineFollowing`, `ForwardTracker`
终止 / Stop	到达 stopDist 或 inshelfMillis 后停车、释放跟踪线程 / stop on `stopDist` or `inshelfMillis`, release thread	`DriveTask.WaitDriveTask`

具体的 MovementDefinition 在 `D:\src\Clumsy\ClumsyDance\ClumsyDance\Movements\` 下，常用例如： The actual MovementDefinitions live under `D:\src\Clumsy\ClumsyDance\ClumsyDance\Movements\`. Representatives:

`AutoShelfFetching_ManyLegs` — 多腿料架（≥ 2 对腿）的循序对齐 / multi-leg rack with two or more leg pairs
`AutoShelfFetching_3Legs` — 三腿料架（V/T 形托架）/ three-leg rack (V / T shaped cradle)
`AutoFetchGood` / `PutGood` — 通用取 / 放的高层调用 / generic high-level pick / place

使用方法 / Usage

任务编写者（Clumsy 任务 / SimpleComposer 业务脚本）按以下方式调用，参数从工位 CAD 配置或 Detour 站点中获取：

Mission authors (Clumsy job, SimpleComposer scenario script) invoke it as follows, with parameters drawn from the station's CAD record or the Detour site.

// In a Clumsy AGV method that derives from SimpleAgvInterface
public void FetchAtStation(double srcX, double srcY, int siteId)
{
    Queue(async () =>
    {
        // 1. Drive to the front of the station using a coarse line follow
        DriveTask.WaitDriveTask(new SteeringLineFollowing
        {
            srcX = currentX, srcY = currentY,
            dstX = srcX,     dstY = srcY,
            basespeed = 600
        }.Follow());

        // 2. Run the station-aware fetch
        DriveTask.WaitDriveTask(new AutoFetchGood
        {
            aX = srcX, aY = srcY,        // approach anchor
            bX = dstX, bY = dstY,        // pickup anchor
            initSpeed   = 600,           // 6 mm tick
            inShelfSpeed= 200,
            stopDist    = 400,
            width       = 1150,          // leg-pair span
            startingSlot= 10
        }.Get());
    });
}

工位的关键参数（腿距、入位深度、对齐速度）通常存在 CAD 配置中，由 CAD工具编辑生成。运行时 SimpleComposer 把这些参数注入到对应的 MovementDefinition 字段。

Per-station geometry (leg span, entry depth, alignment speed) is normally maintained in the CAD layer via the SimpleComposer CAD tool. SimpleComposer injects them into the MovementDefinition fields at runtime.

调试要点 / Tuning tips

参数 / Param	建议值 / Default	含义 / Meaning
`initSpeed`	600 mm/s	进入识别阶段前的逼近速度 / approach speed before detection
`inShelfSpeed`	200 mm/s	进入工位后的精细对位速度 / fine alignment speed inside the station
`stopDist`	400 mm	距腿距离剩余多少时停止 / distance to legs at stop
`width`	工位实测腿距 / measured leg span	检测器用此值过滤伪检测 / detector rejects pairs outside this width
`lookahead`	500 mm	跟踪器纯跟踪前瞻 / pure-pursuit look-ahead
`resolution`	40–50 mm	点云聚类粒度 / cloud-clustering bin
`slots`	30	跟踪槽数（即沿 Y 方向离散化的格数）/ track-slot count
`dfac`	10	距离衰减因子（越大越保守）/ distance weight (higher = more conservative)
`inshelfMillis`	1000	进入工位后跟踪超时 / in-station tracking timeout (ms)
`deaccMillis`	500	收尾减速时间 / final deceleration time (ms)

排错清单 / Troubleshooting:

检测器拿不到腿 → 调大 `LidarDetectTray.blobDist`，或检查激光雷达水平度。
Detector misses legs → bump `LidarDetectTray.blobDist`, or check lidar horizontality.
入位后车体抖动 → 减小 `dfac` 或增大 `inshelfMillis`。
Vehicle oscillates inside → reduce `dfac` or increase `inshelfMillis`.
完成姿态偏差大 → 用 `双雷达标定` 核对外参。
Large terminal pose error → re-calibrate via the dual-lidar guide.