221 lines
11 KiB
Python
221 lines
11 KiB
Python
"""视觉跟踪主控律（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）
|
||
⑧ 机体系 → 局部 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
|
||
|
||
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:
|
||
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） ----
|
||
# 雷达距离不得直接当目标距离：仅关联成立且未超龄才可用（§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))
|
||
if distance_available:
|
||
# Δd = d − d_exp：d > d_exp 前向接近，d < d_exp 减速/后退（§6.2）
|
||
delta_d = snapshot.radar_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
|
||
# 退出近距操作：整体降级保持
|
||
out.mode = ControlMode.DEGRADED_HOLD
|
||
out.degraded_reason = DegradedReason.NO_DISTANCE_EXIT
|
||
return self._finish_and_log(out, "距离未关联，退出近距操作")
|
||
|
||
# ---- ⑧ 机体系 → 局部 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._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
|