MAVLink & FCU

• 基于 Pymavlink 的任务规划实现
• WSL2 环境下 PX4 SITL 多端口 MAVLink 配置指南
• MAVLink Router-安装与使用指南
• 使用 pyulog 分析 PX4 飞控日志
• 基于 VRPN 的 PX4 EKF2 视觉融合基础调试指南
• MAVROS 与 MAVProxy：APM 平台上的使用场景、差异与取舍

基于 Pymavlink 的任务规划实现

1. 概述

本文档使用MAVLink协议和pymavlink库进行航点发送，具有以下特点：

• 直接通信: 直接使用MAVLink协议，与QGC完全一致
• 灵活控制: 可以精确控制消息发送和错误处理
• 高效稳定: 直接通信，性能更好，减少潜在问题
• 完全兼容: 支持与QGroundControl, MAVSDK-Python等应用同时使用

2. 主要功能

2.1 航点管理

• 标准MAVLink航点格式
• 支持航点参数配置（接受半径、停留时间等）
• 自动序列号管理

2.2 任务发送

• 完整的MAVLink任务上传流程
• 支持任务确认和错误处理
• 实时进度监控
• 多航点批量上传: 一次性上传多个航点，具有原子性

2.3 端口兼容性

• 一端口一应用: 一个UDP端口通常只能被一个应用独占接收使用
• 多应用并行: 为每个应用分配不同端口，例如14540、14550，或自定义UDP/TCP端口
• 示例建议: 将QGroundControl使用一个端口，MAVSDK-Python与pymavlink分别使用其他端口

连接配置

# QGroundControl 通常使用一个端口（示例：14540）
# MAVSDK-Python 连接使用另一个端口（示例：14550）
System().connect(system_address="udp://:14550")

# pymavlink 再使用第三个端口（示例：14600，或任意未占用端口）
PyMAVLinkWrapper("drone1")

# 关键点：不同应用使用不同UDP/TCP端口，避免端口冲突

实际使用场景

PX4飞控
  ├─ 14540/udp ←→ QGroundControl
  ├─ 14550/udp ←→ MAVSDK-Python应用
  └─ 14600/udp ←→ pymavlink应用（或用户自定义端口/使用tcp）

3. MAVLink 协议流程

3.1 任务上传流程

1. 发送MISSION_COUNT消息告知航点数量
2. 等待飞控发送MISSION_REQUEST_INT或MISSION_REQUEST请求
3. 根据请求类型发送对应的航点消息：
   • MISSION_REQUEST_INT → MISSION_ITEM_INT
   • MISSION_REQUEST → MISSION_ITEM
4. 等待MISSION_ACK确认消息

3.2 多航点批量上传

• 原子性: 所有航点要么全部成功，要么全部失败
• 高效性: 一次连接完成所有航点上传
• 一致性: 保证航点序列的完整性
• 性能: 减少网络往返次数

3.3 消息类型

• MISSION_COUNT: 任务数量
• MISSION_REQUEST_INT: 航点请求（INT格式）
• MISSION_REQUEST: 航点请求（普通格式）
• MISSION_ITEM_INT: 航点数据（INT格式）
• MISSION_ITEM: 航点数据（普通格式）
• MISSION_ACK: 任务确认
• MISSION_CURRENT: 当前航点

3.4 关键特性

• 异步操作: 支持异步连接和任务发送，所有方法需要使用await关键字
• 双格式支持: 自动处理MISSION_REQUEST和MISSION_REQUEST_INT消息
• 系统ID处理: 自动检测和设置系统ID和组件ID
• 错误处理: 完善的超时和重试机制
• QGC兼容: 与QGroundControl完全兼容，使用相同的MAVLink消息格式
• 灵活配置: 支持所有MAVLink航点参数

3.5 使用注意事项

• 确保无人机已连接并GPS定位正常
• 检查MAVLink连接参数，默认使用udp:127.0.0.1:14540，可根据需要修改
• 必须先调用await connect()方法建立连接
• 航点坐标必须在有效范围内，高度设置要合理（避免碰撞）
• 接受半径要适中（避免过严导致盘旋）
• 大量航点（>100个）建议分批处理

4. 航点格式

waypoint = {
    'sequence': 0,           # 序列号
    'command': 16,          # MAV_CMD_NAV_WAYPOINT
    'frame': 3,             # MAV_FRAME_GLOBAL_RELATIVE_ALT
    'current': 0,           # 是否为当前航点
    'autocontinue': 1,      # 自动继续
    'param1': 0,            # 停留时间
    'param2': 5,            # 接受半径
    'param3': 0,            # 飞越半径
    'param4': 0,            # 航向角
    'param5': lat,          # 纬度
    'param6': lon,          # 经度
    'param7': alt,          # 高度
    'mission_type': 0       # 任务类型
}

5. MAVLink 命令说明

5.1 MAVLink 命令参考

5.2 坐标系说明

6. 使用示例

6.1 安装依赖

首先需要安装pymavlink库：

pip install pymavlink

6.2 基本使用 - 创建航点任务

# 创建发送器
sender = PyMAVLinkWrapper("drone1")

# 连接到飞控
await sender.connect("udp:127.0.0.1:14540")

# 创建正方形航点任务
side_length = 0.001  # 约100米
base_lat, base_lon, base_alt = 39.9042, 116.4074, 50.0  # 基准位置

# 第一个航点：基准位置
sender.add_waypoint(base_lat, base_lon, base_alt)
# 第二个航点：向东
sender.add_waypoint(base_lat + side_length, base_lon, base_alt)
# 第三个航点：东北
sender.add_waypoint(base_lat + side_length, base_lon + side_length, base_alt)
# 第四个航点：北
sender.add_waypoint(base_lat, base_lon + side_length, base_alt)

# 发送任务
success = await sender.send_mission()

6.3 高级配置

自定义航点参数

# 创建发送器并连接
sender = PyMAVLinkWrapper("drone1")
await sender.connect("udp:127.0.0.1:14540")

# 自定义航点参数
sender.add_waypoint(
    lat=39.9042, 
    lon=116.4074, 
    alt=50.0, 
    acceptance_radius=10  # 接受半径10米
)

不同命令类型示例

标准航点 (MAV_CMD_NAV_WAYPOINT)

# 创建发送器并连接
sender = PyMAVLinkWrapper("drone1")
await sender.connect("udp:127.0.0.1:14540")

# 标准航点，默认命令16
sender.add_waypoint(lat=39.9042, lon=116.4074, alt=50.0)

返回起飞点 (MAV_CMD_NAV_RETURN_TO_LAUNCH)

# 返回起飞点，命令20
sender.add_waypoint(
    lat=0, lon=0, alt=0,  # 坐标会被忽略
    command=20,  # MAV_CMD_NAV_RETURN_TO_LAUNCH
    frame=3
)

降落命令 (MAV_CMD_NAV_LAND)

# 降落命令，命令21
sender.add_waypoint(
    lat=39.9042, lon=116.4074, alt=0,
    command=21,  # MAV_CMD_NAV_LAND
    frame=3,
    hold_time=10.0  # 中止高度10米
)

起飞命令 (MAV_CMD_NAV_TAKEOFF)

# 起飞命令，命令22
sender.add_waypoint(
    lat=39.9042, lon=116.4074, alt=50.0,
    command=22,  # MAV_CMD_NAV_TAKEOFF
    frame=3,
    hold_time=15.0,  # 最小俯仰角15度
    yaw=90.0   # 偏航角90度
)

无限盘旋 (MAV_CMD_NAV_LOITER_UNLIM)

# 无限盘旋，命令84
sender.add_waypoint(
    lat=39.9042, lon=116.4074, alt=50.0,
    command=84,  # MAV_CMD_NAV_LOITER_UNLIM
    frame=3,
    hold_time=50.0  # 盘旋半径50米
)

指定时间盘旋 (MAV_CMD_NAV_LOITER_TIME)

# 指定时间盘旋，命令86
sender.add_waypoint(
    lat=39.9042, lon=116.4074, alt=50.0,
    command=86,  # MAV_CMD_NAV_LOITER_TIME
    frame=3,
    hold_time=60.0,  # 盘旋时间60秒
    acceptance_radius=30.0   # 盘旋半径30米
)

调查任务示例

# 创建发送器并连接
sender = PyMAVLinkWrapper("drone1")
await sender.connect("udp:127.0.0.1:14540")

# 调查任务 - 多航点批量上传
survey_waypoints = [
    # 起飞点
    {"lat": 39.9042, "lon": 116.4074, "alt": 0, "command": 22, "hold_time": 0.0},
    # 调查点1
    {"lat": 39.9052, "lon": 116.4084, "alt": 50.0, "command": 16, "hold_time": 30.0},
    # 调查点2
    {"lat": 39.9062, "lon": 116.4094, "alt": 50.0, "command": 16, "hold_time": 30.0},
    # 调查点3
    {"lat": 39.9072, "lon": 116.4104, "alt": 50.0, "command": 16, "hold_time": 30.0},
    # 返回起飞点
    {"lat": 0, "lon": 0, "alt": 0, "command": 20, "hold_time": 0.0}
]

# 批量添加调查航点
for wp in survey_waypoints:
    sender.add_waypoint(
        lat=wp["lat"],
        lon=wp["lon"],
        alt=wp["alt"],
        command=wp["command"],
        hold_time=wp["hold_time"],
        acceptance_radius=5.0
    )

# 一次性上传所有调查航点
success = await sender.send_mission()

参考文档

• PX4 MAVLink 通信概述（MAVLink on PX4）
• QGroundControl 通信设置（UDP 连接）
• MAVSDK 连接URL格式（System.connect）
• pymavlink 文档与连接示例

WSL2 环境下 PX4 SITL 多端口 MAVLink 配置指南

1. 概述

本文档介绍在 WSL2 环境下配置 PX4 1.16.0 stable 版本与 AirSim 1.8.1 进行软件在环（SITL）仿真时，如何通过修改 rcS 启动脚本实现自动开启多个 MAVLink 端口，解决多个设备（如 QGroundControl, MAVROS, MAVSDK 等）同时连接飞控的问题。

2. 环境准备

2.1 系统要求

• 操作系统：Windows 10/11 with WSL2
• WSL2 发行版：Ubuntu 20.04 或更高版本
• PX4 版本：1.16.0 stable
• AirSim 版本：1.8.1
• PX4 源码路径：C:\Users\Administrator\PX4\PX4-Autopilot

3. 自动端口开放配置

3.1 问题背景

在 AirSim 的 SITL 文档中，开启 PX4 SITL 的命令为 make px4_sitl_default none_iris。在此模式下，PX4 默认绑定的地址（如 UDP 14550/14580 或 TCP 4560 等）仅能同时接受一个连接。

当需要使用多个设备（如 QGroundControl, MAVSDK, MAVROS 等）同时连接 SITL 飞控时，需要在 PX4 命令行中手动执行开启端口的命令（如 mavlink start -u 14550 -o 14550 等）才能打开另一个端口。

经测试，可能因 WSL2 的网络配置特殊性，在 WSL2 上同时部署 MAVLink Router 或 MAVProxy 均不能实现在真实的机载计算机上实现的串口/端口路由功能。当需要使用多个设备连接 SITL 飞控时，每次启动飞控就需要手动开启多个端口，特别麻烦。

3.2 修改 rcS 启动脚本实现自动开启多个端口

通过在 PX4 的启动脚本 rcS 中添加 mavlink start 命令，可以在每次编译和启动时自动开启多个 MAVLink 端口，实现多个设备同时连接。这在需要同步进行可视化或多智能体控制无人机的仿真场景下十分有用。如果某个设备只需要收到 MAVLink 数据，而无需发出控制命令到飞控，则仅需通过 QGroundControl 的 MAVLink 转发或其他方式转发即可。

文件路径：C:\Users\Administrator\PX4\PX4-Autopilot\ROMFS\px4fmu_common\init.d-posix\rcS

在 rcS 文件中添加以下内容（建议在文件末尾，MAVLink 相关配置部分之后添加）：

# 开启额外的 MAVLink UDP 端口
# 端口 14550：用于 QGroundControl 地面站连接
mavlink start -u 14550 -o 14550 -t 192.168.31.88 -r 1000000

# 端口 14560：用于 MAVROS 连接
mavlink start -u 14560 -o 14560 -t 192.168.31.77 -r 1000000

# 端口 14540：用于 MAVSDK-Python 或其他应用连接
mavlink start -u 14540 -o 14540 -t 192.168.31.66 -r 1000000

命令参数说明：

• -u <端口>：指定本地 UDP 端口，PX4 在此端口上监听和发送数据
• -o <端口>：指定远程 UDP 端口（通常与本地端口相同）
• -t <IP地址>：指定伙伴 IP 地址（可选，用于指定数据发送的目标 IP）。使用此参数可以将 MAVLink 数据直接发送到指定 IP 地址的设备，实现与多个不同设备的定向连接
• -r <速率>：设置最大发送数据速率（字节/秒），1000000 表示 1MB/s

3.3 验证端口开启状态

启动 PX4 SITL 后，可以通过以下方式验证端口是否成功开启：

# 在 PX4 SITL 的命令行中执行
mavlink status

# 或使用 netstat 检查端口监听状态
netstat -an | grep -E "14540|14550|14560"

# 或使用 ss 命令（Linux）
ss -ulnp | grep -E "14540|14550|14560"

如果配置正确，应该能看到多个 UDP 端口处于监听状态，不同的应用可以连接到不同的端口，实现同时访问飞控。

参考文档

• PX4 官方文档 - SITL 仿真
• AirSim 官方文档
• WSL2 网络配置指南

MAVLink Router-安装与使用指南

1. 概述

MAVLink Router 是一个用于路由 MAVLink 消息的工具，可以在不同的端点（UART、UDP、TCP）之间转发 MAVLink 数据包。在某些 Ubuntu 版本（如 Ubuntu 20.04 Focal）中，可能无法通过 apt 直接安装 mavlink-router 包，此时需要从源码编译安装。

2. 为什么使用 MAVLink Router

2.1 优势

MAVLink Router 解决了无人机系统中多个应用同时访问飞控的问题，优势包括：

多应用并行访问

在无人机系统中，通常需要多个应用同时与飞控通信：

