From b147925d5848de5bdac005975c5fbacd57629950 Mon Sep 17 00:00:00 2001
From: keita <16924591+keita_sakura@user.noreply.gitee.com>
Date: Fri, 9 Oct 2026 11:02:44 +0800
Subject: [PATCH] =?UTF-8?q?=E6=96=B0=E5=A2=9EL1=E8=88=AA=E5=90=91=E4=BC=BA?=
 =?UTF-8?q?=E6=9C=8D=E8=90=BD=E5=9C=B0=E6=96=B9=E6=A1=88(=E6=97=A0?=
 =?UTF-8?q?=E8=B7=9D=E7=A6=BB,=E4=BC=98=E5=85=88=E5=AE=9E=E7=8E=B0)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

核心结论: L1 不用新写控制律 —— TrackingControlLaw 第7步已内建无距离降级
(hold / slow_approach / exit), 复用其限幅/超龄/Coast/无距离/NED旋转(81测试保障)。

三个关键设计:
1. 航向信号用「框架角+脱靶量」而非只用脱靶量 —— 吊舱伺服会把目标保持居中,
   脱靶量稳态≈0 没信号; 框架角才是目标相对机头的方位。且角度输入与 FOV 无关,
   吊舱变焦不会失真(像素路径会)。
2. 接入路线 A1 推荐: ControlSnapshot 加可选 los_beta_deg/los_alpha_deg。
3. tracked 判据不能只信 bit2(实测不复位), 需第二判据。

含改动清单/配置外置/台架 S0-S5 步骤/风险表/L2 接口预留, 以及待拍板三项。
---
 docs/L1航向伺服实现方案.md | 192 +++++++++++++++++++++++++++++++++++++
 1 file changed, 192 insertions(+)
 create mode 100644 docs/L1航向伺服实现方案.md

