无人机利用视觉slam实现位置估计

室内飞行
由于对于室内情况，飞控自身的磁力计在室内近乎无用，所以需要借助外力实现室内的信息获取，一般是无人机的位置以及偏航角

这里我有一个很大的疑惑，对于欧拉角一般是有三个，可是为什么这里只获取了一个，对于欧拉角与四元素，完全还是一个小白的状态。

常见的定位信息获取方式

• 外部定位：利用动作捕捉系统(motion capture system,简称为mocap)等设备测量无人机当前的位置和姿态。北理工模式实验室就有一台，但是作为小白的我还不会用，所以大家一定要努力去好学校，设备啥的太全了
• 机载定位：说的玄乎点就是视觉slam与激光slam,说的难听点就是用视觉传感器测量信息和用激光雷达测量信息。
  两种定位方式如何将信息传给机载电脑：
• 如果是mocap的话，推荐使用vrpn_clinet_ros功能包来得到地面软件发出的定位信息(只需要将地面软件所在的电脑和机载电脑接入同一个局域网，IP设置正确即可。)
• 主流的传感器和SLAM算法一般都有相应的ROS功能包，直接按照官方教程安装，运行起来就可以看到其发布的包含定位数据的话题了

px4飞控地面站的设置
其实我完全不懂为什么通过QGC地面站的设置可以对px4中的运行固件产生影响，这是我现在还不能理解的，只是跟着教程的步骤去做，目前通过了解知道的是相应的PX4模块功中含有的文件params.c里面就定义了我们能在地面站中修改的东西。
为了配合机载电脑中传来的定位信息，有两个和ekf2有关的参数需要修改。其实为什么用和ekf2有关呢，因为在px4内部实现滤波的时候用的就是ekf2算法

• EKF2_AID_MASK 设置为24（即勾选vision position fusion and vision yaw fusion）
• EKF2_HGT_MODE 设置为VISION （默认为气压计）
  因为ekf2中默认是不会融合这个外部的定位信息的，这里相当于在ekf2中启用这个功能。至于ekf2参数中还有很多和这个外部定位相关的滤波参数设置
• EKF2_EV_DELAY:发布信息与捕捉信息之间的时间误差
• EKF2_EV_POS_X, EKF2_EV_POS_Y, EKF2_EV_POS_Z:我认为是由于t265传感器放置的位置所要设置的值。
  从我的理解来看，最后两个参数的设置，默认就好，至少到现在我还没有为其设置过值。
  具体信息流向
• 首先是传感器测量得到，(比如说我们的t265,关于t265的安装与使用，需要去看的博文t265配置以及无人机整体开发过程，后面的老哥写的真的是太详细了，爱了爱了，通过对其t265驱动的安装与相关ros包的安装，我们可以直接由机载电脑的ros话题将其发布出来(所以，我们有时候真的不能说，呵，ros不就是一个通讯连接系统嘛，其实由于各个硬件厂家对ros的支持，ros会真正的成为机器人操作系统，功能太强大了），然后由px4_command功能包的px4_pos_estimator.cpp订阅该话题，进行处理后，转存到另一个话题/mavros/vision_pose/pose并发布。
• Mavros功能包中vision_pose_estimate.cpp订阅/mavros/vision_pose/pose话题，并将其封装成Mavlink消息，发送给飞控。
• 飞控接受mavlink消息，通过mavlink_receiver.cpp接受并处理，转换为相应的uORB消息。
• 飞控中的ekf2算法订阅该uORB消息，并进行相应的计算处理，融合得到无人机的位置速度等状态。
  举例:室内利用t265定位的准备
  对于机载电脑(tx2,nano,而言，其需要安装如下组件:
• t265驱动和t265对应的ROS驱动(realsense-ros)
• ROS系统(最好装全)
• MAVROS包
• 机载电脑与ROS相连接
• t265安装位置对整体坐标变换的影响

t265能够直接给出定位信息的技术：视觉里程计(VIO:Vision-inertial odometer)
VIO是一种用来估计移动物体速度和3D位置(局部位置和姿态)的计算机视觉技术。通常用于在GPS信号缺失或者不可靠的情况下的导航(例如在室内或者在桥下飞行)。 VIO采用视觉里程计(Visual Odometry)，通过相机的图像以及结合设备的IMU的惯性测量结果，去估计设备的位置。
驱动安装与测试：
t265所插入的x86机载电脑，对于不同的信息需要有不同的接口：

• 需要姿态信息(x,y,z position and orientation)板载电脑具有USB2就足够了
• 需要fisheye image streams,板载电脑需要有USB3插口
• 板载电脑的所有USB插口的设置并不是相同的，需要确保板载电脑能够识别设备，在终端下输入：lsusb或者rs-enumerate-devices查看设备是否被识别。
  测试librealsense安装是否成功：
  linux终端下输入：realsense-viewer，如果相应的插口连接了T265,那么可以看到相应的面板出现T265的模型。点击开启设备，移动T265，在intel Realsense Viewer中会看到相应的移动轨迹。
  librealsense安装后形成的SDK里面，会提供python wrapper,名称叫做pyrealsense2,就像realsense-ros是ros wrapper一样。python包的相应位置在~/librealsense/build/wrappers/python
• Update the PYTHONPATH environment variable to add the path to the pyrealsense library: export PYTHONPATH=$PYTHONPATH:/usr/local/lib
• Alternatively, copy the build output (librealsense2.so and pyrealsense2.so in ~/librealsense/build/) next to your script.
• The basic examples provided by Intel can be found in the folder ~/librealsense/wrappers/python/example. Run with python3
  相应的命令

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

cd ~/librealsense/wrappers/python/example

python3 t265_example.py

本文介绍PX4以及机载电脑如何设置VIO。本文介绍的方法是通过ROS将VIO的信息传输给PX4，PX4本身不在乎接收到的里程信息是从何处传来的。
在通过px4_pose_estimator.cpp将传感器信息转化为mavros类型的消息时，代码中需要用到中介包的转换,而这个中介包具有两种，一种是px4官方给出的VIO以及APM给出的t265_to_mavros,根据别人博客的介绍，两种中介安装一种就可以使用，具体的方式，我还没有尝试(待后期尝试再来修改)
其中关于t265_to_mavros官方给出的介绍是:

converts data from vision-based system (external localization system like fiducial tags, VIO, SLAM, or depth image) to corresponding MAVROS topics or MAVLink
大致的意思是将数据从基于视觉的系统（外部定位系统，如基准标签，VIO，SLAM或深度图像）转换为相应的MAVROS主题或MAVLink

但是从图片上说其发布的/mavros/vision_pose/pose的信息来看，好像px4_command中px4_pose_estimator.cpp做的事情与这个中介包重复，具体内容到时候再看官方文档
关于其中介包的具体安装步骤见bolg

QGC地面站参数的设置，见上文。QGC参数被正确配置后，通过QGC地面站中的ANYLAZE中的ODOMETRY中查看实时获取的信息，也可以通过MAVROS发布的ROS节点查看信息。

rostopic echo vision_pose/pose

这个时候，可以移动t265测试数据是否变化

参考博客与文档
[1]博客1