• 地面站软件（如 QGroundControl）需要实时显示飞行状态
• 任务规划工具（如 pymavlink / MAVSDK-Python）需要发送航点任务
• 数据记录工具需要记录飞行数据
• 自定义应用需要执行特定功能

由于串口或单一网络连接通常只能被一个应用独占，MAVLink Router 可以将一个物理连接（如串口）转换为多个网络端点，让多个应用同时访问飞控数据。

注意：QGroundControl 的 MAVLink 转发功能可以实现从飞控接收数据，但无法向飞控发送数据，会提示超时错误。如果需要双向通信（如发送航点任务、参数设置等），必须使用 MAVLink Router、MAVProxy 或其他支持双向转发的工具。

协议转换与桥接

MAVLink Router 支持在不同传输介质之间桥接：

• UART ↔ UDP/TCP：将串口数据转换为网络数据，方便远程访问
• UDP ↔ TCP：在不同网络协议之间转换
• 多端点分发：将一条数据流分发到多个目标

消息路由与过滤

• 智能路由：根据系统 ID 和组件 ID 智能路由消息，避免消息循环
• 消息过滤：可以过滤特定类型的消息，减少网络负载
• 消息去重：自动去除重复消息，提高效率

灵活的网络配置

• 支持 IPv4 和 IPv6
• 支持单播、多播和广播
• 支持客户端和服务器模式
• 自动检测链路本地地址

2.2 典型部署场景

MAVLink Router 在实际应用中有多种部署方式，可以根据具体需求选择机载部署或地面站部署。以下是一个综合的典型部署架构：

graph TB
    FC[飞控<br/>PX4/ArduPilot] -->|UART<br/>/dev/ttyACM0| MR[MAVLink Router]
    
    subgraph CC["机载计算机"]
        MR -->|UDP:14550| QGC[QGroundControl<br/>地面站]
        MR -->|UDP:14540| MP[任务规划工具<br/>pymavlink / MAVSDK-Python]
        MR -->|UDP:14560| MAVROS[MAVROS<br/>ROS/ROS2 节点]
        MR -->|TCP:5760| REMOTE[其他应用<br/>可选]
        
        MAVROS --> APP1[ROS/ROS2 节点]
        MP --> APP2[Python 脚本]
        REMOTE --> APP3[数据记录工具]
    end
    
    
    style FC fill:#e1f5ff
    style CC fill:#fff4e1,stroke:#ff9800,stroke-width:3px
    style MR fill:#e8f5e9
    style QGC fill:#f3e5f5
    style MAVROS fill:#f3e5f5
    style MP fill:#f3e5f5

飞控连接（UART）

飞控通过 UART（如 /dev/ttyACM0）连接到机载计算机，波特率通常为 1500000（1.5Mbps）或 921600。需要确保串口权限正确配置（将用户加入 dialout 组）。一个串口只能被一个 MAVLink Router 实例使用。如果需要在机载计算机和地面站同时访问，需要在机载计算机上部署 MAVLink Router。串口连接延迟低，适合实时控制应用。

QGroundControl 连接（UDP:14550）

QGroundControl 通过 UDP:14550 连接到 MAVLink Router，可以在同一网络内或通过 WiFi/4G 远程连接。QGroundControl 主要用于实时监控飞行状态，接收飞控数据。如果需要发送航点任务，建议使用 pymavlink 或 MAVSDK-Python（通过其他端口）。

注意：QGroundControl 的 MAVLink 转发功能只能接收数据，无法发送数据，会提示超时错误。如果需要双向通信（如发送航点任务、参数设置等），必须使用 MAVLink Router、MAVProxy 或其他支持双向转发的工具。

任务规划工具连接（UDP:14540）

pymavlink 或 MAVSDK-Python 通过 UDP:14540 连接到 MAVLink Router，支持双向通信，可以发送航点任务、参数设置等。注意：一个 UDP 端口通常只能被一个应用独占接收使用。如果需要多个应用并行，为每个应用分配不同端口（如 14540、14550、14600）。pymavlink 和 MAVSDK-Python 可以同时使用，但需要连接不同的端口。

配置示例：

# MAVLink Router 配置
mavlink-routerd /dev/ttyACM0:1500000 -e 127.0.0.1:14540 -e 127.0.0.1:14550

# pymavlink 连接
connection = mavutil.mavlink_connection('udp:127.0.0.1:14540')

# MAVSDK-Python 连接
drone = System()
await drone.connect(system_address="udp://:14550")

MAVROS 连接（UDP:14560）

MAVROS 通过 UDP:14560 连接到 MAVLink Router。MAVROS 是 ROS/ROS2 与 MAVLink 协议之间的桥接节点。配置方式如下：

1. MAVLink Router 配置：

mavlink-routerd /dev/ttyACM0:1500000 -e 127.0.0.1:14560

2. MAVROS 启动（使用命令行参数）：

ros2 launch mavros px4.launch fcu_url:=udp://:14560@127.0.0.1:14560

如果使用 TCP 连接（MAVLink Router 默认监听 TCP:5760），可以使用：

ros2 launch mavros px4.launch fcu_url:=tcp://127.0.0.1:5760

127.0.0.1:14560 配置说明：该配置开放给本地的 MAVROS 使用。当外部连接到伴随计算机的 14560 端口时，MAVLink Router 会自动转发 MAVLink 消息到飞控，实现通过 MAVROS 对飞控的直接操作。这样既支持本地 MAVROS 节点访问，也支持远程通过该端口连接并控制飞控。

使用场景：

• ROS/ROS2 节点通信：通过 MAVROS 发布/订阅飞控话题
• 传感器融合：将机载传感器数据与飞控数据融合
• 自主导航：ROS 导航栈通过 MAVROS 控制无人机
• 视觉处理：视觉 SLAM 节点获取位置信息并发送控制指令

注意事项：

• MAVROS 需要独占一个 UDP 端口
• 可以同时运行多个 ROS 节点共享飞控数据
• 与 ROS 生态系统无缝集成，支持标准的 ROS 话题和服务接口

其他应用连接（TCP:5760）

TCP 服务器默认启用，监听端口 5760，支持多个 TCP 客户端同时连接。TCP 连接比 UDP 更可靠，但延迟略高，适合数据记录、远程监控等对可靠性要求高的应用。可以动态连接和断开，不影响其他端点。

3. 安装方式选择

3.1 使用包管理器安装

在支持的 Ubuntu 版本中，可以直接使用包管理器安装：

sudo apt update
sudo apt install mavlink-router

3.2 从源码编译安装

如果在 Ubuntu Focal 等版本中遇到以下错误：

E: Unable to locate package mavlink-router

则需要从源码编译安装。

4. 从源码编译安装

4.1 安装依赖

首先安装编译所需的依赖包：

sudo apt install git meson ninja-build pkg-config gcc g++ systemd

4.2 克隆源码

git clone https://github.com/mavlink-router/mavlink-router.git
cd mavlink-router
git submodule update --init --recursive

4.3 检查并安装 Meson

检查 Meson 版本：

meson --version

如果版本低于 0.55，需要升级：

pip3 install meson==0.55

4.4 编译和安装

meson setup build . && ninja -C build
sudo ninja -C build install

5. 验证安装

5.1 验证安装成功

安装完成后，可以通过以下命令验证 MAVLink Router 是否安装成功：

mavlink-routerd --help

如果能够正常显示帮助信息，说明安装成功。

5.2 配置串口权限 - 将用户加入 dialout 组

在使用 MAVLink Router 连接飞控串口之前，需要配置串口权限。

Linux 下所有串口 /dev/ttyACM*，/dev/ttyUSB* 默认属于 dialout 组。将用户加入该组可以一次性解决所有串口权限问题：

sudo usermod -a -G dialout $USER

重要：执行上述命令后，必须重启或重新登录系统才能生效：

reboot

重启后检查权限：

ls -l /dev/ttyACM0

应该能看到类似输出：

crw-rw---- 1 root dialout ...

此时用户已经在 dialout 组里，MAVLink Router 就能访问串口了。

6. 命令行使用方法

6.1 参数说明

• TCP 服务器默认启用（监听端口 5760）
• TCP 和 UDP 端点可以多次添加
• 使用 -e 选项添加的 UDP 端点以普通模式启动（发送数据到指定地址和端口）
• 最后一个参数（无键值）可以是 UART 设备或 UDP 连接，UDP 端点将以服务器模式启动（等待传入连接）

6.2 使用示例

从 UART 路由到 UDP

将 MAVLink 数据包从 UART ttyS1 路由到 2 个 UDP 端点：

mavlink-routerd -e 192.168.7.1:14550 -e 127.0.0.1:14550 /dev/ttyS1:1500000

其中 1500000 是 UART 波特率。

从 UDP 路由到 UDP

也可以从传入的 UDP 连接路由 MAVLink 数据包：

mavlink-routerd -e 192.168.7.1:14550 -e 127.0.0.1:14550 0.0.0.0:24550

IPv6 地址格式

IPv6 地址必须用方括号括起来，例如：

mavlink-routerd -e [::1]:14550 /dev/ttyS1:1500000

单播和多播地址都能正确处理，链路本地地址的接口会自动检测。

7. 详细说明

7.1 端点类型

MAVLink Router 支持三种基本端点类型：UART, UDP 链接和 TCP 客户端。此外，它还可以作为 TCP 服务器为动态客户端提供服务（除非明确禁用）。

7.1.1 UART

• 用途：用于遥测无线电或其他串行链接
• 配置：UART 设备路径/名称和波特率
• 行为：无需等待传入数据即可接收和发送数据

7.1.2 UDP

• 配置：模式（客户端或服务器）、IP 地址和端口
• 客户端模式行为：端点配置有目标 IP 和端口组合。启动后可以直接发送 MAVLink 消息，但只有在远程端发送第一条消息后才会接收数据（否则远程端不知道我们的 IP 和端口）
• 服务器模式行为：端点配置有监听端口和 IP 地址。启动后可以直接接收消息，但只有在收到第一条消息后才能发送消息（否则不知道远程 IP 和端口）。MAVLink 消息总是发送到最后一条传入消息的 IP 和端口

7.1.3 TCP 客户端

• 配置：目标 IP 地址和端口，断开连接时的重连间隔
• 行为：TCP 会话建立后立即接收和发送数据

7.2 端点定义

端点通过以下方式创建：

1. 在配置文件中定义端点
2. 通过相应的命令行选项定义端点
3. TCP 客户端连接到 TCP 服务器端口

端点在以下情况下被销毁：

1. TCP 客户端从 TCP 服务器端口断开连接
2. MAVLink Router 终止

（这意味着 UART、UDP 和 TCP 客户端端点在运行期间永远不会被销毁）

7.3 消息路由

7.3.1 基本路由规则

一般来说，在一个端点上接收的每条消息都会被传递到所有已看到该目标系统/组件的端点。如果是广播消息，则传递到所有端点。消息永远不会发送回它来源的同一端点。

7.3.2 详细路由规则

1. 每个端点会记住在其整个生命周期内从哪些系统（系统 ID 和组件 ID）接收过消息
2. 在一个端点上接收的消息会被提供给除接收端点外的所有端点。端点将：
   • 如果消息的发送者地址在此端点的已连接系统列表中，则拒绝该消息（防止消息循环）
   • 根据出站消息过滤器拒绝消息（如果启用）
   • 如果消息的目标是此端点已连接系统列表中的任何系统，则接受该消息。广播规则在检查目标是否可通过此端点到达时适用。没有目标地址的消息视为广播
   • 如果已连接系统列表为空，则只发送系统 ID 广播消息，不发送组件 ID 广播（因为目标系统未知是否可通过此端点到达）
   • 拒绝所有其他消息

7.4 消息过滤

在每个端点上有两个位置可以过滤消息：

• In（入站）：在此端点上从外部接收的消息在路由到其他端点之前，根据相应的过滤规则被丢弃或允许
• Out（出站）：在传输之前，根据端点的过滤规则丢弃或允许消息。这是在内部路由之后（见上面的路由规则章节）

消息过滤器可以基于以下消息标识符之一：

• MsgId：基于 MAVLink 消息 ID（如 HEARTBEAT）过滤消息
• SrcSys：基于 MAVLink 源系统 ID 过滤消息
• SrcComp：基于 MAVLink 源组件 ID 过滤消息

消息过滤器可以是阻止列表或允许列表：

• Block（阻止）：丢弃所有匹配相应标识符的消息（允许所有其他消息）
• Allow（允许）：允许所有匹配相应标识符的消息（丢弃所有其他消息）

注意：在同一标识符上同时使用 Allow 和 Block 过滤器没有意义，但在不同标识符上使用它们可能很有用（例如，只允许特定的出站系统 ID，并阻止该系统发送一些不需要的消息 ID）。

7.5 消息去重

如果启用，每条传入消息都会被检查，是否在过去 DeduplicationPeriod 毫秒内已经收到过另一个副本。如果已知，消息将被丢弃，就像从未收到过一样，该消息的超时计数器将被重置。消息通过包括其标头在内的完整 MAVLink 消息的 std::hash 值进行标识。

只要在配置的周期内没有收到具有完全相同标头序列号和内容的消息，一切正常。最关键的消息是心跳，因为它主要包含静态数据。因此，周期短于最快静态消息的更新周期在任何情况下都可以（对于 1 Hz 心跳，小于 1000 毫秒）。

7.6 端点组

可以将多个端点配置为在同一端点组中。同一组中的端点将共享相同的已连接系统列表。

当使用两个（或更多）并行数据链路时（例如 LTE 和遥测无线电），端点必须在两侧都分组。否则，由于路由规则 1，一个链路将不再被使用。

7.7 消息嗅探

可以通过设置 SnifferSysID 来定义嗅探器。这将把所有流量转发到连接了此 MAVLink 系统 ID 的端点。这可用于记录或查看流经 mavlink-router 的所有消息。

参考文档

• MAVLink Router GitHub 仓库

使用 pyulog 分析 PX4 飞控日志

本文档以实际案例 log_284_2025-11-25-01-15-04.ulg 为例，系统介绍如何使用 Python 的 pyulog 库分析 PX4 飞控生成的 ULog 格式日志文件，通过提取关键数据并生成可视化图表来诊断飞行过程中的潜在问题。

