code-test-drone/docs/L1航向伺服实现方案.md

L1 航向伺服落地方案（无距离，树莓派 / Python）

目标：先把"只修正飞行方向、不需要距离"的部分落地，速度-距离闭环（L2）滞后。 基于结论：本吊舱无激光 → 表4 永不出，且不允许二次开发（部署文档 §10.0 / §10）。 日期：2026-10-09 起，2026-10-19 更新　状态： S0/S1/S2 + SITL + 真吊舱在环已验证（全量 621 passed）。 后续变更：第二判据 require_vision → LOS 运动学一致性（§2.3/§6）； 距离源由「双目」改为规划中的「激光雷达」（§7）。

---

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，2026-10-09）：云台刚性固连机体、朝前不动， frame_yaw_deg 恒 0（"框架角无用"），锁定后也不会自 servo 回中，故脱靶量 长期不≈0。此时 off_target_x_deg 恰好 = 目标相对机头的视线角 β，是唯一 有效信号。因此合成做成配置双路（beta_from_frame / beta_from_off_target）：

• 现在（无电机）：beta_from_frame=false、beta_from_off_target=true → β = off_target_x
• 将来（装电机）：两者皆 true → β = frame_yaw + off_target_x（即上文 §2.1）

"装不装电机"只是翻配置，架构（姿态来自 PX4、角度来自吊舱）不变。装电机后 §2.1 的"脱靶量长期≈0、必须靠框架角"警告才重新生效。

S1 标定结论（2026-10-09）：厂商确认本版电机无用；实测目标在左下时 off_target_x/y 均为负 → 吊舱 X 轴右正（与控制律 β 同向）故 beta_sign=+1； Y 轴上正（与控制律 α"下正"相反）故 alpha_sign=-1。已回填 pod_bridge.json 并加标定回归测试（test_deployed_config_has_calibrated_signs）守护。

2.2 如何接入 TrackingControlLaw（三选一，需拍板）

控制律第 ④ 步现在是硬编码从像素算角度，没有"直接喂角度"的入口：

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 有目标
现行（2026-10-19 起）	bit2 且 遥测未超龄 且 LOS 运动学一致性（pod_bridge/los_consistency.py）

⚠ 2026-10-19 架构变更：后续决定吊舱取代双目 + YOLO 作为唯一目标源， 因此原「我方视觉第二判据」(require_vision / vision_target_fn) 已删除 —— 它要么恒 False（链路永远进不了 TRACK），要么指向吊舱自身（自证，零信息量）。

替代判据：本吊舱无电机、相机刚性固连机体，机头转 Δψ 时真实目标的脱靶量必变 ≈−Δψ。故用 residual = Δβ + Δψ（3s 窗口，激励门槛 20°、残差上限 8°）判断 「角度是活的还是冻住的」。超限 → tracked=False 且 pod_locked() 也返回 False （状态机会退出 TRACK 重新搜索，而不是只让控制律单方面刹车）。

局限：它只能证明「角度是活的」，不能证明「是那个目标」。若吊舱锁错了物体 但角度正常变化，本判据无法识别 —— 那属于识别问题，需另接独立传感器。

---

3. 改动清单（文件级）

计划视图。实际落地状态与逐文件勾选见 §9（含计划外新增的 config.py:load_from_dict）。

#	文件	改动
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. 配置外置（要点，非最终字段名）

// utils/config/pod_bridge.json —— L1 段（已实现，字段名与 HeadingServoConfig 一致）
"heading_servo": {
  "enabled": true,             // true=走 L1(TrackingControlLaw)，false=L2(GuidanceLaw)
  "beta_sign": 1,              // 台架 S1 标定后填 ±1（符号错了会反向转）
  "alpha_sign": 1,
  "beta_from_frame": false,    // 无电机 → 框架角无用；装电机后置 true
  "beta_from_off_target": true,// 无电机 → 脱靶量即 β 主信号
  "alpha_from_frame": false,
  "alpha_from_off_target": true,
  "vertical_enabled": true,    // 决策3：无人机 L1 开垂直（vz 由 tracking.vertical 压低）

  // 第二判据：LOS 运动学一致性（2026-10-19 取代 require_vision，见 §2.3）
  "los_consistency_enabled": true,   // false → 退回"只信 bit2"
  "los_window_ms": 3000,             // 比较窗口
  "los_min_yaw_change_deg": 20.0,    // 窗口内机头至少转这么多才判定（激励门槛）
  "los_max_residual_deg": 8.0        // |Δβ+Δψ| 上限，超过判不一致
}

// tracking 段（内嵌整份 visual_track_control 配置，各分组须齐全；L1 首飞值）
"heading":  { "mode": "rate", "gain": 1.0, "yaw_rate_limit_dps": 20.0, "yaw_slew_limit_dps2": 40.0 },
"distance": { "no_distance_action": "slow_approach",       // 决策3：低速接近（非 hold）
              "no_distance_approach_limit_mps": 0.2 },     // 极低限速起步
"vertical": { "vz_gain_mps_per_deg": 0.05, "vz_limit_mps": 0.5 }  // 开垂直但压低

决策 3：首次上机 no_distance_action = "slow_approach"、限速 0.2 m/s（很低）， 而非原推荐的 hold。安全不受影响——S2 干跑不下发 offboard，符号先验证过再通电。 beta_sign/alpha_sign/增益均为占位，须 S1/S2 标定后回填，标定前不得实飞。

