spdis
284 lines
9.5 KiB
Markdown
284 lines
9.5 KiB
Markdown
# 锥桶避障功能 — 中线变形方案
|
||
|
||
## 1. 核心思路
|
||
|
||
**不改舵机代码，直接修改 `mid_line[]` 数组。** 在锥桶附近把中线"推开"一个凸起，后续巡线逻辑（deviation → deadband → servo）原样工作，车自然跟着变形后的中线绕开锥桶。
|
||
|
||
不使用任何推入计时器、归还逻辑、EMA 平滑——锥桶消失后，下一帧 `image_main()` 重新计算 `mid_line`，自动恢复。
|
||
|
||
---
|
||
|
||
## 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. 去抖机制
|
||
|
||
与之前一致，不改变：
|
||
|
||
```
|
||
#define CONE_MIN_FRAMES 3
|
||
#define CONE_COUNT_DECAY 1
|
||
|
||
g_cone_frames: 连续检测计数
|
||
g_cone_last_row / g_cone_last_col: 位置跟踪 (容差 ±5行/±10列)
|
||
g_cone_confirmed = (g_cone_frames >= CONE_MIN_FRAMES)
|
||
```
|
||
|
||
**关键差异：** 去抖只决定"是否触发变形"，变形本身没有持续时间概念。
|
||
- 锥桶在 → 变形在（每帧重新计算 mid_line 变形）
|
||
- 锥桶消失 → 下帧 image_main() 重新计算 mid_line → 变形自动消失
|
||
- 不去抖时的抖动：锥桶在边界附近闪烁会使 mid_line 来回跳，用去抖平滑
|
||
|
||
---
|
||
|
||
## 7. 舵机与电机
|
||
|
||
**舵机：** 零改动。Step 6 读取 `mid_line[foresee/2]`，看到变形后的中线值，自然输出偏转方向。
|
||
|
||
**电机：** 推入期间减速：
|
||
```
|
||
if (g_cone_confirmed && g_zstate != Z_STOP) {
|
||
effective_speed = target_speed * cone_speed_factor;
|
||
} else {
|
||
effective_speed = target_speed;
|
||
}
|
||
ControlUpdate(effective_speed, g_zstate == Z_STOP);
|
||
```
|
||
|
||
---
|
||
|
||
## 8. 参数配置
|
||
|
||
### 8.1 启动时一次性读取（运行中不重新读文件）
|
||
|
||
| 文件 | 类型 | 默认 | 含义 |
|
||
|---|---|---|---|
|
||
| `./cone_avoid_gain` | double | 0.3 | 归一化推离量 (0=不推, 1=推到赛道边缘) |
|
||
| `./cone_avoid_range` | int | 30 | 斜坡陡峭度 (越小越陡，foresee 行感受越强) |
|
||
|
||
### 8.2 热更新参数
|
||
|
||
| 文件 | 类型 | 默认 | 含义 |
|
||
|---|---|---|---|
|
||
| `./cone_speed` | double | 0.5 | 锥桶触发时速度倍率 |
|
||
| `./cone_min_frames` | int | 3 | 连续确认帧数 |
|
||
| `./cone_margin` | int | 0 | 0=中心点过滤, 1=框边缘过滤 |
|
||
| `./cone_thresh` | double | 0.80 | 锥桶置信度阈值 (业务层, 不改 NMS) |
|
||
|
||
### 8.3 读取实现
|
||
|
||
**main.cpp 启动时（一次性）：**
|
||
```cpp
|
||
double avoid_gain_val = readDoubleFromFile("./cone_avoid_gain");
|
||
int avoid_range_val = (int)readDoubleFromFile("./cone_avoid_range");
|
||
if (avoid_gain_val <= 0) avoid_gain_val = 0.3;
|
||
if (avoid_range_val <= 0) avoid_range_val = 30;
|
||
g_cfg.cone_avoid_gain = avoid_gain_val;
|
||
g_cfg.cone_avoid_range = avoid_range_val;
|
||
```
|
||
|
||
**global.cpp (cfg_load_all，热更新)：**
|
||
```cpp
|
||
g_cfg.cone_speed = readDoubleFromFile(cone_speed_file);
|
||
g_cfg.cone_min_frames = (int)readDoubleFromFile(cone_min_frames_file);
|
||
g_cfg.cone_margin = (int)readDoubleFromFile(cone_margin_file);
|
||
g_cfg.cone_thresh = readDoubleFromFile(cone_thresh_file);
|
||
```
|
||
|
||
**ctl.sh：**
|
||
```sh
|
||
echo 0.3 > "$DIR/cone_avoid_gain" 2>/dev/null
|
||
echo 30 > "$DIR/cone_avoid_range" 2>/dev/null
|
||
echo 0.5 > "$DIR/cone_speed" 2>/dev/null
|
||
echo 3 > "$DIR/cone_min_frames" 2>/dev/null
|
||
echo 0 > "$DIR/cone_margin" 2>/dev/null
|
||
echo 0.80 > "$DIR/cone_thresh" 2>/dev/null
|
||
```
|
||
|
||
---
|
||
|
||
## 9. 代码修改清单
|
||
|
||
```
|
||
lib/global.h:
|
||
├── CfgCache 新增: cone_avoid_gain, cone_avoid_range,
|
||
│ cone_speed, cone_min_frames, cone_margin, cone_thresh
|
||
└── 新增文件名常量
|
||
|
||
src/global.cpp:
|
||
└── cfg_load_all() 追加热更新参数 (不含 avoid_gain/avoid_range)
|
||
|
||
main/main.cpp:
|
||
└── CameraInit 之前: 读取 cone_avoid_gain, cone_avoid_range 写入 g_cfg
|
||
|
||
src/camera.cpp:
|
||
├── #define CONE_CLASS 0
|
||
├── 新增静态变量: g_cone_frames, g_cone_last_row/col, g_cone_confirmed
|
||
├── Step 5 之后: 插入锥桶检测+边界过滤+去抖
|
||
├── Step 5.5: 中线变形 (修改 mid_line[row] for row in [10, cone_row])
|
||
├── Step 7 电机: g_cone_confirmed → effective_speed *= cone_speed_factor
|
||
└── Step 8 LCD: 锥桶框改橙色
|
||
|
||
ctl.sh:
|
||
└── init_pins() 追加 6 个新文件默认值
|
||
```
|
||
|
||
---
|
||
|
||
## 10. 方案对比
|
||
|
||
| | 旧方案（舵机推入） | 新方案（中线变形） |
|
||
|---|---|---|
|
||
| 舵机代码 | 需改动 | 零改动 |
|
||
| deadband | 需绕过 | 自 然生效 |
|
||
| steer_gain | 需单独 cone_gain | 共用 |
|
||
| 归还机制 | 需要计时器+衰减 | 自动 (image_main 重算) |
|
||
| 平滑性 | 需 EMA | 斜坡自带平滑 |
|
||
| 参数数量 | 7 | 6 |
|
||
| 锥桶消失恢复 | 需衰减逻辑 | 下一帧自动 |
|