• 嵌入式
• 物联网
• 机器人
• 自动驾驶
• 智能硬件

导读：本文完整讲解如何在 PX4 上接入 VIO（视觉惯性里程计）与 MoCap（动作捕捉）系统，用外部位姿数据在无 GPS 或 GPS 失效的环境（室内、桥下等）中实现导航。你将掌握 PX4 的 MAVLink 外部位置消息映射、FRD/NED 坐标系变换技巧、EKF2 与 LPE 两套估计器的调参方法、基于 ROS/MAVROS 的数据中继与话题重映射，以及首飞前的完整验证流程。

VIO 与 MoCap 系统都能通过"视觉"信息确定飞行器的位姿（Pose，位置 + 姿态），两者的核心区别在于参考视角：

• VIO使用机载传感器，从飞行器自身视角获取位姿数据（即自运动估计 egomotion）；
• MoCap使用一组机外摄像头，在三维空间中给出飞行器位姿（即由外部系统告诉飞行器"你在哪"）。

无论哪种系统，其位姿数据都可以用于更新 PX4 的局部位置估计（相对于局部原点）；同时可选地融合进飞行器姿态估计。此外，如果外部位姿系统还能提供线速度测量，则可进一步改善状态估计——注意线速度融合目前仅由 EKF2 支持。

本文档面向"将 MoCap/VIO 数据送入 PX4"的完整配置链路，涵盖 MAVLink 直连与 ROS/MAVROS 两种方式，并针对 VICON、OptiTrack 等 MoCap 系统以及 ROVIO、SVO、PTAM 等视觉估计系统给出可操作步骤。

::: info重要前提：以下配置步骤根据你所使用的估计器（EKF2 或 LPE）而有所不同，请先确定目标估计器再按对应章节操作。 :::

PX4 MAVLink 集成：外部位置消息到 uORB 的映射

PX4 使用下列 MAVLink 消息接收外部位置信息，并将其映射为对应的 uORB 主题：

• EKF2只订阅vehicle_visual_odometry主题，因此只能处理前两种消息——MoCap 系统若要与 EKF2 配合，必须发送这两类消息；ODOMETRY 是唯一还能同时携带线速度的消息。
• LPE同时订阅两个主题，因此可以处理上述全部消息。

::: tip EKF2 是 PX4 的默认估计器，比 LPE 经过更充分的测试与支持，应优先选用。 :::

消息应以30Hz（含协方差时）到 50Hz的频率持续流式发送。如果消息率过低，EKF2 将不会融合外部视觉消息。

以下 MAVLink "视觉"消息目前不被 PX4 支持： GLOBAL_VISION_POSITION_ESTIMATE、 VISION_SPEED_ESTIMATE、 VICON_POSITION_ESTIMATE。

源码视角：MAVLink 接收器如何转换位姿数据

PX4 固件侧的消息解析位于 src/modules/mavlink/mavlink_receiver.cpp。以VISION_POSITION_ESTIMATE为例（mavlink_receiver.cpp#L1533-L1568），接收器将消息解码后填充为vehicle_odometry_s结构并发布到vehicle_visual_odometry主题：

• 位置 x/y/z 与姿态（roll/pitch/yaw 欧拉角转四元数）被直接写入；
• 6x6 协方差矩阵上三角中的对角元素被抽取为位置方差与姿态方差（如covariance[0]对应 X 行 0 列，covariance[20]对应偏航方差）；
• 时间戳通过_mavlink_timesync.sync_stamp()做 MAVLink 时间同步校准后再写入timestamp_sample。

