3. Z410-4B-T265视觉版教程

为什么要使用双目相机？

我们在室外飞行的时候，无人机定位是依靠GPS全球定位系统。GPS导航现已十分普及，但GPS的精度目前限制在大约10米。另外，更新速率比较慢，大约是10Hz，并且只能支持户外，因为它们需要接收来自GPS卫星的电子三角测量信号，同时山脉和建筑物的反射会造成多径干扰。所以在高大建筑周边，建筑物室内和隧道等地方就没有GPS信号，我们就需要使用其他方法进行定位导航。

视觉版机型在单目版基础上增加了intel T265双目相机，可以通过v-slam(视觉惯性测距即时定位与地图构建)技术，让无人机进行室内定位导航。英特尔实感追踪摄像头T265是一个完整的嵌入式SLAM解决方案。它根据VIO（视觉惯性里程计）并利用最新的算法来输出6自由度追踪**。**通过对6自由度姿态数据（位置和方向）的处理，发送给无人机定位。

双目定位后，如果室内空间足够，就可以进行悬停飞行，一键返航，航点飞行等。从而为以后室内测试更复杂的避障，导航规划等项目奠定基础。
3.1 一些常用的T265命令

在安装有ubuntu系统的PC端，输入以下命令来实时查看T265视频：

演示视频：

realsense-viewer: RealSense Viewer 是旗舰工具，通过简单的跨平台 UI 提供对大多数相机功能的访问。注意：由于树莓派WIFI带宽有限，通过ssh访问树莓派开启realsense-viewer是有卡顿的。如果要观察T265视频画面，建议使用rs-capture命令。

https://github.com/IntelRealSense/librealsense/tree/master/tools/realsense-viewer

rs-capture：将深度和 RGB 数据流式传输和渲染到屏幕

https://dev.intelrealsense.com/docs/rs-capture

rs-pose: 应用程序将打开一个窗口，在该窗口中打印设备位置相对于初始位置的当前 x、y、z 值。

https://dev.intelrealsense.com/docs/rs-pose

rs-pose-predict: 应用程序应打开一个窗口，在该窗口中打印设备位置相对于其初始位置的预测 x、y、z 值。

https://dev.intelrealsense.com/docs/rs-pose-predict

rs-enumerate-devices：T265在出厂前就对相机进行了标定，每个相机内部都带有相机的内参和外参。在终端中输入此命令，即可读取到T265的配置信息

rs-enumerate-devices -c：如果需要读取到相机内参和外参，以及IMU的参数则可以使用此命令（添加-c参数）
3.2 启用T265定位的飞控参数设置

默认情况下，无人机发货前设置pixhawk飞控使用GPS在室外定位导航。如果要使用T265双目相机进行室内定位，飞控参数需要进行如下设置：

把飞控连接地面站，点击配置/调试菜单---全部参数列表:

先检查以下参数：

AHRS_EKF_TYPE = 3 (EKF类型=EKF3)

EK3_ENABLE = 1 (EKF3启用)

EK2_ENABLE = 0 (EKF2停用)

将第一组定位源参数按照以下设置

EK3_SRC1_POSXY = 6 (水平位置使用外部导航)

EK3_SRC1_POSZ = 1 (垂直位置使用气压计定高)

EK3_SRC1_VELXY = 6 (水平速度使用外部导航)

EK3_SRC1_VELZ = 6 (垂直速度使用外部导航)

EK3_SRC1_YAW = 1 (偏航使用外置罗盘)

COMPASS_USE = 1 (使用外置罗盘)

GPS_TYPE = 0 (禁用GPS)

VISO_TYPE = 2 (IntelT265)

禁用所有速度融合

EK3_SRC_OPTIONS = 0

RTL_ALT=0****室内测试请将返航高度设置为0，让飞行器一当前高度返航。默认是1.5米。

(以下是飞控默认数据，要使用GPS定位改回来就可以了。)

以下是第一组定位源设置，默认情况下不变，使用GPS定位

EK3_SRC1_POSXY = 3（主要水平位置使用GPS）

EK3_SRC1_POSZ = 1（主要垂直位置使用气压计）

