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
76 changes: 76 additions & 0 deletions docs/18.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,76 @@
# Frenet 规划器:用 `KD ∈ [0,1]` 控制“从最左到最右”偏移

你现在的实现里,`KD` 被当作「`offset cost` 的权重」来用:

- `KD` 越大,只会让轨迹更贴近中心线(`l=0`),
- 不能直接表达“期望偏左/偏右”的连续控制。

如果你希望 `KD=0` 表示最左、`KD=1` 表示最右,建议把 `KD` 映射为**目标横向位置**,然后最小化“轨迹终点与目标横向位置的误差”。

---

## 核心改法

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 -> -MAX_ROAD_WIDTH (最左)
# KD=0.5 -> 0 (中间)
# KD=1 -> +MAX_ROAD_WIDTH (最右)
```

2. 用新的偏移误差替换你当前的 `abs(tfp.l[-1])`:

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

3. 最终代价使用 `norm_bias_cost`(而不是原来的 `norm_offset_cost`)。

---

## 可直接替换的关键片段

```python
# =====================================
# final weighted cost
# =====================================
KJ = max(0.0, min(1.0, planner_param[0]))
KD = max(0.0, min(1.0, planner_param[1])) # 这里 KD 改为“左右偏移控制旋钮”

# KD: [0,1] -> 目标横向位置 [-MAX_ROAD_WIDTH, +MAX_ROAD_WIDTH]
l_ref = -MAX_ROAD_WIDTH + 2.0 * MAX_ROAD_WIDTH * KD

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

# 这里给一个稳定的默认权重分配:
# - jerk 权重由 KJ 控制
# - 偏移跟踪固定占一部分(KB),确保 KD 变化有可见效果
# - 剩余给效率项
KB = 0.35
if KJ + KB > 1.0:
KJ = 1.0 - KB
KT = 1.0 - KJ - KB

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

---

## 使用建议

- 先固定 `KJ`(例如 `0.2`),只扫 `KD`:`0.0 -> 0.25 -> 0.5 -> 0.75 -> 1.0`。
- 如果“左右偏移不明显”,增大 `KB`(例如从 `0.35` 到 `0.5`)。
- 如果轨迹变抖,适当增大 `KJ` 或提高 `Ti`。

这样调参时,`KD` 的语义会非常直观:

- **0 = 最左**
- **0.5 = 居中**
- **1 = 最右**