ODOMETRY的处理（mavlink_receiver.cpp#L1571-L1782）则更加细致：接收器根据frame_id（NED/ENU/FRD/FLU）对位置做坐标系换算，例如MAV_FRAME_LOCAL_ENU会被转换为 NED（x↔y交换、z取负），MAV_FRAME_LOCAL_FRD被标记为POSE_FRAME_FRD；线速度则依据child_frame_id转换到对应的速度坐标系（含机体 FRD 系MAV_FRAME_BODY_FRD）。最后根据estimator_type分发：VISION/VIO类型进入_visual_odometry_pub（EKF2 可消费），MOCAP类型进入_mocap_odometry_pub（LPE 可消费），其余类型（如 GPS、LIDAR）会被拒绝并记录错误日志。这正是"EKF2 只能处理视觉类消息、MoCap 必须转换为视觉类消息"这一规则的代码根源。

参考坐标系：FRD、NED 与外部位姿的坐标变换

PX4 在机体坐标系和参考坐标系上都使用FRD约定（XForward 向前、YRight 向右、ZDown 向下）。当使用磁力计航向时，PX4 参考系的 x 轴与北方对齐，因此被称为NED（XNorth 北、YEast 东、ZDown 下）。PX4 估计器的参考系航向与外部位姿估计的参考系航向在大多数情况下并不一致，因此外部位姿估计的参考系被单独命名为 MAV_FRAME_LOCAL_FRD。

根据参考系的来源不同，你需要在发送 MAVLink Vision/MoCap 消息之前对位姿估计应用自定义变换，改变位姿估计的父子坐标系方向，使其符合 PX4 约定。ROS 用户可以参考 MAVROS 的odom插件 中实现的必要变换，更详细说明见下文 参考坐标系与 ROS。

轴交换技巧：以 OptiTrack 为例

例如使用 OptiTrack 框架时，其局部坐标系中 $x$、$z$ 轴位于水平面（x 向前、z 向右），而 $y$ 轴垂直向上。一个简单技巧是交换坐标轴以获得 NED 约定：

如果x_{mav}、y_{mav}、z_{mav}是通过 MAVLink 发送的位置反馈坐标，则：

x_{mav} = x_{mocap} y_{mav} = z_{mocap} z_{mav} = - y_{mocap}

姿态方面：保持四元数的标量部分 $w$ 不变，向量部分 $x$、$y$、$z$ 按同样方式交换。这个技巧适用于任何系统——只要你的 MoCap 输出需要转成 NED 帧，观察其输出并按需交换坐标轴即可。

源码视角：固件对 MAVLink 坐标系声明的处理

值得一提的是，固件侧对ODOMETRY消息本身也做了坐标系声明级的适配（见上文 mavlink_receiver.cpp#L1571-L1782）：若外部系统能正确设置frame_id = MAV_FRAME_LOCAL_FRD，则位置可直接以 FRD 帧存入vehicle_odometry_s，无需用户手工做轴交换；若发送的是MAV_FRAME_LOCAL_NED/MAV_FRAME_LOCAL_ENU/MAV_FRAME_LOCAL_FLU，接收器也会自动换算为 PX4 内部的 NED/FRD 表示。尽量在消息中声明正确的 frame_id，可减少应用层的手工变换负担。

EKF2 调参与配置

本节为快速概览，更详细的信息请参考 使用 PX4 导航滤波器（EKF2）。

使用 EKF2 融合外部位置信息需要设置以下参数（可在QGroundControl>Vehicle Setup > Parameters > EKF2中设置）：

你还可以分别通过 EKF2_GPS_CTRL、EKF2_BARO_CTRL 和 EKF2_RNG_CTRL 禁用 GNSS、气压计与测距仪融合。

::: tip参数修改后需重启飞控才能生效。 :::

源码视角：EKF2 外部视觉参数族的完整定义

EKF2 的外部视觉参数集中在 src/modules/ekf2/params_external_vision.yaml 中定义，除了上表列出的核心参数外，还包含一组噪声与门限参数，帮助你精细控制融合行为：

