spdis
374 lines
14 KiB
Markdown
374 lines
14 KiB
Markdown
# 锥桶避障功能 — 中线变形方案
|
||
|
||
## 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<int> 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 回弹 → 正常 |
|