spdis
95a46c130d
- 搬运 ST API C 源码到 lib/vl53l0x/, 替换 I2C 层为用户态 i2c-dev - 新增 vl53l0x_platform_user.cpp: WriteMulti/ReadMulti 等全平台接口 - 重写 vl53l0x.cpp: start时unbind内核驱动, 单次测距, 无后台轮询 - 高速模式 20000μs timing budget, 模型缓存零污染(41-43ms) - LiDAR 每5帧一读(~20ms), 隔帧模型推理, fps 25.1→29.6 (+18%) - 同事的LiDAR避障集成: 12个配置参数, 中线变形绕行, 刹车注释 - nice -10 + sched_yield 优先级优化
344 lines
13 KiB
Markdown
344 lines
13 KiB
Markdown
# 激光雷达挡板避障设计
|
||
|
||
## 问题
|
||
|
||
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），但仍会遮挡背景赛道导致丢线。判定逻辑依赖的是**丢线**而非**像素颜色**。
|