"""视觉跟踪主控律（TrackingControlLaw）。 对应 C++ 参考实现 reference-c++/visual_track_control/src/tracking_control_law.cpp （规格：docs/物理追踪思路.md §2.3/§4/§5/§6/§7/§8/§9）。 单周期八步流程： ① 超龄判定（姿态优先于视觉，超龄 → 降级保持） ② 判丢 → 零速刹车（进入时清积分与限幅器历史） ③ Coast（is_predicted）→ 清距离积分防饱和 ④ 像素偏移 → 视线角 β/α（全程针孔模型） ⑤ 水平航向（速率式/位置式，A2：位置式合成绝对 NED 航向） ⑥ 垂直统一速度支路（A1：不切姿态微调模式） ⑦ 距离通道 PID → 前向速度；无距离按配置降级（hold/slow_approach/exit， exit 需持续无距离达 no_distance_exit_grace_ms 门限） ⑧ 机体系 → 局部 NED 旋转（A4） 日志纪律与 C++ 版一致：仅状态切换点打印，热路径零日志。 Python 扩展：ControlOutput.vx_body_mps 记录旋转前的机体系前向分量， 供 MAVSDK 机体速度设定点使用。 """ import logging import math from .config import HeadingMode, NoDistanceAction, TrackingConfig from .frame_transform import rotate_body_to_ned from .line_of_sight import PixelToAngle from .pid_controller import PidController, PidParams from .rate_limiter import SlewRateLimiter, clamp from .staleness_checker import is_stale from .types import ControlMode, ControlOutput, ControlSnapshot, DegradedReason logger = logging.getLogger(__name__) _K_PI = 3.14159265358979323846 _K_RAD_TO_DEG = 180.0 / _K_PI _K_DEG_TO_RAD = _K_PI / 180.0 _K_MAX_DT_S = 0.5 # 单步 dt 上限（秒）：异常节拍保护 _MODE_NAMES = { ControlMode.VELOCITY_HEADING: "速度航向", ControlMode.BRAKE_HOVER: "零速刹车", ControlMode.DEGRADED_HOLD: "降级保持", } def _wrap_deg_180(deg: float) -> float: """角度回绕到 [-180, 180)（位置式绝对航向合成用）。""" d = math.fmod(deg + 180.0, 360.0) if d < 0.0: d += 360.0 return d - 180.0 def _build_distance_pid_params(cfg: TrackingConfig) -> PidParams: """由距离配置构造 PID 参数（§6.2）；输出限幅取接近/后退限幅的较大者。""" return PidParams( kp=cfg.distance.kp, ki=cfg.distance.ki, kd=cfg.distance.kd, integral_limit=cfg.distance.integral_limit, derivative_filter_coef=cfg.distance.derivative_filter_coef, output_limit=max(cfg.distance.approach_velocity_limit_mps, cfg.distance.retreat_velocity_limit_mps), ) class TrackingControlLaw: def __init__(self, cfg: TrackingConfig) -> None: self._cfg = cfg self._pixel_to_beta = PixelToAngle(cfg.camera.image_width, cfg.camera.fov_h_deg * _K_DEG_TO_RAD) self._pixel_to_alpha = PixelToAngle(cfg.camera.image_height, cfg.camera.fov_v_deg * _K_DEG_TO_RAD) self._distance_pid = PidController(_build_distance_pid_params(cfg)) self._yaw_deg_limiter = SlewRateLimiter(cfg.heading.yaw_slew_limit_dps2) self._yaw_rate_limiter = SlewRateLimiter(cfg.heading.yaw_slew_limit_dps2) self._vx_limiter = SlewRateLimiter(cfg.accel_limit.ax_mps2) 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 固定周期；异常节拍用配置频率兜底） ---- # 时钟回拨或 now_ms 未前进（同值）时不信任差值，回退配置频率。 dt_s = 1.0 / self._cfg.control.frequency_hz if self._last_now_ms > 0 and now_ms > self._last_now_ms: dt_s = min((now_ms - self._last_now_ms) / 1000.0, _K_MAX_DT_S) self._last_now_ms = now_ms out = ControlOutput() out.mode = ControlMode.VELOCITY_HEADING # 正常路径默认模式，分支可覆盖 # ---- ① 超龄判定（§7.1 / §9.2） ---- # 姿态过期或缺失：禁止空间转换和控制，立即退出正常控制（§9.2） if (not snapshot.attitude_present or is_stale(snapshot.attitude_time_ms, now_ms, self._cfg.control.attitude_stale_ms)): out.mode = ControlMode.DEGRADED_HOLD out.degraded_reason = DegradedReason.ATTITUDE_STALE return self._finish_and_log(out, "姿态过期/缺失，禁止控制") # 视觉追踪结果超龄：不得继续生成正常指令（§7.1） if is_stale(snapshot.track_time_ms, now_ms, self._cfg.control.visual_stale_ms): out.mode = ControlMode.DEGRADED_HOLD out.degraded_reason = DegradedReason.VISION_STALE return self._finish_and_log(out, "视觉追踪结果超龄") # ---- ② 判丢 → 零速刹车（§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() self._yaw_deg_limiter.reset() self._yaw_rate_limiter.reset() self._vx_limiter.reset() self._vz_limiter.reset() out.mode = ControlMode.BRAKE_HOVER # 速度分量保持默认全零 out.valid = True return self._finish_and_log(out, "判丢，零速刹车") # ---- ③ Coast：is_predicted → 清积分防饱和（§6.4） ---- if snapshot.track.is_predicted: self._distance_pid.reset_integral() # 航向通道为纯比例增益映射（§4.2），无积分项，无需清理 out.coast_active = True # ---- ④ 像素偏移 → 视线角（§4.1 水平 β / §5.1 垂直 α） ---- # x_offset = cx − W/2（右正），y_offset = cy − H/2（下正） x_offset = snapshot.track.center[0] - self._cfg.camera.image_width * 0.5 y_offset = snapshot.track.center[1] - self._cfg.camera.image_height * 0.5 beta_deg = self._pixel_to_beta.convert(x_offset) * _K_RAD_TO_DEG alpha_deg = self._pixel_to_alpha.convert(y_offset) * _K_RAD_TO_DEG # ---- ⑤ 水平航向通道（§4.2：增益 → slew → 角速度限幅） ---- if self._cfg.heading.mode is HeadingMode.RATE: # 速率式：β → 偏航角速度指令，先变化率限幅再角速度限幅（FR-033） target_rate = self._cfg.heading.gain * beta_deg slewed = self._yaw_rate_limiter.limit(target_rate, dt_s) out.yaw_rate_dps = clamp(slewed, -self._cfg.heading.yaw_rate_limit_dps, self._cfg.heading.yaw_rate_limit_dps) else: # 位置式：β → 航向修正量，变化率限幅（FR-033）后与当前航向合成 # 绝对 NED 航向——ID 84 的 yaw 字段是绝对值，直接发修正量会让 # 机体转向"修正角"方向（A2 修复）。姿态已在 ① 通过超龄校验。 target_yaw = self._cfg.heading.gain * beta_deg correction_deg = self._yaw_deg_limiter.limit(target_yaw, dt_s) out.yaw_deg = _wrap_deg_180(snapshot.yaw_rad * _K_RAD_TO_DEG + correction_deg) # ---- ⑥ 垂直速度通道（§5：统一速度支路） ---- # 符号约定：α 下正（目标偏低）→ NED 下正 → vz 为正（下降趋近目标高度）。 # 输出模式保持 VELOCITY_HEADING，距离通道前向速度不会被丢弃（A1 修复）。 vz_target = self._cfg.vertical.vz_gain_mps_per_deg * alpha_deg slewed = self._vz_limiter.limit(vz_target, dt_s) # 加速度限幅（§8） out.vz_mps = clamp(slewed, -self._cfg.vertical.vz_limit_mps, self._cfg.vertical.vz_limit_mps) # ---- ⑦ 距离通道（§6.2 / §6.3） ---- # 目标距离来自检测框自身的双目测距：仅 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.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, -self._cfg.distance.retreat_velocity_limit_mps, self._cfg.distance.approach_velocity_limit_mps) else: # 无可用目标距离：清积分防饱和，按配置降级动作（§6.3） self._distance_pid.reset_integral() action = self._cfg.distance.no_distance_action if action is NoDistanceAction.HOLD: # 保持：前向指令清零（安全占位语义） out.vx_mps = 0.0 self._vx_limiter.reset(0.0) elif action is NoDistanceAction.SLOW_APPROACH: # 限速接近：缓速前向，同样经加速度限幅 out.vx_mps = self._vx_limiter.limit( self._cfg.distance.no_distance_approach_limit_mps, dt_s) else: # NoDistanceAction.EXIT # 退出近距操作：双目有效域外无距离是常态（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 修复） ---- # 各通道限幅均在机体系完成（旋转为正交变换，合速度不变，限幅约束 # 不被破坏）；输出端随快照航向旋转为北/东/地。旋转前记录机体系前向 # 分量，供 MAVSDK 机体速度设定点直接使用（免反旋）。 out.vx_body_mps = out.vx_mps out.vx_mps, out.vy_mps, out.vz_mps = rotate_body_to_ned( out.vx_mps, out.vy_mps, out.vz_mps, snapshot.yaw_rad) out.valid = True return self._finish_and_log(out, "Coast 预测跟踪" if out.coast_active else "正常跟踪") def reset(self) -> None: """停止视觉闭环时清全部状态（§6.4）。""" self._distance_pid.reset() self._yaw_deg_limiter.reset() 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 def _finish_and_log(self, out: ControlOutput, reason: str) -> ControlOutput: """仅模式变化时打印状态切换日志（刹车进出、降级切换等）。""" if out.mode is not self._last_mode: logger.info("[ControlLaw] 状态切换: %s -> %s (%s)", _MODE_NAMES[self._last_mode], _MODE_NAMES[out.mode], reason) self._last_mode = out.mode return out