Loongson_2k0300_SmartCar/docs/CONE_DESIGN.md

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

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 启动时（一次性）：

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，热更新)：

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：

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
锥桶消失恢复	需衰减逻辑	下一帧自动