本案例展示了系统性的日志分析流程：从数据探索（识别关键主题）→ 数据提取与预处理 → 理解数据意义 → 可视化分析（生成专业图表）→ 问题诊断（建立因果链条）→ 解决方案（问题排查清单）。这种方法具有系统性、可复现性和实用性，适用于各种飞行日志分析场景，能够帮助开发者快速定位和解决飞行过程中的问题，提升飞行器的安全性和性能。

你可以从以下链接下载该日志文件进行实践：

• log_284_2025-11-25-01-15-04.ulg

1. 从 ULog 中提取有效的数据条目

ULog 是 PX4 飞控系统采用的二进制日志格式，记录了飞行过程中的传感器数据、系统状态、控制指令等丰富信息。要解析 ULog 文件，需要使用 pyulog 库。

1.1 安装 pyulog

首先，确保已安装 Python 环境（推荐 Python 3.7+），然后使用 pip 安装 pyulog：

pip install pyulog

同时，为了进行数据分析和可视化，还需要安装以下依赖：

pip install numpy matplotlib pandas

1.2 读取 ULog 文件

使用 pyulog 读取 ULog 文件的基本方法：

from pyulog import ULog

# 读取 ULog 文件（使用实际案例日志）
ulog = ULog('log_284_2025-11-25-01-15-04.ulg')

# 获取所有消息名称（uORB 主题）
message_names = ulog.get_message_names()
print(f"日志中包含的消息类型: {message_names}")

1.3 解析日志

在实际分析中，我们首先需要了解日志中包含哪些主题，然后根据分析目标识别关键主题。以下脚本可以帮助我们系统地探索日志内容：

from pyulog import ULog
import sys

# 读取日志文件
ulog_file = 'log_284_2025-11-25-01-15-04.ulg'
ulog = ULog(ulog_file)

# 获取所有消息名称
message_names = [dataset.name for dataset in ulog.data_list]
print(f"日志文件: {ulog_file}")
print(f"总共包含 {len(message_names)} 个主题\n")
print("=" * 80)
print("所有主题列表:")
print("=" * 80)

# 按类别分类主题
categories = {
    '飞行状态': ['vehicle_status', 'commander_state', 'vehicle_control_mode'],
    '姿态控制': ['vehicle_attitude', 'vehicle_attitude_setpoint', 'vehicle_rates_setpoint'],
    '位置导航': ['vehicle_local_position', 'vehicle_global_position', 'vehicle_gps_position'],
    'EKF2 融合': ['estimator_local_position', 'estimator_status', 'estimator_innovations'],
    '传感器数据': ['sensor_combined', 'sensor_accel', 'sensor_gyro', 'sensor_mag'],
    '电机/舵机': ['actuator_outputs', 'actuator_controls'],
    '外部定位': ['vehicle_vision_position', 'vehicle_odometry'],
    '其他': []
}

# 分类显示
for category, keywords in categories.items():
    matched = []
    for name in sorted(message_names):
        if any(keyword in name for keyword in keywords):
            matched.append(name)
        elif category == '其他' and not any(name in m for m in categories.values() if m != categories['其他']):
            if name not in [item for sublist in [v for k, v in categories.items() if k != '其他'] for item in sublist]:
                matched.append(name)
    
    if matched:
        print(f"\n【{category}】")
        for name in matched:
            try:
                dataset = ulog.get_dataset(name)
                field_count = len(dataset.data.keys())
                sample_count = len(list(dataset.data.values())[0]) if dataset.data else 0
                print(f"  - {name:40s} (字段数: {field_count:3d}, 样本数: {sample_count:6d})")
            except:
                print(f"  - {name:40s} (无法读取)")

运行后返回

日志文件: log_284_2025-11-25-01-15-04.ulg
总共包含 97 个主题

================================================================================
所有主题列表:
================================================================================

【飞行状态】
  - vehicle_control_mode                     (字段数:  16, 样本数:    114)
  - vehicle_status                           (字段数:  39, 样本数:    114)

【姿态控制】
  - vehicle_attitude                         (字段数:  11, 样本数:   1124)
  - vehicle_attitude_setpoint                (字段数:  11, 样本数:   1124)
  - vehicle_rates_setpoint                   (字段数:   8, 样本数:   2807)

【位置导航】
  - vehicle_local_position                   (字段数:  55, 样本数:    563)
  - vehicle_local_position_setpoint          (字段数:  15, 样本数:    563)

【EKF2 融合】
  - estimator_innovations                    (字段数:  33, 样本数:    112)
  - estimator_innovations                    (字段数:  33, 样本数:    112)
  - estimator_local_position                 (字段数:  55, 样本数:    112)
  - estimator_local_position                 (字段数:  55, 样本数:    112)
  - estimator_status                         (字段数:  40, 样本数:    282)
  - estimator_status                         (字段数:  40, 样本数:    282)
  - estimator_status_flags                   (字段数:  71, 样本数:     72)
  - estimator_status_flags                   (字段数:  71, 样本数:     72)

【传感器数据】
  - sensor_accel                             (字段数:  12, 样本数:     56)
  - sensor_accel                             (字段数:  12, 样本数:     56)
  - sensor_combined                          (字段数:  14, 样本数:  11528)
  - sensor_gyro                              (字段数:  12, 样本数:     56)
  - sensor_gyro                              (字段数:  12, 样本数:     56)
  - sensor_mag                               (字段数:   8, 样本数:     56)

【电机/舵机】
  - actuator_outputs                         (字段数:  18, 样本数:      1)
  - actuator_outputs                         (字段数:  18, 样本数:      1)
  - actuator_outputs                         (字段数:  18, 样本数:      1)

【其他】
  - action_request                           (字段数:   4, 样本数:      5)
  - actuator_armed                           (字段数:   8, 样本数:    114)
  - actuator_motors                          (字段数:  15, 样本数:    563)
  - battery_status                           (字段数:  52, 样本数:    282)
  - can_interface_status                     (字段数:   5, 样本数:    551)
  - can_interface_status                     (字段数:   5, 样本数:    551)
  - config_overrides                         (字段数:   6, 样本数:    115)
  - control_allocator_status                 (字段数:  26, 样本数:    282)
  - cpuload                                  (字段数:   3, 样本数:    113)
  - distance_sensor_mode_change_request      (字段数:   2, 样本数:      1)
  - estimator_aid_src_ev_hgt                 (字段数:  14, 样本数:    112)
  - estimator_aid_src_ev_hgt                 (字段数:  14, 样本数:    112)
  - estimator_aid_src_ev_pos                 (字段数:  21, 样本数:    112)
  - estimator_aid_src_ev_pos                 (字段数:  21, 样本数:    112)
  - estimator_aid_src_ev_yaw                 (字段数:  14, 样本数:    112)
  - estimator_aid_src_ev_yaw                 (字段数:  14, 样本数:    112)
  - estimator_aid_src_gravity                (字段数:  28, 样本数:    112)
  - estimator_aid_src_gravity                (字段数:  28, 样本数:    112)
  - estimator_aid_src_mag                    (字段数:  28, 样本数:    112)
  - estimator_aid_src_mag                    (字段数:  28, 样本数:    112)
  - estimator_attitude                       (字段数:  11, 样本数:    112)
  - estimator_attitude                       (字段数:  11, 样本数:    112)
  - estimator_event_flags                    (字段数:  20, 样本数:     60)
  - estimator_event_flags                    (字段数:  20, 样本数:     60)
  - estimator_innovation_test_ratios         (字段数:  33, 样本数:    112)
  - estimator_innovation_test_ratios         (字段数:  33, 样本数:    112)
  - estimator_innovation_variances           (字段数:  33, 样本数:    112)
  - estimator_innovation_variances           (字段数:  33, 样本数:    112)
  - estimator_odometry                       (字段数:  28, 样本数:    112)
  - estimator_odometry                       (字段数:  28, 样本数:    112)
  - estimator_selector_status                (字段数:  46, 样本数:    589)
  - estimator_sensor_bias                    (字段数:  32, 样本数:     56)
  - estimator_sensor_bias                    (字段数:  32, 样本数:     56)
  - estimator_states                         (字段数:  52, 样本数:    112)
  - estimator_states                         (字段数:  52, 样本数:    112)
  - estimator_status_flags                   (字段数:  71, 样本数:     72)
  - estimator_status_flags                   (字段数:  71, 样本数:     72)
  - event                                    (字段数:  29, 样本数:     25)
  - failsafe_flags                           (字段数:  40, 样本数:    104)
  - failure_detector_status                  (字段数:  11, 样本数:    114)
  - home_position                            (字段数:  13, 样本数:      3)
  - input_rc                                 (字段数:  30, 样本数:    112)
  - magnetometer_bias_estimate               (字段数:  21, 样本数:      3)
  - manual_control_setpoint                  (字段数:  17, 样本数:    282)
  - manual_control_switches                  (字段数:  15, 样本数:     58)
  - navigator_status                         (字段数:   3, 样本数:    114)
  - parameter_update                         (字段数:   9, 样本数:      2)
  - position_setpoint_triplet                (字段数:  70, 样本数:      1)
  - px4io_status                             (字段数:  77, 样本数:     57)
  - rate_ctrl_status                         (字段数:   5, 样本数:    282)
  - rtl_status                               (字段数:   6, 样本数:     28)
  - rtl_time_estimate                        (字段数:   4, 样本数:     28)
  - sensor_baro                              (字段数:   6, 样本数:     56)
  - sensor_selection                         (字段数:   3, 样本数:      4)
  - sensors_status_imu                       (字段数:  35, 样本数:    282)
  - system_power                             (字段数:  17, 样本数:    112)
  - takeoff_status                           (字段数:   3, 样本数:     11)
  - telemetry_status                         (字段数:  38, 样本数:     56)
  - timesync_status                          (字段数:   6, 样本数:     56)
  - trajectory_setpoint                      (字段数:  15, 样本数:    282)
  - transponder_report                       (字段数:  40, 样本数:      1)
  - vehicle_acceleration                     (字段数:   5, 样本数:   1124)
  - vehicle_air_data                         (字段数:   9, 样本数:    282)
  - vehicle_angular_velocity                 (字段数:   8, 样本数:   2807)
  - vehicle_command                          (字段数:  15, 样本数:      3)
  - vehicle_command_ack                      (字段数:   8, 样本数:      4)
  - vehicle_constraints                      (字段数:   4, 样本数:     56)
  - vehicle_imu                              (字段数:  16, 样本数:    112)
  - vehicle_imu                              (字段数:  16, 样本数:    112)
  - vehicle_imu_status                       (字段数:  32, 样本数:     56)
  - vehicle_imu_status                       (字段数:  32, 样本数:     56)
  - vehicle_land_detected                    (字段数:  13, 样本数:     62)
  - vehicle_local_position_setpoint          (字段数:  15, 样本数:    563)
  - vehicle_magnetometer                     (字段数:   7, 样本数:    112)
  - vehicle_thrust_setpoint                  (字段数:   5, 样本数:   2807)
  - vehicle_torque_setpoint                  (字段数:   5, 样本数:   2807)

我们可以清楚地看到日志中包含的所有主题，并根据分析目标选择关键主题。对于 log_284 这个案例，在此文档中我们特别关注：

• 飞行模式：vehicle_status - 了解何时切换到 Position 模式
• EKF2 融合状态：estimator_status - 检查融合器是否正常工作
• 位置估计：estimator_local_position 和 vehicle_local_position - 对比分析位置跳变问题
• 姿态数据：vehicle_attitude - 评估飞行器姿态稳定性

2. 关键数据条目

基于 log_284 案例中识别出的关键主题，本节详细讲解每个数据条目的物理意义、数据单位和提取方法。

2.1 飞行模式与系统状态

vehicle_status - 飞行器系统状态，包含飞行模式、安全状态等关键信息。

vehicle_status = ulog.get_dataset('vehicle_status')
timestamps = np.array(vehicle_status.data['timestamp']) / 1e6
nav_state = np.array(vehicle_status.data['nav_state'])
arming_state = np.array(vehicle_status.data['arming_state'])

# 查找模式切换时间点
mode_changes = np.where(np.diff(nav_state) != 0)[0]
for idx in mode_changes:
    print(f"时间 {timestamps[idx]:.2f} s: 切换到模式 {nav_state[idx+1]}")

2.2 姿态控制数据

vehicle_attitude - 飞行器姿态信息，使用四元数表示。

vehicle_attitude_setpoint - 姿态设定值，用于评估控制器跟踪性能。

2.3 位置与导航数据

vehicle_local_position - 本地坐标系（NED）下的位置、速度和加速度信息，这是对外发布的位置估计。

estimator_local_position - EKF2 内部位置估计，用于分析融合算法性能。字段与 vehicle_local_position 相同，但这是 EKF2 的原始输出，未经过位置控制器的处理。

注意：在 log_284 案例中，对比这两个位置数据源可以发现 Position 模式下的位置跳变问题。

2.4 EKF2 融合状态数据

estimator_status - EKF2 融合器状态，包含融合质量指标和故障标志。

estimator_innovations - EKF2 融合残差（innovation），用于诊断融合质量。

test_ratio 说明：当 test_ratio > 1.0 时，EKF2 会拒绝该次测量数据。在 log_284 案例中，过小的 EKF2_EV_POS_X/Y 参数导致 test_ratio 频繁超过阈值，视觉数据被拒绝，造成位置跳变。

2.5 传感器数据

sensor_combined - 综合传感器数据，包含加速度计、陀螺仪和磁力计的融合读数。

合理范围：

• 加速度计：±20 m/s²（正常飞行），±9.8 m/s²（静止时重力）
• 陀螺仪：±10 rad/s（正常飞行）
• 磁力计：±0.5 G（地球磁场强度）

3. 通过绘图分析关键数据

基于 log_284 案例，本节展示如何将提取的关键数据绘制成图表，直观展示数据变化趋势，为问题诊断提供可视化支持。

读者可以自行下载该日志文件，使用本文提供的脚本进行复现分析。

3.1 飞行模式时间线

通过飞行模式随时间变化的曲线，可以直接观测整段飞行中飞机经历的模式阶段（起飞、悬停、手动介入、稳高/定点、降落等），这是后续所有细节分析的时间框架。