• EKF2_EV_CTRL（bitmask，默认 0，范围 0~15）：bit0 水平位置融合、bit1 垂直位置融合、bit2 3D 速度融合、bit3 偏航融合。
• EKF2_EV_DELAY（float，默认 0，范围 0~300ms，步进 0.1ms，需重启生效）：视觉位置估计器相对 IMU 测量的延迟。
• EKF2_EV_NOISE_MD（enum，默认 0）：0 表示噪声取自视觉消息、EV 噪声参数仅作下限；1 表示直接使用参数作为观测噪声。
• EKF2_EV_QMIN（int32，默认 0，范围 0~100）：外部视觉的最低质量阈值，低于该阈值的融合会被拒绝（质量字段由部分 VIO 系统提供，完全可选）。
• EKF2_EVA_NOISE（float，默认 0.1 rad，范围 0.05~）：视觉角度测量的噪声，用于下限约束或替代消息中的不确定性。
• EKF2_EVP_NOISE（float，默认 0.1 m，范围 0.01~）：视觉位置测量噪声。
• EKF2_EVV_NOISE（float，默认 0.1 m/s，范围 0.01~）：视觉速度测量噪声。
• EKF2_EVP_GATE（float，默认 5.0 SD）：位置融合的新息一致性检验门限（标准差倍数）。
• EKF2_EVV_GATE（float，默认 3.0 SD）：速度融合的新息一致性检验门限。
• EKF2_EV_POS_X/Y/Z（float，默认 0.0 m）：VI 传感器焦点在机体坐标系中的位置，原点相对飞行器重心。

EKF2_EV_DELAY 的调参

EKF2_EV_DELAY 是视觉位置估计器相对于 IMU 测量的延迟。换句话说，它是视觉系统时间戳与"实际"采集时间（按 IMU 时钟——EKF2 的"基准时钟"记录）之差。

从技术上讲，如果 MoCap 与（例如）ROS 计算机之间存在正确的时间戳（不只是到达时间）与时间同步（如 NTP），该值可以设为 0。但实际中，由于 MoCap→PX4 整条链路的延迟高度依赖具体部署，通常需要经验调参——很少有系统能实现全链路同步。

延迟的粗略估计可以从日志中检查 IMU 速率与 EV 速率的偏移得到。要启用 EV 速率日志，请设置 SDLOG_PROFILE 的 bit 7（Computer Vision and Avoidance）。

::: info 上图这类"外部数据 vs 机载估计"的曲线可以使用 FlightPlot 或类似飞行分析工具生成。截至 2021 年 7 月，Flight Review 与 MAVGCL 均不支持该功能。 :::

该值还可以进一步微调：在动态机动过程中，改变参数值并找到使 EKF 新息（innovation）最低的值即可。

LPE 调参与配置

使用 LPE 前，你需要先切换到 LPE 估计器，设置以下参数：LPE_EN（1）、EKF2_EN（0）、ATT_EN（0）。

::: info 若目标是px4_fmu-v2硬件，你还需要使用包含 LPE 模块的固件版本（其他 FMU 系列硬件的固件同时包含 LPE 与 EKF）。LPE 版本可在每个 PX4 发行版的 zip 文件中找到，也可以使用构建命令make px4_fmu-v2_lpe从源码构建。更多细节参见构建代码。 :::

启用外部位姿输入

使用 LPE 融合外部位置信息需要设置以下参数（可在QGroundControl>Vehicle Setup > Parameters > Local Position Estimator中设置）：

禁用气压计融合

如果 VIO 或 MoCap 信息已能提供高精度高度，禁用 LPE 的气压计修正可以减少 Z 轴漂移。在 QGroundControl 中取消勾选 LPE_FUSION 参数的fuse baro选项即可。

调优噪声参数

如果你的视觉或 MoCap 数据精度很高，且希望估计器紧密跟踪它，则应减小标准差参数：LPE_VIS_XY 和 LPE_VIS_Z（用于 VIO）或 LPE_VIC_P（用于 MoCap）。减小它们会使估计器更信任传入的位姿估计。你可能需要将它们设置得低于允许的最小值并强制保存。

