# 使用视觉或运动捕捉系统进行位置估算
视觉惯性测距（VIO）和运动捕捉（MoCap）系统可让载具在全球位置源不可用或不可靠时（如在室内或在桥下飞行时等）进行导航。
VIO 和 MoCap 都能确定载具的 姿势 (quot;视觉"信息（位置和姿态）。它们之间的主要区别在于框架视角：
- VIO 使用 机载传感器 从载具的角度获取姿态数据（见 自我运动 (打开新窗口）).
- MoCap 采用的系统是 车载摄像机 来获取三轴空间中的载具姿态数据（也就是说，它是一个告诉载具姿态的外部系统）。
来自任一类型系统的姿态数据都可用于更新基于 PX4 的自动驾驶仪的本地位置估计（相对于本地原点），也可选择融合到飞行器姿态估计中。此外，如果外部姿态系统还提供线性速度测量值，则可用于改进状态估计（线性速度测量值的融合仅由 EKF2 支持）。
本主题将介绍如何配置基于 PX4 的系统，以便从 MoCap/VIO 系统（通过 ROS 或其他 MAVLink 系统）获取数据，更具体地说，就是如何设置 MoCap 系统（如 VICON 和 Optitrack）和基于视觉的估算系统（如 罗维奥 (打开新窗口）, SVO (打开新窗口） 和 PTAM (打开新窗口）).
备注
根据您使用的是 EKF2 还是 LPE 估算器，说明有所不同。
# PX4 MAVLink 集成
PX4 使用以下 MAVLink 信息获取外部位置信息，并将其映射到 uORB 主题:
| MAVLink | uORB |
|---|---|
| 视觉位置估计 (打开新窗口） | 载具视觉测量 |
奥数 (打开新窗口） (frame_id = mav_frame_local_frd (打开新窗口）) | 载具视觉测量 |
| ATT_POS_MOCAP (打开新窗口） | 载具几何模型 |
奥数 (打开新窗口） (frame_id = mav_frame_mocap_ned (打开新窗口）) | 载具几何模型 |
EKF2 只订阅 载具视觉测量 因此只能处理前两个报文（MoCap 系统必须生成这些报文才能与 EKF2 协同工作）。里程测量报文是唯一能同时向 PX4 发送线性速度的报文。LPE 估算器订阅了两个主题，因此可以处理上述所有报文。
TIP
EKF2 是 PX4 使用的默认估计器。它比 LPE 经过更好的测试和支持，应优先使用。
信息流的频率应在 30 赫兹（如果包含协方差）和 50 赫兹之间。如果信息传输速率过低，EKF2 将无法融合外部视觉信息。
PX4 目前不支持以下 MAVLink "vision" 信息： global_vision_position_estimate (打开新窗口）, 速度估计 (打开新窗口）, Vicon_position_estimate (打开新窗口）
# 参考框架
PX4 使用 FRD（X Forward，Y Right和Z Down)的本地车身框架以及参考框架。当使用磁强计的航向时，PX4 参考框架的 x 轴将与北向对齐，因此称为 NED（X North, Y East，Z D自己的）。在大多数情况下，PX4 估算器的参照系和外部姿态估算器的参照系的航向并不一致。因此，外部姿态估计参考框架的名称有所不同，它被称为 mav_frame_local_frd (打开新窗口）.
根据参考帧的来源，您需要在发送 MAVLink Vision/MoCap 信息之前对姿势估计应用自定义变换。这对于改变姿态估计的父框架和子框架的方向是必要的，以便使其符合 PX4 标准。查看 MAVROS 敖德 插头 (打开新窗口） 进行必要的转换。
TIP
ROS 用户可以在下面的 参考框架和 ROS.
例如，如果使用 Optitrack 框架，本地框架有
如果 x_{mav}, y_{mav} 和 z_{mav} 是作为位置反馈通过 MAVLink 发送的坐标，那么我们可以得到
x_{mav} = x_{mocap} y_{mav} = z_{mocap} z_{mav} = - y_{mocap}
关于方向，保留标量部分 w 的四元数相同，并交换矢量部分 x, y 和 z 用同样的方法。您可以在每个系统中应用这一技巧--如果您需要获得 NED 帧，请查看您的 MoCap 输出并相应地交换轴。
# EKF2 调整参数/配置
注意：这只是一个快速概述。更多详细信息，请查阅 EKF2 调整参数指南
要使用 EKF2 外部位置信息，必须设置以下参数（这些参数可在以下页面中设置 QGroundControl >； 载具设置> 参数> EKF2).
| 参数 | 外部位置估计设置 |
|---|---|
| EKF2_EV_CTRL | 设置 水平位置融合, 垂直视觉融合, 速度融合和 偏航融合根据您所需的融合模式。 |
| EKF2_HGT_REF | 设置为 愿景 将视觉作为高度估计的参考源。 |
| EKF2_EV_DELAY | 设置为测量时间戳与实际捕捉时间之差。更多信息，请参阅 低于. |
| EKF2_EV_POS_X, EKF2_EV_POS_Y, EKF2_EV_POS_Z | 设置视觉传感器（或 MoCap 标记）相对于机器人身体框架的位置。 |
您还可以使用以下功能禁用 GNSS、气压计和测距仪融合功能 EKF2_GPS_CTRL, EKF2_BARO_CTRL 和 EKF2_RNG_CTRL分别为
TIP
重新启动飞行控制器，以使参数更改生效。
# 调整 EKF2_EV_DELAY
EKF2_EV_DELAY 是 视觉位置估计器相对于 IMU 测量的延迟.
或者换句话说，它是视觉系统时间戳与 IMU 时钟（EKF2 的基准时钟）记录的实际捕捉时间之间的差值。
从技术上讲，如果 MoCap 和（例如）ROS 计算机之间有正确的时间戳（不只是到达时间）和时间同步（如 NTP），则可以将其设置为 0。实际上，这需要根据经验进行调整，因为整个 MoCap->PX4 链中的延迟与设置密切相关。很少有系统能设置成完全同步的链！
通过检查 IMU 速率与 EV 速率之间的偏移，可以从日志中大致估算出延迟时间。要启用 EV 速率记录，请将记录中的第 7 位（计算机视觉和规避）设置为 SDLOG_PROFILE.
可以通过改变参数进一步调整该值，以找到在动态机动过程中 EKF 创新最小的值。
# LPE 调整参数/配置
您首先需要 改用 LPE 估计器 通过设置 sys_mc_est_group 参数。
备注
如果瞄准 px4_fmu-v2 您还需要使用包含 LPE 模块的固件版本（其他 FMU 系列硬件的固件同时包含 LPE 和 EKF）。LPE 版本可在每个 PX4 版本的压缩文件中找到，也可使用构建命令从源代码中构建 使 px4_fmu-v2_lpe.湖泊 构建代码 了解更多详情。
# 启用外部姿势输入
要通过 LPE 使用外部位置信息，必须设置以下参数（这些参数可在 QGroundControl >； 载具设置> 参数> 本地位置估算器).
| 参数 | 外部位置估计设置 |
|---|---|
| LPE_FUSION | 如果 熔断视觉位置 选中（默认启用）。 |
| ATT_EXT_HDG_M | 设置为 1 或 2 可启用外部航向集成。设置为 1 时将使用视觉，而设置为 2 时将使用 MoCap 航向。 |
# 禁用气压计融合
如果已经从 VIO 或 MoCap 信息中获得了高精度的高度，则禁用 LPE 中的气压校正以减少 Z 轴上的漂移可能会有所帮助。
可以通过在 QGroundControl 取消选中 保险 选项中的 LPE_FUSION 参数。
# 调整参数噪声参数
如果您的视觉或 MoCap 数据非常精确，而您只想让估算器紧密跟踪这些数据，那么您应该减少标准偏差参数： LPE_VIS_XY 和 LPE_VIS_Z (用于 VIO）或 LPE_VIC_P (用于 MoCap）。减小它们会使估算器更信任传入的姿势估算。您可能需要将其设置为低于允许的最小值，然后强制保存。
TIP
如果性能仍然不佳，请尝试增加 LPE_PN_V 参数。这将使估算器在速度估算过程中更加信任测量结果。
# 使用本地位置启用自动模式
所有 PX4 自动飞行模式（如 使命, 返回, 土地, 保持, 轨道)) 需要一个 全球 位置估计值，通常来自全球定位系统/全球导航卫星系统。
只有 当地 位置估计值（来自 MOCAP、VIO 或类似设备）可以使用 set_gps_global_origin (打开新窗口） MAVLink 消息，将 EKF 的原点设置为特定的全球位置。然后，EKF 将根据原点和本地帧位置提供全球位置估计值。
然后，在规划和执行室内任务时，或在设定本地返回点时，就可以利用这一点。
# 使用 ROS
ROS 不是 所需 但强烈建议使用 PX4，因为它已经与 VIO 和 MoCap 系统很好地集成在一起。PX4 必须已按上述方法设置。
# 将姿势数据导入 ROS
VIO 和 MoCap 系统获取姿态数据的方式不同，有各自的设置和主题。
包括特定系统的设置 低于.对于其他系统，请查阅供应商的设置文档。
# 将姿势数据转发给 PX4
MAVROS 有一些插件，可以使用以下管道从 VIO 或 MoCap 系统中传递视觉估算结果：
| ROS | MAVLink | uORB |
|---|---|---|
| /mavros/vision_pose/pose | 视觉位置估计 (打开新窗口） | 载具视觉测量 |
/mavros/odometry/out (frame_id = odom, child_frame_id = base_link) | 奥数 (打开新窗口） (frame_id = mav_frame_local_frd (打开新窗口）) | 载具视觉测量 |
| /mavros/mocap/pose | ATT_POS_MOCAP (打开新窗口） | 载具几何模型 |
/mavros/odometry/out (frame_id = odom, child_frame_id = base_link) | 奥数 (打开新窗口） (frame_id = mav_frame_local_frd (打开新窗口）) | 载具几何模型 |
您可以将上述任何管道与 LPE 配合使用。
如果使用 EKF2，则只支持 "视觉"管道。要在 EKF2 中使用 MoCap 数据，您必须 重映射 (打开新窗口） 从 MoCap 获取的姿势主题：
- MoCap ROS 类型的主题
geometry_msgs/PoseStamped或geometry_msgs/PoseWithCovarianceStamped必须重映射为/mavros/vision_pose/pose.......。geometry_msgs/PoseStamped由于 MoCap 通常没有与数据相关联的协方差，因此该主题最为常见。 - 如果您通过
导航_msgs/几何ROS 信息，然后将其重新映射为/mavros/odometry/out，确保更新frame_id和子帧标识因此。 - 里程测量框架
frame_id = odom,child_frame_id = base_link可以通过更新mavros/launch/px4_config.yaml.不过，当前版本的 mavros (1.3.0) 需要能够使用 tf 树从frame_id到硬编码帧odom_ned.这同样适用于子帧标识需要在 tf 树中连接到硬编码框架基准链接.如果您使用的是 mavros1.2.0你没有更新文件mavros/launch/px4_config.yaml就可以安全地使用里程测量框架了frame_id = odom,child_frame_id = base_link不用太担心。 - 请注意，如果您使用
child_frame_id = base_link那么您需要确保绹部分导航_msgs/几何消息是 以车身框架表示, 不在惯性框架内!!!!!.
# 参考框架和 ROS
ROS 和 PX4 使用的本地/世界帧和世界帧是不同的。
| 框架 | PX4 | ROS |
|---|---|---|
| 身体 | FRD (X Forward，Y R右，Z D拥有) | FLU (X Forward，Y Lft，Z Up），通常命名为 基本链接 |
| 世界 | FRD 或 NED（X North, Y East，Z D拥有) | FLU 或 ENU（X EY North, Z Up），命名为 敖德 或 地图 |
TIP
参见 REP105：移动平台的坐标框架 (打开新窗口） 了解有关 ROS 框架的更多信息。
下图显示了这两个框架（左侧为 FRD，右侧为 FLU）。
使用 EKF2 进行外部航向估计时，可以忽略磁北，也可以计算并补偿磁北的航向偏移。根据您的选择，偏航角是相对于磁北或当地偏航角给出的。 x.
备注
在 MoCap 软件中创建刚体时，请记住首先对齐机器人的本地 x 与世界接轨 x 轴，否则偏航估计值会有偏移。这会导致外部姿态估计融合无法正常工作。当身体和参照系对齐时，偏航角应为零。
使用 MAVROS，这一操作非常简单。ROS 使用 ENU 框架作为惯例，因此必须以 ENU 提供位置反馈。如果您有 Optitrack 系统，可以使用 mocap_optitrack (打开新窗口） 节点，在 ENU 中已经存在的 ROS 主题上流式传输对象姿态。通过重映射，您可以直接在 mocap_pose_estimate MAVROS 将负责 NED 转换。
MAVROS 的里程测量插件可以轻松处理坐标框架。它使用 ROS 的 tf 软件包。您的外部姿态系统可能采用与 PX4 完全不同的框架约定。外部姿态估计的机身框架可能取决于您在 MOCAP 软件中设置机身框架的方式或您在无人机上安装 VIO 传感器的方式。MAVROS 测距插件需要知道外部姿态的子帧相对于 MAVROS 已知的机身 FRD 或 FLU 主体帧的方向。因此，您必须将外部姿势的主体框架添加到 tf 树中。这可以通过在 ROS 启动文件中加入下面一行的改编版本来实现。
<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"/>；
请确保您更改了偏航、俯仰和滚动的值，以便将外部姿势的身体框架正确连接到 基本链接 或 基准链接.看一看 tf 软件包 (打开新窗口） 以进一步了解如何指定框架之间的变换。您可以使用 rviz 检查是否正确连接了帧。框架名称 external_pose_child_frame 必须与您的 导航_msgs/几何 信息。这同样适用于外部姿势的参照系。您必须将外部姿势的参考帧作为子帧附加到 敖德 或 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 轴向上，则可以将其不带任何旋转（偏航=0、俯仰=0、滚动=0）地连接到 敖德 帧。名称 外置父边框 必须与里程测量报文的 frame_id 匹配。
备注
使用 MAVROS 时 敖德 插件，重要的是没有其他节点在外部姿势的参考帧和子帧之间发布变换。这可能会破坏 tf 树
# 特定系统设置
# OptiTrack MoCap
下面的步骤解释了如何将位置估算值从 光学跟踪 (打开新窗口） 系统到 PX4。假定 MoCap 系统已校准。参见 此视频 (打开新窗口） 获取有关校准过程的教程。
# 步骤 动机 MoCap 软件
- 将机器人的前进方向与 系统 +x 轴 (打开新窗口）
- 在 Motive 软件中定义刚体 (打开新窗口）.给机器人起一个不包含空格的名字，例如
机器人1而不是刚体 1 - 启用 Frame Broadacst 和 VRPN 流媒体 (打开新窗口）
- 将向上轴设置为 Z 轴（默认为 Y 轴）
# 将姿势数据导入 ROS
- 安装
vrpn_client_ros包装 - 您可以通过运行
roslaunch vrpn_client_ros sample.launch 服务器：=<；mocap 机器 IP>；
如果您将刚体命名为 机器人1你会得到这样一个主题 /vrpn_client_node/robot1/pose
# 中继/重映射姿势数据
MAVROS 提供了一个插件，用于转发在 /mavros/vision_pose/pose 到 PX4。假设 MAVROS 正在运行，您只需 重映射 从 MoCap 获取的姿势主题 /vrpn_client_node/<rigid_body_name>/pose 直达 /mavros/vision_pose/pose.请注意，还有一个 模拟 该主题是马弗罗斯为养活 ATT_POS_MOCAP 但不适用于 EKF2。不过，它适用于 LPE。
备注
重新映射姿势主题如上所述 向 PX4 中继姿势数据 (/vrpn_client_node/<rigid_body_name>/pose 类型为 geometry_msgs/PoseStamped).
假定您已按上文所述配置了 EKF2 参数，PX4 现在已设置并融合 MoCap 数据。
现在您可以开始第一次飞行了。
# 首次飞行
设置完上述（特定）系统之一后，现在就可以进行测试了。下面的说明介绍了如何对 MoCap 和 VIO 系统进行测试
# 检查外部估算
请务必在首次飞行前进行以下检查：
- 设置 PX4 参数
MAV_ODOM_LP为 1。然后，PX4 会将接收到的外部姿态以 MAVLink 格式回流 奥数 (打开新窗口） 留言 - 您可以使用 QGroundControl MAVLink 检查员 (打开新窗口）
要做到这一点，需要偏航，直到飞行器上的
奥数信息非常接近单位四元数。(w=1, x=y=z=0) - 此时，车身框架与外部姿势系统的参考框架对齐。如果无法在载具不翻滚或俯仰的情况下使四元数接近单位四元数，则车身框架可能仍有俯仰或翻滚偏移。如果是这种情况，请不要继续，再次检查您的坐标框架。
- 对齐后，你可以将载具从地面拾起，这时你会看到位置的 Z 坐标在减小。向前移动载具，位置的 x 坐标会增加。而向右移动载具则会增加 y 坐标。如果还从外部姿势系统发送了线性速度，则还应检查线性速度。检查线速度是否以 FRD 体框架参照系。
- 设置 PX4 参数
MAV_ODOM_LPPX4 将停止回传该信息流。
如果这些步骤保持一致，您就可以尝试第一次飞行了。
将机器人放在地面上，开始传输 MoCap 反馈信息流。放下左侧（油门）摇杆，启动电机。
此时，将左摇杆置于最低位置，切换到位置控制。此时绿灯亮起。绿灯表示位置反馈可用，位置控制已激活。
将左摇杆置于中间位置，这是死亡区域。通过这个摇杆值，机器人可以保持其高度；抬高摇杆会增加参考高度，而降低数值则会降低参考高度。右摇杆的X和Y值也是如此。
增加左摇杆的数值，机器人就会起飞，之后将其放回中间位置。检查它是否能保持位置不变。
如果它能正常工作，您可能需要设置一个 机外 通过从远程地面站发送位置-设定点来进行实验。