spdis
188 lines
7.0 KiB
Markdown
188 lines
7.0 KiB
Markdown
# 激光雷达挡板避障设计
|
||
|
||
## 概述
|
||
|
||
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 刹车优先级最高（红灯/斑马线停车不绕行）。
|