Skip to content
Open
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
83 changes: 83 additions & 0 deletions docs/18.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,83 @@
# Frenet 规划器:用 `KD ∈ [0,1]` 控制“从最左到最右”偏移(不新增参数版)

你提的问题非常关键:

- 如果新增 `KB`,会让调参维度变多;
- 你原本只有 `KJ/KD/KT` 三个权重,最好保持不变。

所以这里给出**不新增 `KB`** 的做法:

- `KD` 继续保留在 `[0,1]`;
- 但它不再表示“偏移代价权重”,而是表示“横向目标位置(左->右)”;
- 代价权重仍然只用 `KJ/KD/KT` 这三个参数,形式不变。

---

## `KD` 和 `KB` 的关系

在这个版本里:

- **没有 `KB`**;
- **`KD` 既是你的第二个调节参数,也是偏移项的权重参数**;
- 区别是偏移项从“到中心线的距离”改为“到目标横向位置的距离”。

也就是:

- 旧偏移项:`abs(tfp.l[-1])`(永远偏向中心)
- 新偏移项:`abs(tfp.l[-1] - l_ref)`(可偏左/偏右)

---

## 核心改法

1) 把 `KD` 映射到目标横向位置:

```python
KD = np.clip(planner_param[1], 0.0, 1.0)
l_ref = -MAX_ROAD_WIDTH + 2.0 * MAX_ROAD_WIDTH * KD
# KD=0 -> 最左
# KD=0.5 -> 居中
# KD=1 -> 最右
```

2) 用目标偏差替换原偏移误差:

```python
raw_bias_error = abs(tfp.l[-1] - l_ref)
norm_bias_cost = raw_bias_error / (2.0 * MAX_ROAD_WIDTH)
```

3) 最终加权保持三项结构不变(无 `KB`):

```python
KJ = max(0.0, min(1.0, planner_param[0]))
KD = max(0.0, min(1.0, planner_param[1]))
sum_w = KJ + KD
if sum_w > 1.0:
KJ /= sum_w
KD /= sum_w
KT = 1.0 - KJ - KD

tfp.cd = norm_jerk_cost
tfp.cv = norm_speed_cost + norm_acc_cost + norm_progress_cost
tfp.cf = KJ * tfp.cd + KD * norm_bias_cost + KT * tfp.cv
```

---

## 关键说明(避免语义混淆)

因为你希望只保留一个 `KD`,那么 `KD` 实际上承担了两层作用:

1. 决定 `l_ref` 在左-中-右哪个位置;
2. 决定“跟踪这个 `l_ref`”在总代价里的权重。

这在工程上是可用的、也最省参数;如果后续你想把“方向选择”和“跟踪强度”完全解耦,再单独引入第四个参数即可。

---

## 调参建议

- 先固定 `KJ`(如 `0.2`),扫 `KD=0/0.25/0.5/0.75/1.0` 看左右偏置是否符合预期;
- 若偏置不明显,可降低 `KT`(或提高 `KD`);
- 若轨迹抖动,增大 `KJ` 或拉长 `Ti`。