::: tip 如果性能仍然不佳，尝试增大 LPE_PN_V 参数，这会使估计器在速度估计时更信任测量值。 :::

源码视角：LPE 的参数与 MoCap 传感器实现

LPE 的噪声参数在 src/modules/local_position_estimator/params.yaml 中定义：

• LPE_VIS_XY（默认 0.1 m，范围 0.01~1）、LPE_VIS_Z（默认 0.5 m，范围 0.01~100）：VIO 位置标准差。
• LPE_VIC_P（默认 0.001 m，范围 0.0001~1）：Vicon/MoCap 位置标准差。
• LPE_PN_V（默认 0.1，范围 0~1）：速度传播噪声密度，官方注释明确指出"增大以更信任测量、减小以更信任模型"。
• LPE_FUSION（bitmask，默认 145，即二进制 10010001，对应 GPS + baro + land detector）：bit2 为fuse vision position，bit7 为fuse baro，这正是上文"勾选/取消勾选"对应的位操作。

LPE 侧对 MoCap 数据的实际处理在 src/modules/local_position_estimator/sensors/mocap.cpp：它需要至少 20 个样本（REQ_MOCAP_INIT_COUNT = 20）完成初始化统计，超时阈值为 0.2 秒（MOCAP_TIMEOUT = 200000微秒）；若测量标准差超过EP_MAX_STD_DEV = 100.0会被判为无效。初始化成功后，若全局投影参考已就绪且未同时融合视觉估计，LPE 还会以 MoCap 估计作为基准初始化全局原点（经纬度参考），从而为后续局部位置到全局位置的转换提供依据。

仅靠局部位置启用自动模式

PX4 的所有自动飞行模式（如 Mission、Return、Land、Hold、Orbit）都需要_全局_位置估计，通常来自 GPS/GNSS 系统。

只有_局部_位置估计（来自 MOCAP、VIO 或类似系统）的系统，可以使用 SET_GPS_GLOBAL_ORIGIN MAVLink 消息将 EKF 的原点设置为某个特定全局位置。此后 EKF 将基于该原点与局部坐标系位置提供全局位置估计。

这可用于规划与执行室内任务、设置局部返航点等场景。在源码侧，SET_GPS_GLOBAL_ORIGIN的处理同样位于 src/modules/mavlink/mavlink_receiver.cpp（MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN分支）。

与 ROS 协同工作

ROS 并不是提供外部位姿信息的_必需_组件，但强烈推荐使用——它已经与 VIO 和 MoCap 系统有良好的集成。使用前请确保 PX4 已按上文完成配置。

将位姿数据送入 ROS

VIO 与 MoCap 系统获取位姿数据的方式不同，各自有独立的设置与话题。特定系统的设置见下文具体系统配置；其他系统请查阅厂商设置文档。

将位姿数据中继到 PX4

MAVROS 提供了将 VIO 或 MoCap 系统的视觉估计中继出去的插件，可采用以下流水线：

LPE可以使用以上任何流水线。

EKF2只支持"视觉"流水线。要配合 EKF2 使用 MoCap 数据，你必须将来自 MoCap 的位姿话题进行重映射：

• MoCap 的geometry_msgs/PoseStamped或geometry_msgs/PoseWithCovarianceStamped类型话题必须重映射到/mavros/vision_pose/pose。其中geometry_msgs/PoseStamped最常见，因为 MoCap 通常不附带协方差数据。
• 如果通过nav_msgs/OdometryROS 消息获取数据，则需重映射到/mavros/odometry/out，并确保frame_id与child_frame_id相应更新。
• 里程计帧frame_id = odom、child_frame_id = base_link可以通过更新mavros/launch/px4_config.yaml来修改。不过当前版本 mavros（1.3.0）需要能够利用 tf 树找到从frame_id到硬编码帧odom_ned的变换；child_frame_id同样需要与硬编码帧base_link_frd在 tf 树中连通。如果使用 mavros1.2.0且未更新mavros/launch/px4_config.yaml，则可以放心使用frame_id = odom、child_frame_id = base_link的里程计帧。
• 注意：如果使用child_frame_id = base_link向 PX4 发送里程计数据，必须确保nav_msgs/Odometry消息中的twist（速度）部分以**机体坐标系（body frame）**表达，绝不能是惯性坐标系（inertial frame）！

