# 激光雷达挡板避障设计

## 问题

VL53L0X 是单点 ToF 激光测距传感器，仅返回沿光束方向的一个距离值（mm），没有扇形扫描能力。
当激光测到近处有物体时，无法直接从距离值判断它是：

- A) **挡板/障碍物** — 横在赛道上的物体，需要绕行
- B) **赛道边墙** — 弯道处车头对准了侧墙，这是正常行驶状态

**解决思路：用视觉巡线的边界线（left_line / right_line）来区分 A 和 B。**

---

## 关键约束：挡板会导致丢线

挡板立在赛道上时，不仅触发激光近距读数，还会**遮挡摄像头视野中的赛道边界**，
导致从挡板所在位置开始边界线丢失（left_line / right_line = -1）。

```
Camera 俯视视角 (图像坐标):
  row  0 (远)  : ░░░░░░░  ← 被挡板挡住，丢线
  row 15       : ░░░░░░░  ← 挡板上方，丢线
  row 25       : ░░░░░░░  ← 挡板顶部附近，丢线
  row 30       : ███████  ← ★ 挡板所在行，边界线丢失
  row 35       : ■■■■■■■  ← 挡板下方，赛道可见，边界线有效
  row 59 (近)  : ■■■■■■■  ← 车前方，赛道清晰
```

**这意味着：不能像锥桶检测那样"往更远处看边线是否还开着"，因为挡板后面的边线必然丢失。**

正确的判定是检测 **"有效边界 → 丢线"的过渡位置是否与激光近距读数对应**：
- 紧贴着挡板下方（更近处）：赛道应该可见，边界有效
- 挡板位置及上方（更远处）：边界丢失
- 激光读数：短距离 → 同一位置有物理障碍物

三个条件同时成立 → 挡板确认。

---

## 几何模型

```
                        Camera + Laser
                        ｜ (高度 H, 俯角 θ)
                        ｜╲
                        ｜ ╲ laser beam
                        ｜  ╲
  ground  ──────────────┴────███████──── 挡板 at distance d_mm
                              (挡板后方赛道被遮挡)
```

- 激光光束沿车体正前方（图像中轴线）
- 距离 d_mm 越小 → 物体越近 → 映射到图像中越靠下的行（row 大）
- 距离 d_mm 越大 → 物体越远 → 映射到图像中越靠上的行（row 小）

---

## 核心算法

### 第一步：读取激光距离

```
d_mm = vl53l0x.readRange().RangeMilliMeter

if d_mm >= LIDAR_THRESHOLD_MM:
    return CLEAR   // 远处无障碍，不做任何处理
```

`LIDAR_THRESHOLD_MM` 是触发阈值。只有距离小于此值才进入判定。建议默认 ~300mm。

### 第二步：距离 → 图像行映射

```
// 线性模型:
//   row = lt_h-1 (底行, 最近) 对应 D_NEAR
//   row = 10    (最远有效行)   对应 D_FAR
//   clamp 到 [10, lt_h-1]

row = lt_h - 1 - (d_mm - D_NEAR) / (D_FAR - D_NEAR) * (lt_h - 11)
row = clamp(row, 10, lt_h - 1)
```

**标定值（需根据实际安装位置测量）：**

| 参数 | 含义 | 建议初值 |
|------|------|----------|
| `D_NEAR` | row = lt_h-1 对应的物理距离 | 50 mm |
| `D_FAR` | row = 10 对应的物理距离 | 1200 mm |

标定方法：在赛道前方 300mm、600mm、900mm 处各放一个挡板，记录图像中挡板出现的 row，线性拟合。

### 第三步：用边线判定障碍物（核心）

```
// 设 row = distance_to_row(d_mm)，即激光测距对应的图像行

// ── 3a. 检查"挡板下方"（更近处）：赛道应该仍可见 ──
near_valid = false
near_ref_row = -1
for r = min(row + NEAR_START, lt_h-1)  down to  max(row + NEAR_END, lt_h-1):
    if left_line[r] != -1 && right_line[r] != -1:
        near_valid = true
        near_ref_row = r    // 记录有效行，后续绕行时判断宽侧
        break

// ── 3b. 检查"挡板位置及上方"（更远处）：边界应该已丢失 ──
far_lost = false
for r = row  down to  max(row - FAR_SPAN, 10):
    if left_line[r] == -1 || right_line[r] == -1:
        far_lost = true
        break

// ── 3c. 联合判定 ──
if near_valid && far_lost:
    → OBSTACLE_CONFIRMED
    记录: g_lidar_obstacle_row = row
          g_lidar_ref_row      = near_ref_row   // 用于判断绕行方向
else:
    → CLEAR
```

**判定原理（三种典型场景）：**