EK3_SRC1_VELXY = 3（主要水平速度使用GPS）

EK3_SRC1_VELZ = 3（主要垂直速度使用GPS）

EK3_SRC1_YAW = 1（主要偏航/指南针航向）

GPS_TYPE = 1 (GPS类型自动)

VISO_TYPE = 0

本机搭载Intel T265双目相机，在室内没有GPS信号的情况下，代替GPS导航定位。双目定位后，如果室内空间足够，就可以进行悬停飞行，一键返航，失控返航，航点飞行。从而为以后室内测试更复杂的避障，导航规划等项目奠定基础。
3.3 使用T265定位导航的2种方法
3.3.1 使用pyrealsense2启动双目相机

使用pyrealsense2定位原理：将从Realsense T265获得的6自由度姿态数据（位置和方向）和置信度数据交由python脚本处理(该脚本位于vision_to_mavros/scripts/t265_to_mavlink.py)，处理后的结果通过MAVLink发送到ArduPilot无人机，从而实现定位。

该脚本将执行以下任务：

a.使用pyrealsense2的相关API获取6自由度姿势数据并跟踪置信度数据，pyrealesense2是librealsense的Python包装器；

b.执行必要的矩阵变换，以匹配Realsense T265和NED的框架以及其他处理步骤；

c.将姿态数据打包到MAVLink消息VISION_POSITION_ESTIMATE中，并将置信度数据打包到虚拟消息中，然后将它们以预定频率发送到ArduPilot，以免输入数据淹没飞控；

d.自动设置EKF home点，让设置和飞行都更为简单；

飞行器机载电脑已经安装好librealsense和pyrealsense2，根据以下步骤即可启动T265

1.无人机接上电池，打开missionplanner地面站，使用数传连接地面站和飞控；

2.使用ssh远程连接树莓派，操作和之前一样；

3.打开终端，切换路径到脚本所在目录：
```
cd ~/vision_to_mavros/catkin_ws/src/vision_to_mavros/scripts
```
4.执行下面脚本：
```
python3 t265_to_mavlink.py
```
5.地面站软件，快捷键_CTR+F_，点击_Mavlink Inspector_，能看到数据已经上传到飞控了。

同时usb转ttl模块上的rx和tx指示灯会快速闪烁，表示有数据传输；

脚本正常个运行后，并且FCU开始接收VISION_POSITION_ESTIMATE消息，就看到地面站会消息框会显示“ GPS Glitch**”和“ GPS Glitch cleared”****，确认系统已识别了外部本地化数据。**

6.使用Mission Planner: 右键单击地图上的任意点> Set Home Here > Set EKF Origin Here.

稍等片刻直到四轴飞行器图标出现在Mission Planner的地图上；

7.拿起无人机并四处走动，检查地图上是否显示了无人机的位置运动。地图上显示的无人机的轨迹应该反映真实的运动，而不会产生过多的失真或超调。以下是在2m x 2m的正方形中行走的示例。

8.如果外部定位导航数据由于任何原因丢失（跟踪丢失，脚本中断等）,重新运行脚本也不会成功定位，需要重新启动无人机(重新上电)并执行之前的操作。

9.飞行测试：(请有无人机操作经验的人操作)

a.在自稳或则定高模式下解锁起飞，检查无人机飞行是否平稳；

b.使用遥控器操作无人机四处移动，并观察Mission Planner上的位置是否稳定；

c.以上没有问题，保持无人机在0.8~1米左右高度，油门50%左右**，切换到Loiter模式，但是如果出现任何问题，请随时准备切换回Stabilize (自稳)/ Alt-Hold(定高)；**

d.正常情况下，无人机应该稳定悬停在空中；

e.遥控器操作无人机四处缓慢缓慢移动，查看无人机稳定状态。测试时注意始终准备切换回“稳定/保持”状态；

f.如果一切正常，下次就可以在Loiter模式下解锁和起飞。

注意！
请确保双目相机水平向前安装，测试环境光照充足，双目前方特征明显的视觉范围测试。

10.数据查看：

a. 视觉测距信息将出现在VISO 数据闪存日志消息中。

b. EKF的视觉测距信息将显示在XKFD消息中。

