无人机自动驾驶软件系列 E04 ： 深度估计，八叉树地图以及路径规划

前面的几讲中，我们讨论了如何通过python控制无人机以及如何控制无人机在没有GPS的环境下飞行，这些内容是通往无人机自动驾驶的重要一步。本讲中，我们将讨论如何控制无人机在一个未知环境下，只通过一组双目摄像头，在Gazebo中实现路径规划，从点A飞行到点B。本讲内容可以分为如下几个部分：

1. 基于双目摄像头的景深估计；

2. 使用Octomap表示环境；

3. 3D空间中的A*路径规划；

4. 在Gazebo中实验飞行；

5. 路径修剪。

我需要强调的是，我们提供的算法并不是最优的，你可能需要进行参数调优，甚至更换其中的某一或某几个部分的算法，才能实现较为稳定的实际飞行效果。

开始之前，记得更新本地GAAS内容，并且将Gazebo用到的文件拷贝到相应文件夹内：

cd (GAAS_PAHT)
git pull origin master
cp -r (GAAS_PATH)/simulator/launch/* (PX4_FIRMWARE_PATH)/Firmware/launch
cp -r (GAAS_PATH)/simulator/worlds/* ~/catkin_ws/src/Firmware/Tools/sitl_gazebo/worlds/

1. 基于双目摄像头的景深估计

在上一讲中，我们提到了只通过单目相机无法获得场景深度的绝对尺度，RGBD相机对光照及噪声敏感并且价格昂贵。双目相机尽管计算量较大以及对标定要求较高，但是因为其相对低廉的价格，我们选择通过双目相机进行景深估计。若要了解更多关于双目测距的内容，请参考阅读资料 [1]。

我们提供了一个基于C++开发的，基于StereoBM以及WLS滤波的点云估计方法，内容放置在GAAS/software/Obstacle_Map。我们不会讲解细节实现，但是它的工作流程如下：

1. 基于opencv提供的StereoBM的视差图生成；

2. 将视差图投影到相机坐标系的3D空间；

3. 将相机坐标系的3D点云转换为无人机机身坐标系。

编译前置工作：

sudo apt-get install libpopt-dev

编译：

cd (GAAS_PATH)/software/Obstacle_Map
sh generate.sh

编译完成后，首先通过如下命令开启仿真环境：

roslaunch px4 path_planning.launch

Gazebo的仿真窗口将会弹出，并且你将看到一个3x3米的砖墙出现在你的面前，而这个砖墙就是你想在飞行过程中避开的障碍物。飞行的终点是无人机起点前方70米初。

接下来，使用：

sh run.sh

来生成相机坐标系的环境点云；使用下面的命令：

./bin/match_pointcloud_and_slam_pose2

将相机坐标系的点云转为到无人机机身坐标系。你可以通过RVIZ来查看生成的点云。若要查看点云，在RVIZ中选择“add”->"by topic"->"/cloud_in" 。转换为机身坐标系的点云将通过 /cloud_in 发布。

你可以看到RVIZ中出现了一个稠密的点云，尽管不是一个完美的矩形。你可以修改config.yaml中的参数尝试达到一个更好的效果。config.yaml中可以调整的一部分参数如下：

SADWindowSize: 9 
MinDisparity: 3 
NumDisparities: 80
PreFilterCap: 7
UniquenessRatio: 10
SpeckleWindowSize: 7

接下来，我们将介绍Octomap。

2. Octomap 八叉树地图

八叉树地图是一种基于八叉树的算法，它可以将点云稀疏化，从而需要更少的内存空间，更加适合例如Tx2等资源受限的机载电脑。我们将用Octomap表示环境。若要了解更多资料，请参考 阅读资料 [2] 以及 [3]。

安装Octomap:

sudo apt-get install ros-kinetic-octomap-*

开启Octomap 服务之前， 使用rosed将launch文件中的 resolution 改为 0.1; max range 改为 10; 最后一项改为 remap from="cloud_in" to="cloud_in", 将frame_id改为“map”：

rosed octomap_server octomap_mapping.launch
​
如果你更喜欢使用gedit：
​
roscd octomap_server && cd launch
sudo gedit octomap_mapping.launch

更改完毕后你的launch文件如下图所示：

<!--
  Example launch file for octomap_server mapping:
  Listens to incoming PointCloud2 data and incrementally builds an octomap.
  The data is sent out in different representations.
​
  Copy this file into your workspace and adjust as needed, see
  www.ros.org/wiki/octomap_server for details
-->
<launch>
        <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
                <param name="resolution" value="0.1" />
​
                <!-- fixed map frame (set to 'map' if SLAM or localization running!) -->
                <param name="frame_id" type="string" value="/map" />
​
                <!-- maximum range to integrate (speedup!) -->
                <param name="sensor_model/max_range" value="10" />
​
                <!-- data source to integrate (PointCloud2) -->
                <!-- <remap from="cloud_in" to="/narrow_stereo/points_filtered2" /> -->
                <remap from="cloud_in" to="cloud_in" />
​
        </node>
</launch>

接下来，根据上一步生成点云，并根据如下命令调用Ocotmap服务：

roslaunch octomap_server octomap_mapping.launch

在RVIZ中，添加如下topic以便看到Octomap生成效果：

/occupied_cells_vis_array

与点云对应的Octomap如下图所示：

你可以通过rosed来调整例如Octomap分辨率以及最大范围等参数：

rosed octomap_server octomap_mapping.launch

接下来，我们将讨论如何使用提供的 Navigator 来寻找路径并控制无人机飞行。

3. 使用 Navigator 寻找路径并控制飞行

Navigator 提供了一个3D空间的A* 寻路算法，它在3D空间中的效果较差，但是它是一个较好的起点。若要了解更多关于A*的信息，请参考阅读资料 [4]。

在这里，你需要将前面两部分开启（点云生成以及Octomap服务），同时不要忘记在RVIZ中添加如下两个topic，这样你可以在RVIZ中看到规划的路径以及飞机当前的位置。

Navigator存放在下列目录中：

(GAAS_PATH)/software/Navigator

Navigator 默认使用无人机起飞点前方70米处为终点，接下来，在执行Navigator之前，你需要首先起飞无人机：

python px4_mavros_run.py

接下来，开启Navigator：

python Navigator.py

一条红色的飞行路径将会出现在RVIZ之中，无人机会沿着这条路径飞行到设置好的目标终点。

4. 路径修剪

另外一个我们需要注意的是路径修剪。A* 算法生成的原始路径由一系列航点构成，而航点的步长默认设置为0.2米，如果无人机按照0.2米为步长的路径飞行飞行任务会太过繁琐同时飞行时间也会太长，所以我们需要将路径中额外且不重要的航点删除。在此我们提供两种路径修剪方法：

1. 共线判断：对于从A点到B点的一条路径，如果任意两点都共线，我们将只保留路径起点以及重点，删除中间部分；

2. bresenham3D 障碍物判断：对于从A点到B点的一条路径，按照 A 到 A+1， A 到 A+2， A 到 A+3的方式，使用bresenham3D方法进行连线，如果连线和障碍物不重合，且与障碍物距离大于某一设定好的阈值，将连线的终点从原始路径中删除，若连线与障碍物重合，保留上一条连线终点，并将新的连线起点设置为上一条连线终点。

你可以通过如下脚本查看两种算法的处理效果：

python (GAAS_PATH)/software/Navigator/path_optimization/path_pruning.py

在图片中，红色点组成的立方体代表了一个障碍物； 下方红色点组成的路径代表一条原始路径，其中有很多额外的航点； 绿色点组成的路径为原始路径经过共线判断后的路径，其中只保留了转角处的点；蓝色点组成的路径为原始路径经过bresenham3D 障碍物判断处理后的路径。

在Navigator 中我们按照如下逻辑进行路径修剪：

1. A* 生成原始路径；

2. 对原始路径进行共线检查并生成一条新的路径；

3. 对第二步生成的新路径进行bresenham3D 障碍物检测，剔除额外的航点，得到最终航线。

第三步的时间复杂度较高， 所以我们先进行一步共线判断再进行处理。

最后，我们可以发现经过路径修剪后的路径更加的有效且简单，为无人机提供了更好的航线。

Reading Materials

1. ​https://courses.cs.washington.edu/courses/cse455/09wi/Lects/lect16.pdf​

2. ​http://wiki.ros.org/octomap​

3. ​http://wiki.ros.org/octomap_server​

4. ​https://en.wikipedia.org/wiki/A*_search_algorithm​