```
场景 A: 挡板挡路 → 触发绕行
  d_mm = 300mm → row = 35
  row 36~39 (近处): ✓ 赛道可见
  row 30~35 (挡板处): ✗ 丢线 (挡板遮挡)
  → near_valid=true, far_lost=true → ★ 触发

场景 B: 正常直道，无障碍
  d_mm = 1200mm → row = 10, 且 d_mm > THRESHOLD
  → 不进入判定，直接 CLEAR

场景 C: 弯道，激光打到边墙
  d_mm = 200mm → row = 40
  row 41~44 (近处): ✓ 赛道可见
  row 37~40 (弯道处): ✓ 赛道也可能可见（边墙不一定导致丢线）
  → near_valid=true, far_lost=false → CLEAR
```

### 第四步：去抖确认

```
if OBSTACLE_CONFIRMED:
    g_lidar_frames++
else:
    g_lidar_frames = max(0, g_lidar_frames - 1)

g_lidar_confirmed = (g_lidar_frames >= LIDAR_MIN_FRAMES)
```

---

## 绕行策略：中线变形

挡板通常只挡赛道的一部分（偏左或偏右），通过判断挡板下方有效行中哪一侧空间更大，
将中线推向宽侧实现绕行。

### 判断绕行方向

```
// 在 near_ref_row（挡板下方最近的有效行）中判断左右空间
left_space  = mid_line[near_ref_row] - left_line[near_ref_row]
right_space = right_line[near_ref_row] - mid_line[near_ref_row]

// 往宽侧推
dir = (left_space > right_space) ? +1.0 : -1.0
```

### 中线变形

对从 row=10 到挡板位置 row 的所有行施加变形，变形量从远到近线性斜坡上升：

```
// gain    = lidar_avoid_gain     (推离力度, 归一化)
// range   = lidar_avoid_range    (斜坡陡峭度, 行数)
// half_w  = lt_w / 2

for r = 10  to  g_lidar_obstacle_row:
    t    = clamp((r - 10) / range, 0.0, 1.0)  // 斜率上升
    push = t * gain * half_w * dir
    mid_line[r] = clamp(mid_line[r] + push,
                         left_line[r] + 2.0,
                         right_line[r] - 2.0)
```

**变形示意图：**

```
         row 10 ───────●──────────────────────────○ 赛道中心线
         row 20 ────────●─────────────────────────○  (往右绕行)
         row 30 ───────────●──────────────────────○
         row 35 ──────────────●───────────────────○  ← 挡板位置
         row 40 ────────────────████████████────────   ← 挡板 (丢线)
         row 59 ─■────────●───■■■■■■■■■■■■■■───●───■   ← 车底 (近处可见)
              left      mid  方向盘往右打          right
```

### 速度联动

绕行时降速以保证安全：

```
if g_lidar_confirmed || g_lidar_hold_ctr > 0:
    spd *= lidar_speed         // 默认 0.4，即降至 40% 速度
```

### 消失后保持衰减

挡板不再被检测到后，变形不会立即消失，而是线性衰减（类似锥桶 hold 逻辑）：

```
// 确认状态
if g_lidar_confirmed:
    g_lidar_hold_ctr    = 0
    g_lidar_hold_src_row = g_lidar_obstacle_row
    g_lidar_hold_dir    = dir

// 衰减状态（挡板消失后）
else if g_lidar_hold_src_row > 0 && g_lidar_hold_ctr < lidar_hold_frames:
    g_lidar_hold_ctr++
    decay = 1.0 - (double)g_lidar_hold_ctr / lidar_hold_frames   // 线性衰减到 0

    for r = 10  to  g_lidar_hold_src_row:
        t    = clamp((r - 10) / range, 0.0, 1.0)
        push = t * gain * half_w * g_lidar_hold_dir * decay
        mid_line[r] = clamp(mid_line[r] + push, left+2, right-2)
```

---

## 状态机

```
                   ┌────────────────────────┐
                   │                        │
                   ▼                        │
  ┌──────┐  确认   ┌──────┐  消失   ┌──────────┐  保持结束
  │NORMAL│ ─────► │AVOID │ ─────► │HOLD_DECAY│ ────────► NORMAL
  │      │        │      │        │ (衰减)   │
  └──────┘        └──────┘        └──────────┘
     ▲                                │
     └────────────────────────────────┘ 测距恢复正常

NORMAL:      正常行驶，激光测距 > THRESHOLD
AVOID:       挡板确认 → 中线变形绕行 + 减速 × lidar_speed
HOLD_DECAY:  挡板消失 → 变形量线性衰减 (lidar_hold_frames 帧内归零)
             衰减期间若再次检测到挡板 → 立即切回 AVOID
```