diff --git a/docs/L1航向伺服实现方案.md b/docs/L1航向伺服实现方案.md
new file mode 100644
index 0000000..ec3edd1
--- /dev/null
+++ b/docs/L1航向伺服实现方案.md
@@ -0,0 +1,192 @@
+# L1 航向伺服落地方案（无距离，树莓派 / Python）
+
+> 目标：**先把"只修正飞行方向、不需要距离"的部分落地**，速度-距离闭环（L2）滞后。
+> 基于结论：本吊舱无激光 → 表4 永不出，且不允许二次开发（部署文档 §10.0 / §10）。
+> 日期：2026-10-09　状态：**方案待确认，未动代码**
+
+---
+
+## 0. 结论先行：L1 不用新写控制律
+
+`visual_track_control.TrackingControlLaw` **已经内建"无距离"路径**，第 ⑦ 步：
+
+```
+distance_available = snapshot.distance_valid and distance>0 and 未超龄
+   ├─ True  → 距离 PID → 前向速度（这是 L2，滞后做）
+   └─ False → 清积分，按 cfg.distance.no_distance_action 三选一：
+        HOLD          → vx = 0        （只转向，不前近）← 首次上机用这个
+        SLOW_APPROACH → vx = 固定保守限速  ← 航向验证通过后用这个
+        EXIT          → 持续无距离达 grace 才降级
+```
+
+所以 L1 = **`TrackingControlLaw` + `distance_valid=False` + 无距离动作配置**，
+航向/垂直通道、限幅、超龄、Coast 清积分、NED 旋转全部复用既有实现
+（该包有 81 个测试保障）。**要新写的只有"胶水层"**：把吊舱表2-2 的角度量喂进去。
+
+---
+
+## 1. 现有构件盘点（能复用什么）
+
+| 构件 | 位置 | L1 怎么用 |
+|---|---|---|
+| `TrackingControlLaw` | `visual_track_control/tracking_control_law.py` | 主控制律，直接用 |
+| `ControlSnapshot` / `TrackState` | `visual_track_control/types.py` | 输入结构，`distance_valid=False` |
+| `PixelToAngle` | `line_of_sight.py` | ❌ **绕过**（见 §2.1，吊舱给的已是角度） |
+| `PidController` / `SlewRateLimiter` | 同包 | 被控制律内部使用 |
+| `PodBridge` 主循环 | `pod_bridge/bridge.py` | 改：把 GuidanceLaw 换成 TrackingControlLaw |
+| `_pod_locked()` | `bridge.py:173` | 目标门控（⚠ 有坑，见 §2.3） |
+| 表2-2 角度量 | `protocol.py:150-160` | `off_target_x/y_deg`、`frame_yaw/pitch_deg` |
+
+---
+
+## 2. 三个关键设计决策
+
+### 2.1 🔴 航向信号要用「框架角 + 脱靶量」，不能只用脱靶量
+
+这是本方案**最容易踩空**的地方：
+
+> 吊舱锁定后**内部伺服会自己把目标保持在画面中心**，所以脱靶量长期 ≈ 0。
+> 若只用脱靶量驱动航向，**稳态时根本没有信号**，飞机会"不知道该往哪转"。
+
+正确的信号是**框架角**（`frame_yaw_deg`，云台相对机体的方位角）：目标居中时它
+依然非零，它就是"目标相对机头的方位"。合成：
+
+```
+β = frame_yaw_deg  +  off_target_x_deg      （水平，右正）
+α = frame_pitch_deg + off_target_y_deg      （垂直，下正）
+```
+
+- 目标居中时：`off_target ≈ 0`，β 由框架角提供（主信号）
+- 云台滞后 / 大偏差时：脱靶量提供动态补充
+
+控制律让 β → 0，等价于**把机体航向转向目标、让云台回中**（标准 gimbal-slaved
+航向控制）。这正是"用吊舱修正飞行方向"的正确做法。
+
+⚠ **必须在台架标定**：① 框架角是否确为"相对机体"（而非绝对方位）；
+② β/α 的正方向是否与控制律约定（右正 / 下正）一致。**符号错了会反向转**。
+
+**额外好处**：走角度输入就**与视场 FOV 无关**——吊舱会变焦，若走像素换算
+（`PixelToAngle` 反推），变焦后 FOV 变化会直接失真。这是不用像素路径的硬理由。
+
+### 2.2 如何接入 TrackingControlLaw（三选一，需拍板）
+
+控制律第 ④ 步现在是**硬编码从像素算角度**，没有"直接喂角度"的入口：
+
+```python
+x_offset = snapshot.track.center[0] - image_width * 0.5   # tracking_control_law.py:135
+beta_deg = self._pixel_to_beta.convert(x_offset) * RAD_TO_DEG
+```
+
+| 路线 | 做法 | 评价 |
+|---|---|---|
+| **A1（推荐）** | `ControlSnapshot` 加可选字段 `los_beta_deg` / `los_alpha_deg`（默认 `None`）；第 ④ 步：**提供了就直接用，否则走原像素路径** | 改动约 10 行，**向后兼容**、现有 81 测试不受影响，⑤~⑧ 步（限幅/超龄/Coast/无距离/NED 旋转）全部复用 |
+| A2 | 不动上游，在 `pod_bridge` 里写 `PodHeadingServo` 薄封装，复用 `PidController`/`SlewRateLimiter`/`is_stale` | 不碰稳定模块，但**要重寫 ⑤⑥⑦⑧ 的逻辑**，与既有实现双份维护，易走偏 |
+| B | 把角度反推成"等价像素偏移"喂进现有字段 | ❌ 不推荐：需要 FOV，**吊舱变焦即失真**，且多绕一层徒增误差 |
+
+**建议 A1**。代价是动一下 `visual_track_control`（需补 3~5 条测试守住"默认 None
+仍走像素路径"的兼容性）。
+
+### 2.3 ⚠ `tracked` 判据不能只信 bit2
+
+已知事实（部署文档 §10）：**锁定位置位 bit2 在目标丢失后不复位**，且脱靶量
+**没有无效标志位、丢失时表现为 0**。后果：
+
+- 只信 bit2 → 目标早已丢失，仍认为"锁定中"继续转；
+- 只看脱靶量 0 → 误判为"跟得很好"。
+
+L1 的 `tracked` 判据按阶段递进：
+
+| 阶段 | 判据 |
+|---|---|
+| 台架（S1~S2） | bit2 锁定 **且** 遥测未超龄（现有 `_pod_locked()`）+ **人工在旁观察**，随时按键 `u` 解锁 |
+| 接入视觉后（S3+） | bit2 **且** 我方 `stereo_yolo`/`target_tracker` 有目标（**第二判据**，必做） |
+
+---
+
+## 3. 改动清单（文件级）
+
+| # | 文件 | 改动 |
+|---|---|---|
+| 1 | `utils/visual_track_control/types.py` | `ControlSnapshot` 加 `los_beta_deg` / `los_alpha_deg: Optional[float] = None`（路线 A1） |
+| 2 | `utils/visual_track_control/tracking_control_law.py` | 第 ④ 步：角度直入优先，否则回退像素路径 |
+| 3 | `utils/pod_bridge/bridge.py` | 新增 `_build_snapshot()`：表2-2 → `ControlSnapshot`（β/α 按 §2.1，符号由配置）；`step()` 改走 `TrackingControlLaw`；`tracked` 用 §2.3 判据 |
+| 4 | `utils/pod_bridge/config.py` | 新增 L1 配置段（信号源、符号、门限、是否禁用垂直） |
+| 5 | `utils/config/pod_bridge.json` | 落配置值（铁律：数值不写死在代码里） |
+| 6 | `utils/tests/...` | 新增测试：角度直入路径、默认 None 兼容性、bridge L1 组装（FakePod） |
+
+**暂不动**：`guidance.py`（L2 地理引导）保留，按配置在 L1 / L2 间切换，
+等 L2 有标定过的距离源再启用。
+
+---
+
+## 4. 配置外置（要点，非最终字段名）
+
+```jsonc
+// utils/config/pod_bridge.json —— L1 段
+"heading_servo": {
+  "enabled": true,
+  "beta_sign": 1,            // 台架标定后填 ±1（框架角 + 脱靶量合成的符号）
+  "alpha_sign": 1,
+  "use_off_target": true,    // β 是否叠加脱靶量（false = 只用框架角）
+  "vertical_enabled": false, // L1 先禁垂直，避免撞地；vz=0
+  "require_vision": false    // true = 需我方视觉作为第二判据（S3 起开）
+}
+```
+
+```jsonc
+// visual_track_control 侧（沿用既有结构，只改值）
+"heading":  { "mode": "rate", "gain": ..., "yaw_rate_limit_dps": 20.0, "yaw_slew_limit_dps2": 40.0 },
+"distance": { "no_distance_action": "hold",          // 首次上机 = hold
+              "no_distance_approach_limit_mps": 0.3 },
+"vertical": { "vz_gain_mps_per_deg": 0.0 }          // L1 禁用
+```
+
+> 首次上机一律 `no_distance_action = "hold"`：**只转向、不前近**。
+> 航向验证通过后再切 `slow_approach` 并把限速压到 0.3 m/s 起步。
+
+---
+
+## 5. 台架落地步骤（增量，每步过了再下一步）
+
+| 步骤 | 做什么 | 通过判据 |
+|---|---|---|
+| **S0** 纯软单测 | 新增测试 + 全量回归 | 全绿（当前 372 → 增加） |
+| **S1** 标定 | `pod_console.py` 观察云台转动 / 移动目标时 `frame_yaw/frame_pitch`、`off_target_x/y` 的变化 | 确认框架角是"相对机体"；定出 `beta_sign` / `alpha_sign` |
+| **S2** 干跑 | `bridge` 以 dry-run 跑 L1，**不下发 offboard**，只打印 yaw_rate | 目标偏左 → yaw_rate 为负（左转），符号正确；目标居中 → 趋于 0 |
+| **S3** 不装桨接飞控 | 真实 offboard | 机体转向目标、云台逐步回中；人工可随时接管 |
+| **S4** 切 slow_approach | vx = 0.3 m/s 起步 | 缓慢接近不振荡；异常立即 RC 切手动 |
+| **S5** 装桨试飞 | — | 最后一步，先在空旷场地 |
+
+⚠ S3 起必须：**不装桨 / 空旷场地 / RC 随时可切手动 / 有人值守**。
+
+---
+
+## 6. 风险与判据
+
+| 风险 | 判据 / 对策 |
+|---|---|
+| **符号反了** → 反向转 | S1 标定 + S2 干跑必验；配置化而非硬编码 |
+| bit2 粘滞 → 丢失后仍转 | 第二判据（我方视觉）；台架期人工值守 |
+| 框架角定义不符（绝对而非相对机体） | S1 实测确认，不符则改用脱靶量积分等替代 |
+| 云台归中/搜索态框架角乱转 | 仅锁定态启用；非锁定 → `tracked=False` → BRAKE_HOVER |
+| 垂直通道撞地 | L1 强制 `vz_gain=0` |
+| 测距噪声（L2 才有） | L2 滞后，且**不用连续 PID**，改分段限速 |
+
+---
+
+## 7. 与 L2 的接口预留
+
+L1 落地时就留好口子，将来 L2 只是"换个输入源"，不动结构：
+
+- `ControlSnapshot.distance_valid` / `target_distance_m` / `distance_time_ms` 字段
+  **现在就按 `False` 填全**，L2 时换成 `stereo_ranger` 的 `forward_distance_m`；
+- `bridge.step()` 按配置在 `TrackingControlLaw`（L1）与 `GuidanceLaw`（L2）间切换；
+- `guidance.py` 的输入接口保持不变，等有标定过的距离源再启用。
+
+---
+
+## 8. 待你拍板
+
+1. **接入路线**：A1（改 `ControlSnapshot` 加可选角度字段，推荐）/ A2（pod_bridge 侧薄封装）/ B（像素反推，不推荐）；
+2. **目标类型**：人车（地面）/ 无人机（空中）—— 影响 L2 的几何反推法能否用，L1 不受影响；
+3. **首次上机无距离动作**：`hold`（只转不前，我建议）/ `slow_approach`（0.3 m/s）。