---

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 粘滞 → 丢失后仍转	LOS 运动学一致性（pod_bridge/los_consistency.py，2026-10-19 取代旧「我方视觉」方案）：机头转 Δψ 而脱靶量不动 → 判丢；台架期仍人工值守
框架角定义不符（绝对而非相对机体）	S1 实测确认，不符则改用脱靶量积分等替代
云台归中/搜索态框架角乱转	仅锁定态启用；非锁定 → tracked=False → BRAKE_HOVER
垂直通道撞地	L1 强制 vz_gain=0（本机实际按决策3 开垂直但压低限速）
测距噪声（L2 才有）	L2 滞后，且不用连续 PID，改分段限速

---

7. 与 L2 的接口预留

L1 落地时就留好口子，将来 L2 只是"换个输入源"，不动结构：

• ControlSnapshot.distance_valid / target_distance_m / distance_time_ms 字段 现在就按 False 填全；将来接激光雷达距离源后填入（原计划的 stereo_ranger.forward_distance_m 已随"吊舱取代双目"一并搁置）；
• bridge.step() 按配置在 TrackingControlLaw（L1）与 GuidanceLaw（L2）间切换；
• guidance.py 的输入接口保持不变，等有标定过的距离源再启用。

---

8. 已拍板（2026-10-09）

1. 接入路线 = A1：ControlSnapshot 加可选 los_beta_deg/los_alpha_deg，第④步角度直入优先、否则回退像素路径。已实现（types.py / tracking_control_law.py，补 5 条兼容性测试）。
2. 目标类型 = 无人机（空中）：影响 L2 几何反推法，L1 无关。
3. 首次上机 = slow_approach + 极低限速 0.2 m/s（非 hold）；L1 开垂直通道，但 vz 限速压低（vz_limit_mps=0.5、vz_gain=0.05）防撞。

补充决策（BridgeConfig 结构）：BridgeConfig 直接内嵌 TrackingConfig 字段 + heading_servo 段；运行时按 heading_servo.enabled 在 L1(TrackingControlLaw)/L2(GuidanceLaw) 间切换，L2 接口天然保留（对齐 §7）。姿态来自 PX4、β/α 来自吊舱（决策 2 见 §2.1 无电机说明）。

---

9. 实现状态（S0 已完成）

#	文件	状态
1	visual_track_control/types.py	✅ ControlSnapshot 加 los_beta_deg/los_alpha_deg: Optional[float]=None
2	visual_track_control/tracking_control_law.py	✅ 第④步角度直入优先，否则回退像素
3	visual_track_control/config.py	✅ 拆出 load_from_dict() 供 bridge 内嵌复用
4	pod_bridge/config.py	✅ 加 HeadingServoConfig + 内嵌 TrackingConfig，严格校验符号(±1)
5	pod_bridge/bridge.py	✅ _build_snapshot()；step() 按 enabled 切 L1/L2；_dispatch 对 DEGRADED_HOLD 补发刹车保心跳；vision_target_fn 注入点 → 已删除，改为 LOS 一致性判据（los_consistency.py，§2.3）
6	config/pod_bridge.json	✅ 落 L1 首飞值（slow_approach/0.2、垂直压低、无电机 → 仅脱靶量）
7	tests/	✅ 新增 24 条（vtc 5 + config 9 + bridge L1 10）；全量 396 passed
8	pod_bridge/run_bridge.py	✅ 闭环 CLI 入口：--dry-run(S2 只接吊舱/不发 offboard/打印 β→yaw_rate) 与实飞(S3+，PodBridge.connect+信号优雅停机)；配套 run_bridge_test.py 5 条，全量 401 passed
9	pod_bridge/bridge.py:run()	✅ 循环周期改按生效律取频率（L1=tracking.control，L2=guidance），修 L1 live 频率错配
10	pod_bridge/virtual_pod.py	✅ 模拟吊舱（SITL/台架工具）：按无电机语义合成表2-2（框架角恒0、无激光、不可见→bit2=0+脱靶量归0），符号与 S1 标定一致
11	l1_sitl_test.py（根目录，原 pod_sitl_test.py）	✅ L1 SITL 全流程：解锁→起飞→锚点→offboard→L1 闭环(VirtualPod→PodBridge→控制律)→丢目标→刹车→RTL→落地→上锁，轨迹落 CSV；飞行编排不塞进 run_bridge（保持其纯 offboard 闭环入口）
12	tests/pod_bridge/virtual_pod*_test.py	✅ 15 条：S1 符号回归 + VirtualPod×PodBridge×控制律集成（右转/下降/慢速前近/丢目标刹车）；全量 417 passed
13	pod_bridge/los_consistency.py（2026-10-19）	✅ 目标有效性第二判据：LOS 运动学一致性（Δβ+Δψ，3s 窗口/20° 激励/8° 残差）；pod_locked() 叠加该判据；配套 los_consistency_test.py 12 条 + 桥接层 3 条

待办（非代码，台架）：S1 标定 已完成（2026-10-09）：目标在左下时 off_target_x/y 均为负 → 吊舱 X 轴右正（与 β 同向）beta_sign=+1；Y 轴上正（与 α 下正相反） alpha_sign=-1（已回填 pod_bridge.json，并加标定回归测试守住）。厂商确认本版 电机无用 → beta_from_frame/alpha_from_frame 恒 false。

已完成的后续验证（2026-10-19）：S2 干跑（run_bridge.py --dry-run，真吊舱 COM5，20Hz 闭环无异常）、SITL 全流程（l1_sitl_test.py 与 task_runner_sitl.py 均实跑）、 真吊舱在环全流程（task_runner_real.py --port COM5，无目标时走"搜索超时→返航"安全路径）。 剩：heading.gain 标定、吊舱真实目标识别（bit2 0→1）、S3 不装桨接飞控。