11.参考视频：
3.3.2 ROS系统下启用T265

这是第二种使用双目相机定位的方法。

ROS下双目相机T265的定位原理：从realsense-ros节点获得的位置数据由vision_to_mavros节点处理，并通过主题/ mavros / vision_pose / pose发送到mavros节点。 mavros将负责ENU-NED帧转换，并通过MAVLink将其发送到ArduPilot无人机，从而实现室内定位。

**本机已经安装好 librealsense、realsense-ros、mavros、vision_to_mavros 等必要的组件，根据以下教程即可启动 T265 定位。
1.主从机设置完成
2.无人机接上电池，打开 missionplanner地面站，使用数传连接地面站和飞控；
3.使用ssh远程连接树莓派；
4.开启一个终端，运行 realsense-ros节点
```
roslaunch realsense2_camera rs_t265.launch
```
/camera/odom/sample/ 和 /tf主题将以200Hz频率发布。
5.开启另一个终端，运行 mavros节点
```
roslaunch mavros apm.launch
```
rostopic echo /mavros/vision_pose/pose未发布。
6.开启第三个终端，运行 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的频率发布。

以上3个节点运行成功，并且FCU开始接收VISION_POSITION_ESTIMATE消息，就看到地面站会消息框会显示“ GPS Glitch”和“ GPS Glitch cleared”，确认系统已识别了外部本地化数据。
7.设置 EKF HOME 点

a. 使用Mission Planner: 右键单击地图上的任意点> Set Home Here > Set EKF Origin Here.

b. 使用脚本设置EKF HOME点，开启第四个终端，运行：
```
rosrun vision_ to_mavros set_origin.py
```
8.打开地面站软件，快捷键CTR+F，点击Mavlink Inspector，能看到数据已经上传到飞控了。室内定位运行成功，切换到loiter悬停模式，指示灯是蓝色，表示可以解锁；如果切换到loiter变成黄灯，表示室内定位运行失败，请重新检查再启动。

同时usb转ttl模块上的rx和tx指示灯会快速闪烁，表示有数据传输；
9.稍等片刻直到四轴飞行器图标出现在 Mission Planner 的地图上；
10.拿起无人机并四处走动，检查地图上是否显示了无人机的位置运动。地图上显示的无人机的轨迹应该反映真实的运动，而不会产生过多的失真或超调。以下是在2m * 2m的正方形中行走的示例
11.飞行测试：(请有无人机操作经验的人操作)

a.在自稳或则定高模式下解锁起飞，检查无人机飞行是否平稳；

b.使用遥控器操作无人机四处移动，并观察Mission Planner和rviz上的位置，以查看跟踪是否稳定；

c.以上没有问题，保持无人机在0.8~1米左右高度，油门50%左右，切换到Loiter，但是如果出现任何问题，请随时准备切换回Stabilize / Alt-Hold。

d.正常情况下，无人机应稳定悬停并能够保持其位置。使用遥控器缓慢移动无人机2-3米，验证比例（便于在rviz上查看）

e.如果一切正常，下次就可以在Loiter模式下解锁和起飞。

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

f.如果外部定位导航数据由于任何原因丢失（跟踪丢失，脚本中断等），重新运行脚本也不会成功定位，需要重新启动无人机(重新上电)并执行之前的操作。
12.数据查看：

视觉测距信息将出现在VISO 数据闪存日志消息中。
演示视频：
13.成功运行以上节点，下次可以只需要开启一个终端，使用下面这个命令一次启动所有节点：
```
roslaunch vision_to_mavros t265_all_nodes.launch
```
此命令一次执行3个launch：

rs_t265.launch

apm.launch

t265_tf_to_mavros.launch

这样可避免开启多个终端，简化操作，方便飞行。

再使用脚本设置EKF HOME点：
```
rosrun vision_to_mavros set_origin.py
```

Z410-4B视觉版操作教程

3. Z410-4B-T265视觉版教程

3.1 一些常用的T265命令

3.2 启用T265定位的飞控参数设置

3.3 使用T265定位导航的2种方法

3.3.1 使用pyrealsense2启动双目相机

3.3.2 ROS系统下启用T265