脚本下载：plot_flight_mode.py

• 图表含义：

  • 横轴：时间（秒），从日志开始后的相对时间，范围 0-55 秒。
  • 纵轴：飞行模式（Flight Mode），显示 PX4 中的各个模式，包括 MANUAL, ALTCTL, POSCTL, AUTO_MISSION, AUTO_LOITER, AUTO_RTL, ACRO, OFFBOARD 等。
  • 数据系列：蓝色线条和圆形标记表示当前激活的飞行模式，模式切换时线条发生垂直跳变。
  • 关键信息：每一次模式切换的精确时间点、模式持续时长，以及模式切换的频率和模式。

• 分析：

  • 数据观察：
    • 飞行模式序列（0-55秒）：
      • 0-13.5秒：ALTCTL（高度控制）模式：飞行开始阶段，飞机处于高度控制模式，此阶段对水平位置估计的依赖较弱，主要依赖气压计和加速度计进行高度控制。
      • 13.5-17.5秒：POSCTL（位置控制）模式：首次切换到位置控制模式，持续约4秒。这是第一个关键时间点，飞机开始依赖水平位置估计进行控制。
      • 17.5-25.5秒：ALTCTL（高度控制）模式：切回高度控制模式，持续约8秒。可能由于位置控制出现问题，飞手或系统自动切回高度控制模式。
      • 25.5-37.5秒：POSCTL（位置控制）模式：再次切换到位置控制模式，持续约12秒。这是第二个关键时间点，也是持续时间最长的位置控制阶段。
      • 37.5-55秒：ALTCTL（高度控制）模式：最后切回高度控制模式，直到日志结束。
    • 模式切换特征：
      • 飞机仅在 ALTCTL 和 POSCTL 之间切换，没有进入其他模式（如 MANUAL, AUTO_MISSION, OFFBOARD 等），说明飞行过程相对简单。
      • 在约55秒的飞行过程中，发生了4次模式切换（ALTCTL→POSCTL→ALTCTL→POSCTL→ALTCTL），频繁切换往往意味着飞手在尝试使用位置控制模式但遇到问题，或系统检测到异常自动触发保护逻辑。
      • 两次 POSCTL 模式分别持续约4秒和12秒，持续时间较短，特别是第一次仅4秒，可能表明位置控制模式启动后很快出现问题。
  • 关键发现：
    • 时间锚点与问题关联：
      • 13.5秒（首次切入 POSCTL）：这是第一个关键时间点，结合后续分析可以发现，此时开始出现水平位置漂移和姿态控制异常。
      • 25.5秒（再次切入 POSCTL）：这是第二个关键时间点，也是持续时间最长的位置控制阶段，可能对应最严重的异常阶段（如位置对比图中35-45秒的异常峰值期）。
    • 模式切换与异常的关系：每次从 ALTCTL 切换到 POSCTL 后，飞机开始依赖水平位置估计，若位置估计本身存在问题（如 EKF2 对外发布位置跳变），就会导致位置控制异常，表现为水平位置漂移和姿态控制异常；切回 ALTCTL 后，对水平位置估计的依赖减弱，异常现象可能暂时缓解。
  • 结论验证：
    这一飞行模式时间线为后续所有细节分析提供了明确的时间框架。通过将姿态角异常、位置跳变、加速度计波动等数据与模式切换时间点对齐，可以清晰地建立模式切换 → 位置估计异常 → 控制失效的因果链条，为问题诊断提供关键的时间锚点。

3.2 姿态角时间序列

姿态角（Roll/Pitch/Yaw）时间序列是最直观反映飞控控制性能的图表之一。通过对比问题飞行（log_284）和正常飞行（log_287），可以很快判断控制回路是否处于健康工作区间。

脚本下载：plot_attitude.py

问题飞行

正常飞行

• 图表含义：

  • 横轴：时间（秒），从日志开始后的相对时间。
  • 纵轴：
    • 姿态角图：角度（度），显示 Roll（横滚角，绕 X 轴，蓝色）、Pitch（俯仰角，绕 Y 轴，橙色）、Yaw（偏航角，绕 Z 轴，绿色）随时间的变化。
    • 角速度图：角速度（deg/s），显示 Roll Rate（蓝色）、Pitch Rate（橙色）、Yaw Rate（绿色）随时间的变化。
  • 数据系列：
    • 问题飞行（log_284）：显示约52秒的飞行数据，包含姿态角和角速度两个子图。
    • 正常飞行（log_287）：显示约125秒的飞行数据，包含姿态角和角速度两个子图。
  • 关键标记：蓝色点标记快速角度变化（>10°/s），用于突出异常行为。

• 分析：

  • 数据观察：
    • log_284（问题飞行）：
      • Roll 和 Pitch 轴：姿态角在整个约52秒的飞行过程中保持相对稳定，角度值基本在 $\pm 10^\circ$ 范围内小幅波动，接近水平状态；角速度主要波动在 $\pm 500$ deg/s 范围内，属于正常的控制响应范围。
      • Yaw 轴（严重异常）：姿态角出现频繁、剧烈的瞬时反转，在多个时间段（3-8秒、15-18秒、20-22秒、25-28秒、30-38秒、40-42秒）内，Yaw 角在 $-170^\circ$ 和 $+170^\circ$ 之间快速跳变，在图表上呈现为近垂直的线条，表明航向在短时间内发生 $340^\circ$ 的巨大变化；角速度出现极端峰值，幅度可达 $\pm 7500$ deg/s，与 Yaw 角的快速跳变完全对应；蓝色点标记集中在 Yaw 角的跳变区间，进一步突出了异常行为的严重性；42秒后 Yaw 角稳定在 $+170^\circ$ 附近。
    • log_287（正常飞行）：
      • Roll 和 Pitch 轴：姿态角在整个约125秒的飞行过程中持续振荡，角度值在 $-5^\circ$ 到 $+10^\circ$ 范围内波动，围绕 $0^\circ$ 进行小幅调整，这是正常飞行中为保持姿态稳定而进行的持续修正；角速度显示高频、高幅振荡，频繁达到 $\pm 40$ deg/s 的峰值，甚至超出显示范围，表明系统在积极进行姿态修正，这是健康控制系统的正常表现。
      • Yaw 轴（非常稳定）：姿态角在整个125秒内极其稳定，始终保持在 $95-96^\circ$ 附近，波动极小，几乎呈水平直线，表明航向估计和控制非常可靠；角速度在整个飞行过程中非常接近 0 deg/s，波动仅在 $\pm 2$ deg/s 范围内，表明几乎没有绕 Z 轴的旋转运动，航向保持稳定。
  • 关键发现：
    • Yaw 轴的极端对比：这是最关键的发现——log_284 中 Yaw 角出现频繁、剧烈的瞬时反转（角速度峰值达 $\pm 7500$ deg/s），而 log_287 中 Yaw 角极其稳定（角速度接近 0），这种极端对比直接证明了问题出在航向估计或控制逻辑，而非机械结构。
    • Roll/Pitch 表现差异：log_284 中 Roll/Pitch 相对稳定但缺乏主动修正，而 log_287 中 Roll/Pitch 持续振荡但处于健康控制状态，这种差异可能与飞行模式、控制参数或外部干扰有关，但不是导致问题的根本原因。
    • 问题定位：Roll/Pitch 在 log_284 中基本正常，而 Yaw 出现严重异常，说明问题不在基本姿态控制环路本身，而在更高层（如位置/航向估计或上层控制模式）。Yaw 的频繁跳变通常意味着位置/航向估计不稳定（EKF2 融合质量差），导致航向参考频繁跳变；或控制器在错误反馈信号驱动下不断反向调整，形成失控感。
  • 结论验证：
    通过这一对比，可以直接观察到：同一架飞机、相似的飞行场景，仅仅因为参数/配置不同，Yaw 控制就可以从稳定可控变为频繁反转、接近失控，从而证明问题根源在于位置估计与控制逻辑的配置不当（特别是 EKF2 航向估计和 Position 模式下的控制逻辑），而非机械结构或硬件故障。

3.3 EKF2 内部位置 vs 对外发布位置

位置对比图是 log_284 案例中最关键的证据之一：它将 EKF2 内部估计的位置（来自 estimator_local_position）与飞控对外发布的位置（如 vehicle_local_position）叠加到同一坐标系中，直观展示 EKF2 内部位置估计与对外发布位置之间的差异。

脚本下载：plot_position.py

• 图表含义：

  • 横轴：时间（秒），从日志开始后的相对时间，范围 0-55 秒。
  • 纵轴：位置（米），三个子图分别显示 X 轴（前后方向）、Y 轴（左右方向）、Z 轴（高度方向）的位置随时间的变化。
  • 数据系列：
    • 蓝色线：estimator_local_position（EKF2 内部估计位置），表示 EKF2 滤波器内部的位置估计值。
    • 橙色线：vehicle_local_position（对外发布位置），表示飞控对上层控制和外部模块发布的位置值。
  • 关键标记：红色虚线标记 “Position Mode Start”（位置模式启动时间点，约14秒），用于标识模式切换的关键时刻。

• 分析：

  • 数据观察：
    • X 轴位置对比（前后方向）：
      • Position 模式启动前（0-14秒）：两条曲线紧密重合，在 4-6 米范围内平滑波动，表明此阶段位置估计整体可信。
      • Position 模式启动后（14秒起）：橙色线（对外发布位置）立即开始剧烈波动，与蓝色线（内部估计）出现明显分离。
      • 异常峰值期（35-45秒）：橙色线出现极端跳变，峰值接近 10 米，最低降至 2 米以下，呈现锯齿状、高幅度的来回抖动；蓝色线在此期间也出现振荡，但幅度和频率明显小于橙色线，整体更平滑。
      • 恢复期（45秒后）：两条曲线重新收敛，从约 6 米平滑下降至 4 米并趋于稳定，表明系统恢复正常。
    • Y 轴位置对比（左右方向）：
      • Position 模式启动前（0-14秒）：两条曲线紧密重合，在 12.5-14.5 米范围内稳定波动，与 X 轴表现一致。
      • Position 模式启动后（14秒起）：橙色线开始显著偏离蓝色线，波动加剧。
      • 异常峰值期（35-45秒）：橙色线出现极端跳变，峰值接近 15 米，最低降至 10 米以下，与 X 轴同步出现大幅锯齿状抖动；蓝色线同样显示振荡但幅度更小。
      • 恢复期（45秒后）：两条曲线收敛，从约 12 米平滑上升至 13 米并稳定，系统恢复正常。
    • Z 轴位置对比（高度方向）：
      • 整体一致性：Z 轴两条曲线的一致性明显优于 X/Y 轴，即使在 Position 模式启动后，橙色线的波动也相对较小。
      • 0-14秒：两条曲线基本重合，从 -0.2 米平滑下降至 -0.55 到 -0.6 米，保持稳定。
      • 14秒后：橙色线波动稍大于蓝色线，但偏差远小于 X/Y 轴。
      • 38-42秒：两条曲线同步出现快速下降至 -0.85 米，随后快速上升至 -0.2 米（48秒），橙色线在此期间略显锯齿状，但整体趋势一致。
  • 关键发现：
    • X/Y 轴异常跳变：Position 模式启动后，对外发布的 X/Y 位置（橙色线）出现极端跳变和锯齿状抖动，而 EKF2 内部估计（蓝色线）虽然也有振荡，但幅度和频率明显更小，说明问题出在 EKF2 对外发布位置的处理环节，而非内部估计本身。
    • Z 轴相对稳定：Z 轴两条曲线的一致性明显好于 X/Y 轴，表明高度估计和发布机制相对正常，问题主要集中在水平位置（X/Y）的发布环节。
    • 异常峰值期（35-45秒）：这是整个飞行过程中最严重的异常阶段，X/Y 轴对外发布位置出现极端跳变，峰值可达正常值的 2-3 倍，这直接导致 Position 控制器基于错误反馈产生剧烈修正动作。
    • 恢复机制：45秒后两条曲线重新收敛，可能与模式切换、飞手介入或 EKF2 重新收敛有关，说明系统具备恢复能力，但异常期间已造成位置控制失效。
  • 结论验证：
    • 这一对比图与前文对 EKF2 test_ratio 和参数 EKF2_EV_POS_X/Y 过于严格的分析结论形成闭环：EKF2 外部位置数据被拒绝 → 内部估计与对外发布位置分离 → 对外发布位置出现跳变 → Position 控制器基于错误反馈输出错误控制量 → 飞机在 Position 模式下出现失控漂移。
    • 在 PX4 的 Position 模式中，飞控会在切入该模式时将当前飞机所在的 X/Y 位置视为新的保持目标点，若对外发布的位置本身发生跳变，控制器会基于错误的位置反馈持续调整控制量，导致飞机出现缓慢甚至剧烈漂移；当退出 Position 模式，改为 Altitude 或 Manual 并由飞手直接控制姿态/油门时，对位置估计的依赖减弱，漂移现象立即消失。

3.4 加速度计时域对比

加速度计 X 轴和 Y 轴时域对比图，将问题飞行（log_284）与正常飞行（log_287）在同一时间标度下叠加，帮助我们从机体振动和动态响应的角度，验证系统是否处在一个合理的工作环境中。

脚本下载：plot_accel.py

• 图表含义：

  • 横轴：时间（秒），分别对两段日志做相对时间对齐，范围 0-130 秒。
  • 纵轴：加速度（m/s²），两个子图分别显示 X 轴（前后方向，通常是机头指向）和 Y 轴（左右方向）的加速度随时间的变化。
  • 数据系列：
    • 蓝色线：log_284 accel X/Y（问题飞行），显示存在问题的飞行中的加速度数据。
    • 橙色线：log_287 accel X/Y（正常飞行），显示参数/配置优化后的稳定飞行中的加速度数据。
  • 关键信息：通过对比两条曲线可以直观判断系统是否工作在健康的振动水平，识别异常振动和动态响应问题。

