Loongson_2k0300_SmartCar/docs/LIDAR_AVOID.md

激光雷达挡板避障设计

概述

VL53L0X 单点 ToF 激光测距传感器，返回沿光束方向的距离值（mm）。 当距离低于阈值时触发绕行：中线变形推向右侧，消失后保持衰减。

当前实现： 纯距离触发 + 预触发去抖，不检查视觉边界线。绕行方向固定朝右。

---

VL53L0X 驱动集成

驱动已编译（src/vl53l0x.cpp + lib/vl53l0x/ C API），在 CameraInit() 中初始化：

CameraInit():
  if g_cfg.lidar_enable:
      g_lidar_ok = g_lidar_sensor.init()      // open /dev/stmvl53l0x_ranging
      g_lidar_sensor.startMeasure()            // 启动首次测量
  else:
      g_lidar_ok = false                       // 禁用

cameraDeInit():
  if g_lidar_ok → g_lidar_sensor.stop()

硬件未连接时 init() 失败 → g_lidar_ok = false → lidar_avoid_process() 直接跳过，不影响正常行驶。

---

流水线位置

lidar_avoid_process() 位于 CameraHandler Step 4，在视觉巡线之后、模型推理之前：

Step 3  image_main()           → left_line[], right_line[], mid_line[]
Step 4  lidar_avoid_process()  → 修改 mid_line[] (绕行变形)  ★
Step 5  run_model_inference()
Step 6c cone_detect_and_deform() → 也修改 mid_line[] (叠加在 lidar 之上)

lidar 先于 cone 修改 mid_line，优先级更高。cone 的 clamp 确保不超出边界。

---

核心算法

1. 读取激光距离（每 2 帧一次）

lidar_avoid_process():
  if !g_lidar_ok || !g_cfg.lidar_enable → return
  if ++lidar_skip < 2 → return          // 每 2 帧一读 (~15Hz)
  lidar_skip = 0

  data = g_lidar_sensor.readResult()     // 非阻塞读取上次测量结果
  d_mm = data.RangeMilliMeter
  g_lidar_sensor.startMeasure()          // 立即启动下次测量 (后台 ~20ms)

2. 预触发去抖

  if d_mm == 0:                          // 读失败, 跳过
      skip

  if d_mm >= g_cfg.lidar_pre:            // 距离超出预触发窗口 (默认 800mm)
      g_lidar_frames = max(0, frames-1)  // 衰减但不归零
      skip

  // d_mm < lidar_pre → 进入触发判定

lidar_pre（默认 800mm）是预触发窗口：距离进入此范围就开始攒帧，但要 < lidar_thresh（默认 300mm）才真正确认。

3. 距离 → 图像行映射

  row = lt_h - 1 - (d_mm - lidar_near) × (lt_h - 11) / (lidar_far - lidar_near)
  row = clamp(row, 10, lt_h - 1)

参数	默认	含义
lidar_near	50 mm	row = lt_h-1（最近行）对应的距离
lidar_far	1200 mm	row = 10（最远有效行）对应的距离

4. 触发判定

  g_lidar_frames++
  g_lidar_obstacle_row = row

  g_lidar_confirmed = (d_mm < lidar_thresh) && (g_lidar_frames >= lidar_min_frames)

注意： 不检查视觉边界线（near_valid / far_lost），纯距离触发。 配置中的 lidar_near_start、lidar_near_end、lidar_far_span 参数被加载但未使用（为设计预留）。

5. 确认 → 记录状态

  if g_lidar_confirmed:
      g_lidar_hold_ctr     = 0
      g_lidar_hold_src_row = g_lidar_obstacle_row
      g_lidar_hold_dir     = 1.0          // 固定朝右绕行
      g_lidar_frames       = 0            // 重置计数

方向固定为右（+1.0），不根据赛道左右空间动态判断。

---

中线变形

当 lidar_is_active()（确认中或保持衰减中）时，对 row 10 ~ 障碍行施加变形：

  decay = (确认中) 1.0 : (1.0 - hold_ctr / hold_frames)   // 保持期线性衰减

  for row = 10  to  g_lidar_hold_src_row:
      t    = clamp((row - 10) / lidar_avoid_range, 0.0, 1.0)
      push = t × lidar_avoid_gain × half_w × dir × decay
      mid_line[row] = clamp(mid_line[row] + push,
                             left_line[row] + 2.0,
                             right_line[row] - 2.0)

---

状态机

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

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

---

速度联动

motor_update() 中包含挡板减速代码（lidar_is_active() → speed × lidar_speed）， 但当前已注释。绕行仅靠中线变形，不减速。如需启用，取消注释即可。

---

配置参数

文件	类型	默认	含义
./lidar_enable	int(0/1)	1	激光避障总开关（0=禁用，init 也不调用）
./lidar_thresh	int	300	障碍判定距离阈值 (mm)，低于此值触发确认
./lidar_pre	int	800	预触发窗口 (mm)，低于此值开始攒帧
./lidar_near	int	50	row=lt_h-1 对应的物理距离 (mm)
./lidar_far	int	1200	row=10 对应的物理距离 (mm)
./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_near_start	int	1	(已加载但未使用，预留)
./lidar_near_end	int	4	(已加载但未使用，预留)
./lidar_far_span	int	6	(已加载但未使用，预留)

---

边界情况 & 注意事项

1. 激光硬件未连接 — init() 失败时 g_lidar_ok = false，后续跳过，不阻塞。
2. 弯道边墙误判 — 急弯处激光可能打到边墙产生近距读数。依赖 lidar_thresh + lidar_min_frames 去抖抑制。
3. 方向固定右绕 — 当前不判断挡板在赛道左侧还是右侧，一律朝右推中线。对大多数场景够用，但若挡板在右侧则绕行方向不理想。
4. 标定建议 — 在平坦路段用 debug 模式打印 d_mm 和 row，确认 lidar_near / lidar_far 映射准确。
5. VL53L0X 读取模式 — 非阻塞：先 startMeasure() 启动后台测量 (~20ms)，下次调用时 readResult() 取结果。每 2 帧一读不阻塞主循环。
6. 多传感器优先级 — lidar 先于 cone 修改 mid_line。zebra/tl 的 block 刹车优先级最高（红灯/斑马线停车不绕行）。