songhaoyu/code-test-drone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

双目测距方案重构与其他小修改

Browse Source
This commit is contained in:
keita
2026-09-10 16:16:05 +08:00
parent 3325465359
commit 538ae4c6c7
39 changed files with 2285 additions and 1444 deletions
Download Patch File Download Diff File Expand all files Collapse all files
utils/visual_track_control/tracking_control_law.py
+30 -11
View File
@@ -10,7 +10,8 @@
④ 像素偏移 → 视线角 β/α（全程针孔模型）
⑤ 水平航向（速率式/位置式，A2：位置式合成绝对 NED 航向）
⑥ 垂直统一速度支路（A1：不切姿态微调模式）
⑦ 距离通道 PID → 前向速度；无距离按配置降级（hold/slow_approach/exit）
⑦ 距离通道 PID → 前向速度；无距离按配置降级（hold/slow_approach/exit，
exit 需持续无距离达 no_distance_exit_grace_ms 门限）
⑧ 机体系 → 局部 NED 旋转（A4）
日志纪律与 C++ 版一致：仅状态切换点打印，热路径零日志。
@@ -78,6 +79,10 @@ class TrackingControlLaw:
self._vz_limiter = SlewRateLimiter(cfg.accel_limit.az_mps2)
self._last_now_ms = 0
self._last_mode = ControlMode.DEGRADED_HOLD
# EXIT 无距离计时（§6.3：持续无距离达 no_distance_exit_grace_ms 才退出，
# 任一有效距离帧重置）
self._no_distance_active = False
self._no_distance_since_ms = 0
def update(self, snapshot: ControlSnapshot, now_ms: int) -> ControlOutput:
# ---- 节拍推算（§7.1 固定周期；异常节拍用配置频率兜底） ----
@@ -107,6 +112,7 @@ class TrackingControlLaw:
# ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 BRAKE_HOVER） ----
if not snapshot.track.tracked:
self._no_distance_active = False # 刹车期间不计 EXIT 无距离时长
if self._last_mode is not ControlMode.BRAKE_HOVER:
# 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅）
self._distance_pid.reset()
@@ -156,14 +162,16 @@ class TrackingControlLaw:
self._cfg.vertical.vz_limit_mps)
# ---- ⑦ 距离通道（§6.2 / §6.3） ----
# 雷达距离不得直接当目标距离：仅关联成立且未超龄才可用（§6.3、FR-041）
distance_available = (snapshot.associated_to_target
and snapshot.radar_distance_m > 0.0
and not is_stale(snapshot.radar_time_ms, now_ms,
self._cfg.control.radar_stale_ms))
# 目标距离来自检测框自身的双目测距：仅 distance_valid 且未超龄才可用
# （§6.3、FR-041）
distance_available = (snapshot.distance_valid
and snapshot.target_distance_m > 0.0
and not is_stale(snapshot.distance_time_ms, now_ms,
self._cfg.control.distance_stale_ms))
if distance_available:
self._no_distance_active = False # 任一有效距离帧重置 EXIT 计时
# Δd = d − d_exp：d > d_exp 前向接近，d < d_exp 减速/后退（§6.2）
delta_d = snapshot.radar_distance_m - self._cfg.distance.d_exp_m
delta_d = snapshot.target_distance_m - self._cfg.distance.d_exp_m
vx_raw = self._distance_pid.update(delta_d, dt_s)
slewed = self._vx_limiter.limit(vx_raw, dt_s) # 加速度限幅（§8）
out.vx_mps = clamp(slewed,
@@ -182,10 +190,20 @@ class TrackingControlLaw:
out.vx_mps = self._vx_limiter.limit(
self._cfg.distance.no_distance_approach_limit_mps, dt_s)
else: # NoDistanceAction.EXIT
# 退出近距操作：整体降级保持
out.mode = ControlMode.DEGRADED_HOLD
out.degraded_reason = DegradedReason.NO_DISTANCE_EXIT
return self._finish_and_log(out, "距离未关联，退出近距操作")
# 退出近距操作：双目有效域外无距离是常态（D4 远场定速接近），
# 首个无效拍不退出；持续无距离达 no_distance_exit_grace_ms
# （疑似传感器级故障）才降级。grace 期间按 SLOW_APPROACH 行为
# 执行（经加速度限幅，vx 平滑过渡）；grace=0 保留旧"立即退出"行为
if not self._no_distance_active:
self._no_distance_active = True
self._no_distance_since_ms = now_ms
if (now_ms - self._no_distance_since_ms
>= self._cfg.distance.no_distance_exit_grace_ms):
out.mode = ControlMode.DEGRADED_HOLD
out.degraded_reason = DegradedReason.NO_DISTANCE_EXIT
return self._finish_and_log(out, "持续无距离达门限，退出近距操作")
out.vx_mps = self._vx_limiter.limit(
self._cfg.distance.no_distance_approach_limit_mps, dt_s)
# ---- ⑧ 机体系 → 局部 NED 旋转（§2.3，A4 修复） ----
# 各通道限幅均在机体系完成（旋转为正交变换，合速度不变，限幅约束
@@ -207,6 +225,7 @@ class TrackingControlLaw:
self._yaw_rate_limiter.reset()
self._vx_limiter.reset()
self._vz_limiter.reset()
self._no_distance_active = False
self._last_now_ms = 0
self._last_mode = ControlMode.DEGRADED_HOLD