diff --git a/docs/物理追踪思路.md b/docs/物理追踪思路.md index 5dc77c0..57a9684 100644 --- a/docs/物理追踪思路.md +++ b/docs/物理追踪思路.md @@ -80,8 +80,8 @@ - `ControlIntentType::kManualOverride`(禁止自动输出/人工接管意图): 遥控器接管时停止任务控制输出。 - `Px4Setpoint` 使用 `SetpointType::kVelocity`(速度设定值,分量写入 - `x`、`y`、`z`)或 `SetpointType::kAttitude`(姿态微调,仅限垂直小误差 - 支路,见第 5 章);航向写入 `yaw_deg` 或 `yaw_rate_dps`; + `x`、`y`、`z`;垂直通道不再使用 `SetpointType::kAttitude`,理由见 + 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); -- **角度误差较大**:调整高度,使用速度设定值(`SetpointType::kVelocity` - 的 `z` 分量,局部 NED 下为垂直速度)趋近目标高度。 +``` +vz = clamp( slew( k_vz * α ), -vz_limit, +vz_limit ) +``` -角度误差切换阈值、姿态角限幅、垂直速度限幅均**待定**(第 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 统一速度支路) | 待定 | | 垂直速度限幅 | 上升/下降速度上限 | 待定 | -| 姿态角限幅 | 姿态微调支路的角与角速率上限 | 待定 | | 前向速度限幅 | 接近/后退速度上限,随距离收敛 | 待定 | | 加速度限幅 | 各轴加速度上限 | 待定 | | 控制周期频率 | 控制线程固定周期 | 待定 |