• 分析：

  • 数据观察：
    • log_284（问题飞行）：
      • 在前约50秒内，X 轴和 Y 轴加速度均显示出更大的振幅波动，振荡范围约在 $\pm 6$ 到 $\pm 7.5$ m/s² 之间，明显高于正常水平。
      • 曲线呈现不规则、高频的抖动特征，表明此阶段机体振动水平确实偏高。
      • 数据在大约50秒后停止记录，这与飞行模式时间线中显示的飞行终止时间点一致。
    • log_287（正常飞行）：
      • 整体振幅显著更小，大部分时间加速度值稳定在 $\pm 2.5$ m/s² 范围内，基线接近 $0$ m/s²。
      • 曲线平滑度明显优于 log_284，表明系统工作在健康的振动环境中。
      • 存在少量尖锐的峰值（如45秒、65秒、95秒、105秒附近），这些可能是正常的机动动作（如快速转向、急停等），峰值后迅速恢复到稳定状态。
      • 数据持续记录整个测量期间（约130秒），飞行过程完整。
  • 关键发现：
    • 振动水平对比：log_284 在前50秒内确实存在更高的振动水平，这可能与 Position 模式下的位置控制异常导致的频繁修正动作有关，而非单纯的机械振动问题。
    • 系统优化效果：log_287 的平滑曲线证明，在相同的机械结构下，通过优化 EKF2 参数和位置控制逻辑，系统可以工作在低振动、稳定的状态。
    • 问题根源验证：通过对比两张图中曲线的整体平滑度和峰值大小，可以直观判断系统是否工作在一个健康的振动水平，为问题诊断提供重要的参考依据。
  • 结论验证：
    这一对比进一步验证了前文结论：问题的根源在于 EKF2 配置不当导致的位置估计跳变，进而引发 Position 控制器的异常响应和频繁修正，表现为加速度数据的剧烈波动；而非机械结构或硬件故障导致的振动问题。

3.5 Test Ratio 与位置跳变关联分析

Test Ratio 与位置跳变关联分析图是 log_284 案例中最关键的诊断图表之一：它将 EKF2 的 test ratio（测试比率）、数据拒绝事件与位置跳变、Yaw 角异常叠加在同一时间轴上，直观展示 EKF2 数据拒绝机制与位置控制异常之间的因果关系。

脚本下载：plot_position_jump.py

• 图表含义：

  • 横轴：时间（秒），从日志开始后的相对时间，范围 0-55 秒。
  • 纵轴：三个子图分别显示不同的数据指标。
    • 上子图：X 位置（米，左轴）和 Yaw 角（度，右轴），显示 EKF2 内部估计位置（蓝色）、对外发布位置（橙色）和 Yaw 角（绿色虚线）。
    • 中子图：X Test Ratio（无单位），显示 EKF2 对 X 轴位置数据的测试比率（红色实线）和拒绝阈值（黑色虚线，值为 1.0）。
    • 下子图：数据拒绝事件（二进制指标），红色点表示数据被拒绝的时刻。
  • 关键标记：红色虚线标记 “POSCTL” 和 “ALTCTL”，用于标识飞行模式切换的时间点。
  • 关键信息：通过对比三个子图，可以直观观察 test ratio 峰值、数据拒绝事件与位置跳变、Yaw 角异常之间的时间对应关系。

• 分析：

  • 数据观察：
    • ALTCTL 模式阶段（0-13.5秒、17.5-25.5秒、37.5-55秒）：
      • X 位置：EKF2 内部估计（蓝色）与对外发布位置（橙色）紧密重合，在 4-6 米范围内稳定波动，位置估计整体可信。
      • Yaw 角：稳定在约 $10^\circ$ 附近，波动较小，航向控制正常。
      • X Test Ratio：大部分时间保持在拒绝阈值（1.0）以下，表明传感器数据质量良好，EKF2 正常接受数据。
      • 数据拒绝事件：几乎没有或极少，系统运行稳定。
    • POSCTL 模式阶段（13.5-17.5秒、25.5-37.5秒）：
      • X 位置：切换到 POSCTL 模式后，对外发布位置（橙色）开始出现波动和偏差，与内部估计（蓝色）出现分离。
      • Yaw 角：在模式切换时刻出现急剧变化，随后在 POSCTL 模式下波动加剧。
      • X Test Ratio：出现峰值，在约 13.5 秒、20 秒、32 秒等时刻超过拒绝阈值（1.0），最高峰值可达 30 以上。
      • 数据拒绝事件：在 test ratio 超过阈值时出现，与位置跳变和 Yaw 角异常的时间点高度一致。
    • 异常峰值期（35-45秒）：
      • X 位置：对外发布位置出现极端跳变，从约 6 米跳至 9 米，随后降至 2 米以下，呈现锯齿状、高幅度的来回抖动，与内部估计位置严重分离。
      • Yaw 角：出现频繁、剧烈的瞬时反转，在 $-150^\circ$ 和 $+150^\circ$ 之间快速跳变，航向控制完全失效。
      • X Test Ratio：出现连续多次峰值，数值在 10-15 之间，持续超过拒绝阈值，表明 EKF2 在此阶段频繁拒绝传感器数据。
      • 数据拒绝事件：在 35-40 秒期间出现密集的拒绝事件，红色点集中分布，与位置跳变和 Yaw 角异常完全对应。
    • 恢复期（45秒后）：
      • 切回 ALTCTL 模式后，X 位置两条曲线重新收敛，稳定在约 4 米；Yaw 角稳定在 $0^\circ$ 附近；test ratio 降至阈值以下；数据拒绝事件消失，系统恢复正常。
  • 关键发现：
    • 时间对应关系：test ratio 峰值、数据拒绝事件与位置跳变、Yaw 角异常在时间上高度一致，特别是在 POSCTL 模式下和异常峰值期（35-45秒），这种对应关系清晰地表明 EKF2 数据拒绝机制是导致位置跳变的直接原因。
    • 模式切换触发：每次从 ALTCTL 切换到 POSCTL 时，test ratio 都会出现峰值并超过阈值，导致数据拒绝事件，随后位置跳变和 Yaw 角异常开始出现；切回 ALTCTL 后，test ratio 恢复正常，位置和 Yaw 角也趋于稳定。
    • 异常峰值期的严重性：35-45 秒期间，test ratio 连续多次超过阈值，数据拒绝事件密集出现，对应最严重的位置跳变（峰值可达正常值的 2-3 倍）和 Yaw 角失控（$340^\circ$ 的巨大变化），这是整个飞行过程中最危险的阶段。
    • EKF2 数据拒绝机制的影响：当 test ratio > 1.0 时，EKF2 拒绝外部位置数据（如视觉定位数据），只能依赖 IMU 预测，位置逐渐漂移；下次数据被接受时，位置突然"跳回"，形成锯齿状跳变；这种不稳定的位置反馈导致 Position 控制器产生错误的控制指令，进而引发 Yaw 角异常和水平位置漂移。
  • 结论验证：
    • 这一关联分析图与前文对 EKF2 test_ratio 和参数 EKF2_EV_POS_X/Y 过于严格的分析结论形成完整的因果链条：EKF2 参数设置过小（EKF2_EV_POS_X/Y 过小） → innovation test 过于严格 → test ratio 频繁超过阈值 → 外部位置数据被拒绝 → 内部估计与对外发布位置分离 → 对外发布位置出现跳变 → Position 控制器基于错误反馈输出错误控制量 → Yaw 角异常和水平位置漂移 → 飞机在 Position 模式下出现失控漂移。
    • 在 ALTCTL 模式下，系统对水平位置估计的依赖较弱，即使出现数据拒绝，影响也较小；但在 POSCTL 模式下，位置估计的准确性直接决定控制性能，数据拒绝导致的位置跳变会立即引发控制异常，这解释了为什么问题只在 POSCTL 模式下暴露出来。

4. 问题诊断

基于 log_284 案例的可视化分析，本节系统性地讲解如何通过图表识别和诊断飞行过程中的常见问题，并提供相应的解决方案。

4.1 位置估计异常分析

基于 log_284 案例的完整分析，位置估计异常问题具有以下典型特征：

• 问题症状：

  • estimator_local_position（EKF2 内部估计位置）与 vehicle_local_position（对外发布位置）出现明显偏差
  • X/Y 轴位置出现锯齿状跳变，位置在相邻采样之间快速来回抖动
  • Z 轴（高度）相对稳定，问题主要集中在水平位置（X/Y）
  • Yaw 角出现频繁、剧烈的瞬时反转
  • 问题仅在 Position 模式下暴露，在 Altitude 模式下表现正常

• 诊断方法：

  • 对比 EKF2 内部估计位置与对外发布位置（见 3.3 节）
  • 分析 EKF2 test ratio 和数据拒绝事件（见 3.5 节）
  • 观察飞行模式切换与位置跳变的时间对应关系（见 3.1 节）
  • 对比问题飞行与正常飞行的姿态角响应（见 3.2 节）

• 问题原因分析（基于 log_284）：

  1. EKF2 数据拒绝机制触发：

     • 当 test_ratio > 1.0 时，EKF2 拒绝外部位置数据（如视觉定位数据），只能依赖 IMU 预测
     • 位置逐渐漂移；下次数据被接受时，位置突然"跳回"，形成锯齿状跳变
     • 在 log_284 中，test ratio 峰值可达 30 以上，大部分在 5-20 之间

  2. 参数设置过小导致过度拒绝：

     • EKF2_EV_POS_X/Y 设置过小（如 0.1），导致 innovation test 过于严格
     • 视觉数据频繁被拒绝，特别是在 Position 模式启动时和异常峰值期（35-45秒）
     • 数据拒绝事件集中在模式切换时刻和异常峰值期，与位置跳变完全对应

  3. 控制回路反馈振荡：

     • Position 模式下，位置控制器基于不稳定的位置反馈产生控制指令
     • 位置跳变导致控制器不断尝试修正"错误"的位置，形成振荡反馈
     • 这种振荡反馈进一步加剧了位置估计的不稳定性，形成恶性循环

  4. 模式依赖性问题：

     • 在 Altitude 模式下，系统对水平位置估计的依赖较弱，即使出现数据拒绝，影响也较小
     • 在 Position 模式下，位置估计的准确性直接决定控制性能，数据拒绝导致的位置跳变会立即引发控制异常
     • 这解释了为什么问题只在 Position 模式下暴露出来

4.2 解决方案

基于 log_284 案例的诊断结果，建议按照以下问题排查清单逐步检查和调整：

问题排查清单：

1. 检查并调整 EKF2 参数：

   • 检查 EKF2_EV_POS_X 和 EKF2_EV_POS_Y 的当前值，如果过小（如 0.1），建议调整到 0.3-0.5
     • 这些参数定义了外部位置数据（如视觉定位）的标准偏差
     • 增大这些值可以放宽 innovation test 的严格程度，减少数据被过度拒绝的情况
   • 检查 EKF2_EVP_GATE（innovation gate 阈值），如果默认值为 3.0，可以尝试调整到 5.0
     • 这个参数控制 innovation test 的拒绝阈值，增大后可以减少数据拒绝
   • 调整后重新飞行并记录日志，检查 test ratio 是否降低，数据拒绝事件是否减少，确认位置跳变和 Yaw 角异常是否消失

2. 检查外部定位数据质量：

   • 验证视觉定位数据（VRPN）的噪声水平
     • 检查数据转发脚本的过滤逻辑是否合理
     • 确认数据更新频率是否在合理范围内（建议 20-50 Hz）
     • 验证数据是否包含异常值或跳变
   • 优化数据预处理流程
     • 在数据转发脚本中添加异常值过滤
     • 实现数据平滑滤波，减少噪声
     • 确保数据时间戳同步准确

3. 验证传感器校准状态：

   • 重新校准 IMU（加速度计、陀螺仪）
     • 使用 PX4 的校准工具进行完整校准
     • 确保传感器数据质量，减少 IMU 预测误差
   • 检查传感器安装情况
     • 确认传感器安装牢固，无松动
     • 检查是否存在电磁干扰
     • 验证传感器数据是否在合理范围内

4. 考虑固件和配置优化：

   • 检查是否有可用的固件升级
     • 考虑升级/降级 PX4 固件版本，可能包含位置处理逻辑的优化
     • 查看固件更新日志，了解 EKF2 相关的改进
   • 根据实际飞行环境调整其他相关参数
     • EKF2_AID_MASK：控制哪些辅助数据源被使用
     • EKF2_HGT_MODE：高度估计模式选择
     • 根据实际飞行环境和传感器配置进行优化

5. 调整飞行策略（临时方案）：

   • 避免频繁模式切换
     • 在 Position 模式下保持稳定飞行，减少模式切换频率
     • 如果必须切换，确保在 Altitude 模式下停留足够时间，让 EKF2 重新收敛
   • 如果问题持续存在，考虑使用其他飞行模式
     • 可以使用 Altitude 模式进行飞行
     • 或者使用 Manual 模式，由飞手直接控制姿态和油门

验证和测试注意事项：

• 每次参数调整后，先进行地面测试和短时间悬停测试
• 记录日志并对比调整前后的 test ratio、数据拒绝事件和位置稳定性
• 逐步调整参数，避免一次性大幅修改导致其他问题
• 参考 log_287（正常飞行）的参数配置，作为调整的参考基准

4.3 小结

本文档以 log_284 为实际案例，展示了使用 pyulog 库分析 PX4 飞控日志的完整流程。通过系统性的数据提取、可视化和诊断分析，成功定位了 Position 模式下位置跳变问题的根本原因。

log_284 案例要点：

• 问题现象：切换到 Position 模式后，X/Y 轴出现锯齿状位置跳变，Yaw 角频繁反转
• 根本原因：EKF2 参数（EKF2_EV_POS_X/Y）设置过小，导致 innovation test 过于严格，视觉数据频繁被拒绝
• 诊断方法：通过对比 EKF2 内部估计位置与对外发布位置、分析 test ratio 和数据拒绝事件、观察飞行模式切换与异常的时间对应关系，建立了完整的因果链条
• 解决方案：调整 EKF2_EV_POS_X/Y（从 0.1 调整到 0.3-0.5）和 EKF2_EVP_GATE（从 3.0 调整到 5.0），问题得到解决

参考文档

• pyulog GitHub 仓库
• PX4 ULog 文件格式文档
• PX4 日志分析指南
• Flight Review 在线日志分析工具
• PlotJuggler 数据可视化工具

