spdis
16 KiB
16 KiB
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 1059 有可靠数据；row 09 的 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 |
视频文件流读取 | 已实现但无调用 |
当前已知缺陷 / 未利用能力
- cls=0 锥桶 — 模型已检测但被忽略（CONE_DESIGN.md 设计了方案）
- cls=1 红灯 / cls=2 绿灯 — 检测框跳过不画（LCD 渲染中
if g_boxes[i].cls == 1 || g_boxes[i].cls == 2: continue），无决策 - 电机开环 — 无编码器反馈，PID 类已实现但无调用点，
MotorController无updateSpeed()方法 - 舵机死区 — 用
abs(deviation) < deadband比较像素值，deadband 单位是显示空间像素 - 中线 row 范围 — 丢线补全仅覆盖 row 10
59，row 09 的 mid_line 保持 -1 不可靠 - mid_val == 255 防御 — 代码中写死但 255 永远不会出现在 mid_line 中（当前实现下）
- LCD 坐标缩放硬编码 — 检测框绘制用
newWidth/160.0f，假设 raw_frame 宽=160。回退模式下 raw_frame=640 时出错