spdis
303 lines
16 KiB
Markdown
303 lines
16 KiB
Markdown
# SmartCar 决策总架构：从视频帧到执行器
|
||
|
||
## 坐标系统一览
|
||
|
||
```
|
||
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
|
||
|
||
**⚠ 回退模式：** 当 CONVERT_RGB=0 未生效时，raw_frame = 640×480，模型框坐标也在 640×480 空间。
|
||
此时 LCD 渲染仍硬编码除以 160/120（会出错），但正常运行时不会进入此模式。
|
||
|
||
---
|
||
|
||
## 主循环 (main.cpp → CameraHandler)
|
||
|
||
```
|
||
main()
|
||
├── cfg_load_all() # 从工作目录文件读取全部配置
|
||
├── CameraInit(0, dest_fps, 320, 240) # 打开摄像头, LCD, 模型, I2C
|
||
├── ControlInit() # 初始化双电机 GPIO + PWM
|
||
│
|
||
└── while(running):
|
||
└── CameraHandler() # ★ 以下逐帧执行
|
||
```
|
||
|
||
---
|
||
|
||
## 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)
|
||
|
||
```
|
||
raw_frame (160×120 或 640×480 BGR)
|
||
│
|
||
├── cv::resize → (line_tracking_w × line_tracking_h) 典型 80×60
|
||
│
|
||
├── image_binerize():
|
||
│ BGR→HSV → H通道Otsu(THRESH_BINARY_INV) → S通道Otsu → bitwise_or
|
||
│ 赛道=255(白), 背景=0(黑)
|
||
│ 原理：蓝底赛道H偏聚集(100~130)、S高；灰路面S低
|
||
│ bitwise_or 取并集，H或S任一判为赛道即保留
|
||
│
|
||
├── find_road():
|
||
│ MORPH_OPEN(2×2 CROSS) → 种子点(lt_w/2, lt_h-10)
|
||
│ → 种子点处画实心圆(半径10, 255) 防种子落在黑色区域
|
||
│ → floodFill(loDiff=20, upDiff=20, 8邻域, newVal=128)
|
||
│ → mask 提取 ROI → track (赛道内部=128, 外部=0)
|
||
│
|
||
├── 逐行最长连续段搜索:
|
||
│ uchar(*IMG)[lt_w] = track.data
|
||
│ 每行扫描，找最长连续非零段
|
||
│ 有多个白段时取最后一个(≥取等，偏右)
|
||
│ → left_line[row], right_line[row]
|
||
│ → 全零行: left=right=-1
|
||
│
|
||
└── 中线 + 丢线补全 (row = lt_h-1 → 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 (偏左)
|
||
```
|
||
|
||
**边界线区间有效性：** row 10~59 有可靠数据；row 0~9 的 mid_line 保持初始值 -1（不可靠，不要写入 box 过滤逻辑）。
|
||
|
||
---
|
||
|
||
## 舵机控制数学
|
||
|
||
```
|
||
foresee = g_cfg.foresee ← 前瞻行索引（像素，显示空间），默认 40
|
||
check_row = foresee / calc_scale ← 转为巡线空间行号（calc_scale=2, int 整除）
|
||
mid_val = mid_line[check_row] ← 该行中线列坐标 (0~79)
|
||
|
||
如果 mid_val == 255 → 跳过（丢线补全也写不到 255，仅作防御）
|
||
否则：
|
||
deviation = mid_val × 2 - newWidth / 2 ← 转为显示空间偏差(px)
|
||
deviation -= g_cfg.center_bias ← 中心偏置修正
|
||
g_steer_deviation = deviation / (newWidth/2) ← 归一化到 [-1, 1]
|
||
|
||
if |deviation| < deadband:
|
||
servo = 1500000 ns (直行)
|
||
else:
|
||
norm = deviation / (newWidth/2)
|
||
offset = norm × steer_gain × 300000
|
||
duty = clamp(1500000 + offset, 1200000, 1800000)
|
||
servo.setDutyCycle(duty)
|
||
```
|
||
|
||
**舵机量程：** 1,200,000 ~ 1,800,000 ns，中位 1,500,000 ns，±300,000 ns 对应满偏。
|
||
**deadband 单位：** 显示空间像素（deviation 的单位）。
|
||
|
||
---
|
||
|
||
## 电机控制数学
|
||
|
||
```
|
||
ControlUpdate(speed, zebra_block):
|
||
|
||
if zebra_block || !g_cfg.start:
|
||
motor[0].updateduty(0) → 两电机停转
|
||
motor[1].updateduty(0)
|
||
if !g_cfg.start: mortorEN.setValue(0) → GPIO73 拉低
|
||
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 方向)
|
||
motor[1].updateduty(spd) → 右电机 (pwmchip8/pwm1, gpio13 方向)
|
||
mortorEN.setValue(1) → GPIO73 拉高使能
|
||
```
|
||
|
||
**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()` 方法，无编码器反馈。
|
||
`PIDController` 类存在但从未被实例化或调用。
|
||
MotorController.h 中也无 `updateSpeed()` 方法（之前的文档中误记了此函数）。
|
||
|
||
---
|
||
|
||
## 斑马线状态机
|
||
|
||
```
|
||
检测到 cls=3 → 取第一个斑马线框 → 去抖计数+追踪最远cy
|
||
|
||
ZNORMAL 期间:
|
||
zebra_seen: g_zc_frames++, g_zc_min_cy = min(g_zc_min_cy, zebra_cy)
|
||
未 seen: g_zc_frames = max(0, g_zc_frames - 2)
|
||
g_zc_frames==0 → g_zc_min_cy=120 (重置)
|
||
|
||
触发条件 (仅 ZNORMAL):
|
||
zebra_near (cy > g_cfg.zebrasee)
|
||
&& g_zc_frames >= ZEBRA_MIN_FRAMES (5)
|
||
&& g_zc_min_cy <= ZEBRA_FAR_CY (50)
|
||
→ play_zebra_audio() → ZSTOP
|
||
|
||
┌──────┐ 条件: 连续5帧 + 起源于远(cy_min≤50) + 当前近(cy>zebrasee)
|
||
│NORMAL│ ─────────────────────────────────────► ┌──────┐
|
||
│ │ ◄───────────────────────────────────── │ STOP │
|
||
└──────┘ 冷却5秒结束 │ │
|
||
▲ └──┬───┘
|
||
│ 4秒后 │
|
||
│ ┌──────────┐ ◄─────────────────────┘
|
||
└───────────│ COOLDOWN │
|
||
└──────────┘
|
||
|
||
NORMAL: 允许通行，检测斑马线
|
||
STOP: 刹停 4 秒，g_zstate==Z_STOP 传给 ControlUpdate 第二个参数
|
||
COOLDOWN: 恢复行驶但斑马线状态机停止检测 5 秒（防止重复触发）
|
||
仅跳过检测，不影响其他功能（舵机/巡线正常）
|
||
```
|
||
|
||
**关键变量：**
|
||
- `g_zc_min_cy`：NORMAL 期间追踪斑马线出现时的最小 cy（越远值越小）。未检测到斑马线时衰减减2/帧，归零后重置为 120。
|
||
- `ZEBRA_FAR_CY = 50`：斑马线的 cy 必须曾在 ≤50 处出现过（即"从远处来"）
|
||
- `zebrasee`（默认 60）：当前斑马线 cy > 此值视为"足够近"，触发停车
|
||
- 去抖衰减速度：未检测到时每次 `-2`（比锥桶设计中的 `-1` 更快下降）
|
||
- Box 遍历在找到第一个 cls=3 后 `break`，忽略同帧其他斑马线框
|
||
|
||
---
|
||
|
||
## 配置系统
|
||
|
||
所有配置通过工作目录下的纯文本文件读写。文件不存在时 readDoubleFromFile 返回 0：
|
||
|
||
| 文件 | 类型 | 代码默认 | ctl.sh 写入 | 含义 |
|
||
|---|---|---|---|---|
|
||
| `./speed` | double | 60 | 11 | 目标速度 (% 占空比) |
|
||
| `./start` | int(0/1) | 0 | 0 | 使能开关，1=电机运行 |
|
||
| `./debug` | int(0/1) | 0 | (不写入) | 每7帧重载配置+允许存图 |
|
||
| `./showImg` | int(0/1) | 0 | (不写入) | LCD 显示（每10帧轮询→g_lcd_on） |
|
||
| `./foresee` | double | 80 | 40 | 舵机前瞻行 (显示空间像素) |
|
||
| `./deadband` | double | 5 | 8 | 舵机死区 (显示空间像素) |
|
||
| `./steer_gain` | double | 1.0 | 1.5 | 舵机增益 |
|
||
| `./center_bias` | double | 0 | (不写入) | 中线偏置修正 |
|
||
| `./kp`,`./ki`,`./kd` | double | - | 3.5/0.3/2.0 | PID 参数 (当前未使用) |
|
||
| `./mortor_kp/ki/kd` | double | - | 0.6/0.2/0 | 电机编码器 PID (当前未使用) |
|
||
| `./zebrasee` | double | 60 | (不写入) | 斑马线近界阈值 (模型空间cy) |
|
||
| `./destfps` | double | 30* | (不写入) | 目标帧率 (*代码兜底值) |
|
||
| `./saveImg` | int(0/1) | - | (不写入) | 保存帧图像 (需 debug=1) |
|
||
|
||
**debug 模式：** `./debug` = 1 时，主循环每7帧调用一次 `cfg_load_all()` 重读全部配置，支持热更新调参。同时允许 `saveImg` 保存图像。
|
||
|
||
**注意**：`global.h` 中的 `CfgCache` 默认值和 `ctl.sh` 写入的值不一致（如 speed: 60 vs 11）。`ctl.sh` 写入的值覆盖代码默认值，是实际运行参数。
|
||
|
||
---
|
||
|
||
## 代码中未使用的模块
|
||
|
||
以下 `.cpp` 文件已编译进 `common_lib` 但主循环中从未调用：
|
||
|
||
| 文件 | 功能 | 状态 |
|
||
|---|---|---|
|
||
| `PIDController.cpp` | 位置式/增量式 PID | 类存在但无实例化，无调用 |
|
||
| `serial.cpp` | VOFA 串口可视化 (vofa_justfloat/vofa_image) | 已实现但无调用 |
|
||
| `Timer.cpp` | 定时器线程 | 已实现但无调用（Video 依赖它但 Video 也未用） |
|
||
| `video.cpp` | 视频文件流读取 | 已实现但无调用 |
|
||
|
||
---
|
||
|
||
## 当前已知缺陷 / 未利用能力
|
||
|
||
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 时出错
|