spdis/Loongson_2k0300_SmartCar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

C路: VL53L0X 用户态I2C直驱 + 高速模式 29.6fps

Browse Source 
- 搬运 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 优先级优化
This commit is contained in:
spdis
2026-06-17 13:47:13 +08:00
parent 06a73b0f02
commit 95a46c130d
31 changed files with 11645 additions and 191 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/ARCHITECTURE.md
+243 -134
View File
@@ -3,119 +3,98 @@
## 坐标系统一览
```
Camera 输出 640×480 MJPEG
│
CONVERT_RGB=0 + IMREAD_REDUCED_COLOR_4
→ 1/4 MJPEG 解码 → raw_frame = 160×120
│
┌───────────┤
▼ ▼ (raw_frame.cols/rows 决定分母)
巡线空间 显示空间 模型空间
(line_tracking (newWidth × newHeight) 160×120 (正常)
_width × calc_scale = 2 640×480 (回退模式)
line_tracking newWidth = lt_w × 2
_height) newHeight = lt_h × 2
典型值 80×60
Camera 输出 640×480 MJPEG
│
CONVERT_RGB=0 + IMREAD_REDUCED_COLOR_4
→ 1/4 MJPEG 解码 → raw_frame = 160×120
│
┌───────────┤
▼ ▼ (raw_frame.cols/rows 决定分母)
巡线空间 显示空间 模型空间
(line_tracking (newWidth × newHeight) 160×120 (正常)
_width × calc_scale = 2 640×480 (回退模式)
line_tracking newWidth = lt_w × 2
_height) newHeight = lt_h × 2
典型值 80×60
```
**关键换算关系（正常模式 raw_frame = 160×120）：**
- 模型空间 → 巡线空间：`col_lt = cx * line_tracking_width / 160`，`row_lt = cy * line_tracking_height / 120`
- 巡线空间 → 显示空间：`pixel = value * calc_scale`（calc_scale = 2）
- newWidth / newHeight 取决于屏幕物理分辨率，由 `CameraInit` 动态计算
- line_tracking_width / height = newWidth/calc_scale, newHeight/calc_scale
- `line_tracking_width / height` = `newWidth / calc_scale`, `newHeight / calc_scale`
- newWidth / newHeight 由 `CameraInit` 读取 `/dev/fb0` 屏幕分辨率后动态计算
**⚠ 回退模式：** 当 CONVERT_RGB=0 未生效时，raw_frame = 640×480，模型框坐标也在 640×480 空间。
此时 LCD 渲染仍硬编码除以 160/120（会出错），但正常运行时不会进入此模式。
此时 LCD 渲染的硬编码缩放 `newWidth/160.0f` 会出错，但正常运行时不会进入此模式。
---
## 主循环 (main.cpp → CameraHandler)
## 主循环 (`main.cpp`)
```
main()
├── cfg_load_all() # 从工作目录文件读取全部配置
├── CameraInit(0, dest_fps, 320, 240) # 打开摄像头, LCD, 模型, I2C
├── ControlInit() # 初始化双电机 GPIO + PWM
├── cfg_load_all() # 从工作目录文件读取全部配置
├── 补读 cone_avoid_gain/range/hold_frames
├── CameraInit(0) # 打开摄像头, LCD, 模型, I2C, 计算巡线尺寸
├── ControlInit() # 初始化双电机 GPIO + PWM + 启动编码器线程
│
└── while(running):
└── CameraHandler() # ★ 以下逐帧执行
├── CameraHandler() # ★ 逐帧执行
└── target_speed = g_cfg.speed # (debug 模式下每 7 帧重载)
```
`CameraInit` 现在只接受 `camera_id` 一个参数（之前有 dest_fps, width, height 三个遗留参数已移除）。
巡线分辨率由屏幕自适应算法决定：`line_tracking_w = newWidth/2`, `line_tracking_h = newHeight/2`。
---
## CameraHandler 11 步流水线
```
Step 1 capture_frame() 640×480 MJPEG → IMREAD_REDUCED_COLOR_4
取帧+解码 → raw_frame = 160×120 BGR
(回退: raw_mat 三通道 → 直用 640×480)
Step 2 save_image_if_requested() debug=1 && saveImg 文件=1 → 存 ./image/XXXXX.jpg
保存图像 (条件)
Step 3 image_main() raw_frame → resize(lt_w×lt_h) → HSV 双通道Otsu
视觉巡线 → floodFill(种子 (lt_w/2, lt_h-10), 半径5)
→ 逐行最长连续段搜索 → 丢线补全(row 10~59)
输出: left_line[], right_line[], mid_line[]
Step 4 run_model_inference() 每 2 帧推理一次
模型推理 raw_frame (160×120) → model_v10_detect()
4 类: 0=锥桶 1=红灯 2=绿灯 3=斑马线
g_thresh = {0.90, 0.75, 0.80, 0.90}
→ g_boxes[16], g_box_count
Step 5a zebra_process() 去抖计数 + ZNORMAL→ZSTOP(4s)→ZCOOLDOWN(5s)
斑马线状态机 触发: 连续5帧 + 起源于远(cy≤50) + 当前近(cy>zebrasee)
刹停时 I2C 语音播报
Step 5b traffic_light_process() TLNORMAL→TLSTOP→TLWAIT_GREEN
红绿灯状态机 红灯≥3帧 → 停车; 红灯消失 → 等绿灯; 绿灯≥3帧 → 通行
Step 5c cone_detect_and_deform() 去抖确认 + 中线变形 + 消失后保持衰减
锥桶检测 & 中线变形 cone_speed 减速 + cone_hold_frames 保持
Step 6 steering_update() foresee/2 → check_row → mid_line[row]×2 - newWidth/2
舵机控制 deadband 过滤 → servo duty: 1500000 ± offset ns
Step 7 motor_update() 开环PWM + 编码器比例刹车 + 弯道减速 + 锥桶减速
电机控制
Step 8 lcd_render() track→BGR→ROI + 边界线(红/绿/蓝) + 检测框 + 状态指示
LCD 渲染 RGB565 → /dev/fb0 mmap
Step 9 fps_log() 每 15 帧输出总帧率 + 分步耗时 ms
FPS 统计
```
---
## CameraHandler 9 步流水线
```
┌─────────────────────┐
Step 1 │ cap.read(raw_mat) │ 640×480 MJPEG 原始字节流
取帧+解码 │ raw_mat 单通道(字节) │ → IMREAD_REDUCED_COLOR_4
│ → cv::imdecode │ → raw_frame = 160×120 BGR
│ raw_frame = decoded │ (回退: raw_mat 三通道→直用 640×480)
└────────┬────────────┘
│
Step 2 ┌────────▼────────────┐
保存图像 (条件) │ g_cfg.debug==1 │ 写入 ./image/image_XXXXX.jpg
│ && saveImg==1 → │ (双重条件，缺一不可)
│ saveCameraImage() │
└────────┬────────────┘
│
Step 3 ┌────────▼────────────┐
视觉巡线 │ image_main() │ raw_frame → resize(lt_w×lt_h)
(每帧都跑) │ │ → HSV双通道Otsu → floodFill
│ 输出: left_line[] │ → 逐行最长连续段搜索
│ right_line[] │ → 丢线补全(仅 row 10~59)
│ mid_line[] │ → track (赛道蒙版 128/0)
└────────┬────────────┘
│
Step 4 ┌────────▼────────────┐
模型推理 │ model_v10_detect() │ 每 2 帧推理一次
(每 2 帧) │ raw_frame (160×120) │ 4 类: 0=锥桶 1=红灯 2=绿灯 3=斑马线
│ → g_boxes[16] │ g_thresh = [0.80,0.80,0.80,0.75]
│ → g_box_count │ 框坐标 ∈ raw_frame 空间
└────────┬────────────┘
│
Step 5 ┌────────▼────────────┐
斑马线状态机 │ 遍历 g_boxes │ 仅处理 cls=3，取第一个斑马线框
│ 去抖计数+远近判断 │ NORMAL→STOP(4s)→COOLDOWN(5s)
│ │ I2C 语音播报 @ 0x34
└────────┬────────────┘
│
Step 6 ┌────────▼────────────┐
舵机控制 │ if g_cfg.start: │
│ foresee→check_row │ 偏差 = mid_line[row]×2 - newWidth/2
│ mid_line[check_row]│ → g_steer_deviation ∈ [-1, 1]
│ → deviation │ → servo duty: 1500000 ± offset
│ → deadband 过滤 │ clamp [1.2M, 1.8M] ns
│ → servo.setDuty()│ mid_val==255 → 跳过(无效行)
└────────┬────────────┘
│
Step 7 ┌────────▼────────────┐
电机控制 │ ControlUpdate( │
(开环) │ target_speed, │ if zebra_STOP: duty=0 (刹停)+GPIO73=0
│ g_zstate==Z_STOP) │ else: speed × curve
│ │ curve = 1 - |deviation|×0.4
│ │ curve ∈ [0.6, 1.0]
│ │ 左右电机同速(无差速)
└────────┬────────────┘
│
Step 8 ┌────────▼────────────┐
LCD 渲染 │ if g_lcd_on: │ g_lcd_on = g_cfg.showImg 的缓存
│ track→BGR→ROI │ (每10帧刷新一次缓存)
│ + 边界线(红) │ 检测框坐标 bx=newWidth/160 缩放
│ + 中线(蓝) │ 状态指示 N(绿)/S(红)/C(黄)
│ + 检测框标注 │ → RGB565 → /dev/fb0 mmap
│ (L1条状:跳过红灯) │
└────────┬────────────┘
│
Step 9 ┌────────▼────────────┐
FPS 统计 │ 每 15 帧输出 │ stdout: "fps=XX.X|rd=X vi=X md=X ct=X ms"
│ 分步计时+总帧率 │ 15帧平均
└─────────────────────┘
```
---
## 视觉巡线深度展开 (image_main)
## 视觉巡线深度展开 (`image_main`)
```
raw_frame (160×120 或 640×480 BGR)
@@ -130,7 +109,7 @@ raw_frame (160×120 或 640×480 BGR)
│
├── find_road():
│ MORPH_OPEN(2×2 CROSS) → 种子点(lt_w/2, lt_h-10)
│ → 种子点处画实心圆(半径10, 255) 防种子落在黑色区域
│ → 种子点处画实心圆(半径5, 255) 防种子落在黑色区域
│ → floodFill(loDiff=20, upDiff=20, 8邻域, newVal=128)
│ → mask 提取 ROI → track (赛道内部=128, 外部=0)
│
@@ -141,28 +120,29 @@ raw_frame (160×120 或 640×480 BGR)
│ → left_line[row], right_line[row]
│ → 全零行: left=right=-1
│
└── 中线 + 丢线补全 (row = lt_h-1 → 10):
└── 中线 + 丢线补全 (row = lt_h-2 → 10):
有边界: mid = (left+right)/2, int 整除
丢线: mid[row] = mid[row+1] (继承下行)
若 mid > lt_w/2 → left=mid, right=lt_w-1 (偏右)
若 mid ≤ lt_w/2 → left=0, right=mid (偏左)
底行兜底: 丢线时用 lt_w/2
```
**边界线区间有效性：** row 10~59 有可靠数据；row 0~9 的 mid_line 保持初始值 -1（不可靠，不要写入 box 过滤逻辑）。
**边界线区间有效性：** row 10~59 有数据；row 0~9 的 mid_line 保持初始值 -1（不可靠）。
---
## 舵机控制数学
```
foresee = g_cfg.foresee ← 前瞻行索引（像素，显示空间），默认 40
check_row = foresee / calc_scale ← 转为巡线空间行号（calc_scale=2, int 整除）
mid_val = mid_line[check_row] ← 该行中线列坐标 (0~79)
foresee = g_cfg.foresee ← 前瞻行索引（显示空间像素），默认 40
check_row = (int)foresee / calc_scale ← 转为巡线空间行号（calc_scale=2, int 整除）
mid_val = mid_line[check_row] ← 该行中线列坐标 (0~79)
如果 mid_val == 255 → 跳过（丢线补全也写不到 255，仅作防御）
如果 mid_val == -1 → 跳过（丢线行无效）
否则：
deviation = mid_val × 2 - newWidth / 2 ← 转为显示空间偏差(px)
deviation -= g_cfg.center_bias ← 中心偏置修正
deviation -= g_cfg.center_bias ← 中心偏置修正
g_steer_deviation = deviation / (newWidth/2) ← 归一化到 [-1, 1]
if |deviation| < deadband:
@@ -179,19 +159,32 @@ mid_val = mid_line[check_row] ← 该行中线列坐标 (0~79)
---
## 电机控制数学
## 电机控制数学 (`ControlUpdate`)
```
ControlUpdate(speed, zebra_block):
ControlUpdate(speed, zebra_or_tl_block):
if zebra_block || !g_cfg.start:
if !g_cfg.start:
motor[0].updateduty(0) → 两电机停转
motor[1].updateduty(0)
if !g_cfg.start: mortorEN.setValue(0) → GPIO73 拉低
mortorEN.setValue(0) → GPIO73 拉低
return
curve = 1.0 - |g_steer_deviation| × 0.4
curve = max(curve, 0.6) ← 最低不低于 60% 速度
if zebra_or_tl_block: ← 斑马线或红灯刹停
读取 g_enc_speed (编码器线程, 脉冲/秒)
读取 g_enc_dir (编码器方向)
if cur_speed > 0.5:
brake_ns = cur_speed × g_cfg.brake_scale ← 比例刹车
brake_ns = clamp(brake_ns, 0, g_cfg.brake_max)
brake_pct = brake_ns / 500 ← ns → % (period=50000ns)
dir = cur_dir ? 1 : 0
motor[i]->updateduty(dir ? -brake_pct : brake_pct) ← 反转刹车
else:
motor[i]->updateduty(0) ← 已静止，不刹车
return
curve = 1.0 - |g_steer_deviation| × 0.4 ← 弯道减速
curve = max(curve, 0.6) ← 最低 ≥ 60%
spd = speed × curve
motor[0].updateduty(spd) → 左电机 (pwmchip8/pwm2, gpio12 方向)
@@ -199,22 +192,31 @@ ControlUpdate(speed, zebra_block):
mortorEN.setValue(1) → GPIO73 拉高使能
```
**编码器刹车（encoder brake）：** 当斑马线或红灯触发停车时，根据实时编码器速度计算反向刹车占空比，
刹车力度与当前车速成正比（`brake_scale`），有上限（`brake_max` ns）。
速度低于 0.5 pps 时不再施加刹车。
**motor_update 额外逻辑：** `motor_update()` 在调用 `ControlUpdate` 之前，若锥桶已触发（`cone_is_slow()`），
会将 speed 乘以 `g_cfg.cone_speed`（默认 0.5），实现锥桶路段减速。
**motor[0] vs motor[1]：** 左/右区别仅在于 gpioNum（12 vs 13）和 pwm通道（2 vs 1），函数调用完全相同。
**updateduty(duty) 内部：**
- `pwm_period * |duty| / 100` → 转占空比 ns 写入 sysfs
- duty > 0 → GPIO 方向 = 1（正转），duty ≤ 0 → GPIO 方向 = 0（反转）
**速度控制是开环的：** `MotorController` 仅包含 `updateduty()` 方法，无编码器反馈。
**速度控制是开环的：** `MotorController` 仅包含 `updateduty()` 方法，正常行驶无编码器反馈。
`PIDController` 类存在但从未被实例化或调用。
MotorController.h 中也无 `updateSpeed()` 方法（之前的文档中误记了此函数）。
**编码器线程** (`ControlInit` → `encoder_thread`)：独立线程高频轮询 GPIO67（LSB脉冲）+ GPIO72（方向），
每 100ms 计算一次速度 → `g_enc_speed`（脉冲/秒），含 500µs sleep 防止 CPU 饿死。
---
## 斑马线状态机
```
检测到 cls=3 → 取第一个斑马线框 → 去抖计数+追踪最远cy
检测到 cls=3 → 取第一个斑马线框（之后 break）→ 去抖计数+追踪最远cy
ZNORMAL 期间:
zebra_seen: g_zc_frames++, g_zc_min_cy = min(g_zc_min_cy, zebra_cy)
@@ -231,14 +233,14 @@ ZNORMAL 期间:
│NORMAL│ ─────────────────────────────────────► ┌──────┐
│ │ ◄───────────────────────────────────── │ STOP │
└──────┘ 冷却5秒结束 │ │
▲ └──┬───┘
│ 4秒后 │
│ ┌──────────┐ ◄─────────────────────┘
└───────────│ COOLDOWN │
└──────────┘
▲ └──┬───┘
│ 4秒后 │
│ ┌──────────┐ ◄─────────────────────┘
└──────────│ COOLDOWN │
└──────────┘
NORMAL: 允许通行，检测斑马线
STOP: 刹停 4 秒，g_zstate==Z_STOP 传给 ControlUpdate 第二个参数
STOP: 刹停 4 秒，g_zstate==Z_STOP 传给 motor_update → 编码器刹车
COOLDOWN: 恢复行驶但斑马线状态机停止检测 5 秒（防止重复触发）
仅跳过检测，不影响其他功能（舵机/巡线正常）
```
@@ -247,14 +249,99 @@ COOLDOWN: 恢复行驶但斑马线状态机停止检测 5 秒（防止重复触
- `g_zc_min_cy`：NORMAL 期间追踪斑马线出现时的最小 cy（越远值越小）。未检测到斑马线时衰减减2/帧，归零后重置为 120。
- `ZEBRA_FAR_CY = 50`：斑马线的 cy 必须曾在 ≤50 处出现过（即"从远处来"）
- `zebrasee`（默认 60）：当前斑马线 cy > 此值视为"足够近"，触发停车
- 去抖衰减速度：未检测到时每次 `-2`（比锥桶设计中的 `-1` 更快下降）
- 去抖衰减速度：未检测到时每次 `-2`（比锥桶设计的 `-1` 更快下降）
- Box 遍历在找到第一个 cls=3 后 `break`，忽略同帧其他斑马线框
---
## 红绿灯状态机
```
检测到 cls=1 (红灯) / cls=2 (绿灯) → 去抖计数 → 状态转移
┌──────┐ 红灯≥3帧 ┌──────┐ 红灯消失 ┌───────────┐
│NORMAL│ ───────────► │ STOP │ ──────────► │WAIT_GREEN │
│ │ ◄─────────── │ │ │ │
└──────┘ 绿灯≥3帧 └──────┘ └─────┬─────┘
▲ │
└───────────────────────────────────────────┘
TL_NORMAL: 允许通行，检测红绿灯
TL_STOP: 红灯→停车（编码器刹车），等红灯消失
TL_WAIT_GREEN: 红灯已消失，等待绿灯出现（≥3帧）→ 恢复通行
```
**返回值：** `traffic_light_process()` 返回 `true` 当 `g_tl_state != TL_NORMAL`，
传递给 `motor_update` → `ControlUpdate` 触发编码器刹车。
---
## 锥桶检测 & 中线变形
```
cone_detect_and_deform():
(仅在 Z_NORMAL && TL_NORMAL 时运行)
┌─ 寻找最近锥桶 ─┐
│ 遍历 g_boxes, cls=0, conf >= cone_thresh
│ 模型空间坐标 → 巡线空间坐标
│ 过滤: rl < 10 或 无边界线的行 → 跳过
│ cone_margin==0 → 中心点; ==1 → 框边缘 (左右均在边界线内)
│ 取 cy 最大(最近)的锥桶
│
├─ 去抖确认 ──────────────────────────────
│ 位置容忍: row_tol = max(2, lt_h/12), col_tol = max(3, lt_w/8)
│ 连续出现且位置接近 → g_cone_frames++
│ 丢失 → g_cone_frames = max(0, g_cone_frames - 1)
│ g_cone_confirmed = (g_cone_frames >= cone_min_frames)
│
├─ 保持/衰减 ────────────────────────────
│ 确认时: g_cone_hold_ctr=0, 记录锥桶位置
│ 未确认但有历史: g_cone_hold_ctr++, 衰减推离量
│ 超过 cone_hold_frames → 停止影响
│
└─ 中线变形 ─────────────────────────────
dir = (锥桶偏左) ? +1.0 : -1.0 ← 向远离锥桶方向推
decay = (确认) 1.0 : (1 - hold_ctr/hold_frames)
for row = 10 → src_row:
t = clamp((row-10) / cone_avoid_range, 0, 1) ← 斜坡上升
push = t × cone_avoid_gain × half_w × dir × decay
mid_line[row] = clamp(mid_line[row] + push,
left_line[row]+2, right_line[row]-2)
```
**速度联动：** `cone_is_slow()` 在锥桶确认或保持期间返回 true，
`motor_update` 将当前速度乘以 `cone_speed`（默认 0.5）。
---
## LCD 渲染
```
lcd_render():
g_lcd_on 每 10 帧从 g_cfg.showImg 刷新
track (128/0 灰度) → resize(newWidth×newHeight) → GRAY→BGR
→ 居中拷贝到 lcd_fbImage (screenHeight × screenWidth) 的 ROI 区域
边界线绘制: 红=左边界, 绿=右边界, 蓝=中线
(跳过 mid_line[row]==-1 的行)
检测框绘制: 遍历 g_boxes, bx=newWidth/raw_frame.cols
跳过 cls=1(红灯) 和 cls=2(绿灯) 的框
cls=0 锥桶 → 橙色框, cls=3 斑马线 → 紫色框
标注框类别+置信度
状态指示 (左下角): N=正常, S=斑马线停车, C=斑马线冷却,
R=红灯停车, G=等绿灯
→ convertMatToRGB565 → /dev/fb0 mmap
```
---
## 配置系统
所有配置通过工作目录下的纯文本文件读写。文件不存在时 readDoubleFromFile 返回 0：
所有配置通过工作目录下的纯文本文件读写。文件不存在时 `readDoubleFromFile` 返回 0：
| 文件 | 类型 | 代码默认 | ctl.sh 写入 | 含义 |
|---|---|---|---|---|
@@ -271,32 +358,54 @@ COOLDOWN: 恢复行驶但斑马线状态机停止检测 5 秒（防止重复触
| `./zebrasee` | double | 60 | (不写入) | 斑马线近界阈值 (模型空间cy) |
| `./destfps` | double | 30* | (不写入) | 目标帧率 (*代码兜底值) |
| `./saveImg` | int(0/1) | - | (不写入) | 保存帧图像 (需 debug=1) |
| `./cone_avoid_gain` | double | 0.3 | 0.3 | 锥桶中线变形推离量 (归一化) |
| `./cone_avoid_range` | int | 30 | 30 | 变形斜坡陡峭度 |
| `./cone_speed` | double | 0.5 | 0.5 | 锥桶触发时速度倍率 |
| `./cone_min_frames` | int | 3 | 3 | 锥桶连续确认帧数 |
| `./cone_margin` | int | 0 | 0 | 0=中心点, 1=框边缘检查 |
| `./cone_thresh` | double | 0.80 | 0.80 | 锥桶置信度阈值 |
| `./cone_hold_frames` | int | 30 | 30 | 锥桶消失后保持变形帧数 |
| `./brake_scale` | double | 10 | 10 | 刹车速度→占空比系数 |
| `./brake_max` | int | 10000 | 10000 | 最大刹车占空比 (ns) |
**debug 模式：** `./debug` = 1 时，主循环每7帧调用一次 `cfg_load_all()` 重读全部配置，支持热更新调参。同时允许 `saveImg` 保存图像。
**注意**：`global.h` 中的 `CfgCache` 默认值和 `ctl.sh` 写入的值不一致（如 speed: 60 vs 11）。`ctl.sh` 写入的值覆盖代码默认值，是实际运行参数。
**注意：** `global.h` 中的 `CfgCache` 默认值和 `ctl.sh` 写入的值不一致（如 speed: 60 vs 11）。`ctl.sh` 写入的值覆盖代码默认值，是实际运行参数。
---
## 代码中未使用的模块
以下 `.cpp` 文件已编译进 `common_lib` 但主循环中从未调用：
以下 `.cpp` 文件存在于 `src/` 中但已被 `CMakeLists.txt` 的 `list(FILTER ... EXCLUDE)` 排除编译：
| 文件 | 功能 | 状态 |
| 文件 | 功能 | 排除原因 |
|---|---|---|
| `PIDController.cpp` | 位置式/增量式 PID | 类存在但无实例化，无调用 |
| `serial.cpp` | VOFA 串口可视化 (vofa_justfloat/vofa_image) | 已实现但无调用 |
| `Timer.cpp` | 定时器线程 | 已实现但无调用（Video 依赖它但 Video 也未用） |
| `video.cpp` | 视频文件流读取 | 已实现但无调用 |
| `vl53l0x.cpp` | 激光测距模块 | 硬件未连接 |
| `zebra_detect.cpp` | 经典斑马线检测 | 已被 Mild 模型替代 |
| `PIDController.cpp` | 位置式/增量式 PID | 电机开环直驱，无调用点 |
| `serial.cpp` | VOFA 串口可视化 | 未使用 |
**注意：** `PIDController.h` 仍在 `lib/` 中，`MotorController` 仅提供 `updateduty()`（开环占空比），
不提供 `updateSpeed()` 方法。
---
## 当前已知缺陷 / 未利用能力
## 模型：Mild v12
1. **cls=0 锥桶** — 模型已检测但被忽略（CONE_DESIGN.md 设计了方案）
2. **cls=1 红灯 / cls=2 绿灯** — 检测框跳过不画（LCD 渲染中 `if g_boxes[i].cls == 1 || g_boxes[i].cls == 2: continue`），无决策
3. **电机开环** — 无编码器反馈，PID 类已实现但无调用点，`MotorController` 无 `updateSpeed()` 方法
4. **舵机死区** — 用 `abs(deviation) < deadband` 比较像素值，deadband 单位是显示空间像素
5. **中线 row 范围** — 丢线补全仅覆盖 row 10~59，row 0~9 的 mid_line 保持 -1 不可靠
6. **mid_val == 255 防御** — 代码中写死但 255 永远不会出现在 mid_line 中（当前实现下）
7. **LCD 坐标缩放硬编码** — 检测框绘制用 `newWidth/160.0f`，假设 raw_frame 宽=160。回退模式下 raw_frame=640 时出错
- **推理引擎**：`src/model_v10.{hpp,cpp}`（名称为兼容保留，内部为 Mild v3）
- **输入**：160×120 BGR
- **输出**：4 类 + 背景 → 9 通道：0=锥桶, 1=红灯, 2=绿灯, 3=斑马线
- **架构**：3→9→9→13→13→19→19→19→44→44→64→64 → head(64→64,dw) → out(64→9)
- **阈值**：`{0.90, 0.75, 0.80, 0.90}`（锥桶/红灯/绿灯/斑马线）
- **权重文件**：`mild_v12.bin`（105KB 自定义二进制格式）
- **推理频率**：每 2 帧一次（跳帧节省 CPU）
---
## 已知局限
1. **电机开环** — 正常行驶无编码器反馈，仅制动时使用编码器速度做比例刹车。PID 类已实现但无调用点。
2. **舵机死区** — 用 `abs(deviation) < deadband` 比较像素值，deadband 单位是显示空间像素。
3. **中线 row 范围** — 丢线补全仅覆盖 row 10~59，row 0~9 的 mid_line 保持 -1 不可靠。
4. **LCD 坐标缩放硬编码** — 检测框绘制用 `newWidth/160.0f`，假设 raw_frame 宽=160。回退模式（raw_frame=640）下会出错。
5. **锥桶变形可能跳过 row 0~9** — 变形循环从 row=10 开始，row 0~9 不会改变。
docs/LIDAR_AVOID.md
+343
View File
@@ -0,0 +1,343 @@
# 激光雷达挡板避障设计
## 问题
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），但仍会遮挡背景赛道导致丢线。判定逻辑依赖的是**丢线**而非**像素颜色**。