# 用于非GPS导航的VIO跟踪摄像机

## 用于非GPS导航的ROS和VIO跟踪摄像机[¶](https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html#ros-and-vio-tracking-camera-for-non-gps-navigation)

此 wiki 页面介绍了如何将 [VIO 跟踪摄像头](https://www.intelrealsense.com/visual-inertial-tracking-case-study/)（如[英特尔实感 T265](https://www.intelrealsense.com/tracking-camera-t265/)）与 ROS 一起使用，以促进非 GPS 飞行。

注意

为简洁起见，系统某些部分的解释将保持简短。更深入的讨论可以在以下博客文章第 [1](https://discuss.ardupilot.org/t/integration-of-ardupilot-and-vio-tracking-camera-part-1-getting-started-with-the-intel-realsense-t265-on-rasberry-pi-3b/43162) 部分、第 [2 部分](https://discuss.ardupilot.org/t/integration-of-ardupilot-and-vio-tracking-camera-part-2-complete-installation-and-indoor-non-gps-flights/43405)、[第 3 部分](https://discuss.ardupilot.org/t/integration-of-ardupilot-and-vio-tracking-camera-part-3-indoor-autonomous-flights-and-performance-tests/43626)中找到。

### 视频[¶](https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html#video)

#### 硬件要求[¶](https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html#hardware-requirements)

* 传感器：[英特尔实感跟踪摄像头 T265](https://www.intelrealsense.com/tracking-camera-t265/)
* 配套电脑：[树莓派3 B型](https://www.raspberrypi.org/products/raspberry-pi-3-model-b/)

注意

根据您对 T265 的需求，配套计算机应具有 USB2（仅姿势数据）或 USB3（**姿势** + **图像**数据）。对于定位和导航，我们只需要捕获姿势数据，因此RPi 3B足以完成任务。

#### 系统概述[¶](https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html#system-overview)

[![../\_images/ros-vio-connection.png](https://ardupilot.org/dev/_images/ros-vio-connection.png)](https://ardupilot.org/dev/_images/ros-vio-connection.png)

从节点获取的位置数据将由节点处理，并通过主题发送到节点。 将负责ENU-NED帧转换，并通过MAVLink将其发送到ArduPilot。`realsense-rosvision_to_mavrosmavros/mavros/vision_pose/posemavros`

#### 安装`librealsense`[¶](https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html#install-librealsense)

注意

Realsense T265 在 Windows 和 Linux 上通过 [librealsense](https://github.com/IntelRealSense/librealsense) 提供支持。不同系统的安装过程差异很大。有关特定系统的说明，请参阅[官方github页面](https://github.com/IntelRealSense/librealsense)，例如[Jetson](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation_jetson.md)，[Odroid](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation_odroid.md)，[Windows](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation_windows.md)，[Raspbian](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation_raspbian.md)。

以下步骤适用于 Ubuntu Linux。由于没有可用于 RPi 的 Debian 软件包，因此必须从源代码构建。`librealsense`

* **安装操作系统**（如果您尚未这样做）：[Ubuntu MATE 16.04 LTS](https://ubuntu-mate.org/blog/ubuntu-mate-xenial-final-release/)。
* **增加交换大小**：RPi 没有足够的 RAM 来编译 SDK，因此需要增加交换大小。2048 （2GB） 的交换大小似乎运行良好，但您可以使用更少的大小。

```
# Toggle swap off
sudo dphys-swapfile swapoff

# Edit the config file
sudo nano /etc/dphys-swapfile

# Find and edit the variable CONF_SWAPSIZE=2048

# Toggle swap back on
sudo dphys-swapfile swapon

# Reboot your raspberry
sudo reboot

# After reboot, check that swap size changed
free

# Should show something like Swap: 2097148
```

* **克隆 librealsense 存储库并编译 SDK**

```
# Update system
sudo apt update
sudo apt upgrade -y

# Install dependencies
sudo apt install git libssl-dev libusb-1.0-0-dev pkg-config -y
sudo apt install cmake python3-dev raspberrypi-kernel-headers -y

# Clone the repository under directory of your chosing
cd ~
git clone https://github.com/IntelRealSense/librealsense.git
cd librealsense

# Install udev rules
sudo cp config/99-realsense-libusb.rules /etc/udev/rules.d/
sudo udevadm control --reload-rules && udevadm trigger

# Create the destination directory
mkdir build
cd build

# Remove extra files if this is not your first run
xarg sudo rm < install_manifest.txt
rm CMakeCache.txt
export CC=/usr/bin/gcc-6
export CXX=/usr/bin/g++-6
cmake -D CMAKE_BUILD_TYPE="Release"\
-D FORCE_LIBUVC=ON \
-D BUILD_PYTHON_BINDINGS=ON \
-D BUILD_EXAMPLES=ON ..
make -j4
sudo make install
sudo ldconfig

# Finally, reboot the pi:
sudo reboot
```

* **测试librealsense是否正确安装**：最简单的方法是插入T265并使用SDK中包含的[示例](https://github.com/IntelRealSense/librealsense/tree/master/examples)和[工具](https://github.com/IntelRealSense/librealsense/tree/master/tools)。
  * 如果您插入了显示器，您可以通过在终端中输入以下内容来打开[英特尔实感查看器](https://github.com/IntelRealSense/librealsense/tree/master/tools/realsense-viewer)：。如果连接了 T265，该设备将在左侧面板上可用。单击滑块以启动设备并切换到 3D 视图。移动 T265，您应该会看到它的轨迹。`realsense-viewer`
  * 如果没有连接显示器，也可以从终端启动其他演示，例如、、等。`rs-posers-pose-predictrs-capture`
* **测试 pyrealsense2 python** 包装器：如果您启用了构建 Python 包装器（标志），则编译后的库位于构建文件夹中：。`BUILD_PYTHON_BINDINGS~/librealsense/build/wrappers/python`
  * 更新环境变量以将路径添加到库中：。或者，复制脚本旁边的生成输出（和文件，位于 中）。`PYTHONPATHpyrealsenseexport PYTHONPATH=$PYTHONPATH:/usr/local/liblibrealsense2.sopyrealsense2.so~/librealsense/build/`
  * 英特尔提供的基本示例可以在文件夹中找到。使用 Python3 运行它。`~/librealsense/wrappers/python/example`

```
export PYTHONPATH=$PYTHONPATH:/usr/local/lib

cd ~/librealsense/wrappers/python/example

# You should see a stream of data coming from the T265.
python3 t265_example.py
```

注意

带有 USB3 的 RPi 2B 无法处理图像流。尝试显示鱼眼图像可能会导致应用程序崩溃。

#### 安装`realsense-ros`[¶](https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html#install-realsense-ros)

* ROS的安装步骤很简单，您可以[按照此处](https://github.com/IntelRealSense/realsense-ros#installation-instructions)官方存储库上的说明进行操作。
* 启动节点：。所有传感器数据将作为 ROS 主题发布：`roslaunch realsense2_camera rs_t265.launch`

```
/camera/odom/sample
/camera/accel/sample
/camera/gyro/sample
/camera/fisheye1/image_raw (not viewable on RPi 3B)
/camera/fisheye2/image_raw (not viewable on RPi 3B)
```

* 在另一个终端中，运行以查看当前的 CPU 使用率。`top`

注意

的版本需要与 的版本匹配，因此每次更新 ros/lib 时，另一个也应该更新。`realsense-roslibrealsense`

#### 安装`mavros`[¶](https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html#install-mavros)

* 建立串行连接：[使用 MAVLink 将 RPi 连接到 ArduPilot](https://ardupilot.org/dev/docs/raspberry-pi-via-mavlink.html#raspberry-pi-via-mavlink)。
  * 如果RPi-ArduPilot之间的连接是通过UART串行端口建立的，请[同时更改/boot/config.txt中的设置](https://discuss.ardupilot.org/t/communicating-with-raspberry-pi-3b/39269/8)。
* 安装 ：按照我们[维基页面上](https://ardupilot.org/dev/docs/ros-install.html#ros-install)的说明进行操作。`mavros`
* 在 ROS 中建立连接：[使用 MAVROS连接到 ArduPilot](https://ardupilot.org/dev/docs/ros-connecting.html#ros-connecting)。

#### 安装`vision_to_mavros`[¶](https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html#install-vision-to-mavros)

* 克隆并构建包：

```
# Navigate to catkin workspace
cd ~/catkin_ws/src

# Clone and build the repo
git clone https://github.com/hoangthien94/vision_to_mavros.git
cd ..
catkin_make
source ~/.bashrc

# Add source command to .bashrc for future use
# echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
```

* 根据 T265 摄像头的方向修改参数：为**前置摄像头**设置默认参数。在启动文件中，从值列表中修改参数 、、 或使用与您的设置对应的您自己的值。`t265_tf_to_mavros.launchroll_campitch_camyaw_camgamma_world`

注意

存在一个[已知问题](https://github.com/IntelRealSense/librealsense/issues/4080)，即**朝下**方向的偏航角不一致，并且已经讨论了一些解决方法。在撰写本文时，似乎有效的方法如下：当相机开始流式传输姿势（启动或调用 的 API 以调用姿势数据）时，它需要稍微倾斜（即不完全平坦）。否则，世界坐标的偏航角可能会随机初始化。`realsense-roslibrealsense`

#### 配置 ArduPilot[¶](https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html#configure-ardupilot)

使用地面站（即任务规划器）连接到飞行控制器，并检查是否设置了如下所示的参数：

* [AHRS\_EKF\_TYPE](https://ardupilot.org/copter/docs/parameters.html#ahrs-ekf-type) = 2（默认值）以使用 EKF2（在撰写本文时，处理外部导航数据不支持 EKF3）
* [EK2\_ENABLE](https://ardupilot.org/copter/docs/parameters.html#ek2-enable) = 1（默认值）
* [EK3\_ENABLE](https://ardupilot.org/copter/docs/parameters.html#ek3-enable) = 0（默认值）
* [GPS\_TYPE](https://ardupilot.org/copter/docs/parameters.html#gps-type) = 0 禁用 GPS
* [EK2\_GPS\_TYPE](https://ardupilot.org/copter/docs/parameters.html#ek2-gps-type) = 3 禁用 EKF 对 GPS 的使用
* [EK2\_POSNE\_M\_NSE](https://ardupilot.org/copter/docs/parameters.html#ek2-posne-m-nse-gps-horizontal-position-measurement-noise-m) = 0.1
* [EK2\_VELD\_M\_NSE](https://ardupilot.org/copter/docs/parameters.html#ek2-veld-m-nse-gps-vertical-velocity-measurement-noise-m-s) = 0.1
* [EK2\_VELNE\_M\_NSE](https://ardupilot.org/copter/docs/parameters.html#ek2-velne-m-nse-gps-horizontal-velocity-measurement-noise-m-s) = 0.1
* [COMPASS\_ENABLE](https://ardupilot.org/copter/docs/parameters.html#compass-enable) = 0、[COMPASS\_USE](https://ardupilot.org/copter/docs/parameters.html#compass-use-use-compass-for-yaw) = 0、[COMPASS\_USE2](https://ardupilot.org/copter/docs/parameters.html#compass-use2) = 0、[COMPASS\_USE3](https://ardupilot.org/copter/docs/parameters.html#compass-use3) = 0 以禁用 EKF 对指南针的使用，而是依赖来自外部导航数据的航向。

修改参数后，重启飞控。

#### 验证所有 ROS 节点是否正常工作[¶](https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html#verify-that-all-ros-nodes-are-working)

此设置中运行 3 个 ROS 节点：、 和 。在 3 个独立的终端中启动：`realsense-rosmavrosvision_to_mavros`

* `realsense-ros`节点：。`roslaunch realsense2_camera rs_t265.launch`
  * 该主题，应以 200Hz 发布。`/camera/odom/sample//tf`
* `mavros`节点：（修改了您的设置中的其他参数）。`roslaunch mavros apm.launchfcu_urlapm.launch`
  * `rostopic echo /mavros/state`应显示 FCU 已连接。
  * `rostopic echo /mavros/vision_pose/pose`未发布。
* `vision_to_mavros`节点：`roslaunch vision_to_mavros t265_tf_to_mavros.launch`
  * `rostopic echo /mavros/vision_pose/pose`现在应该显示来自 T265 的姿势数据。
  * `rostopic hz /mavros/vision_pose/pose`应显示该主题正在以 30Hz 发布。
* 要验证ArduPilot是否在Mission Planner上接收：按下并单击“Mavlink Inspector”，您应该能够看到传入的数据。`VISION_POSITION_ESTIMATECtrl+F`

[![../\_images/ros-vio-check-data.png](https://ardupilot.org/dev/_images/ros-vio-check-data.png)](https://ardupilot.org/dev/_images/ros-vio-check-data.png)

* 一旦所有节点都可以成功运行，下次您可以使用 一次启动所有节点，这将启动：`roslaunch vision_to_mavros t265_all_nodes.launch`
  * `rs_t265.launch`，如最初提供的，无需修改。`realsense-ros`
  * `apm.launch`，使用您自己的配置进行修改。
  * `t265_tf_to_mavros.launch`参数根据您的 T265 方向。

#### 地面测试[¶](https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html#ground-test)

* 开机后，启动所有ROS节点。
* 通常（但并非总是如此），一旦运行并且FCU开始接收消息，您将看到消息“GPS故障”和“GPS故障清除”，确认系统正在识别外部定位数据。`mavrosVISION_POSITION_ESTIMATE`
* 现在你需要告诉EKF车辆在世界上的位置（即将**EKF设置为家**），否则传入的数据将不会融合。为此，您需要[SET\_GPS\_GLOBAL\_ORIGIN](https://mavlink.io/en/messages/common.html#SET_GPS_GLOBAL_ORIGIN)和[SET\_HOME\_POSITION](https://mavlink.io/en/messages/common.html#SET_HOME_POSITION)发送 MAVLink 消息。有许多选项可以完成此操作：
  * **使用任务规划器：**&#x53F3;键单击地图上的任意点> >。`Set Home HereSet EKF Origin Here`

    [![../\_images/zed-set-ekf-origin.png](https://ardupilot.org/dev/_images/zed-set-ekf-origin.png)](https://ardupilot.org/images/zed-set-ekf-origin.png)
  * **使用代码**：通过 MAVLink 将所需的消息发送到 FCU。在 Python 中，您可以在[set\_origin.py使用此](https://github.com/anbello/aruco_gridboard/blob/master/script/set_origin.py)脚本。
    * 安装 ：按照[此处](https://github.com/ArduPilot/pymavlink#installation)的说明进行操作。`pymavlink`
    * 运行脚本：。`rosrun vision_to_mavros set_origin.py`
* 您应该会看到地图上出现一个四轴飞行器图标。
* 拿起车辆并四处走动，检查车辆的位置运动是否显示在地图上。车辆在地图上的轨迹应反映真实运动，没有太多失真或超调。下面是一个在 2m x 2m 正方形中行走的示例。

[![../\_images/ros-vio-ground-test.png](https://ardupilot.org/dev/_images/ros-vio-ground-test.png)](https://ardupilot.org/dev/_images/ros-vio-ground-test.png)

#### 飞行测试[¶](https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html#flight-test)

对于您的首次飞行： - 起飞在稳定或Alt-Hold，检查飞行器是否稳定。

* 移动车辆并观察任务规划器上的位置，并查看跟踪是否稳定。`rviz`
* 切换到 Loiter，但随时准备在出现任何问题时切换回稳定/Alt-Hold。
* 否则，车辆应稳定悬停并能够保持其位置。来回移动车辆2-3米，验证刻度（更容易查看）。`rviz`
* 以不同的速度移动车辆（平移、旋转），随时准备切换回稳定/Alt-Hold。

如果一切按预期进行，下次您可以在 Loiter 模式下武装和起飞。

提示

在切换到 Loiter 模式之前，请务必确认位置反馈运行正常。注意环境中的工作边界，即由于缺乏功能、快速或旋转移动而可能丢失跟踪的位置。

#### 数据记录[¶](https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html#data-logging)

* 视觉里程计信息将显示在数据闪存日志消息中。`VISO`

#### 引用[¶](https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html#references)

* 原始博客文章：第 [1 部分](https://discuss.ardupilot.org/t/integration-of-ardupilot-and-vio-tracking-camera-part-1-getting-started-with-the-intel-realsense-t265-on-rasberry-pi-3b/43162/1)、第 2 部分、[第 3 部分](https://discuss.ardupilot.org/t/integration-of-ardupilot-and-vio-tracking-camera-part-2-complete-installation-and-indoor-non-gps-flights/43405/1)。
* [ROS框架约定](http://www.ros.org/reps/rep-0105.html)
* [ENU-NED协调](https://dev.px4.io/en/ros/external_position_estimation.html#setup_specific_systems)。
