# 使用视觉或运动捕捉系统进行位置估算

视觉惯性测距(VIO)和运动捕捉(MoCap)系统可让载具在全球位置源不可用或不可靠时(如在室内或在桥下飞行时等)进行导航。

VIO 和 MoCap 都能确定载具的 姿势 (quot;视觉"信息(位置和姿态)。它们之间的主要区别在于框架视角:

  • VIO 使用 机载传感器 从载具的角度获取姿态数据(见 自我运动 (打开新窗口)).
  • MoCap 采用的系统是 车载摄像机 来获取三轴空间中的载具姿态数据(也就是说,它是一个告诉载具姿态的外部系统)。

来自任一类型系统的姿态数据都可用于更新基于 PX4 的自动驾驶仪的本地位置估计(相对于本地原点),也可选择融合到飞行器姿态估计中。此外,如果外部姿态系统还提供线性速度测量值,则可用于改进状态估计(线性速度测量值的融合仅由 EKF2 支持)。

本主题将介绍如何配置基于 PX4 的系统,以便从 MoCap/VIO 系统(通过 ROS 或其他 MAVLink 系统)获取数据,更具体地说,就是如何设置 MoCap 系统(如 VICON 和 Optitrack)和基于视觉的估算系统(如 罗维奥 (打开新窗口), SVO (打开新窗口)PTAM (打开新窗口)).

备注

根据您使用的是 EKF2 还是 LPE 估算器,说明有所不同。

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 前方和 z 右),而 y 轴是垂直向上的。一个简单的窍门是交换轴,以获得 NED 惯例。

如果 x_{mav}, y_{mav}z_{mav} 是作为位置反馈通过 MAVLink 发送的坐标,那么我们可以得到

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

关于方向,保留标量部分 w 的四元数相同,并交换矢量部分 x, yz 用同样的方法。您可以在每个系统中应用这一技巧--如果您需要获得 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_CTRLEKF2_RNG_CTRL分别为

TIP

重新启动飞行控制器,以使参数更改生效。

# 调整 EKF2_EV_DELAY

EKF2_EV_DELAY视觉位置估计器相对于 IMU 测量的延迟.

或者换句话说,它是视觉系统时间戳与 IMU 时钟(EKF2 的基准时钟)记录的实际捕捉时间之间的差值。

从技术上讲,如果 MoCap 和(例如)ROS 计算机之间有正确的时间戳(不只是到达时间)和时间同步(如 NTP),则可以将其设置为 0。实际上,这需要根据经验进行调整,因为整个 MoCap->PX4 链中的延迟与设置密切相关。很少有系统能设置成完全同步的链!

通过检查 IMU 速率与 EV 速率之间的偏移,可以从日志中大致估算出延迟时间。要启用 EV 速率记录,请将记录中的第 7 位(计算机视觉和规避)设置为 SDLOG_PROFILE.

ekf2_ev_delay 日志

备注

外部数据与机载估计值(如上所述)的对比图可以通过以下方法生成 飞行图 或类似的飞行分析工具。在编写本报告时(2021 年 7 月),既没有 飞行回顾 也不 MAVGCL 支持这一功能。

可以通过改变参数进一步调整该值,以找到在动态机动过程中 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_XYLPE_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/PoseStampedgeometry_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 树中连接到硬编码框架 基准链接.如果您使用的是 mavros 1.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 软件

# 将姿势数据导入 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_LP PX4 将停止回传该信息流。

如果这些步骤保持一致,您就可以尝试第一次飞行了。

将机器人放在地面上,开始传输 MoCap 反馈信息流。放下左侧(油门)摇杆,启动电机。

此时,将左摇杆置于最低位置,切换到位置控制。此时绿灯亮起。绿灯表示位置反馈可用,位置控制已激活。

将左摇杆置于中间位置,这是死亡区域。通过这个摇杆值,机器人可以保持其高度;抬高摇杆会增加参考高度,而降低数值则会降低参考高度。右摇杆的X和Y值也是如此。

增加左摇杆的数值,机器人就会起飞,之后将其放回中间位置。检查它是否能保持位置不变。

如果它能正常工作,您可能需要设置一个 机外 通过从远程地面站发送位置-设定点来进行实验。