keita
477ad5f5de
- pod_bridge: 协议G版编解码/地理位置引导律/PX4网关/串口链路/闭环主循环 - 含三项安全修复: position_time_ms拆分、表2-2锁定门控、run()异常护栏 - 测试92条全绿; 附树莓派4B(Ubuntu 24.04)部署注意事项与最小依赖清单
205 lines
9.3 KiB
Python
205 lines
9.3 KiB
Python
"""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
|