查看“自动识别工位并取放货”的源代码

<languages/>
== 概述 / 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.

{| class="wikitable"
! 阶段 / 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.

<syntaxhighlight lang="csharp">
// 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());
    });
}
</syntaxhighlight>

工位的关键参数（腿距、入位深度、对齐速度）通常存在 CAD 配置中，由 [[Special:MyLanguage/开发手册 - SimpleComposer界面开发 - 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 ==
{| class="wikitable"
! 参数 / 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`.
* 完成姿态偏差大 → 用 `[[Special:MyLanguage/使用手册 - 双雷达标定手册|双雷达标定]]` 核对外参。
* Large terminal pose error → re-calibrate via the dual-lidar guide.

== 相关页面 / See also ==
* [[Special:MyLanguage/使用叉车自动取托盘功能|使用叉车自动取托盘功能]]
* [[Special:MyLanguage/自动识别托盘取放货|自动识别托盘取放货]]
* [[Special:MyLanguage/托盘识别|托盘识别]]
* [[Special:MyLanguage/使用自动对准料架功能|使用自动对准料架功能]]
* [[Special:MyLanguage/使用叉车料架堆叠功能|使用叉车料架堆叠功能]]

[[Category:运动控制使用手册]]