# 锥桶避障功能 — 中线变形方案 ## 1. 核心思路 **不改舵机代码，直接修改 `mid_line[]` 数组。** 在锥桶附近把中线"推开"一个凸起，后续巡线逻辑（deviation → deadband → servo）原样工作，车自然跟着变形后的中线绕开锥桶。 锥桶消失后进入**保持衰减**阶段（`cone_hold_frames` 帧内变形量线性衰减），随后进入**回弹**阶段（`cone_return_frames` 帧内朝锥桶方向反推，帮助车回到赛道中央）。 ### 1.1 原始中线快照 (`mid_line_raw`) 避障变形会原地修改 `mid_line[]`，导致方向判断被污染。为解决此问题，在 `image_main()` 之后立即保存一份快照： ``` image_main() → mid_line[] = 赛道真实中线 mid_line_raw = mid_line → 快照 (60 个 int, 240 字节, 每帧复制) lidar_avoid_process() → 修改 mid_line[] (lidar 推力) cone_detect_and_deform() → 修改 mid_line[] (锥桶推力) steering_update() → 读 mid_line[] (变形后) 控制舵机 ``` | 变量 | 含义 | 来源 | 被修改 | 用途 | |------|------|------|--------|------| | `mid_line_raw[]` | 赛道在哪 | `image_main()` 每帧重算 | 否 | 方向判断 | | `mid_line[]` | 车该往哪走 | `mid_line_raw` + 避障推力 | 是 | 舵机控制 | **所有方向判断（锥桶在赛道左侧还是右侧、回弹方向）均基于 `mid_line_raw`**， 避免变形后的中线导致方向翻转。 --- ## 2. 坐标与边界过滤 ``` 模型空间 (raw_frame): 正常模式: 160×120 (MJPEG 1/4解码) 回退模式: 640×480 巡线空间映射: cone_col = box.cx * line_tracking_width / raw_frame.cols (典型 0~79) cone_row = box.cy * line_tracking_height / raw_frame.rows (典型 0~59) 换算速查 (正常模式 160×120 → 典型 80×60): cone_col = cx / 2 cone_row = cy / 2 ``` 边界过滤 (cls=0, conf ≥ cone_thresh): ``` row_lt < 10 → 丢弃 (row 0~9 无可靠巡线数据) row_lt >= line_tracking_height → 丢弃 left_line[row_lt] == -1 → 丢弃 (该行丢线) right_line[row_lt] == -1 → 丢弃 col_lt < left_line[row_lt] → 丢弃 (赛道外) col_lt > right_line[row_lt] → 丢弃 (赛道外) ``` --- ## 3. 中线变形 ### 3.1 方向 ``` mid_at_cone = mid_line[cone_row] cone_col < mid_at_cone → 锥在左 → push_mid 向右 (+) cone_col > mid_at_cone → 锥在右 → push_mid 向左 (-) direction = (cone_col < mid_at_cone) ? +1 : -1 ``` ### 3.2 变形区域 从 `start_row = max(10, cone_row - range)` 到 `cone_row`，共 (cone_row - start_row) 行： ``` row 0 ───────────────────────────────── 远 │ (不变形) row 10 ───────────────────────────────── │ start_row ────────────── t=0 │ 线性斜坡 │ foresee 行在这里看到部分偏移 │ │ cone_row ────────────── t=1 row 59 ───────────────────────────────── 近 (车前方) ``` ### 3.3 数学 ``` for row in [start_row, cone_row]: t = (row - start_row) / (cone_row - start_row) // [0, 1] push = t * cone_avoid_gain * (line_tracking_width / 2) * direction mid_line[row] = original_mid_line[row] + push mid_line[row] = clamp(mid_line[row], left_line[row] + 2, right_line[row] - 2) ``` **参数：** - `cone_avoid_gain` (double, 0.3)：归一化推离量。1.0 = 推到半幅赛道宽。默认 0.3 表示推到赛道 30% 宽度处。 - `cone_avoid_range` (int, 30)：斜坡覆盖行数。必须足够大使 foresee 行感受到偏移。 --- ## 4. 数字算例 **场景：** 直道，锥桶在赛道左侧。`lt_w=80, lt_h=60, cone_avoid_gain=0.3, cone_avoid_range=30` ``` 原始数据: cone_col=25, cone_row=50, mid_at_cone=40 (锥在左侧15列) direction = +1 (推中线向右) start_row = max(10, 50-30) = 20 half_w = 40 变形计算: push_max = 0.3 * 40 * 1 = 12 (行50处的最大位移, 巡线空间列) row=20 (start, t=0): mid_line = 40 + 0 = 40 → deviation = 40*2-80 = 0 → 舵机中位 row=25 (t=5/30=0.17): mid_line = 40 + 2.04 = 42 → deviation = 42*2-80 = +4 → 略右 row=35 (t=15/30=0.5): mid_line = 40 + 6.0 = 46 → deviation = 46*2-80 = +12 → 明显右转 row=50 (t=1.0): mid_line = 40 + 12 = 52 → deviation = 52*2-80 = +24 → 大幅右转 foresee=40 (显示空间) → check_row=40/2=20 (巡线空间): mid_line[20] 未被变形 (start_row=20, t=0) → 舵机看不到! 需要减小 range 或增大 foresee ``` **问题暴露：** `foresee=40` → `check_row=20`，但 `start_row=20` 时 t=0 无偏移。 **修正：** 确保变形起点在 `check_row` 以上。两种方式： 1. 增大 `cone_avoid_range`（如 40），让 start_row 更低 2. 让斜坡向上延伸超过 start_row（即从 row=10 就开始） **采用方案 2：** 变形始终从 `row=10` 开始，`range` 参数控制斜坡的"陡峭度"而非范围。 --- ## 5. 修正后的变形公式 ``` range = cone_avoid_range // 陡峭度参数 (默认 30) total_rows = cone_row - 10 // 实际跨度 for row in [10, cone_row]: t_raw = (row - 10.0) / total_rows // 线性 [0, 1] t = clamp(t_raw * (total_rows / range), // 用 range 缩放斜率 0.0, 1.0) push = t * cone_avoid_gain * (line_tracking_width / 2) * direction mid_line[row] = clamp(original_mid + push, left_line[row] + 2, right_line[row] - 2) ``` **效果（cone_avoid_range=30, cone_row=50, total_rows=40）：** ``` total_rows/range = 40/30 = 1.33 ← 斜率因子 >1, 斜坡更陡 row=10: t_raw=0, t=0*1.33=0 → push=0 row=20: t_raw=0.25, t=0.25*1.33=0.33 → push=0.33*12=4.0 → deviation=+8 (刚好过死区) row=35: t_raw=0.625,t=0.83 → push=10 → deviation=+20 row=50: t_raw=1.0, t=1.0 → push=12 → deviation=+24 ``` foresee 行 (row=20) 收到 33% 的偏移，产生 +8px 偏差 → 刚好越过 deadband(8px) → 舵机开始右转。 **调参规则：** - 增大 `cone_avoid_range` → 斜坡更缓 → foresee 行偏移更小 → 转向更柔和 - 减小 `cone_avoid_range` → 斜坡更陡 → foresee 行偏移更大 → 转向更猛 - 增大 `cone_avoid_gain` → 整体偏移量增大 → 转向更猛 --- ## 6. 去抖机制 ``` g_cone_frames: 连续检测计数 位置容忍 (动态): row_tol = max(2, line_tracking_height / 12) 典型值 5 col_tol = max(3, line_tracking_width / 8) 典型值 10 首次出现: g_cone_frames = 1, 记录位置 位置接近 (容差内): g_cone_frames++, 更新位置 位置突变: g_cone_frames = 1, 更新位置 (重新开始计数) 未检测到: g_cone_frames = max(0, g_cone_frames - 1) g_cone_confirmed = (g_cone_frames >= cone_min_frames) ``` **cone_min_frames 默认值为 1（单帧即确认）。** 原因：视觉巡线（`image_main()` 的 floodFill） 对赛道上的锥桶响应极快——锥桶进入视野后 1~2 帧内车就开始转弯，锥桶随即离开视野。 模型检测的窗口天然只有 1~2 帧，多帧去抖门槛（如 3）永远达不到。 边界过滤（row 有效、赛道内、conf ≥ cone_thresh）已经提供了足够的误触发保护。 --- ## 7. 保持衰减 + 回弹 锥桶消失后不会立刻恢复正常巡线，而是经历三个阶段： ``` 确认 (g_cone_confirmed && cone_seen) → g_cone_hold_ctr = 1, g_cone_return_ctr = 0 → 冻结位置: hold_src_row/col = cone_row/col → 记录回弹方向: 锥桶在中线左侧 → return_dir = -1 (回弹推左) 锥桶在中线右侧 → return_dir = +1 (回弹推右) 确认但不可见 (g_cone_confirmed && !cone_seen, 去抖衰减中) → g_cone_hold_ctr = 1 (保持重置, 但不覆盖冻结位置) → 使用 hold_src_row/col 继续避让变形 保持衰减 (!g_cone_confirmed, 1 ~ cone_hold_frames 帧) → g_cone_hold_ctr++ → decay = 1.0 - hold_ctr / hold_frames (线性衰减到 0) → 中线变形量乘 decay → 逐渐恢复 回弹 (保持结束后 1 ~ cone_return_frames 帧) → g_cone_return_ctr++ → 反方向推中线 (return_dir), 力度 = cone_return_gain → 斜坡范围 = cone_avoid_range × 1.2 (比避让略缓) → 覆盖全部有效行 (row 10 ~ lt_h-1), 不限于锥桶位置 → decay = 1.0 - return_ctr / return_frames (线性衰减) → 回弹结束: 清零所有状态变量 ``` **cone_is_slow() 在三个阶段中均返回 true**（确认 / 保持 / 回弹），但锥桶减速代码当前**已注释**。 ### 7.1 已修复的 BUG：hold_src_row 被 -1 覆盖导致回弹永远不执行 **问题**：锥桶消失后，由于去抖计数 `g_cone_frames` 逐帧 -1 衰减，`g_cone_confirmed` 在锥桶消失后仍保持 true 多帧（如锥桶可见 20 帧 → 消失后 confirmed 维持 18 帧）。 在这些帧中 `cone_detect_and_deform()` 内的局部变量 `cone_row = -1`（没找到锥桶）， 但旧代码在 `if (g_cone_confirmed)` 块中无条件执行： ```cpp // ★ 旧代码 (BUG): if (g_cone_confirmed) { g_cone_hold_ctr = 1; g_cone_hold_src_row = cone_row; // cone_row = -1, 覆盖有效值! g_cone_hold_src_col = cone_col; // 同上 g_cone_return_ctr = 0; double ms = mid_line[cone_row]; // mid_line[-1] 越界访问! g_cone_return_dir = (cone_col < ms) ? -1.0 : 1.0; } ``` 后果链： 1. `hold_src_row` 被 -1 覆盖 → 有效位置丢失 2. `mid_line[-1]` 越界访问 → 未定义行为 3. 后续所有阶段 `src_row = hold_src_row = -1` → `if (src_row <= 10) return` 提前退出 4. 保持变形 **从不执行**，回弹变形 **从不执行** ``` 时间轴: 锥桶可见 20帧 去抖衰减 18帧 hold 15帧 return 30帧 避让变形: ████████████████████ (src_row=-1,退出) (同上,-1) (同上,-1) 保持变形: 从不执行 从不执行 回弹变形: 从不执行 ``` **修复**：只在 `cone_seen` 为 true 时更新冻结位置和回弹方向： ```cpp // ★ 修复后: if (g_cone_confirmed) { g_cone_hold_ctr = 1; g_cone_return_ctr = 0; if (cone_seen) { // 只在锥桶实际可见时更新 g_cone_hold_src_row = cone_row; g_cone_hold_src_col = cone_col; double ms = mid_line[cone_row]; g_cone_return_dir = (cone_col < ms) ? -1.0 : 1.0; } } ``` 同时修复 `src_row` 选择逻辑： ```cpp // ★ 旧代码: int src_row = g_cone_confirmed ? cone_row : g_cone_hold_src_row; // 当 confirmed=true 但 cone_seen=false 时 → src_row = cone_row = -1 → 提前退出 // ★ 修复后: int src_row = (g_cone_confirmed && cone_seen) ? cone_row : g_cone_hold_src_row; // confirmed 但不可见时 → 使用冻结位置, 避让变形继续生效 ``` 修复后的时间轴： ``` 时间轴: 锥桶可见 20帧 去抖衰减 18帧 hold 15帧 return 30帧 避让变形: ████████████████████ ████████████████████ ▓▓▓▓▓▓▓▓▓▓▓▓▓▓ 保持变形: (用冻结位置继续避让) (线性衰减→0) 回弹变形: ░░░░░░░░░░░░░░ ``` ## 8. 舵机与电机 **舵机：** 零改动。steering_update 读取 `mid_line[foresee/2]`，看到变形后的中线值，自然输出偏转方向。 **电机：** `motor_update()` 中存在锥桶减速代码（`cone_is_slow()` → `speed × cone_speed`）， 但当前**已注释**，仅弯道减速生效。如需启用，取消 `motor_update()` 中对应行的注释即可。 --- ## 9. 参数配置 所有锥桶参数均通过 `cfg_load_all()` 统一读取（`src/global.cpp`），debug 模式下每 30 帧热更新。 | 文件 | 类型 | 默认 | 含义 | |---|---|---|---| | `./cone_avoid_gain` | double | 0.3 | 归一化推离量 (0=不推, 1=推到赛道边缘) | | `./cone_avoid_range` | int | 30 | 斜坡陡峭度 (越小越陡，foresee 行感受越强) | | `./cone_speed` | double | 0.5 | 锥桶触发时速度倍率 (当前减速代码已注释) | | `./cone_min_frames` | int | 1 | 确认帧数 (1=单帧即确认) | | `./cone_margin` | int | 0 | 0=中心点过滤, 1=框边缘过滤 | | `./cone_thresh` | double | 0.80 | 锥桶置信度阈值 (业务层, 不改 NMS) | | `./cone_hold_frames` | int | 15 | 消失后保持变形帧数 | | `./cone_return_gain` | double | 1.0 | 回弹力度 (1.0=等同避让力度) | | `./cone_return_frames` | int | 30 | 回弹持续帧数 | --- ## 10. 代码位置 ``` lib/image_cv.h: └── extern mid_line_raw (原始中线快照声明) src/image_cv.cpp: └── std::vector mid_line_raw (定义) lib/global.h: └── CfgCache 包含: cone_avoid_gain, cone_avoid_range, cone_speed, cone_min_frames, cone_margin, cone_thresh, cone_hold_frames, cone_return_gain, cone_return_frames src/global.cpp: └── cfg_load_all() 统一读取所有锥桶参数 src/camera.cpp: ├── CameraHandler(): image_main() 后执行 mid_line_raw = mid_line (快照) ├── 静态变量: g_cone_frames, g_cone_last_row/col, g_cone_confirmed, │ g_cone_hold_ctr, g_cone_hold_src_row/col, │ g_cone_return_ctr, g_cone_return_dir ├── cone_is_slow(): 判断是否处于避让/保持/回弹任一阶段 ├── cone_detect_and_deform(): 方向判断基于 mid_line_raw, 变形修改 mid_line ├── motor_update(): cone_speed 减速 (已注释) └── lcd_render(): 锥桶框橙色绘制 ctl.sh: └── init_pins() 写入 cone_avoid_gain/range/speed/min_frames/margin/thresh/hold_frames ``` --- ## 11. 方案对比 | | 旧方案（舵机推入） | 当前方案（中线变形） | |---|---|---| | 舵机代码 | 需改动 | 零改动 | | deadband | 需绕过 | 自然生效 | | steer_gain | 需单独 cone_gain | 共用 | | 归还机制 | 需计时器+衰减 | hold 衰减 + return 回弹 | | 平滑性 | 需 EMA | 斜坡自带平滑 | | 参数数量 | 7 | 9 (含 hold/return) | | 锥桶消失恢复 | 需手动衰减 | 自动: hold 衰减 → return 回弹 → 正常 |