模型推理移至管道末尾降舵机延迟60→16ms + 视觉Mat复用 + 端到端延迟计时 + OpenCV 4.13 + WASD遥控
This commit is contained in:
spdis
+170
-80
@@ -4,7 +4,27 @@
|
||||
|
||||
**不改舵机代码，直接修改 `mid_line[]` 数组。** 在锥桶附近把中线"推开"一个凸起，后续巡线逻辑（deviation → deadband → servo）原样工作，车自然跟着变形后的中线绕开锥桶。
|
||||
|
||||
不使用任何推入计时器、归还逻辑、EMA 平滑——锥桶消失后，下一帧 `image_main()` 重新计算 `mid_line`，自动恢复。
|
||||
锥桶消失后进入**保持衰减**阶段（`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`**，
|
||||
避免变形后的中线导致方向翻转。
|
||||
|
||||
---
|
||||
|
||||
@@ -158,126 +178,196 @@ foresee 行 (row=20) 收到 33% 的偏移，产生 +8px 偏差 → 刚好越过
|
||||
|
||||
## 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)
|
||||
|
||||
位置容忍 (动态):
|
||||
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)
|
||||
```
|
||||
|
||||
**关键差异：** 去抖只决定"是否触发变形"，变形本身没有持续时间概念。
|
||||
- 锥桶在 → 变形在（每帧重新计算 mid_line 变形）
|
||||
- 锥桶消失 → 下帧 image_main() 重新计算 mid_line → 变形自动消失
|
||||
- 不去抖时的抖动：锥桶在边界附近闪烁会使 mid_line 来回跳，用去抖平滑
|
||||
**cone_min_frames 默认值为 1（单帧即确认）。** 原因：视觉巡线（`image_main()` 的 floodFill）
|
||||
对赛道上的锥桶响应极快——锥桶进入视野后 1~2 帧内车就开始转弯，锥桶随即离开视野。
|
||||
模型检测的窗口天然只有 1~2 帧，多帧去抖门槛（如 3）永远达不到。
|
||||
边界过滤（row 有效、赛道内、conf ≥ cone_thresh）已经提供了足够的误触发保护。
|
||||
|
||||
---
|
||||
|
||||
## 7. 舵机与电机
|
||||
## 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;
|
||||
确认 (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;
|
||||
}
|
||||
ControlUpdate(effective_speed, g_zstate == Z_STOP);
|
||||
```
|
||||
|
||||
后果链：
|
||||
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()` 中对应行的注释即可。
|
||||
|
||||
---
|
||||
|
||||
## 8. 参数配置
|
||||
## 9. 参数配置
|
||||
|
||||
### 8.1 启动时一次性读取（运行中不重新读文件）
|
||||
所有锥桶参数均通过 `cfg_load_all()` 统一读取（`src/global.cpp`），debug 模式下每 30 帧热更新。
|
||||
|
||||
| 文件 | 类型 | 默认 | 含义 |
|
||||
|---|---|---|---|
|
||||
| `./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_speed` | double | 0.5 | 锥桶触发时速度倍率 (当前减速代码已注释) |
|
||||
| `./cone_min_frames` | int | 1 | 确认帧数 (1=单帧即确认) |
|
||||
| `./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
|
||||
```
|
||||
| `./cone_hold_frames` | int | 15 | 消失后保持变形帧数 |
|
||||
| `./cone_return_gain` | double | 1.0 | 回弹力度 (1.0=等同避让力度) |
|
||||
| `./cone_return_frames` | int | 30 | 回弹持续帧数 |
|
||||
|
||||
---
|
||||
|
||||
## 9. 代码修改清单
|
||||
## 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
|
||||
└── 新增文件名常量
|
||||
└── 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() 追加热更新参数 (不含 avoid_gain/avoid_range)
|
||||
|
||||
main/main.cpp:
|
||||
└── CameraInit 之前: 读取 cone_avoid_gain, cone_avoid_range 写入 g_cfg
|
||||
└── cfg_load_all() 统一读取所有锥桶参数
|
||||
|
||||
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: 锥桶框改橙色
|
||||
├── 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() 追加 6 个新文件默认值
|
||||
└── init_pins() 写入 cone_avoid_gain/range/speed/min_frames/margin/thresh/hold_frames
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 10. 方案对比
|
||||
## 11. 方案对比
|
||||
|
||||
| | 旧方案（舵机推入） | 新方案（中线变形） |
|
||||
| | 旧方案（舵机推入） | 当前方案（中线变形） |
|
||||
|---|---|---|
|
||||
| 舵机代码 | 需改动 | 零改动 |
|
||||
| deadband | 需绕过 | 自 然生效 |
|
||||
| deadband | 需绕过 | 自然生效 |
|
||||
| steer_gain | 需单独 cone_gain | 共用 |
|
||||
| 归还机制 | 需要计时器+衰减 | 自动 (image_main 重算) |
|
||||
| 归还机制 | 需计时器+衰减 | hold 衰减 + return 回弹 |
|
||||
| 平滑性 | 需 EMA | 斜坡自带平滑 |
|
||||
| 参数数量 | 7 | 6 |
|
||||
| 锥桶消失恢复 | 需衰减逻辑 | 下一帧自动 |
|
||||
| 参数数量 | 7 | 9 (含 hold/return) |
|
||||
| 锥桶消失恢复 | 需手动衰减 | 自动: hold 衰减 → return 回弹 → 正常 |
|
||||
|
||||
Reference in New Issue
Block a user