Loongson_2k0300_SmartCar/docs/LIDAR_AVOID.md

激光雷达挡板避障设计

问题

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 速度联动

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

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

---

复用现有 VL53L0X 驱动

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

// 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），但仍会遮挡背景赛道导致丢线。判定逻辑依赖的是丢线而非像素颜色。