"""guidance.py —— 地理位置引导律：载机位置 + 目标经纬度 → offboard 速度/航向。 吊舱（表4）给出目标经纬度，本律计算到目标的方位角/距离，生成"朝目标飞"的 控制意图，输出复用 visual_track_control.types.ControlOutput，可直接喂现有 MavsdkCommandGateway（速率式分支：vx_body_mps 前向 + yaw_rate_dps 偏航）。 与 visual_track_control.TrackingControlLaw 的区别：后者由像素视线角驱动（视觉 伺服），本律由绝对地理坐标驱动（航位推算）。复用其纯算法构件（PID / 限幅 / 超龄判定），故 pod_bridge 依赖 visual_track_control 作为控制原语库。 控制策略（速率式，避开"绝对航向为正北 0° 被网关误判为无航向指令"的歧义）： * 方位误差 err = wrap180(目标方位 − 当前航向) → 偏航角速度（比例 + 变化率限幅） * 距离误差 = 地面距离 − 停止半径 → 前向基准速度（PID + 限幅） * 对准门控：前向 = 基准速度 · max(0, cos(err))，未对准时先原地转向再前进 * 垂直：高度保持（vz = 0）；高度逼近策略待后续 config 化 * 到达停止半径 / 目标或载机位置不可用 / 超龄 → 零速刹车（BRAKE_HOVER） """ import logging import math from dataclasses import dataclass from visual_track_control.pid_controller import PidController, PidParams from visual_track_control.rate_limiter import SlewRateLimiter, clamp from visual_track_control.staleness_checker import is_stale from visual_track_control.types import ControlMode, ControlOutput logger = logging.getLogger(__name__) _K_DEG_TO_RAD = math.pi / 180.0 _K_RAD_TO_DEG = 180.0 / math.pi _K_EARTH_RADIUS_M = 6371008.8 # WGS84 平均半径 _K_MAX_DT_S = 0.5 # 单步 dt 上限（秒）：异常节拍保护 # --- 大地测量（纯函数）----------------------------------------------------- def haversine_distance_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float: """两点地面距离（米，haversine 大圆距离）。""" phi1 = lat1 * _K_DEG_TO_RAD phi2 = lat2 * _K_DEG_TO_RAD dphi = (lat2 - lat1) * _K_DEG_TO_RAD dlmb = (lon2 - lon1) * _K_DEG_TO_RAD a = (math.sin(dphi / 2.0) ** 2 + math.cos(phi1) * math.cos(phi2) * math.sin(dlmb / 2.0) ** 2) return 2.0 * _K_EARTH_RADIUS_M * math.asin(min(1.0, math.sqrt(a))) def initial_bearing_deg(lat1: float, lon1: float, lat2: float, lon2: float) -> float: """从点 1 指向点 2 的初始方位角（度，正北为 0，顺时针 [0, 360)）。""" phi1 = lat1 * _K_DEG_TO_RAD phi2 = lat2 * _K_DEG_TO_RAD dlmb = (lon2 - lon1) * _K_DEG_TO_RAD x = math.sin(dlmb) * math.cos(phi2) y = (math.cos(phi1) * math.sin(phi2) - math.sin(phi1) * math.cos(phi2) * math.cos(dlmb)) return (math.atan2(x, y) * _K_RAD_TO_DEG) % 360.0 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 # --- 配置 / 输入 ------------------------------------------------------------ @dataclass class GuidanceConfig: """引导律参数（全部可调，后续 config.py 从 JSON 加载并校验）。""" frequency_hz: float = 10.0 target_stale_ms: int = 1000 # 目标经纬度超龄门限 position_stale_ms: int = 1000 # 载机位置超龄门限 stop_radius_m: float = 5.0 # 到达此半径内悬停（不再前进） max_speed_mps: float = 5.0 # 前向速度上限 speed_kp: float = 0.5 # 距离比例增益（mps / m） speed_ki: float = 0.0 speed_kd: float = 0.0 speed_integral_limit: float = 0.0 speed_derivative_filter_coef: float = 0.0 speed_slew_mps2: float = 2.0 # 前向加速度限幅 max_yaw_rate_dps: float = 30.0 # 偏航角速度上限 yaw_kp: float = 1.0 # 方位误差比例增益（dps / deg） yaw_slew_dps2: float = 60.0 # 偏航角速度变化率限幅 @dataclass class GuidanceInput: """单周期引导输入：载机状态（PX4 遥测）+ 目标（吊舱表4），各带取样时间戳。""" own_lat_deg: float = 0.0 own_lon_deg: float = 0.0 own_yaw_deg: float = 0.0 # 当前航向（NED，度，正北 0 顺时针） own_position_valid: bool = False # GPS 定位/位置是否可用 own_position_time_ms: int = 0 target_lat_deg: float = 0.0 target_lon_deg: float = 0.0 target_valid: bool = False # 表4 目标经纬度是否有效 target_time_ms: int = 0 # --- 引导律 ----------------------------------------------------------------- class GuidanceLaw: """地理位置引导主控律。update() 单周期返回 ControlOutput。 诊断量（last_distance_m / last_bearing_deg / last_heading_err_deg）每周期 更新，供桥接层日志使用；刹车/降级时为最近一次有效计算值或 NaN。 """ def __init__(self, cfg: GuidanceConfig) -> None: self._cfg = cfg self._speed_pid = PidController(PidParams( kp=cfg.speed_kp, ki=cfg.speed_ki, kd=cfg.speed_kd, integral_limit=cfg.speed_integral_limit, derivative_filter_coef=cfg.speed_derivative_filter_coef, output_limit=cfg.max_speed_mps, )) self._speed_limiter = SlewRateLimiter(cfg.speed_slew_mps2) self._yaw_limiter = SlewRateLimiter(cfg.yaw_slew_dps2) self._last_now_ms = 0 self._last_mode = ControlMode.BRAKE_HOVER self.last_distance_m = float("nan") self.last_bearing_deg = float("nan") self.last_heading_err_deg = float("nan") def update(self, inp: GuidanceInput, now_ms: int) -> ControlOutput: dt_s = self._advance_dt(now_ms) out = ControlOutput() # ---- 可用性 / 超龄判定 ---- position_ok = (inp.own_position_valid and not is_stale(inp.own_position_time_ms, now_ms, self._cfg.position_stale_ms)) target_ok = (inp.target_valid and not is_stale(inp.target_time_ms, now_ms, self._cfg.target_stale_ms)) if not position_ok or not target_ok: return self._brake(out, "载机位置或目标不可用/超龄，零速刹车") distance = haversine_distance_m(inp.own_lat_deg, inp.own_lon_deg, inp.target_lat_deg, inp.target_lon_deg) bearing = initial_bearing_deg(inp.own_lat_deg, inp.own_lon_deg, inp.target_lat_deg, inp.target_lon_deg) heading_err = _wrap_deg_180(bearing - inp.own_yaw_deg) self.last_distance_m = distance self.last_bearing_deg = bearing self.last_heading_err_deg = heading_err # ---- 到达停止半径 → 悬停 ---- if distance <= self._cfg.stop_radius_m: return self._brake(out, "已到达停止半径，悬停") # ---- 偏航角速度（比例 + 变化率限幅 + 上限）---- target_rate = clamp(self._cfg.yaw_kp * heading_err, -self._cfg.max_yaw_rate_dps, self._cfg.max_yaw_rate_dps) out.yaw_rate_dps = self._yaw_limiter.limit(target_rate, dt_s) # ---- 前向速度（距离 PID + 对准门控 + 加速度限幅 + 上限）---- base_speed = clamp(self._speed_pid.update(distance - self._cfg.stop_radius_m, dt_s), 0.0, self._cfg.max_speed_mps) alignment = max(0.0, math.cos(heading_err * _K_DEG_TO_RAD)) out.vx_body_mps = self._speed_limiter.limit(base_speed * alignment, dt_s) out.vz_mps = 0.0 # 高度保持 out.mode = ControlMode.VELOCITY_HEADING out.valid = True return self._finish_and_log(out, "朝目标引导") def reset(self) -> None: """停止闭环时清全部内部状态。""" self._speed_pid.reset() self._speed_limiter.reset() self._yaw_limiter.reset() self._last_now_ms = 0 self._last_mode = ControlMode.BRAKE_HOVER def _advance_dt(self, now_ms: int) -> float: """节拍推算：异常节拍/时钟回拨用配置频率兜底，单步上限 0.5s。""" dt_s = 1.0 / self._cfg.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 return dt_s def _brake(self, out: ControlOutput, reason: str) -> ControlOutput: """零速刹车：进入时清积分与限幅器历史，恢复引导后指令从零渐变。""" if self._last_mode is not ControlMode.BRAKE_HOVER: self._speed_pid.reset() self._speed_limiter.reset() self._yaw_limiter.reset() out.mode = ControlMode.BRAKE_HOVER out.valid = True # 速度分量保持默认全零 return self._finish_and_log(out, reason) def _finish_and_log(self, out: ControlOutput, reason: str) -> ControlOutput: """仅模式变化时打印状态切换日志（热路径零日志）。""" if out.mode is not self._last_mode: logger.info("[Guidance] 状态切换: %s -> %s (%s)", self._last_mode.name, out.mode.name, reason) self._last_mode = out.mode return out