优化文件

docs/物理追踪思路.md

@@ -80,8 +80,8 @@
   - `ControlIntentType::kManualOverride`（禁止自动输出/人工接管意图）：
     遥控器接管时停止任务控制输出。
 - `Px4Setpoint` 使用 `SetpointType::kVelocity`（速度设定值，分量写入
-  `x`、`y`、`z`）或 `SetpointType::kAttitude`（姿态微调，仅限垂直小误差
+  `x`、`y`、`z`；垂直通道不再使用 `SetpointType::kAttitude`，理由见
-  支路，见第 5 章）；航向写入 `yaw_deg` 或 `yaw_rate_dps`；
+  5.2 设计修订）；航向写入 `yaw_deg` 或 `yaw_rate_dps`；
   无效设定值（如刹停完成）也需发送，避免过期指令（`valid` 标志语义见
   `include/common/types.h`）。
 
@@ -199,19 +199,33 @@
    （等价于早期草稿中的 `Δh = d × cos(90° − α)`；此处 α 为垂直视线角，
    与水平视场角 θ_h、垂直视场角 θ_v 明确区分命名。）
 
-雷达未关联时，垂直通道只能使用角度误差做姿态/速率级控制，
+雷达未关联时，垂直通道仍由角度误差驱动（统一速度支路，见 5.2），
-不得假设已知高度差。
+只是缺少高度差校核，不得假设已知高度差。
 
-### 5.2 控制策略
+### 5.2 控制策略（设计修订：双支路已合并为统一速度支路）
 
-垂直控制按角度误差大小分两支（沿用原草稿思路，阈值待定）：
+**现行设计**：垂直控制采用统一速度支路——垂直视线角 α 直接映射为
+垂直速度设定值，全程保持 `ControlIntentType::kVelocityHeading` 的
+`SetpointType::kVelocity` `z` 分量输出，无误差分支切换：
 
-- **角度误差较小**：直接微调姿态角（`SetpointType::kAttitude`），
+```
-  仅作辅助，必须有严格姿态角与姿态变化率限幅（需求分析.md FR-033）；
+vz = clamp( slew( k_vz * α ), -vz_limit, +vz_limit )
-- **角度误差较大**：调整高度，使用速度设定值（`SetpointType::kVelocity`
+```
-  的 `z` 分量，局部 NED 下为垂直速度）趋近目标高度。
 
-角度误差切换阈值、姿态角限幅、垂直速度限幅均**待定**（第 8 章）。
+其中 `k_vz` 为角度-速度增益，`slew` 为垂直加速度限幅（第 8 章），
+`clamp` 为垂直速度限幅。小角度误差自然映射为小垂直速度，大小误差
+表现为连续的增益、饱和与限速，不产生模式跳变。
+
+**设计修订记录**：原草稿按角度误差大小分两支——小误差微调姿态角
+（`SetpointType::kAttitude`，姿态限幅要求见需求分析.md FR-033），
+大误差用垂直速度调整高度，两支按角度误差切换阈值切换。参考实现
+`reference-c++/visual_track_control`（`tracking_control_law.cpp`
+垂直速度通道）已将两支**合并**为上述统一速度支路，Python 移植
+（`utils/visual_track_control`）沿用同一实现。合并原因：输出切换为
+姿态设定值时，同一周期的距离通道前向速度会随输出模式切换被丢弃，
+导致水平逼近停滞；统一速度支路消除了该问题，且无需维护切换阈值。
+
+垂直角度-速度增益 `k_vz` 与垂直速度限幅均**待定/配置化**（第 8 章）。
 
 ## 6. 距离控制
 
@@ -298,12 +312,11 @@
 
 | 项目 | 说明 | 状态 |
 |------|------|------|
-| 水平/垂直角度误差切换阈值 | 姿态微调与高度/速率控制的分支条件 | 待定 |
 | 射程 `d_exp` | 由拦截设备接口与配置给出 | 待定 |
 | 距离通道 PID 参数 | Kp / Ki / Kd、积分限幅 | 待定 |
 | 航向通道增益与限幅 | 偏航角速度上限、方向变化率上限 | 待定 |
+| 垂直角度-速度增益 | 垂直视线角到垂直速度的映射增益（5.2 统一速度支路） | 待定 |
 | 垂直速度限幅 | 上升/下降速度上限 | 待定 |
-| 姿态角限幅 | 姿态微调支路的角与角速率上限 | 待定 |
 | 前向速度限幅 | 接近/后退速度上限，随距离收敛 | 待定 |
 | 加速度限幅 | 各轴加速度上限 | 待定 |
 | 控制周期频率 | 控制线程固定周期 | 待定 |