无人机跟随无人机自动降落算法

无人机自动降落到船舶上 --- 算法设计思路

系统架构

整体设计思路

复制代码

┌─────────────────────────────────────────────────────┐
│           主线程 (Matplotlib 3D)                    │
│          实时渲染 + 交互控制                        │
└──────────────┬──────────────────────────────────────┘
               │
        ┌──────┴──────┐
        │  状态队列   │  命令队列
        └──────┬──────┘
               │
    ┌──────────┼──────────┐
    ▼          ▼          ▼
┌─────────┐┌─────────┐┌─────────┐
│ 导引线程 ││船位置线程││状态发布 │ 
│ (30ms) ││ (60ms) ││ (50ms) │
└─────────┘└─────────┘└─────────┘
    ▲          ▲
    └──────┬───┘
       shared_state
      (Lock 保护)

关键特性

组件	功能	周期	输入输出
导引线程	计算速度指令，维护无人机状态	30ms	输入：无人机位置、速度；输出：速度指令
船位置线程	更新船的绝对位置	60ms	基于船运动模型更新坐标
状态发布线程	快照状态供可视化	50ms	读取共享状态，发布给UI
可视化线程	3D 渲染 + 交互	~60Hz	接收状态，响应用户命令

ZEM/ZEV 最优制导原理

目标： 使无人机在有限时间内到达船舶，同时速度降至零（零脱靶量 + 零末速度）

船运动模型

设计需求

模拟真实船舶运动的随机性和延迟性，包括：

速度波动（风浪影响）
航向偏转（操舵响应延迟）
非直线轨迹（真实感）

Ornstein-Uhlenbeck (OU) 随机过程

基本方程

d X t = − θ ( X t − μ ) d t + σ d W t d X_t = -\theta (X_t - \mu) dt + \sigma dW_t dXt=−θ(Xt−μ)dt+σdWt

特点：

均值回归： 波动会衰减至平均值
相关性： 相邻时刻相关，不是纯白噪声
稳态性： 长期统计性质稳定
物理意义： 模拟阻力、回复力等

离散化（Euler-Maruyama 格式）

X t + d t = X t − θ X t ⋅ d t + σ e f f d t ⋅ N ( 0 , 1 ) X_{t+dt} = X_t - \theta X_t \cdot dt + \sigma_{eff} \sqrt{dt} \cdot N(0, 1) Xt+dt=Xt−θXt⋅dt+σeffdt ⋅N(0,1)

其中：

σ e f f = σ 2 θ \sigma_{eff} = \sigma \sqrt{2\theta} σeff=σ2θ

速度扰动模型

目标：模拟波浪对船速的影响

参数：

参数	值	含义
`SHIP_SPEED_NOMINAL`	0.583 m/s	标称船速
`SHIP_SPEED_FLUCTUATION`	0.15	波动比例（15%）
`SHIP_OU_THETA`	0.5 s⁻¹	均值回归率
`SHIP_SPEED_MULTIPLIER`	2.0	速度倍率（可调）

稳态特性：

标准差： σ = 0.583 × 0.15 ≈ 0.087 \sigma = 0.583 \times 0.15 \approx 0.087 σ=0.583×0.15≈0.087 m/s
相关时间： τ = 1 / θ = 2 \tau = 1 / \theta = 2 τ=1/θ=2 s
波动范围： ± 0.175 \pm 0.175 ±0.175 m/s（±30% 限幅）

更新方程：

python 复制代码

dt = 0.06  # 60ms
sigma_eff = 0.583 * 0.15 * sqrt(2 * 0.5)
delta_v_pert += -0.5 * delta_v_pert * dt + sigma_eff * sqrt(dt) * N(0,1)
delta_v_pert = clamp(delta_v_pert, -0.175, 0.175)  # ±30% 限幅

航向扰动模型

目标：模拟操舵延迟和风浪对航向的影响，产生明显弯曲的航迹

参数：

参数	值	含义
`SHIP_HEADING_RAD`	31°	标称航向
`SHIP_HEADING_FLUCTUATION_DEG`	20°	波动标准差
`SHIP_HEADING_OU_THETA`	0.1 s⁻¹	均值回归率（低回归）

稳态特性：

标准差： 20 ° 20° 20°
相关时间： τ = 1 / θ = 10 \tau = 1 / \theta = 10 τ=1/θ=10 s
波动范围： ± 45 ° \pm 45° ±45° 限幅（允许大幅偏转）

特点：

相比速度扰动，回归率更低（0.1 vs 0.5）
相关时间更长（10s vs 2s）
允许 ± 45 ° \pm 45° ±45° 的大幅偏转
结果：航迹呈现明显的弯曲和蛇形运动

更新方程：

python 复制代码

