绕障行走
概述 / Overview
"绕障行走"指 AGV 在 巡线行走 的基础上叠加 局部障碍感知与降速 / 短距偏避能力。MDCS 中的绕障 不是动态重规划（那是调度层 SimpleCore 的工作），而是 线段内的安全反应：当前向激光检测到障碍时，先减速到停，必要时小幅横向避让；如果障碍持续存在 > 阈值时间，调度会接管做全局重路径。
"Obstacle-aware line following" adds local obstacle perception with deceleration and small lateral avoidance on top of line following. MDCS does not do dynamic global re-planning here — that's SimpleCore's job. The Movement-level logic is in-segment safety reaction: decelerate-to-stop on a detected obstacle, optionally small lateral nudge; if the obstacle persists beyond a threshold, the scheduler takes over with a full reroute.
与全局重规划的分工 / Division with global replanning
| 层 / Layer | 触发 / Trigger | 反应 / Reaction |
|---|---|---|
| Clumsy Movement (本页) | 前向激光 ROI 内有点 / Front lidar ROI has points | 减速 / 停车 / 小幅横向避让 |
| SimpleCore 调度 | 障碍持续 > T_obstacle_stale (默认 8 s) | 重新规划该车的路径段 |
| 全局地图重建 | 障碍很久不消失（> T_map_invalid） | 标记地图段为永久不可达 |
The split is deliberate: Movement-level responses must be ≤ tens of milliseconds; scheduler-level replanning is hundreds of milliseconds and global. They cooperate via the per-segment "stuck" flag.
关键参数 / Key parameters
绕障字段附加在 `SteeringLineFollowing` 之上（或它的"绕障变体"`SteeringLineFollowingWithAvoid`）：
These fields are layered on top of `SteeringLineFollowing` (or its avoidance-aware sibling `SteeringLineFollowingWithAvoid`):
| 字段 / Field | 默认 / Default | 含义 / Meaning |
|---|---|---|
| `obstacleROIStart` / `obstacleROIEnd` | 视场前向 ±15° | 障碍检测的角度区间 / FOV slice for obstacle detection |
| `obstacleROILength` | 1500 mm | 检测距离上限 / max distance considered |
| `obstacleSlowDist` | 1500 mm | 进入此距离开始降速 / start decelerating |
| `obstacleStopDist` | 400 mm | 在此距离停车 / hard stop |
| `obstacleMinPoints` | 4 | 多少个点视为"真障碍"（去抗 ghost） |
| `lateralAvoid` | `false` | 是否允许小幅横向偏避（仅全向车 / 双阿克曼蟹行可用） |
| `lateralAvoidMax` | 250 mm | 横向最大偏移 / max lateral nudge |
| `stuckTimeoutMs` | 8000 | 持续受阻多久后向调度告警 / report-stuck timeout |
工作机制 / How it works
1. 每个 tick 读最新激光点云 `cachedLidar`（参见 2D 激光适配）。 2. 用 ROI 过滤：只关心角度在 `[obstacleROIStart, obstacleROIEnd]`、距离在 `[0, obstacleROILength]` 的点。 3. 统计 ROI 内点数 ≥ `obstacleMinPoints` 时认定"有障碍"，记录 最近距离 d_obs。 4. 计算速度上限：
- d_obs ≥ obstacleSlowDist → basespeed - obstacleStopDist < d_obs < obstacleSlowDist → 线性插值降速 - d_obs ≤ obstacleStopDist → 0（停车）
5. 如果 `lateralAvoid = true` 且持续受阻 > 500 ms，启用 小幅横向偏避：把虚拟前瞻点向无障碍一侧偏 `lateralAvoidMax`，重新做纯跟踪。 6. 如果连续受阻 > `stuckTimeoutMs`，把 segment stuck 标志置 true → SimpleCore 调度立即重新寻路。
1. Each tick read `cachedLidar` from the front lidar. 2. ROI filter on the angle/distance slice. 3. Points ≥ `obstacleMinPoints` → mark obstacle, record nearest distance d_obs. 4. Speed cap: linear ramp between `obstacleStopDist` (0) and `obstacleSlowDist` (basespeed). 5. If `lateralAvoid` is on, after 500 ms of blockage shift the look-ahead point sideways. 6. After `stuckTimeoutMs` continuous blockage, flag the segment as stuck → scheduler replans.
调用样例 / Example
public void DriveAvoiding(double sx, double sy, double dx, double dy)
{
Queue(async () =>
{
DriveTask.WaitDriveTask(new SteeringLineFollowingWithAvoid
{
srcX = sx, srcY = sy, dstX = dx, dstY = dy,
basespeed = 800,
obstacleSlowDist = 1500,
obstacleStopDist = 400,
obstacleMinPoints = 4,
lateralAvoid = false, // 普通阿克曼车关闭
stuckTimeoutMs = 8000
}.Follow());
});
}
调试要点 / Tuning
- 误触发停车 / False positives → 增大 `obstacleMinPoints`（地面反射或飞虫的少量点会被忽略）。
- 反应迟钝 / Slow to react → 减小 `obstacleSlowDist` 接近 `obstacleStopDist`，但太接近会跌停（无平滑减速）。
- 横向避让撞两侧 / Lateral nudge into walls → 减小 `lateralAvoidMax`；或在调度可达性中声明 两侧不可避。
- 反复 stuck / replan 抖动 / Flap between stuck / unstuck → 增大 `stuckTimeoutMs`，避免短暂遮挡引发重路径。
全向车与 蟹行避让 / Omni & crab avoidance
全向车与双阿克曼车型可以 真 横向避让（不改变车头朝向就能侧移）。对应 Movement 是 `OmniLineFollowingWithAvoid` 与 `CrabLineFollowingWithAvoid`，它们的 `lateralAvoidMax` 可以放宽到 600 mm。
Omni and double-Ackermann vehicles can truly translate laterally without yawing. Use `OmniLineFollowingWithAvoid` / `CrabLineFollowingWithAvoid`; `lateralAvoidMax` can be opened up to 600 mm safely.
安全 / Safety
绕障 Movement 不是安全 e-stop —— 它的反应时间是 50 ms 量级（Movement tick），而安全 e-stop 必须 < 10 ms 并 独立于 Movement 逻辑。`obstacleStopDist` 之外还必须有：
The Movement is not a safety e-stop — its reaction is at the ~50 ms tick. Always provide:
- 激光雷达紧急停止区（< 300 mm 内任何点）—— 接到 Medulla 上位 `eStop` 信号上。
- 物理安全条 / 急停按钮。
- SIL-2 / PL-d 的功能安全设备（重载场景）。