Loongson_2k0300_SmartCar/docs/CONE_DESIGN.md

锥桶避障功能 — 中线变形方案

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） 对赛道上的锥桶响应极快——锥桶进入视野后 12 帧内车就开始转弯，锥桶随即离开视野。 模型检测的窗口天然只有 12 帧，多帧去抖门槛（如 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) 块中无条件执行：

// ★ 旧代码 (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 时更新冻结位置和回弹方向：

// ★ 修复后:
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 选择逻辑：

// ★ 旧代码:
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 回弹 → 正常