sigma_h_eff = rad(20°) * sqrt(2 * 0.1)
delta_hdg_pert += -0.1 * delta_hdg_pert * dt + sigma_h_eff * sqrt(dt) * N(0,1)
delta_hdg_pert = clamp(delta_hdg_pert, -rad(45°), rad(45°))

# 计算实际船速分量
speed = (SHIP_SPEED_NOMINAL + delta_v_pert) * SHIP_SPEED_MULTIPLIER
heading = SHIP_HEADING_RAD + delta_hdg_pert
ship_vx = speed * cos(heading)
ship_vy = speed * sin(heading)

航位更新

python 复制代码

ship_x += ship_vx * dt
ship_y += ship_vy * dt

线程协调机制

共享状态保护

所有共享变量通过 threading.Lock 保护，确保线程安全：

python 复制代码

self._lock = threading.Lock()

# 受保护的共享变量
with self._lock:
    self.ship_pos        # 船绝对位置
    self.ship_vel        # 船速度
    self._drone_pos      # 无人机绝对位置（由外部注入）
    self._drone_vel      # 无人机速度（由外部注入）
    self.velocity_cmd    # 速度指令（导引线程写，可视化线程读）
    self.trajectory      # 轨迹点（导引线程写，可视化线程读）

线程职责划分

导引线程 (30ms)

复制代码

1. 读取命令队列（非阻塞，检查 reset 命令）
2. 接收外部注入的无人机位置和速度
3. 调用 compute_guidance() 计算速度指令
4. 更新轨迹点（deque，自动淘汰旧点）
5. 睡眠至下一周期

时序特点：

单调钟定时：next_wakeup += GUIDANCE_PERIOD
消除漏斗累积（Scheduling Drift）
相对精准的 30ms 周期

船位置线程 (60ms)

复制代码

1. 调用 update_ship() 更新船位置
2. 内部维护 OU 状态变量（速度、航向扰动）
3. 计算新的船速分量和位置
4. 睡眠至下一周期

独立性：

不依赖导引线程，周期独立
只读导引线程的状态（在可视化中观察）

状态发布线程 (50ms)

复制代码

1. 定期快照完整状态
2. 推送到可视化队列
3. 若队列满，丢弃旧数据（保留最新）

缓冲机制：

使用 queue.Queue(maxsize=5) 限制内存
满时自动丢弃最旧数据
避免UI卡顿

通信流

复制代码

┌──────────────────────────┐
│  导引线程                 │
│  - compute_guidance()    │
│  - 维护 velocity_cmd     │
│  - 维护 trajectory       │
└──────────┬───────────────┘
           │ 共享状态
           │ (Lock)
           ▼
┌──────────────────────────┐
│  状态发布线程            │
│  - get_state()           │
│  - 推送到队列            │
└──────────┬───────────────┘
           │ 状态队列
           ▼
┌──────────────────────────┐
│  可视化线程 (主线程)      │
│  - 3D 渲染               │
│  - 处理用户交互          │
└──────────────────────────┘

右侧：
┌──────────────────────────┐
│  船位置线程              │
│  - update_ship()         │
│  - OU 状态维护           │
└──────────────────────────┘

无阻塞操作

所有队列操作采用 get_nowait()/put_nowait() 防止死锁：

python 复制代码

# 导引线程：检查命令（非阻塞）
try:
    while True:
        cmd = cmd_queue.get_nowait()
        if cmd.get("command") == "reset":
            tracker.reset()
except queue.Empty:
    pass

# 可视化线程：获取最新状态（非阻塞）
def _drain_queue(self):
    state = None
    try:
        while True:
            state = self.state_queue.get_nowait()
    except queue.Empty:
        pass
    return state

自适应参数设计

周期配置的意义

周期	频率	目的	依赖关系
30ms	33.3 Hz	导引计算，轨迹采样	独立
60ms	16.7 Hz	船位置更新	独立
50ms	20 Hz	状态发布	依赖前两者
~60Hz	UI刷新	3D 渲染	从队列读数据

设计原理：

导引频率最高（30ms），关乎控制精度
船更新频率次之（60ms），足以捕捉低频运动
状态发布介于两者（50ms），既不过载也不漏数据
3D 渲染频率由 matplotlib 动画自动调节

距离判决

python 复制代码

self.state = "landing" if dist < 15.0 else "tracking"

追踪状态： 应用完整的 ZEM/ZEV 制导
着陆状态： 接近目标（< 15m），仅用于状态指示，实际制导无变化

轨迹记录限制

python 复制代码

MAX_TRAJECTORY_POINTS = 2000
self.trajectory = collections.deque(maxlen=MAX_TRAJECTORY_POINTS)

# 时间覆盖：2000 点 × 30ms = 60s

作用：

自动淘汰最旧的点，防止无限增长
覆盖完整的追踪过程（60 秒）
O(1) 追加操作，高效

数值计算特性

无累积误差的速度积分

关键思想： 速度指令总是从外部注入的真值开始，增量为制导加速度。

