巡线行走
概述 / Overview
"巡线行走"是 MDCS Clumsy 最基础的运动原语：给一个起点 (srcX, srcY) 和终点 (dstX, dstY)，AGV 沿直线段从起点驶到终点。它由 `SteeringLineFollowing`（正向）和 `SteeringLineFollowingReverse`（反向）两个 `MovementDefinition` 子类实现，所有其它复杂动作（取放货、绕障、双车联动）都是在它之上组合而成。
"Line following" is the most basic Clumsy motion primitive: given (srcX, srcY) and (dstX, dstY), the AGV runs a straight segment between them. It's implemented by `SteeringLineFollowing` (forward) and `SteeringLineFollowingReverse` (reverse), both `MovementDefinition` subclasses. Every higher action (pick/place, obstacle avoidance, twin-car coordination) composes on top.
文件 / Files: `D:\src\Clumsy\ClumsyCore\Legacy\SteeringLineFollowing.cs`、`SteeringLineFollowingReverse.cs`。
使用场景 / When to use
- 站点对点的直线行驶（无需绕障 / 无需精对位）。
- 取放货的 粗对位阶段（开 工位识别 之前的逼近）。
- 反向出库（用 `Reverse` 变体）。
- 简单调度的最小可用配置。
- Point-to-point straight driving on uncongested aisles.
- Coarse-approach stage before station-aware pickup.
- Reverse withdraw (the `Reverse` variant).
- Minimum config for a quick-running scheduler.
关键字段 / Key fields
| 字段 / Field | 类型 | 含义 / Meaning |
|---|---|---|
| `srcX`, `srcY` | `double` | 起点 / segment start (mm) |
| `dstX`, `dstY` | `double` | 终点 / segment end (mm) |
| `basespeed` | `int` | 巡航速度 / cruise speed (mm/s) |
| `lookAhead` | `float` | 纯跟踪前瞻距离 / pure-pursuit look-ahead (mm) |
| `dFactor` | `float` | 横向偏差转化为转向的增益 / lateral-error → steer gain |
| `slowdown` | `bool` | 临近终点时降速 / decelerate near goal (true by default) |
| `finSpeed` | `int` | 终点最低速度（slowdown 触发后）/ final speed when slowdown active |
| `slowdownDist` | `float` | 进入减速段的距离阈值 / decel-zone size (mm) |
`SteeringLineFollowingReverse` 字段完全一致；语义为车头朝向 不变、车体后退到目标。 `SteeringLineFollowingReverse` has the same fields; the car drives in reverse while keeping its heading unchanged.
工作机制 / How it works
1. `MovementDefinition.Get()` 是一个 可枚举的 bool 序列，每个 tick 返回一次 `true`（表示继续），直到完成时返回 `false` / 退出枚举。 2. `DriveTask.WaitDriveTask(IEnumerable<bool>)` 把这个序列每隔 `Configuration.conf.DriveTaskInterval` ms 调一次，并阻塞调用方直到序列结束。 3. 每个 tick 内部：
- 读当前位姿 `(x, y, th)`（来自 Detour，已订阅）。 - 计算前瞻点（沿目标线段往前 `lookAhead`）。 - 用纯跟踪法求转向角；`vCmd` 取 `basespeed`（若进入减速段则衰减到 `finSpeed`）。 - 写到 Medulla 上位 IO。
4. 当车体距 `(dstX, dstY)` 小于阈值（默认 50 mm），返回 `false` 结束。
1. `MovementDefinition.Get()` returns an `IEnumerable<bool>` ticked once per control cycle; `true` = keep going, end-of-iteration = done. 2. `DriveTask.WaitDriveTask(...)` ticks at `Configuration.conf.DriveTaskInterval` ms and blocks the caller until the iteration ends. 3. Each tick: read pose → compute look-ahead point → pure-pursuit steer → write `vCmd` / `steerCmd`. 4. Exits when within ~50 mm of `(dstX, dstY)`.
调用样例 / Example
public void DriveStraight(double sx, double sy, double dx, double dy)
{
Queue(async () =>
{
DriveTask.WaitDriveTask(new SteeringLineFollowing
{
srcX = sx, srcY = sy,
dstX = dx, dstY = dy,
basespeed = 600,
lookAhead = 500,
dFactor = 1.2f,
slowdown = true,
finSpeed = 100
}.Follow());
});
}
调试要点 / Tuning
| 现象 / Symptom | 调整 / Adjustment |
|---|---|
| 路径偏移 / Drifts off the line | 增大 `dFactor`（默认 1.2），但太大会震荡 |
| 在终点震荡 / Oscillates at goal | 启用 `slowdown` 并增大 `slowdownDist` |
| 终点冲过 / Overshoots | 降低 `basespeed` 或减小 `finSpeed` |
| 弯道内切 / Cuts inside corner (短线段段拼接时) | 减小 `lookAhead` |
| 慢速时跟不准 / Poor tracking at low speed | `dFactor` 与速度成反比；低速时增大 |
| 反向跑歪 / Reverse drifts | 检查后向激光雷达视野；可能未启用，导致姿态误差累积 |
与其它 Movement 的衔接 / Composing with other Movements
- 取货 / 放货前的接近 — 后接 `AutoFetchGood` / `AutoShelfFetching_ManyLegs`。
- 绕障 — 见 绕障行走。
- 反向出库 — 用 `SteeringLineFollowingReverse`，搭配同样的字段。
- 长路径 — SimpleComposer 调度会把长路径切成若干段 LineFollowing 拼接，每段一个 `srcX/Y → dstX/Y`。
注意 / Caveat
LineFollowing 是 几何意义上的直线段跟踪，与车型无关。但具体 Movement 内部隐含假设：阿克曼 / 差速车型可以 转向即改向。对全向车应改用 `OmniLineFollowing`（见 全向车适配案例），它允许车身朝向独立于运动方向。
`SteeringLineFollowing` is a geometric line-segment tracker but implicitly assumes Ackermann / differential kinematics. For omni vehicles use `OmniLineFollowing` (heading independent of motion direction) — see the omni case study.