参考坐标系与 ROS

ROS 与 PX4 使用的局部/世界坐标系不同：

::: tip 关于 ROS 坐标系更多信息参见 REP105: Coordinate Frames for Mobile Platforms。 :::

两种坐标系对比如下图所示（左为 FRD，右为 FLU）：

使用 EKF2 进行外部航向估计时，磁北可以被忽略，也可以计算并补偿相对磁北的航向偏移。根据你的选择，偏航角将分别以磁北或局部x为基准给出。

::: info 在 MoCap 软件中创建刚体时，务必先将机器人局部x轴与世界x轴对齐，否则偏航估计会产生偏移，可能导致外部位姿估计融合失效。当机体与参考坐标系对齐时，偏航角应为零。 :::

使用 MAVROS 时，这个操作很直接。ROS 使用 ENU 坐标系约定，因此位置反馈必须以 ENU 提供。如果你有 OptiTrack 系统，可以使用mocap_optitrack节点，它直接将物体位姿以 ENU 发布在 ROS 话题上。通过重映射，你可以将其原样发布到mocap_pose_estimate，无需任何变换，MAVROS 会自动处理 NED 转换。

MAVROS 的 odometry 插件借助 ROS 的 tf 包简化了坐标系处理。你的外部位姿系统可能有完全不同的坐标系约定，与 PX4 不匹配——外部位姿估计的机体坐标系取决于你在 MOCAP 软件中如何设置机体坐标系，或 VIO 传感器如何安装。MAVROS odometry 插件需要知道外部位姿的子坐标系相对于 MAVROS 已知的机体 FRD 或 FLU 坐标系的方向。因此，你必须将外部位姿的机体坐标系加入 tf 树。这可以通过在 ROS launch 文件中加入以下代码行（按需调整）实现：

<node pkg="tf" type="static_transform_publisher" name="tf_baseLink_externalPoseChildFrame" args="0 0 0 <yaw> <pitch> <roll> base_link <external_pose_child_frame> 1000"/>

请确保修改 yaw、pitch、roll 的值，使外部位姿的机体坐标系正确连接到base_link或base_link_frd。关于如何指定坐标系间的变换，可参考 tf 包 的说明。你可以用 rviz 检查坐标系是否正确连接。external_pose_child_frame的名称必须与nav_msgs/Odometry消息的child_frame_id一致。

同样适用于外部位姿的参考坐标系：你需要将外部位姿的参考坐标系作为子坐标系连接到odom或odom_frd帧。相应地调整以下代码行：

<node pkg="tf" type="static_transform_publisher" name="tf_odom_externalPoseParentFrame" args="0 0 0 <yaw> <pitch> <roll> odom <external_pose_parent_frame> 1000"/>

如果参考坐标系的 z 轴指向上方，你可以无旋转（yaw=0, pitch=0, roll=0）地将其连接到odom帧。external_pose_parent_frame的名称必须与里程计消息的frame_id一致。

::: info 使用 MAVROSodom插件时，务必确保没有其他节点发布外部位姿参考系与子坐标系之间的变换，否则可能破坏 tf 树。 :::

具体系统配置

OptiTrack MoCap

以下步骤说明如何将 OptiTrack 系统的位置估计馈送给 PX4。前提是 MoCap 系统已完成标定（校准教程可参考相应视频）。

在MotiveMoCap 软件中的步骤

• 将机器人前进方向与系统的 +x 轴对齐（参见 OptiTrack 标定文档）。
• 在 Motive 软件中定义一个刚体。给机器人起一个不含空格的名称，例如用robot1而不是Rigidbody 1。
• 启用 Frame Broadcast 与 VRPN 流式传输。
• 将 Up 轴设置为 Z 轴（默认是 Y）。