python 复制代码

# 不采用：v_cmd_next = v_cmd_current + a * dt  ❌ 会累积舍入误差

# 而是：
v_cmd = drone_vel + a * dt  ✓ 从真值开始

优点：

仿真中无累积误差
在真实系统中，可直接接收 GPS/IMU 数据替代 drone_vel
闭环反馈结构

单调时钟定时

python 复制代码

next_wakeup = time.monotonic()  # 基于单调钟，不受系统时间调整影响
while True:
    # ... 处理业务逻辑 ...
    next_wakeup += GUIDANCE_PERIOD
    sleep_dur = next_wakeup - time.monotonic()
    if sleep_dur > 0:
        time.sleep(sleep_dur)

优点：

消除漏斗累积（Scheduling Drift）
长期周期精度高
不受系统时钟调整影响

OU 过程的 Euler-Maruyama 离散化

python 复制代码

# OU 微分方程：dX = -θ·X·dt + σ_eff·√dt·dW

X_new = X + (-θ * X * dt) + sigma_eff * sqrt(dt) * N(0, 1)

数值特性：

一阶强收敛
简单稳定
适合实时应用

向量计算优化

所有水平面计算使用向量分量，避免频繁三角函数调用：

python 复制代码

# ✓ 高效：直接使用分量
ax = -6 * rel_x / t_go2 - 4 * vrel_x / t_go
ay = -6 * rel_y / t_go2 - 4 * vrel_y / t_go
a_mag = sqrt(ax^2 + ay^2)  # 仅在需要时计算模

# ❌ 低效：频繁转换极坐标
dist = sqrt(rel_x^2 + rel_y^2)
angle = atan2(rel_y, rel_x)
a_mag_cmd = 6 * dist / t_go2
# ... 再转换回分量 ...

总体算法流程

完整的控制循环（30ms）

复制代码

导引线程 (30ms 周期)
├─ 检查重置命令（非阻塞）
├─ 接收外部注入的无人机位置和速度
├─ compute_guidance(drone_pos, drone_vel)
│  ├─ 加速度和速度限幅
│  └─ 返回速度指令
├─ 更新轨迹（deque.append）
└─ 睡眠至下一周期

同时进行 (60ms 周期)
船位置线程 (独立)
├─ OU 速度扰动更新
├─ OU 航向扰动更新
├─ 计算实际船速和航向
└─ 航位积分

结果：
状态发布 (50ms 周期)
├─ 读取完整状态快照
└─ 推送给可视化队列

可视化 (主线程)
├─ 从队列接收状态
├─ 3D 渲染
└─ 响应用户交互（重置）

可视化层反馈

复制代码

用户点击 "重置" 按钮
  │
  ├─ 清空 UI 轨迹缓存
  ├─ 推送 reset 命令到命令队列
  │
导引线程 检测到命令
  │
  ├─ 调用 tracker.reset()
  │   ├─ 清空所有状态
  │   ├─ 从初始位置重新开始
  │   └─ 重初始化 OU 状态
  │
仿真重新启动
  └─ 轨迹从初始位置开始绘制

扩展与改进方向

现有设计的可扩展性

传感器融合： 用 GPS/IMU 传感器读数直接替代 drone_vel 和 drone_pos 注入
风场模型： 船速扰动可演变为风场模型
多目标跟踪： 轻易扩展到多个船舶和无人机
动态轨迹规划： 每次调用自动重规划，支持动态约束

参数调优建议

参数	调优方向	效果
`MAX_ACCEL`	增大	更快追踪，更大加速度尖峰
`MAX_SPEED`	增大	更快速度，更易超调
`T_GO_MIN`	减小	允许更激进的制导，风险增大
`SHIP_SPEED_MULTIPLIER`	增大	船更快，追踪难度增大
`SHIP_HEADING_OU_THETA`	减小	航向波动更持久，轨迹更弯曲

无人机跟随 无人机自动降落算法

无人机自动降落到船舶上 --- 算法设计思路

目录

系统架构

整体设计思路

关键特性

ZEM/ZEV 最优制导原理

船运动模型

设计需求

Ornstein-Uhlenbeck (OU) 随机过程

基本方程

离散化（Euler-Maruyama 格式）

速度扰动模型

航向扰动模型

航位更新

线程协调机制

共享状态保护

线程职责划分

导引线程 (30ms)

船位置线程 (60ms)

状态发布线程 (50ms)

通信流

无阻塞操作

自适应参数设计

周期配置的意义

距离判决

轨迹记录限制

数值计算特性

无累积误差的速度积分

单调时钟定时

OU 过程的 Euler-Maruyama 离散化

向量计算优化

总体算法流程

完整的控制循环（30ms）

可视化层反馈

扩展与改进方向

现有设计的可扩展性

参数调优建议

无人机跟随无人机自动降落算法