# 无人机自动驾驶软件系列 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，先进行第一次配置：

![](https://1955112758-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LYUhlGdK9Y1iLhupMFC%2F-LugcWZf8Fa_DXCbYf7l%2F-LuggrOEIRTUAK-LTzIe%2Fimage.png?alt=media\&token=676cab70-e40a-44a0-92f1-5ca02bee9360)

指定OPENCV\_EXTRA\_MODULES\_PATH到opencv\_contrib，并勾选WITH\_QT和ENABLE\_CXX11：

![](https://1955112758-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LYUhlGdK9Y1iLhupMFC%2F-LugcWZf8Fa_DXCbYf7l%2F-LughzC-_iWiANnu-FCn%2Fimage.png?alt=media\&token=9ad216de-e65d-496b-9eda-d5f43f9c9164)

![](https://1955112758-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LYUhlGdK9Y1iLhupMFC%2F-LugcWZf8Fa_DXCbYf7l%2F-Lugi6rj7RDUmvU7KfJg%2Fimage.png?alt=media\&token=64da989d-8457-4eba-a666-4278efdbadb6)

![](https://1955112758-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LYUhlGdK9Y1iLhupMFC%2F-LugcWZf8Fa_DXCbYf7l%2F-LugiLZfYF3OxHe0dzvX%2Fimage.png?alt=media\&token=cf79e20a-8249-4d75-9732-b30a355f4aab)

**再点击一次Configure**，即可看到BUILD\_opencv\_cvv选项，勾选之后点击Generate即可生成目标makefile，进行编译即可。

**注意，点击Generate只是在你指定的目录下生成了Makefile，并没有进行编译。你需要进入目录执行**`make && sudo make install`

![](https://1955112758-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LYUhlGdK9Y1iLhupMFC%2F-LugcWZf8Fa_DXCbYf7l%2F-Lugic4ErpCOJk5rO7XS%2Fimage.png?alt=media\&token=9f680b5a-4b27-438d-8ae4-a4e3656f0d49)

由于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时会看到如下警告——这是正常现象，不影响使用：

![](https://1955112758-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LYUhlGdK9Y1iLhupMFC%2F-LugcWZf8Fa_DXCbYf7l%2F-LugoECJ9BMftcIuu9-_%2Fimage.png?alt=media\&token=20bfb316-9fe7-4e41-a8bd-46b216b7253f)

### 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文件，你会发现如下参数：

```
<arg name="vehicle" default="iris_stereo_gray_no_gps"/>
```

你可以看到我们使用的是"iris\_stereo\_gray\_no\_gps" 无人机模型，在此模型中我已经将GPS模块删除了，所以如果你现在通过如下命令打开QGroundControl：

```
./QGroundControl.AppImage # if you downloaded AppImage
```

![Fig 1, QGroundControl](https://1955112758-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LYUhlGdK9Y1iLhupMFC%2F-LcJ1Omf3Zgmwz1jUVzn%2F-LcJ6nZr60ERNe4lNLxk%2Ft3-qgc.png?alt=media\&token=23877b86-af1c-4d1f-9f57-19c854e302ac)

QGroundControl 在打开后会自动连接到Gazebo仿真，同时在窗口中你可以看到"No GPS Lock for Vehicle"， 这代表无人机中的GPS已被禁用，我们将会使用SLAM作为水平位置估计手段。

![](https://1955112758-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LYUhlGdK9Y1iLhupMFC%2F-LgaYGT7xNerBUpDSdtg%2F-LgaYY-BOQcIXA0TxxjC%2FWechatIMG16845.jpeg?alt=media\&token=2238e2ed-e58a-4eb0-be59-2bd7c6428806)

{% hint style="info" %}
运行 Gazebo 时，如果一切正常但出现以上报错信息，该报错信息并不会影响 Gazebo 运行，可以继续进行到下一步。

如果您知道如何解决以上报错，欢迎在 GAAS GitHub 页面提交 PR，帮助我们减少不必要的报错信息。
{% endhint %}

接下来，开启SLAM：

```
./bin/EurocStereoVIO_ros ./examples/simulationCamera.yaml
```

或者也可以运行：

```
sh run.sh
```

此时会弹出一个窗口，你可以看到机载摄像头中的实时画面如下图所示，注意图中的彩色特征点，此时SLAM已经初始化，否则请确保你已经更新了无人机模型，否则SLAM将不会正常初始化。

![Fig 2, SLAM initialized](https://1955112758-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LYUhlGdK9Y1iLhupMFC%2F-Lcs_pS2V7kso7T4m0cI%2F-LcsdeA0NVKhJemWAHt8%2Ft3-slam-initialized.png?alt=media\&token=8490dbb3-3814-4563-af5a-b314adafb822)

在QGroundControl中，点击左上角的齿轮按钮，选择最下方的"Parameters"， 在搜索栏里输入“vision”，将“EKF2\_AID\_MASK”的参数改为“vision position fusion"，对应值为 “8”。点击“save”保存参数更改。

![Fig 3, QGroundContrl, select vision position fusion](https://1955112758-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LYUhlGdK9Y1iLhupMFC%2F-LcJ1Omf3Zgmwz1jUVzn%2F-LcJ9Wy8b8KmFDnRCIIO%2Ft3-gqc-2.png?alt=media\&token=018615de-cb76-49cc-a70d-5abf5fd3405b)

我们只会使用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信息实现较为复杂的任务。

![Fig 4, SLAM position history](https://1955112758-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LYUhlGdK9Y1iLhupMFC%2F-LcOSonnJRTEx5YMXkpa%2F-LcOStKHHzNig1o9Bcm3%2Ft3-slam-4.png?alt=media\&token=14dd895f-b48d-4817-b05a-c2ddecc32cb6)

完成预定轨迹后，无人机会降落在地面上，你可以通过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环境下的无人机飞行控制并完成了一个简单任务。

凡在本教程出现的信息，均仅供参考。本教程将尽力确保所提供信息的准确性及可靠性，但不保证有关资料的准确性及可靠性；任何实际机器测试前请尽可能多地进行测试，并对任何自主决定的行为负责。本教程对有关资料所引致的错误、不确或遗漏，概不负任何法律责任（包括侵权责任、合同责任和其它责任）。