| 状态 | 电机 | 舵机 | 说明 |
|------|------|------|------|
| NORMAL | 正常速度 | 正常巡线 | 无变形 |
| AVOID | speed × lidar_speed | 中线偏转绕行 | 基于 left/right 空间判断方向 |
| HOLD_DECAY | speed × lidar_speed | 变形量线性衰减 | 确保车完全通过后再恢复 |

---

## 与现有流水线的集成

`lidar_avoid_process()` 放在 cone_detect_and_deform 之前执行（两者都修改 mid_line，
lidar 优先级更高）：

```
// CameraHandler 中 Step 5:
bool zebra_block  = zebra_process();
bool tl_block     = traffic_light_process();
lidar_avoid_process();      // ★ 新增：直接修改 mid_line
cone_detect_and_deform();

steering_update();
motor_update(zebra_block, tl_block);
```

### 中线修改的优先级

lidar 和 cone 都会修改 `mid_line[]`。由于 lidar 绕行是结构性避障（避开整个挡板），
应**先执行 lidar 变形，再执行 cone 变形**。
cone 的 clamp 到 `[left+2, right-2]` 会确保不超出 lidar 变形后的安全区间。

### motor_update 速度联动

```cpp
// motor_update 中新增：
if (g_lidar_confirmed || g_lidar_hold_ctr > 0) {
    spd *= g_cfg.lidar_speed;
}
```

不通过 `block` 参数刹车（绕行不需要停车），而是降速 + 变形。

---

## 复用现有 VL53L0X 驱动

已有驱动（当前被 `CMakeLists.txt` 排除编译）：

```cpp
// lib/vl53l0x.h
VL53L0X sensor;
sensor.init();                                      // open /dev/stmvl53l0x_ranging
VL53L0X_RangingMeasurementData_t data;
sensor.readRange(data);                             // data.RangeMilliMeter
sensor.stop();
```

**集成时需做的事：**
1. 从 `CMakeLists.txt` 的 EXCLUDE 列表中移除 `vl53l0x.cpp`
2. 在 `CameraInit()` 中调用 `sensor.init()`（硬件未连接时优雅降级）
3. 在 `cameraDeInit()` 中调用 `sensor.stop()`

---

## 配置参数

| 文件 | 类型 | 建议默认 | 含义 |
|------|------|----------|------|
| `./lidar_thresh` | int | 300 | 障碍判定距离阈值 (mm)，低于此值触发判定 |
| `./lidar_near` | int | 50 | row=lt_h-1 对应的物理距离 (mm) |
| `./lidar_far` | int | 1200 | row=10 对应的物理距离 (mm) |
| `./lidar_near_start` | int | 1 | near_valid 检测起始偏移 (row + N) |
| `./lidar_near_end` | int | 4 | near_valid 检测终止偏移 |
| `./lidar_far_span` | int | 6 | far_lost 检测跨度 (从 row 往上检查 N 行) |
| `./lidar_min_frames` | int | 3 | 连续确认帧数 |
| `./lidar_avoid_gain` | double | 0.4 | 绕行中线推离力度 (归一化) |
| `./lidar_avoid_range` | int | 30 | 绕行斜坡陡峭度 (行数) |
| `./lidar_speed` | double | 0.4 | 绕行时速度倍率 |
| `./lidar_hold_frames` | int | 30 | 挡板消失后变形保持帧数 |
| `./lidar_enable` | int(0/1) | 1 | 激光避障总开关（0=禁用） |

---

## 边界情况 & 注意事项

1. **激光硬件未连接** — `sensor.init()` 失败时，`lidar_avoid_process()` 直接返回 false，不阻塞正常行驶。
2. **弯道边墙误判** — 急弯处赛道边墙可能导致激光近距 + 边线丢失同时出现。依赖 `lidar_thresh` 和 `lidar_min_frames` 调参抑制。误判时车会短暂绕向一侧，弯道通过后立即恢复。
3. **左右空间相等** — `left_space == right_space` 时默认 `dir = -1.0`（往右绕），可通过配置 `lidar_default_dir` 调整。
4. **上坡/下坡** — 车辆俯仰变化会影响距离→行的映射精度。建议在平坦路段标定。
5. **多传感器优先级** — lidar 先于 cone 修改 mid_line。zebra/tl 的 block 刹车优先级最高（红灯/斑马线停车不绕行）。
6. **首次集成建议** — 先调通数据采集：打印 `d_mm`、对应 `row`、以及 `near_ref_row` 处的 `left/right/mid` 值，跑几圈确认标定参数无误后再开启绕行。
7. **挡板材质** — 深色挡板不会被 HSV-Otsu 判为赛道（白色 255），但仍会遮挡背景赛道导致丢线。判定逻辑依赖的是**丢线**而非**像素颜色**。