基于 VRPN 的 PX4 EKF2 视觉融合基础调试指南

本文档不包含 ROS 2, MAVROS, Gazebo 等工具的基础使用方法，也没有如何安装、编译或运行这些软件的具体步骤； 而是专注于：VRPN 位姿数据是如何进入系统、在各个设备中如何被处理与融合、以及如何通过日志和脚本分析这些数据。

• 阅读本文档默认你已经具备以下基础能力：
• 能够在自己的环境中安装并使用 ROS 2, MAVROS 和 Gazebo
• 能够启动 PX4 飞控, QGroundControl，并、完成连接
• 能够运行简单的 Python 脚本和 ROS / ROS 2 节点命令

1. 飞控的数据融合流程：从 VRPN 到位置融合数据

1.1 完整数据流

VRPN 动捕系统的位置和姿态数据经过多个环节处理，最终融合到飞控的位置估计中。整个系统涉及三个主要设备：动捕系统，伴随计算机 和 飞控。

graph TD
    subgraph 动捕系统["动捕系统设备"]
        A[VRPN动捕系统<br/>位置和姿态测量]
    end
    
    subgraph 伴随计算机["伴随计算机"]
        B[VRPN转发脚本<br/>get_pose.py<br/>数据预处理和过滤]
        C[MAVROS<br/>ROS与PX4通信桥梁]
    end
    
    subgraph 飞控["飞控设备"]
        E[IMU传感器<br/>加速度计/陀螺仪]
        D[PX4 EKF2融合器<br/>多传感器融合]
        F[PX4位置控制器<br/>位置控制算法]
    end
    
    A -->|1. /vrpn_mocap/drone1/pose<br/>PoseStamped原始数据| B
    B -->|2. 数据有效性检查<br/>位置跳变过滤| C
    C -->|3. /mavros/vision_pose/pose<br/>转换为MAVLink并发送给PX4| D
    E -->|4. IMU原始数据| D
    D -->|5. estimator_local_position<br/>融合后的位置估计| F
    F -->|6. vehicle_local_position<br/>作为控制回路反馈| C
    C -->|7. /mavros/local_position/pose<br/>提供给上层应用使用| G[外部应用<br/>MAVSDK/QGC等]
    
    style 动捕系统 fill:#e8f5e9,stroke:#009900,stroke-width:3px
    style 伴随计算机 fill:#fff4e1,stroke:#ff9900,stroke-width:3px
    style 飞控 fill:#e1f5ff,stroke:#0066cc,stroke-width:3px
    style D fill:#e1f5ff,stroke:#0066cc,stroke-width:3px
    style C fill:#fff4e1,stroke:#ff9900,stroke-width:2px
    style B fill:#fff4e1,stroke:#ff9900,stroke-width:2px
    style G fill:#fce4ec,stroke:#cc0066,stroke-width:2px

设备功能说明：

1.2 关键话题说明

1.3 视觉转发脚本

在伴随计算机上，需要一个“视觉转发脚本”get_pose.py，负责把动捕系统输出的位姿数据转换成飞控可以理解的形式，并转发给MAVROS。
可以简单理解为：MAVROS是 ROS ↔ PX4 的桥，而 get_pose.py 则是 VRPN ↔ MAVROS 的桥——负责在进入 MAVROS 之前，把视觉数据清洗、规范好。
其功能可以概括为：

1. 订阅动捕系统的位姿话题（例如 /vrpn_mocap/drone1/pose，类型为geometry_msgs/PoseStamped）。
2. 检查数据有效性：过滤掉包含 NaN、无穷大或严重不合法四元数的数据。
3. 限制位置跳变：如果相邻两帧的位置差异超过阈值（如 0.5 m），认为数据异常并丢弃。
4. 可选：限制数据超时，如果长时间收不到新数据，则暂停转发，避免向飞控提供过期数据。
5. 转发到MAVROS：将清洗后的位姿发布到 /mavros/vision_pose/pose，并将 frame_id 统一设为 map 坐标系，供飞控 EKF2 使用。
6. 可选：记录日志，将实际转发的数据写入日志文件，方便之后用 Python 分析。

下面是一个基于 rospy 的简化示例，展示 get_pose.py 的核心逻辑（省略异常处理和完整启动代码）：

#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import PoseStamped
import math


class VisionRelay(object):
    def __init__(self):
        # 最大允许位置跳变（米）
        self.max_position_change = rospy.get_param("~max_position_change", 0.5)
        self.last_pose = None

        # 订阅 VRPN 动捕位姿
        self.sub = rospy.Subscriber(
            "/vrpn_mocap/drone1/pose",
            PoseStamped,
            self.pose_callback,
            queue_size=10,
        )

        # 发布到 MAVROS，供飞控 EKF2 使用
        self.pub = rospy.Publisher(
            "/mavros/vision_pose/pose",
            PoseStamped,
            queue_size=10,
        )

    def _is_pose_valid(self, pose):
        """检查位置和四元数是否有效"""
        p = pose.position
        q = pose.orientation

        # 1）检查 NaN / Inf
        for v in (p.x, p.y, p.z, q.x, q.y, q.z, q.w):
            if math.isnan(v) or math.isinf(v):
                return False

        # 2）检查四元数近似归一化
        q_norm = math.sqrt(q.x**2 + q.y**2 + q.z**2 + q.w**2)
        if abs(q_norm - 1.0) > 0.1:
            return False

        return True

    def _position_jump_too_large(self, new_pose):
        """检查与上一帧相比是否位置跳变过大"""
        if self.last_pose is None:
            return False

        p1 = self.last_pose.pose.position
        p2 = new_pose.pose.position
        dx = p2.x - p1.x
        dy = p2.y - p1.y
        dz = p2.z - p1.z
        dist = math.sqrt(dx * dx + dy * dy + dz * dz)
        return dist > self.max_position_change

    def pose_callback(self, msg):
        """接收 VRPN 位姿并进行过滤，再转发到 MAVROS"""
        if not self._is_pose_valid(msg.pose):
            # 丢弃无效数据
            return

        if self._position_jump_too_large(msg):
            # 丢弃位置跳变过大的数据
            return

        # 通过检查，更新 last_pose
        self.last_pose = msg

        # 转发到 MAVROS，这里主动将 frame_id 规范到 \"map\" 坐标系
        # 这样可以避免动捕系统原始 frame_id (如 \"world\" / 机体名等) 与 PX4/MAVROS 期望的全局坐标系不一致，
        # 方便在下游（EKF2、本地位置话题、可视化工具）中进行统一的坐标变换和调试。
        out = PoseStamped()
        out.header.stamp = msg.header.stamp
        out.header.frame_id = "map"
        out.pose = msg.pose
        self.pub.publish(out)


if __name__ == "__main__":
    rospy.init_node("vision_relay")
    node = VisionRelay()
    rospy.spin()

无论使用 ROS1（rospy）还是 ROS2（rclpy），关键点都是：订阅 VRPN 位姿 → 进行数据过滤 → 以 /mavros/vision_pose/pose 的形式转发给 MAVROS，从而让飞控的 EKF2 接收到可靠的视觉位姿输入。

1.4 PX4 参数配置：使用视觉+IMU 进行EKF2融合

要让飞控真正使用视觉位置和高度，需要在 PX4 中正确配置 EKF2 相关参数，使其融合 External Vision（EV）和自身 IMU，并在需要时停用 GPS 融合。下面给出一个典型的配置思路（以 QGroundControl 参数界面为例）：

1. 启用视觉位置融合（EKF2_AID_MASK）

   • 在 QGC 中搜索参数 EKF2_AID_MASK。
   • 确保勾选/包含：
     • Vision position（视觉位置）
     • （可选）Vision yaw（视觉航向），如果动捕系统提供可靠的 yaw。
   • 如果不希望使用 GPS：取消 GPS position、GPS yaw 等相关选项，或者直接在飞控硬件上不接 GPS。

2. 选择高度来源（EKF2_HGT_MODE / EKF2_EV_CTRL）

   • 将 EKF2_HGT_MODE 设置为 Vision / External Vision（具体名称取决于 PX4 版本）。
   • 在 EKF2_EV_CTRL 中启用高度相关融合选项（position + height）。

3. 合理设置视觉噪声参数（EKF2_EV_POS_X / EKF2_EV_POS_Y 等）

   • 在日志分析中，如果发现视觉数据经常被拒绝（test_ratio 很大），说明噪声参数过小或门限过严。
   • 通常可以将：
     • EKF2_EV_POS_X / EKF2_EV_POS_Y 从 0.1 调整到 0.3–0.5。
     • EKF2_EVP_NOISE 调整到与 EV_POS_X/Y 相近的数值。
     • EKF2_EVP_GATE 调整到 5–7 之间，允许合理的残差。

4. 只保留需要的传感器融合

   • 确保 EKF2_AID_MASK 中只启用你实际在用的传感器（IMU 必须，视觉按需，GPS 可停用）。
   • 这样可以避免 EKF2 在 GPS 和视觉之间来回切换，导致位置跳变。

5. 保存参数并重启飞控

   • 修改完参数后，在 QGC 中保存参数并重启飞控，让新的融合配置生效。

完成以上配置后，EKF2 应当可以以 IMU 为高频预测源，以视觉位姿为慢速绝对参考，在无 GPS 的环境中完成稳定的位置和高度融合。

1.5 EKF2融合过程

EKF2（Extended Kalman Filter 2）是PX4的核心融合算法，将VRPN数据与IMU数据融合：

graph LR
    A[VRPN位置数据] -->|External Vision| C[EKF2融合器]
    B[IMU数据] -->|高频预测| C
    C -->|融合算法| D[位置估计]
    D -->|输出| E[local_position/pose]
    
    style C fill:#e1f5ff,stroke:#0066cc,stroke-width:3px
    style E fill:#fce4ec,stroke:#cc0066,stroke-width:2px

融合步骤：

1. IMU预测阶段：使用IMU数据（加速度计、陀螺仪）以高频（~200Hz）预测位置和姿态
2. VRPN更新阶段：接收VRPN数据，计算innovation（预测值与测量值的差）
3. 数据有效性检查：计算test_ratio，如果超过阈值则拒绝数据
4. 状态更新：融合有效数据，更新位置和姿态估计

1.6 验证融合

检查 /mavros/local_position/pose 话题输出：

# 检查话题是否存在
rostopic list | grep local_position

# 检查话题频率（应该>10Hz）
rostopic hz /mavros/local_position/pose

# 查看话题内容
rostopic echo /mavros/local_position/pose

正常输出示例：

header:
  seq: 12345
  stamp:
    secs: 1234567890
    nsecs: 123456789
  frame_id: "map"
pose:
  position:
    x: 1.234
    y: 2.345
    z: -0.567
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0

如果话题有正常输出且频率>10Hz，说明VRPN数据已成功融合到飞控位置信息中。
只要你使用 MAVROS 并持续发布：

rostopic echo /mavros/vision_pose/pose

MAVROS 会自动将其转换为 MAVLink 的 VISION_POSITION_ESTIMATE，在 EKF2 参数配置正确（例如 EKF2_AID_MASK 中启用 Vision 相关选项）的前提下，EKF2 就会融合这些视觉数据，并通过 /mavros/local_position/pose 话题输出。

2. 融合结果验证与快速排查

2.1 使用 MAVROS 话题快速确认 EKF2 输出

当怀疑 EKF2 没有正确融合 VRPN 视觉数据时，可以先用 MAVROS 话题做一个“最小自检”：

# 直接查看局部位置输出
rostopic echo /mavros/local_position/pose

# 检查话题频率（建议 >10Hz）
rostopic hz /mavros/local_position/pose

# 查看话题信息，确认发布者与消息类型
rostopic info /mavros/local_position/pose

如果 /mavros/local_position/pose 话题不存在、频率明显偏低，或者数据长时间不更新，说明 EKF2 融合过程存在问题，需要进一步检查 PX4 内部状态和日志。

如果 /mavros/vision_pose/pose 话题不存在，说明 get_pose.py 脚本运行存在问题，需要进一步检查和调试。

2.2 检查 PX4 EKF2 状态与常见失败原因

PX4 内部通过 uORB 话题提供 EKF2 的详细状态与告警信息，可以直接在 ROS2 环境下查看：

# 查看 EKF2 的时间戳信息（是否在正常更新）
ros2 topic echo /fmu/out/ekf2_timestamps

# 查看 EKF2 估计器状态（融合标志、test_ratio 等）
ros2 topic echo /fmu/out/estimator_status

在 estimator_status 中，重点关注以下几个方面：

• Innovation Test Ratios：各传感器的残差检验值，如果长期明显大于 1，说明该传感器数据经常被拒绝，可能存在噪声模型或数据质量问题。
• Control Status / Fusion Flags：哪些传感器被真正纳入融合（如 GPS、视觉位置、气压高度等），可用来确认视觉融合是否已实际启用。
• 告警与错误信息：PX4 会通过状态位和日志给出融合失败的直接原因。

常见 EKF2 融合失败原因包括：

• GPS 信号丢失或不稳定（但参数中仍启用 GPS 融合，导致状态在 GPS/视觉间频繁切换）。
• IMU 数据异常（震动过大、传感器损坏或安装不当）。
• 磁力计干扰或偏差过大（航向不可靠，影响位置估计）。
• 参数配置错误（例如 EKF2_AID_MASK 未启用 Vision Position，或高度来源与实际数据来源不一致）。

2.3 结合日志与 Flight Review 做进一步确认

如果通过话题和 uORB 状态仍无法快速判断问题根因，可以进一步查看 PX4 记录的 .ulg 日志：

1. 使用 QGroundControl 下载 .ulg 日志文件（参考后文“4.1 QGroundControl日志下载”）。
2. 使用 QGroundControl 自带的日志浏览器或 Flight Review 网站打开日志。
3. 在日志中重点查看 Estimator Status / EKF2 相关曲线：
   • Innovation Test Ratios：确认视觉位置、气压高度、IMU 等传感器的残差是否在合理范围。
   • Control Status / Fusion Flags：确认 External Vision 是否真正被纳入融合，而不是一直处于“待用”或“拒绝”状态。
   • Warning / Error 信息：例如 GPS quality insufficient，Mag fusion failed，Bad IMU data 等。

