# 无人机自动驾驶软件系列 E03： 在无GPS环境下通过SLAM实现位置估计
我们建议开发者使用镜像来安装 GAAS，这样可以节省很多的时间和精力：
{% content-ref url="../handy-tools/gaas-v0.7-release-jing-xiang-x86" %}
[gaas-v0.7-release-jing-xiang-x86](https://gaas.gitbook.io/guide/handy-tools/gaas-v0.7-release-jing-xiang-x86)
{% endcontent-ref %}
**注意，在你没有把握的情况下，我们建议你不要轻易进行实机测试。并且测试的时候，请保持距离，并注意安全。本课的内容虽然室内外都可以使用，但我们建议你先在室外空地测试。**
GPS 可以为无人机提供较为准确的位置信息，但是某些时候可能会没有 GPS 信号，或者 GPS 信号不够稳定；比如说桥下，室内以及高楼林立的城市内。那么为了能够允许无人机在这些环境中飞行，我们需要提供其它位置估计手段，比如说SLAM。SLAM使用摄像头作为其主要传感器，传感器可以是单目摄像头，双目摄像头以及RGBD深度摄像头。这几种摄像头有着其对应的优缺点；比如说单目摄像头虽然价格低，体积小，但是它需要其它传感器的辅助，否则无法计算环境深度，也就没有办法为SLAM提供尺度信息；双目摄像头可以通过三角测量提供景深，但是计算量较大，并且需要良好标定；RGBD深度相机可以主动测距，但是对于环境较为敏感，测量距离受限，噪声较大，同时对于阳光以及玻璃等环境下无法正常工作。本讲我们将使用基于双目的SLAM为PX4提供位置估计，实现在无GPS环境下的定点飞行。
本讲内容在实机上通过测试，可以实现较为稳定的定点，但是还有很大的优化空间。**大家没有把握的情况下我不建议实机测试，因为当SLAM信号中断时会有潜在危险。**本讲中的内容会通过Gazebo展示，但是实机上的设置方法也非常类似。
本讲内容可以分为三个部分：
1. 环境更新及设置；
2. SLAM及使用；
3. Gazebo验证。
## 1. 环境更新及设置
如果你完成了前两讲内容，到此你已经设置好了ROS，MAVROS，Gazebo，并且编译好了PX4固件。现在通过如下命令克隆GAAS到本地环境：
```
git clone https://github.com/generalized-intelligence/GAAS
```
或者通过如下命令更新GAAS：
```
cd （GAAS_PATH）
git pull origin master
```
将ROS的launch文件拷贝到PX4相应文件夹内:
```
cp -r (GAAS_PATH)/simulator/launch/* (PX4_FIRMWARE_PATH)/Firmware/launch
```
我们最近对Gazebo用到的仿真模型进行了修改，如果你已经完成了前两课，那么你需要将“(PX4\_FIRMWARE\_PATH)/Firmware/Tools/sitl\_gazebo/models" 中的对应模型删除，并重新拷贝：
```
cp -r (GAAS_PATH)/simulator/models/* ~/catkin_ws/src/Firmware/Tools/sitl_gazebo/models/
```
### a. 编译SLAM
GAAS使用的SLAM是基于如下项目修改的：
```
https://github.com/gaoxiang12/ygz-stereo-inertial
```
在编译之前，请按照上述项目中的描述安装依赖：
```
Mavros msgs: sudo apt install ros-kinetic-mavros-msgs
Pangolin (for visualization): https://github.com/stevenlovegrove/Pangolin
Eigen3: sudo apt-get install libeigen3-dev
g2o: sudo apt-get install libcxsparse-dev libqt4-dev libcholmod3.0.6 libsuitesparse-dev qt4-qmake
glog (for logging): sudo apt-get install libgoogle-glog-dev
ROS Octomap: sudo apt-get install ros-kinetic-octomap-*
```
可以采用以下命令安装以上依赖（不包括 Pangolin）：
```
sudo apt-get install libeigen3-dev
sudo apt-get install ros-kinetic-mavros-msgs
sudo apt-get install libgoogle-glog-dev
sudo apt-get install ros-kinetic-octomap-*
sudo apt-get install libglew-dev
sudo apt-get install libxkbcommon-x11-dev
sudo apt-get install libqt4-dev libsuitesparse-dev qt4-qmake
#以下适用于Ubuntu16.04
sudo apt-get install libqglviewer-dev libcholmod3.0.6
#以下适用于Ubuntu18.04
sudo apt-get install libqglviewer-headers libcholmod3
```
对于Ubuntu 18.04 LTS 系统，将 `ros-kinetic-` 替换为 `ros-melodic-` 即可。
**注意，你需要单独编译安装Pangolin。**
之后，安装DBoW3，PCL, g2o 以及 opencv，项目链接如下：
```
# 记得编译完 make install
# PCL
https://github.com/PointCloudLibrary/pcl
# DBoW3
https://github.com/rmsalinas/DBow3
# g2o
https://github.com/RainerKuemmerle/g2o
# opencv，建议使用版本高于3.4.5
https://github.com/opencv/opencv
```
在编译opencv的过程中，需要确保cmake生成的opencv编译配置中，包括了opencv\_cvv组件。为此，你需要首先克隆opencv\_contrib：
```
https://github.com/opencv/opencv_contrib
```
进入克隆下来的opencv目录，切换版本到一个较高版本——此处使用版本号3.4.5：
```
git checkout 3.4.5
```
**注意，对于opencv和opencv\_contrib都需要切换版本，且必须保证两者版本一致。**
然后使用cmake进行配置——此处配置较为复杂，若对cmake不熟悉则可以使用cmake-gui：
```
sudo apt-get install cmake-qt-gui
cmake-gui
```
首先指定编译目录，点击Configure并选择Unix Makefiles，先进行第一次配置：
指定OPENCV\_EXTRA\_MODULES\_PATH到opencv\_contrib，并勾选WITH\_QT和ENABLE\_CXX11：
**再点击一次Configure**，即可看到BUILD\_opencv\_cvv选项，勾选之后点击Generate即可生成目标makefile，进行编译即可。
**注意，点击Generate只是在你指定的目录下生成了Makefile，并没有进行编译。你需要进入目录执行**`make && sudo make install`
由于ROS自带了低版本的opencv，为了防止cmake在生成SLAM的makefile的过程中无法找到正确的opencv版本，需要在SLAM的cmake配置文件中指定：
```
cd （GAAS_PATH)/software/SLAM/ygz_slam_ros
gedit CMakeLists.txt
```
配置文件的第53行：
`find_package(OpenCV REQUIRED)`
在OpenCV后面加上版本号（注意不要省略空格），若你安装了其他版本（必须不低于3.4.5），则将3.4.5改为你安装的版本号：
`find_package(OpenCV 3.4.5 REQUIRED)`
完成后，通过如下命令编译SLAM：
```
cd （GAAS_PATH)/software/SLAM/ygz_slam_ros
# 编译过程较长，不建议在虚拟机里进行
sh generate.sh
```
对于Ubuntu 18.04 LTS ，在编译SLAM时会看到如下警告——这是正常现象，不影响使用：
### b. 下载QGroundControl
我们将会使用QGroundControl来设置并查看无人机参数及状态。如果你使用的是UBUNTU 16.04，可以通过如下链接下载：
```
https://s3-us-west-2.amazonaws.com/qgroundcontrol/latest/QGroundControl.AppImage
```
下载后，通过如下命令开启：
```
chmod +x ./QGroundControl.AppImage
./QGroundControl.AppImage （或者双击）
```
你也可以通过如下链接查看更多信息：
```
https://docs.qgroundcontrol.com/en/getting_started/download_and_install.html
```
如果你已经成功编译好了ygz\_slam\_ros并且下载完QGroundControl，你可以继续到下一部分。
## 2. SLAM及使用
在 ygz\_slam\_ros/examples文件夹内，你可以看到一个名为simulationCamera.yaml的配置文件，此文件是SLAM的配置文件，文件包含ROS的左右图像topic名，相机内参以及一些其它参数。其中一部分内容如下：
```
%YAML:1.0
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
# camera and imu topics
Left: /gi/simulation/left/image_raw
Right: /gi/simulation/right/image_raw
# not used for now
Imu: /mavros/imu/data
# Camera calibration and distortion parameters (OpenCV)
# if running in pure stereo vision mode
PureVisionMode: true
# do we need visualization?
UseViewer: false
# display mappoints?
displayMapPoints: false
Camera.fx: 376.0
Camera.fy: 376.0
Camera.cx: 376.0
Camera.cy: 240.0
Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
```
如果想测试自己的相机，请更改相机topic，注意此时并没有使用IMU信息，并且此项目还未完成，会有不稳定情况出现。
## 3. Gazebo验证
现在让我们继续到Gazebo仿真部分。
首先通过如下命令开启仿真环境：
```
roslaunch px4 slam.launch
```
一个Gazebo窗口将会打开，一架无人机会出现在一个空旷环境内，如我在前几课反复强调，一定记得查看MAVROS的连接情况：
```
rostopic echo /mavros/state
```
确保输出的结果为"connected: True"。
{% hint style="info" %}
若Gazebo此步没有发布图像，请尝试 sudo apt install ros-kinetic-gazebo-control
{% endhint %}
继续之前，如果你打开slam.launch文件，你会发现如下参数：
```
```
你可以看到我们使用的是"iris\_stereo\_gray\_no\_gps" 无人机模型，在此模型中我已经将GPS模块删除了，所以如果你现在通过如下命令打开QGroundControl：
```
./QGroundControl.AppImage # if you downloaded AppImage
```
QGroundControl 在打开后会自动连接到Gazebo仿真，同时在窗口中你可以看到"No GPS Lock for Vehicle"， 这代表无人机中的GPS已被禁用，我们将会使用SLAM作为水平位置估计手段。
{% hint style="info" %}
运行 Gazebo 时，如果一切正常但出现以上报错信息，该报错信息并不会影响 Gazebo 运行，可以继续进行到下一步。
如果您知道如何解决以上报错，欢迎在 GAAS GitHub 页面提交 PR，帮助我们减少不必要的报错信息。
{% endhint %}
接下来，开启SLAM：
```
./bin/EurocStereoVIO_ros ./examples/simulationCamera.yaml
```
或者也可以运行：
```
sh run.sh
```
此时会弹出一个窗口，你可以看到机载摄像头中的实时画面如下图所示，注意图中的彩色特征点，此时SLAM已经初始化，否则请确保你已经更新了无人机模型，否则SLAM将不会正常初始化。
在QGroundControl中，点击左上角的齿轮按钮，选择最下方的"Parameters"， 在搜索栏里输入“vision”，将“EKF2\_AID\_MASK”的参数改为“vision position fusion"，对应值为 “8”。点击“save”保存参数更改。
我们只会使用SLAM信息来估计水平位置移动，忽略SLAM的YAW信息以及高度信息，并且使用PX4的气压计作为定高手段。
接下来，点击“Clear”，选择“Tools”，选择“reboot vehicle”重启无人机，使参数设置生效。
{% hint style="info" %}
实际上此时你不需要重启无人机即可使得参数生效，但是为了教程的完整性（尤其是在实机测试的时候），我将重启也作为了这里的一步。
{% endhint %}
在终端中，输入：
```
rostopic echo /mavros/vision_pose/pose
```
你会看到当前的实时SLAM位置估计信息：
```
---
header:
seq: 1489
stamp:
secs: 3453
nsecs: 368000000
frame_id: ''
pose:
position:
x: 0.000944685346145
y: -0.00012923774903
z: 0.000286279467091
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0
---
```
接下来，进入到“(GAAS\_PATH)/demo/tutorial\_3”文件夹，起飞无人机：
```
python px4_mavros_run.py
```
待无人机悬停后，在另外一个终端，通过如下脚本控制无人机飞一个 3x3 米的正方形：
```
python square.py
```
你可以看到无人机能够实现较为理想的位置估计，并且无人机可以依赖SLAM信息实现较为复杂的任务。
完成预定轨迹后，无人机会降落在地面上，你可以通过SLAM窗口实时观察运行轨迹。
以上内容在实机上进行过测试，但是表现不稳定，为了实现较为理想的效果，我们需要花费一定精力进行调参并改善SLAM效果。在实机测试过程中我们发现了如下问题：
1. SLAM有时会出错并终止运行，此时无人机会跳到“ALTITUDE”模式；
2. 有时会出现无人机漂移情况，尽管相关参数已经设定并且SLAM处于运行状态；
3. 快速运动情况下定位精度不理想。
如果你想在实机上测试，典型过程如下：
1. 进行良好的摄像头标定；
2. 开启SLAM;
3. 启动MAVROS，确保连接正常；
4. 在QGroundControl中设置相关参数；
5. 确保 /mavros/local\_position/pose 发布的位置信息是有“意义”的，此信息是飞控融合SLAM信息后的位置信息，如果xy方向的值处于 10^-20 的范围，那么此时的位置估计是没有意义的，你需要检查MAVROS连接，QGroundControl参数设定以及SLAM状态；
6. 通过遥控器起飞无人机后切换到定点模式，或者通过提供的python脚本起飞无人机。
{% hint style="info" %}
在Gazebo验证完成前，以及在有合适场地前，不要盲目实机测试。
{% endhint %}
总而言之，本讲中我们讲述了如何使用SLAM，如何通过QGroundControl设置参数，如何在Gazebo中进行无GPS环境下的无人机飞行控制并完成了一个简单任务。
凡在本教程出现的信息，均仅供参考。本教程将尽力确保所提供信息的准确性及可靠性，但不保证有关资料的准确性及可靠性；任何实际机器测试前请尽可能多地进行测试，并对任何自主决定的行为负责。本教程对有关资料所引致的错误、不确或遗漏，概不负任何法律责任（包括侵权责任、合同责任和其它责任）。
---
# Agent Instructions: Querying This Documentation
If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question.
Perform an HTTP GET request on the current page URL with the `ask` query parameter:
```
GET https://gaas.gitbook.io/guide/software-realization-build-your-own-autonomous-drone/wu-ren-ji-zi-dong-jia-shi-xi-lie-part-3-zai-wu-gps-huan-jing-xia-tong-guo-slam-shi-xian-wei-zhi-gu-j.md?ask=
```
The question should be specific, self-contained, and written in natural language.
The response will contain a direct answer to the question and relevant excerpts and sources from the documentation.
Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.