将位姿数据送入 ROS

• 安装vrpn_client_ros包。
• 通过以下命令获取每个刚体在独立话题上的位姿：

roslaunch vrpn_client_ros sample.launch server:=<mocap machine ip>

如果将刚体命名为robot1，你将得到类似/vrpn_client_node/robot1/pose的话题。

中继/重映射位姿数据

MAVROS 提供了将发布在/mavros/vision_pose/pose上的位姿数据中继到 PX4 的插件。假设 MAVROS 正在运行，你只需将 MoCap 的位姿话题/vrpn_client_node/<rigid_body_name>/pose重映射到/mavros/vision_pose/pose。注意 MAVROS 还有一个mocap话题可以向 PX4 馈送ATT_POS_MOCAP，但它不适用于 EKF2；不过它可以与 LPE 配合使用。

::: info 话题重映射已在上文"将位姿数据中继到 PX4"中说明（/vrpn_client_node/<rigid_body_name>/pose的类型是geometry_msgs/PoseStamped）。 :::

假设你已按上文配置好 EKF2 参数，PX4 现在就绪并开始融合 MoCap 数据，可以着手首次飞行了。

首次飞行

完成上述任一（具体）系统配置后，即可开始测试。以下说明适用于 MoCap 与 VIO 系统。

检查外部估计

首次飞行前务必完成以下检查：

• 将 PX4 参数MAV_ODOM_LP设为 1。PX4 会将收到的外部位姿以 MAVLink ODOMETRY 消息流回。
• 使用QGroundControl的 MAVLink Inspector 检查这些 MAVLink 消息。操作时旋转飞行器，直到ODOMETRY消息的四元数非常接近单位四元数（w=1, x=y=z=0）。
• 此时机体坐标系与外部位姿系统的参考坐标系对齐。如果无法在不滚动或俯仰飞行器的情况下让四元数接近单位四元数，说明坐标系可能仍有俯仰或横滚偏移。此时不要继续，请再次检查坐标系。
• 对齐后，将飞行器从地面拿起，应看到位置的 z 坐标减小；向前移动飞行器，x 坐标应增大；向右移动，y 坐标应增大。如果外部位姿系统还发送线速度，还应检查线速度——确保线速度以FRD机体坐标系表达。
• 将 PX4 参数MAV_ODOM_LP设回 0。PX4 将停止回传该消息。

在固件侧，回传 ODOMETRY 的实现位于 src/modules/mavlink/streams/ODOMETRY.hpp：该流订阅vehicle_odometry主题，将位姿帧（NED/FRD）与速度帧（NED/FRD/BODY_FRD）映射回对应的 MAVLinkframe_id/child_frame_id，并填充位置、四元数、速度、角速度、协方差、reset_counter与quality等字段，最后以MAV_ESTIMATOR_TYPE_AUTOPILOT标识发送——这正好解释了MAV_ODOM_LP使能后你能在地面站看到的外部位姿回环。

起飞操作

如果上述步骤一致，就可以尝试首次飞行。

将机器人放在地面并开始流式传输 MoCap 反馈。将左手（油门）摇杆拉到底并解锁电机。

此时，保持左摇杆处于最低位置，切换到位置控制。你应该看到绿色指示灯——绿灯表示位置反馈可用且位置控制已激活。

将左摇杆置于中间位置，这是死区。在该摇杆值下，机器人保持高度；抬高摇杆会增加参考高度，降低则会减小高度。右摇杆对 x、y 的操作同理。

提高左摇杆的值，机器人将起飞；随后立即将其放回中间。检查它能否保持位置。

如果一切正常，你可能希望设置一个机外（offboard）实验，从远程地面站发送位置设定点。

参考实现与源码速查

• 嵌入式
• 物联网
• 机器人
• 自动驾驶
• 智能硬件