通过“话题自检 + uORB 状态 + 日志回放”这一套组合排查，可以较快定位是传感器数据本身的问题（如动捕数据不稳定、IMU 噪声过大）、参数配置错误，还是EKF2 内部对某类数据一直处于拒绝状态。

3. 可视化验证：Gazebo中的位置监控

3.1 系统架构

使用Gazebo仿真环境和ROS2 bridge，可以实时可视化飞机的位置和姿态，对比实机位置数据与仿真显示：

graph TD
    A[实机飞控] -->|MAVLink| B[MAVROS]
    B -->|/mavros/local_position/pose| C[ROS2 Bridge]
    C -->|ROS2话题| D[Gazebo插件]
    D -->|可视化| E[Gazebo场景]
    
    F[VRPN数据] -->|/vrpn_mocap/drone1/pose| G[转发脚本]
    G -->|/mavros/vision_pose/pose| B
    
    style E fill:#fce4ec,stroke:#cc0066,stroke-width:2px
    style C fill:#fff4e1,stroke:#ff9900,stroke-width:2px
    style B fill:#e1f5ff,stroke:#0066cc,stroke-width:2px

3.3 Gazebo可视化插件

创建Gazebo插件订阅飞机位置话题，在Gazebo场景中显示飞机模型：

<!-- Gazebo模型文件示例 -->
<model name="drone1">
  <pose>0 0 0 0 0 0</pose>
  <static>false</static>
  <plugin name="pose_visualizer" filename="libpose_visualizer.so">
    <ros2_topic>/mavros/local_position/pose</ros2_topic>
    <update_rate>50</update_rate>
  </plugin>
</model>

3.4 位置数据对比

在Gazebo中可以同时显示：

1. 实机位置：来自/mavros/local_position/pose（EKF2融合后的位置）
2. VRPN原始位置：来自/vrpn_mocap/drone1/pose（动捕系统原始数据）
3. 期望位置：来自位置控制器setpoint

graph LR
    A[实机位置<br/>mavros/local_position/pose] -->|对比| D[Gazebo显示]
    B[VRPN原始位置<br/>vrpn_mocap/drone1/pose] -->|对比| D
    C[期望位置<br/>setpoint] -->|对比| D
    
    style D fill:#fce4ec,stroke:#cc0066,stroke-width:3px

观察要点：

• 位置差异：实机位置与VRPN原始位置的差异反映了EKF2融合的效果
• 姿态差异：如果姿态显示不一致，可能是EKF2融合或IMU数据问题
• 延迟：观察数据更新的延迟，如果延迟过大可能影响控制性能
• 跳变：如果位置突然跳变，可能是数据过滤失效或EKF2融合异常

4. 异常诊断：使用 pyulog 库分析飞控日志

4.1 QGroundControl日志下载

当发现位置异常或融合问题时，需要下载飞行日志进行分析。

4.1.1 连接飞控

1. 打开QGroundControl（QGC）
2. 通过USB或无线连接飞控
3. 等待连接建立（状态栏显示"Connected"）

4.1.2 下载日志文件

graph TD
    A[QGC连接飞控] -->|连接成功| B[Vehicle Setup]
    B -->|Log Files| C[查看日志列表]
    C -->|选择日志| D[下载.ulg文件]
    D -->|保存到本地| E[日志文件]
    
    style E fill:#e8f5e9,stroke:#009900,stroke-width:2px

操作步骤：

1. 在QGC主界面，点击左上角菜单 → Vehicle Setup
2. 选择 Log Files 标签页
3. 查看日志列表，选择需要分析的日志（通常是最新的）
4. 点击 Download 按钮下载
5. 日志文件保存为.ulg格式

日志文件命名：

• 格式：log_XXX_YYYY-MM-DD-HH-MM-SS.ulg
• 例如：log_287_2025-11-25-05-30-36.ulg

4.2 Python脚本分析工具

使用Python脚本解析.ulg文件，提取关键数据并生成可视化图表。

4.2.1 安装依赖

# 安装pyulog库（用于解析.ulg文件）
pip install pyulog

# 安装其他依赖
pip install numpy matplotlib pandas

4.2.2 基础分析脚本

下面是一个简单的分析脚本示例： 从 .ulg 日志中读取 EKF 内部估计位置（estimator_local_position）和融合后对外发布的位置（vehicle_local_position）。
在一张图上绘制二者的 X/Y/Z 三轴曲线对比图并保存为PNG文件，同时在终端打印保存信息。

from pyulog import ULog
import matplotlib.pyplot as plt


# 这里直接指定需要分析的 ulog 文件
ulog_file = 'log_284_2025-11-25-01-15-04.ulg'

# 从文件名中提取「ulog 编号」（例如 log_0_2025-... -> log_0）
base_name = ulog_file.split('.')[0]
ulog_id = '_'.join(base_name.split('_')[:2])
output_png = f'{ulog_id}.png'

# 读取日志
ulog = ULog(ulog_file)

# 获取 EKF 估计位置
try:
    elp = ulog.get_dataset('estimator_local_position0')
except Exception:
    elp = ulog.get_dataset('estimator_local_position')

# 获取 vehicle_local_position
vlp = ulog.get_dataset('vehicle_local_position')

# 时间戳（秒）
t_el = elp.data['timestamp'] / 1e6
t_vl = vlp.data['timestamp'] / 1e6

# 位置数据
x_el = elp.data['x']
y_el = elp.data['y']
z_el = elp.data['z']

x_vl = vlp.data['x']
y_vl = vlp.data['y']
z_vl = vlp.data['z']

# 只做 XYZ 对比并输出图片
fig, axes = plt.subplots(3, 1, figsize=(12, 10), sharex=True)

# X
ax = axes[0]
ax.plot(t_el, x_el, label='estimator_local_position.x', alpha=0.7)
ax.plot(t_vl, x_vl, label='vehicle_local_position.x', alpha=0.7)
ax.set_ylabel('X (m)')
ax.legend()
ax.grid(True, alpha=0.3)

# Y
ax = axes[1]
ax.plot(t_el, y_el, label='estimator_local_position.y', alpha=0.7)
ax.plot(t_vl, y_vl, label='vehicle_local_position.y', alpha=0.7)
ax.set_ylabel('Y (m)')
ax.legend()
ax.grid(True, alpha=0.3)

# Z
ax = axes[2]
ax.plot(t_el, z_el, label='estimator_local_position.z', alpha=0.7)
ax.plot(t_vl, z_vl, label='vehicle_local_position.z', alpha=0.7)
ax.set_ylabel('Z (m)')
ax.set_xlabel('Time (s)')
ax.legend()
ax.grid(True, alpha=0.3)

plt.tight_layout()
plt.savefig(output_png, dpi=150)
print(f'图表已保存为 {output_png}')

plt.show()

4.3 执行分析

graph TD
    A[飞行测试 / 采集数据] -->|QGC下载| B[.ulg 日志文件]
    B -->|Python基础分析脚本<br/>main.py| C[提取关键数据]
    C -->|生成图表| D[位置 / 高度时间序列图]
    C -->|生成图表| E[EV融合状态 / test_ratio 图]
    D -->|对比| F[定位异常时间与位置跳变]
    E -->|对比| F
    F -->|在 Gazebo 中对比| G[实机 / VRPN / 期望轨迹可视化]
    G -->|综合判断| H[确定问题原因]
    H -->|调整参数 / 修改脚本| I[EKF2参数 / VRPN转发脚本 / 控制逻辑]
    I -->|重新起飞并采集新日志| A
    
    style A fill:#e8f5e9,stroke:#009900,stroke-width:2px
    style B fill:#e8f5e9,stroke:#009900,stroke-width:2px
    style C fill:#fff4e1,stroke:#ff9900,stroke-width:2px
    style D fill:#fff4e1,stroke:#ff9900,stroke-width:2px
    style E fill:#fff4e1,stroke:#ff9900,stroke-width:2px
    style F fill:#fff4e1,stroke:#ff9900,stroke-width:2px
    style G fill:#e1f5ff,stroke:#0066cc,stroke-width:2px
    style H fill:#fce4ec,stroke:#cc0066,stroke-width:2px
    style I fill:#fce4ec,stroke:#cc0066,stroke-width:2px

在图示流程中，当通过日志分析、Gazebo 可视化完成参数调整或问题修复后，应当重新回到流程起点（从新一轮飞行测试与日志采集开始），形成闭环迭代的调试过程。

4.4 分析调试过程案例

本节展示的是同一套系统在同一场景下的三轮重复测试与分析过程：每次起飞都记录 .ulg 日志，用前文的 Python 脚本生成三轴位置对比图，逐步调整参数与脚本，直到 estimator_local_position 与 vehicle_local_position 的 X/Y/Z 曲线基本重合。

第一次测试：log_284——初步评估

log_284 是该案例中的第一次测试飞行，通过 Python 分析脚本生成的三轴位置对比图如下：

你可以下载对应的原始 .ulg 日志文件以便自行复现分析过程：

• log_284_2025-11-25-01-15-04.ulg

在 log_284 中可以看到：在切换到 Position 模式之前，estimator_local_position 与 vehicle_local_position 的三轴曲线基本重合，XYZ 一致性良好，说明 EKF2 内部状态与对外发布的位置估计在非 Position 模式下工作正常。
当通过 QGC 将飞控切换到 Position 模式后，图中的 X/Y 轴曲线开始出现明显的锯齿状位置跳变，飞控开始拒绝超范围的位置输入，平面位置估计在相邻采样之间快速来回抖动，最终导致控制回路输出剧烈变化，飞机进入失控状态。

问题原因分析：

1. Position 模式下的控制回路反馈振荡：在 Position 模式下，位置控制器会读取 vehicle_local_position 作为反馈信号，计算位置误差并生成控制指令。当位置估计不稳定时，会形成反馈循环：EKF2 内部估计（estimator_local_position）出现波动 → 位置控制器基于不稳定的位置反馈产生控制指令 → 控制指令导致飞机实际运动，进一步影响位置估计 → 形成振荡反馈，导致锯齿状跳变。这解释了为什么问题只在切换到 Position 模式后才暴露出来。

2. EKF2 数据拒绝机制导致的间歇性跳变：EKF2 的 innovation test 会计算预测值与测量值的残差（innovation），如果 test_ratio 超过门限（如 EKF2_EVP_GATE），会拒绝该次视觉数据。视觉数据被拒绝时，EKF2 只能依赖 IMU 进行预测，位置逐渐漂移；下次视觉数据被接受时，位置突然"跳回"到正确值。这种"拒绝-接受"的切换导致位置曲线出现锯齿状跳变。在 log_284 中，由于视觉噪声参数（EKF2_EV_POS_X/Y）设置过小，视觉数据频繁被拒绝，导致锯齿状跳变非常明显。

3. 两个位置数据源的不同处理逻辑：estimator_local_position 是 EKF2 内部状态，相对平滑但可能有延迟；vehicle_local_position 是经过位置控制器和范围限制处理后的对外发布值。当 EKF2 内部估计与控制器期望不一致时，vehicle_local_position 可能被限制或修正，导致两者出现偏差。在 log_284 中，两个曲线"打架"的情况正是这种处理逻辑差异的体现。

这一轮测试为后续调试建立了清晰的"问题基线"——问题只在 Position 模式下暴露，且主要集中在平面位置 X/Y 的估计质量上。

第二次测试：log_286——参数调整

在第二次测试 log_286 中，在飞行前对 EKF2_EV_POS_X，EKF2_EV_POS_Y 等视觉噪声参数以及相关 innovation gate（EKF2_EVP_GATE）进行了调整，希望减少视觉数据被拒绝的次数，并改善 EKF2 对外发布的位置输出质量：

对应的原始日志文件为：

• log_286_2025-11-25-04-42-44.ulg

对比 log_284 与 log_286 的分析图可以看到：在切换到 Position 模式时，X/Y 轴上虽然依然存在大量锯齿状跳变，但 estimator_local_position 与 vehicle_local_position 在两个平面轴上的轨迹已经基本重合，不再出现前一轮测试中那种明显彼此"打架"的情况。

改进原因分析：

通过调整 EKF2_EV_POS_X/Y 和 innovation gate（EKF2_EVP_GATE），减少了视觉数据被 EKF2 拒绝的频率。在 log_284 中，由于视觉噪声参数设置过小，EKF2 的 innovation test 频繁拒绝视觉数据，导致位置在"IMU 预测漂移"和"视觉数据跳回"之间快速切换，形成锯齿状跳变。参数调整后，虽然仍有跳变（说明问题尚未完全解决），但两个位置曲线已经基本重合，说明视觉数据被拒绝的频率显著降低，EKF2 内部状态与对外发布的位置估计已经趋于一致。

这表明视觉测量与 EKF2 内部状态本身是一致的，问题更可能来自当前 PX4 固件版本或其他高级参数配置，而不是 VRPN 数据链路或噪声模型本身。

第三次测试：log_287——融合效果收敛

第三次测试 log_287 展示的是在前两次测试基础上，更换 PX4 固件版本、重新配置 EKF2 相关参数并完成电机与遥控器校准之后，系统运行较为稳定的一次飞行：

你可以下载该次飞行的日志文件进行进一步分析：

• log_287_2025-11-25-05-30-36.ulg

在这次飞行中，从姿态控制到切换到 Position 模式的整个过程中，XYZ 三轴的 estimator_local_position 与 vehicle_local_position 曲线始终平滑且高度重合，X/Y 轴不再出现锯齿状位置跳变，Position 模式下飞行轨迹稳定可控，高度曲线也未出现明显漂移或突变。

通过三次测试的逐步改进，验证了问题的根本原因和解决方案：

1. 参数调整的作用（log_286）：通过调整 EKF2_EV_POS_X/Y 和 innovation gate（EKF2_EVP_GATE），减少了视觉数据被拒绝的频率，使两个位置曲线基本重合，证明了参数配置的重要性。

2. 固件升级与完整校准的协同效果（log_287）：更换 PX4 固件版本、重新配置 EKF2 参数并完成传感器校准后，彻底消除了锯齿状跳变。这说明问题不仅来自参数配置，也可能与固件版本的位置处理逻辑有关。新固件版本可能修复了 Position 模式下的位置处理逻辑问题，或者优化了 EKF2 与位置控制器之间的接口，使得两个位置数据源能够更好地同步。

