无人机自动驾驶软件系列 E05： 视觉引导降落

本讲将给出一个通过二维码进行视觉辅助降落的例子。

我们建议开发者使用镜像来安装 GAAS，这样可以节省很多的时间和精力：

pageGAAS v0.7 Release 镜像 - x64

前几讲中，我们讲述了如何设置仿真环境，如何通过python控制无人机，如何在没有GPS的环境下通过SLAM实现位置控制，以及如何通过一组双目摄像头进行避障。本讲中，我会简要介绍如何通过二维码进行视觉辅助降落。注意，此实现比较简单，实际情况下需要使用更为复杂的方法。

二维码提供了丰富的视觉信息，它可以用来定位，跟踪，或者识别。它通常由很多黑色矩形组成。关于二维码的更多信息，请参考 阅读资料[1]。

我们将使用pyzbar这个python包来进行二维码的检测以及解析。

Fig 1, 二维码

我们主要关注的是二维码的四个角，它们起到了二维码定位作用，提供了二维码的位置信息。下面的图片展示了此二维码的识别结果，你可以看到二维码的四个角的位置被标记出来。

此二维码被放置在一个752 x 480 的白色背景上。

Fig 2, 二维码的解析结果

给出二维码的四角位置，鉴于它们处于同一平面，我们可以通过单应性矩阵以及气压计的高度指数来恢复摄像头到放置到平面上的指定二维码的旋转以及平移。

1. 依赖安装

我们首先需要安装pyzbar以及numpy：

do apt-get install libzbar0
pip install pyzbar numpy

如果你完成了前面的几个课程，你应该已经安装了rospy以及opencv。

最后， 记得更新GAAS：

cd GAAS_PATH
git pull origin master

将simulator文件夹中的二维码模型，仿真世界，以及启动文件拷贝到px4对应文件夹下。

2. 单应性矩阵，旋转以及平移

计算机视觉中，关于一个平面的两张图片可以通过一个名为单应性矩阵的变换矩阵联系在一起，此矩阵包含了两张关于同一平面的图片的旋转以及平移信息。通常，为了获得两张图片的单应性矩阵，我们需要两张图片的超过三对不共线的匹配好的特征点。换一个说法就是，我们需要在两张图片中找到超过三组匹配好的特征。而通过二维码解析出来的四角位置可以用来进行单应性矩阵的计算。

Fig 3, 相机中的二维码以及给定的二维码特征匹配

通过两张图片恢复出相对旋转以及位移的过程可以分为以下几步：

1. 给定一张目标图片（也被称为待寻图片），此图片需要包含一个二维码；

2. 通过机载摄像头不断接收图像，检测二维码，如果找到二维码，解析二维码，得到四个角在图像（也被称为训练图像）中的位置，并进行到第三步；

3. 给定待寻图片的二维码在图片中的位置以及摄像头图像（训练图像）中的二维码在图像中的位置，我们可以得到四对匹配好的特征点，这样我们可以通过opencv的getPerspectiveTransform函数来计算单应性矩阵；

4. 通过opencv的decomposeHomographyMat函数来分解上一步得到的单应性矩阵，通常会得到四组不同的解， 每一组解包含一个旋转，平移，以及目标二维码在当前相机坐标系的法线向量；

5. 因为目标二维码被平放在水平地面上，所以它在当前图像坐标系中的法线向量应该为（0， -1， 0），那么对应的旋转以及平移就是我们想要的结果。

Fig 4, 相机坐标系 (来源自阅读资料【2】)

训练图像通过单应性矩阵恢复出来的二维码如下图所示：

Fig 5, 通过单应性矩阵恢复出来的二维码

我们可以发现，恢复出来的图像和我们给定的二维码非常相似，代表我们求得的单应性矩阵是合理的。

关于二维码的检测，单应性矩阵计算，旋转以及平移的恢复等信息可以在qr_code.py中找到。

3. 测试

首先打开仿真环境：

roslaunch px4 landing.launch 

一个gazebo仿真环境会弹出，我们可以看到一张二维码被放置在无人机起点前方几米处，这张二维码就是我们在前面提到的待寻图片。

Fig 6, 仿真环境及待寻图片

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

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

接下来， 起飞无人机：

python px4_mavros_run.py

开启二维码检测：

python sample_fly.py

无人机此时应该会起飞到2米处，同时二维码检测函数线程也被开启。

二维码检测此时会一直失败，因为待寻图片距离无人机太远。所以你需要将无人机移动到二维码出现在无人机图像中为止。无人机的摄像头朝前，而待寻二维码平放在地面上，这种情况给二维码检测带来了一些难度，所以你可能需要将无人机移动的”恰到好处“， 此时才能正确的检测到待寻二维码。

# 打开一个终端
ipython

# 在ipython中
from commander import Commander
com = Commander()
com.move(8, 0, 0)

通常来说， 我们最好将无人机用来检测目标的摄像头朝下放置，或者与水平方向呈45度角向下放置，这样能够得到比较好的二维码检测精度。

Fig 7, 摄像头图像检测结果展示

我们此时将只考虑恢复出来的平移部分。当发现并成功解析待寻二维码后，无人机会被控制并降落在待寻二维码前方。你会发现无人机并不会降落在二维码上面，这是由我们给出的二维码在图片中的位置决定的。

匹配是指二维码在当前图像中和给出图像中的位置匹配。计算出来的结果会有误差，同时相机位置和机身位置也有位移。

此讲内容旨在提供一个简单的降落算法。如果你发现任何问题，或有不明白的地方，欢迎提 Issue 或者加入我们的微信群聊。一般来说提交 Issue 是更好的讨论方式。

4. 阅读资料

1. https://en.wikipedia.org/wiki/Homography_(computer_vision)

2. https://docs.opencv.org/3.4.1/d9/dab/tutorial_homography.html