3. 控制回路反馈振荡的消除：在 log_287 中，由于位置估计稳定，Position 模式下的控制回路不再出现振荡反馈。位置控制器能够基于稳定的位置反馈产生平滑的控制指令，避免了锯齿状跳变。

4. 数据拒绝机制的优化：通过参数调整和固件升级，EKF2 的 innovation test 能够更准确地评估视觉数据的质量，减少了不必要的拒绝，使得位置估计更加连续和平滑。

结合 Gazebo 可视化，对比 /mavros/local_position/pose 与 /vrpn_mocap/drone1/pose 可以看到，VRPN 融合后的整体轨迹与动捕原始轨迹保持一致。
通过这一个案例的三次测试与分析，可以将"日志采集 → Python 分析 → 参数/脚本调整 → 重新飞行验证"的闭环流程具体化，直到最终实现三轴位置曲线的高度重合，方便在后续调试中快速套用同样的方法。

参考文档

• PX4 User Guide - External Vision
• PX4 User Guide - EKF2 Tuning Guide
• MAVROS 官方文档
• ROS 1 rostopic 命令参考
• ROS 2 CLI 工具参考
• Gazebo 官方文档
• VRPN - Virtual-Reality Peripheral Network
• Flight Review 日志分析网站
• pyulog GitHub 仓库

MAVROS 与 MAVProxy：APM 平台上的使用场景、差异与取舍

本文围绕 ArduPilot/APM 飞控平台，系统梳理 MAVROS 与 MAVProxy 的定位、工作原理、典型使用场景、差异点与组合策略，并评估在不同任务形态下它们的必要性以及可替代方案。

flowchart TB
  A["APM/ArduPilot 飞控<br/>(MAVLink)"] --> RP["MAVProxy（路由/桥接）"]
  P["PX4 飞控<br/>(MAVLink)"] --> RR["MAVLink Router（路由）"]
  RP --> O("MAVLink 扇出")
  RR --> O
  O --> Q["QGC/地面站"]
  O --> S["MAVSDK 服务/后端"]
  O --> M
  
  %% ROS 子图（组合与数据流）
  subgraph ROS
    direction TB
    M["MAVROS"]
    RA["ROS 算法/感知/规划"]
    Tpub["状态/位置话题<br/>(/mavros/state, /local_position, /imu 等)"]
    Tsub["控制/Setpoint 话题<br/>(/setpoint_position, /setpoint_raw 等)"]
    Tsensor["传感器话题<br/>(IMU/里程计/相机/视觉定位)"]
    
    M --> Tpub
    Tpub --> RA
    RA --> Tsub
    Tsub --> M
    Tsensor --> RA
    Tsensor --> M
  end
  
  %% 可选：飞控直连地面站（仅人控/参数/任务管理）
  

• 只需“把数据给多人看/多服务用”：用 MAVProxy（或 MAVLink Router）。
• 需要“在 ROS 内做算法并控制飞机”：用 MAVROS；若还要多路分发，再叠加 MAVProxy。
• 面向 APM 的通用稳健方案：APM → MAVLink 路由工具 →（QGC | MAVROS | MAVSDK…）。

1. 概念

• MAVLink：飞控与外部系统之间的轻量级消息协议。消息是结构化的二进制帧，包含系统/组件 ID、消息 ID、序号、时间戳等；通过串口或 UDP/TCP 承载。关键点是“谁与谁在对话、以何种传输介质与频率对话”。
• APM/ArduPilot：在飞控端负责生成/消费 MAVLink 消息（心跳、状态、位置、参数、模式、任务、RCIO 等）；可配置消息流速率与输出端口。
• 典型链路：飞控串口/UDP → 路由/桥接（可选） → 地面站/算法/服务。路由关注“扇入/扇出与可靠转发”，桥接关注“协议域间的语义映射（如 MAVLink↔ROS）”。

2. MAVProxy：MAVLink 路由工具

2.1 概念

• 定位：轻量“地面站/路由工具”。核心目标是“从一个或多个输入稳定扇出到多个输出”，不改变 MAVLink 协议语义；由 ArduPilot 官方维护。
• 职责边界：以路由/转发为核心能力，其他能力按需启用。

2.2 工作机制

• 输入（master）：显式指定串口或 UDP/TCP 源，只转发声明的输入。
• 输出（out）：可并行配置多个 UDP/TCP 目的端，逐帧复制转发。
• 流控（streamrate）：向飞控申请消息组频率；需与其他客户端协同，避免相互抢写导致抖动。
• 插件化：按需加载日志、地形、地理围栏、仿真辅助等模块。
• 功能要点：
  • 稳定扇出与可观测：输出目的端清晰，异常易于定位（心跳/计数异常等）。
  • 低侵入与易部署：无需 ROS/DDS 等环境，常驻机载计算机做统一扇出。
  • 直接交互飞控：读写参数、切换模式、解锁/上锁。
  • 任务/航点：任务上传下载与流程管理。
  • 速率管理：统一申请与调整消息组频率，控制链路负载。
  • 日志记录：事件/日志输出，便于复现与追踪。
  • 地理与仿真：地理围栏、地形数据、SITL/联调辅助。

注意：关于 QGC 的 MAVLink 转发

QGC 的“MAVLink 转发”仅会将“QGC 接收到的 MAVLink 帧”单向转发到目标地址；它不会把来自该目标地址的控制/参数写入等指令再回送给飞控。因此，经由 QGC 转发链路发送控制命令通常会因无法到达飞控而超时。若需要可控的双向链路，请在飞控侧或近端使用 MAVProxy、MAVLink Router 等路由工具建立端到端会话。

2.3 典型使用场景

• 多下游并发：同一飞控流同时提供给 QGC、MAVSDK 服务、日志录制、状态监控。
• 网络解耦：在不改飞控输出的前提下，本机扇出多路流。
• 安全隔离：结合端口控制与防火墙/ACL 管控可达范围。
• 多飞控集中接入：一台伴随计算机统一接入多块飞控，集中扇出与会话管理。
• 蜂群/编队分发：为编队提供稳定的遥测与指令分发通道，易于规模化监控。
• 异构链路聚合：串口/UDP/蜂窝/以太网等多链路统一转发与限流。
• 伴随端常驻：作为“入口与分发”常驻机载，向地面站、算法、日志稳定供数。
• 诊断与复现：配合日志/事件与可观测性指标，快速定位断流、环路与高负载。

2.4 优势与局限

• 优势：简单可靠、部署门槛低、与 APM 生态契合度高、职责边界清晰。
• 局限：无语义映射与高层控制 API；需配合其他组件完成算法/定位/融合。

2.5 MAVProxy 与 PX4

• MAVProxy 是基于 MAVLink 协议层的路由/地面站工具，由 ArduPilot 社区维护；并非 APM 专属。任何产出/消费 MAVLink 的系统（APM、PX4、SITL、外设网关等）都可接入与转发。
• PX4 官方推荐在机载计算机侧采用其自有框架与工具链进行路由/桥接，例如 MAVLink Router（推荐）或基于 microROS/uXRCE-DDS 的通信栈，而非 MAVProxy。本推荐见 PX4 官方“机载电脑”文档。
• MAVProxy 能够稳定转发 PX4 的遥测与指令流；实际工程中仍应优先评估 PX4 推荐方案，只有在需要命令行交互、插件生态与现场诊断等特性时再考虑使用 MAVProxy。
• 建议：
  • 以 PX4 官方支持路径、长期维护与系统服务形态为优先：倾向 MAVLink Router 或 uXRCE-DDS。
  • 需要命令行交互、插件能力与快速诊断：可选 MAVProxy（注意协调 SYSID/COMPID、速率与签名/隔离策略）。
• 注意：对 PX4 而言，MAVProxy“可用但非官方推荐/非必要”；对 APM 生态契合度最高，但并非唯一可选的路由工具。

3. MAVROS：ROS↔MAVLink

3.1 概念

• MAVROS 是 ROS 的一个软件包，用于在 ROS 与支持 MAVLink 的飞控之间进行通信与语义映射；是连接 ROS 与 PX4/APM 的关键桥梁，使开发者可直接在 ROS 中以话题/服务的形式使用飞控能力。
• 若不使用 MAVROS，开发者需自行解析 MAVLink 并构建框架，无法复用 ROS 的大量工具链与第三方算法生态；使用 MAVROS 可显著降低集成成本与学习曲线。

3.2 工作机制

• 插件化：如 state、setpoint_position、setpoint_raw、global_position、param、cmd 等，分别负责子域消息的解码、校验与发布/订阅。
• 语义映射：将 MAVLink 原语映射为 ROS 常用消息类型（geometry_msgs、sensor_msgs 等）。
• 控制与时间/坐标：提供 Offboard 控制接口；可启用时间同步与 TF/外参管理以保证闭环稳定。

3.3 典型使用场景

• Offboard/伴随计算：在 ROS 中执行路径规划、轨迹跟踪、任务编排，通过 setpoint 接口控制飞控。
• 传感器/定位融合：将里程计/SLAM/视觉定位等结果映射到飞控或用于外环控制。
• 仿真闭环：在 Gazebo/Ignition 等仿真环境中与 PX4/APM 形成闭环，快速迭代算法与任务逻辑。

3.4 优势与局限

• 优势：强语义桥接、算法栈与工具链丰富、成熟的 Offboard 接口与可视化支持（RViz、rqt 等）。
• 局限：引入 ROS 运行时的资源与复杂度；需管理话题队列、调度与时延；多客户端需协调参数写入与消息速率，避免竞争。

3.5 MAVROS 与 PX4/APM

• 同时支持 PX4 与 APM，是两者与 ROS 之间的主流桥接路径之一。
• 在 PX4 侧，亦可选用 uXRCE-DDS（ROS2）等官方通信路径；选择取决于算法栈与系统架构。
• 与 MAVProxy 的关系：二者互补。常见组合为“APM/PX4 → 路由（MAVProxy/Router）→ MAVROS/算法栈”。

4. 场景对比与实践策略

• 定位差异：
  • MAVProxy：面向链路与路由，最小可行转发；不改变消息语义。
  • MAVROS：面向算法与应用，完成协议域到 ROS 域的语义投射与控制接口。
• 部署复杂度：MAVProxy < MAVROS（需要 ROS 运行时与插件管理）。
• 资源占用：MAVProxy 轻；MAVROS 随插件与话题规模增长。
• 实时性/时延：
  • 路由层（MAVProxy）时延极低，主要受网络与系统负载影响。
  • ROS 桥接（MAVROS）受调度、话题队列、时间同步影响，需工程化调优（CPU 亲和、QoS/队列深度、发布频率）。
• 容错与观测：
  • MAVProxy 输出侧可快速定位断流或下游异常。
  • MAVROS 通过 ROS 工具链（rqt、roswtf、tf 树）进行问题定位，粒度更细但系统更复杂。

4.1 使用建议

• 何时用 MAVProxy：
  • 需要“把飞控数据转给若干消费方”，且无 ROS 依赖。
  • 需要在机载端稳定地“一个输入、多输出”扇出，减少对飞控端改动。
• 何时用 MAVROS：
  • 主要运行环境在 ROS 内：路径规划、定位融合、任务执行、仿真闭环等。
  • 不需要对外再扇出多条原始 MAVLink 流，或扇出可由 ROS 内部其他方式满足。
• 何时组合使用：
  • 在机载端用 MAVProxy 统一从飞控读取并扇出：一支给 QGC/监控，另一支给本机或远端的 MAVROS/算法栈。
  • 这样可以把“链路可用性/端口安全”与“算法语义处理”解耦，减少相互干扰。

4.2 可替代/互补方案对比

• MAVLink Router（mavlink-routerd）：
  • 职能与 MAVProxy 的“路由/扇出”类似，C 实现，资源占用更低，配置文件式管理，常见于 PX4，但同样适用 ArduPilot。
  • 优点：性能优、守护进程形态、基于规则的管控；缺点：交互/调试能力不如 MAVProxy 命令行直观。
• MAVSDK / MAVSDK-Server：
  • 面向应用开发的高层 API（C++/Python/Go 等），便于快速构建控制/任务逻辑。
  • 与 MAVProxy/MAVROS 并不冲突：常见架构是“APM→路由→MAVSDK、QGC 等并列消费”。
• cmavnode / 自研网关：
  • 轻量桥接/转发工具；适合极小型系统或定制需求，但生态/文档沉淀一般。
• 直接地面站（QGC/Mission Planner）：
  • 仅人控与参数/任务管理时可以直连；不解决“多消费者并发”“程序化接口”问题。

4.3 工程选择建议

• 若主要诉求是“稳定扇出”与“运维可控”，首选 MAVProxy 或 MAVLink Router（择其一）。
• 若主要诉求是“ROS 内算法闭环”，首选 MAVROS；是否叠加路由视是否有多消费者与安全隔离需求而定。

5. 项目实践的思考

• 链路梳理：明确输入端（串口/UDP 单播/广播）、下游清单（QGC、服务、算法端）。
• 频率与带宽：
  • 统一在路由侧申请/调整流速，避免多个客户端抢写导致消息抖动。
  • 关注高频消息（姿态/位置/IMU），在无线链路下控制总体带宽与丢包敏感度。
• ID 与签名：
  • 多客户端写参数/控制时，确保系统/组件 ID 唯一；在需要时启用 MAVLink 签名与网络隔离。
• 资源与实时性：
  • 在机载电脑上为路由与关键节点设定 CPU 亲和与优先级，保持时延稳定。
• 可观测性与排障：
  • 路由侧：定期检查下游端口存活状态、丢包与心跳计数；
  • ROS 侧：关注时间同步、话题队列、TF 树一致性与控制回路频率。

参考文档

• MAVROS 官方文档
• ArduPilot 官方文档
• QGroundControl 用户指南
• MAVLink 协议规范
• MAVProxy 文档（ArduPilot）
• PX4 机载电脑（官方推荐路由：MAVLink Router / uXRCE-DDS）
• ArduPilot 开源飞控之 MAVProxy 简介（应用拓扑与多机场景）
• MAVROS 和 MAVLink 的关联与区别（CSDN）