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

我们建议开发者使用镜像来安装 GAAS，这样可以节省很多的时间和精力：
GAAS v0.7 Release 镜像 - x64
前面的几讲中，我们讨论了如何通过python控制无人机以及如何控制无人机在没有GPS的环境下飞行，这些内容是通往无人机自动驾驶的重要一步。本讲中，我们将讨论如何控制无人机在一个未知环境下，只通过一组双目摄像头，在Gazebo中实现路径规划，从点A飞行到点B。本讲内容可以分为如下几个部分：
1.基于双目摄像头的景深估计；
2.使用Octomap表示环境；
3.3D空间中的A*路径规划；
4.在Gazebo中实验飞行；
5.路径修剪。
我需要强调的是，我们提供的算法并不是最优的，你可能需要进行参数调优，甚至更换其中的某一或某几个部分的算法，才能实现较为稳定的实际飞行效果。
开始之前，记得更新本地GAAS内容，并且将Gazebo用到的文件拷贝到相应文件夹内：
1
cd (GAAS_PAHT)
2
git pull origin master
3
cp -r (GAAS_PATH)/simulator/launch/* (PX4_FIRMWARE_PATH)/Firmware/launch
4
cp -r (GAAS_PATH)/simulator/worlds/* ~/catkin_ws/src/Firmware/Tools/sitl_gazebo/worlds/
Copied!
1. 基于双目摄像头的景深估计
在上一讲中，我们提到了只通过单目相机无法获得场景深度的绝对尺度，RGBD相机对光照及噪声敏感并且价格昂贵。双目相机尽管计算量较大以及对标定要求较高，但是因为其相对低廉的价格，我们选择通过双目相机进行景深估计。若要了解更多关于双目测距的内容，请参考阅读资料 [1]。
我们提供了一个基于C++开发的，基于StereoBM以及WLS滤波的点云估计方法，内容放置在GAAS/software/Obstacle_Map。我们不会讲解细节实现，但是它的工作流程如下：
1.基于opencv提供的StereoBM的视差图生成；
2.将视差图投影到相机坐标系的3D空间；
3.将相机坐标系的3D点云转换为无人机机身坐标系。
编译前置工作：
1
sudo apt-get install libpopt-dev
Copied!
编译：
1
cd (GAAS_PATH)/software/Obstacle_Map
2
sh generate.sh
Copied!
编译完成后，首先通过如下命令开启仿真环境：
1
roslaunch px4 path_planning.launch
Copied!
Gazebo的仿真窗口将会弹出，并且你将看到一个3x3米的砖墙出现在你的面前，而这个砖墙就是你想在飞行过程中避开的障碍物。飞行的终点是无人机起点前方70米初。
Fig 1, Gazebo Simulation Environment
运行 Gazebo 时，如果一切正常但出现以上报错信息，该报错信息并不会影响 Gazebo 运行，可以继续进行到下一步。
如果您知道如何解决以上报错，欢迎在 GAAS GitHub 页面提交 PR，帮助我们减少不必要的报错信息。
接下来，使用：
1
sh run.sh
Copied!
来生成相机坐标系的环境点云；使用下面的命令：
1
./bin/match_pointcloud_and_slam_pose2
Copied!
将相机坐标系的点云转为到无人机机身坐标系。你可以通过RVIZ来查看生成的点云。若要查看点云，在RVIZ中选择“add”->"by topic"->"/cloud_in" 。转换为机身坐标系的点云将通过 /cloud_in 发布。
Fig 2, select topic for RVIZ visualization
Fig 3, Point Cloud in RVIZ
你可以看到RVIZ中出现了一个稠密的点云，尽管不是一个完美的矩形。你可以修改config.yaml中的参数尝试达到一个更好的效果。config.yaml中可以调整的一部分参数如下：
1
SADWindowSize: 9 
2
MinDisparity: 3 
3
NumDisparities: 80
4
PreFilterCap: 7
5
UniquenessRatio: 10
6
SpeckleWindowSize: 7
Copied!
接下来，我们将介绍Octomap。
2. Octomap 八叉树地图
八叉树地图是一种基于八叉树的算法，它可以将点云稀疏化，从而需要更少的内存空间，更加适合例如Tx2等资源受限的机载电脑。我们将用Octomap表示环境。若要了解更多资料，请参考 阅读资料 [2] 以及 [3]。
安装Octomap:
1
sudo apt-get install ros-kinetic-octomap-*
Copied!
开启Octomap 服务之前， 使用rosed将launch文件中的 resolution 改为 0.1; max range 改为 10;  最后一项改为 remap from="cloud_in" to="cloud_in",  将frame_id改为“map”：
1
rosed octomap_server octomap_mapping.launch
2
​
3
如果你更喜欢使用gedit：
4
​
5
roscd octomap_server && cd launch
6
sudo gedit octomap_mapping.launch
Copied!
更改完毕后你的launch文件如下图所示：
1
<!--
2
  Example launch file for octomap_server mapping:
3
  Listens to incoming PointCloud2 data and incrementally builds an octomap.
4
  The data is sent out in different representations.
5
​
6
  Copy this file into your workspace and adjust as needed, see
7
  www.ros.org/wiki/octomap_server for details
8
-->
9
<launch>
10
        <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
11
                <param name="resolution" value="0.1" />
12
​
13
                <!-- fixed map frame (set to 'map' if SLAM or localization running!) -->
14
                <param name="frame_id" type="string" value="/map" />
15
​
16
                <!-- maximum range to integrate (speedup!) -->
17
                <param name="sensor_model/max_range" value="10" />
18
​
19
                <!-- data source to integrate (PointCloud2) -->
20
                <!-- <remap from="cloud_in" to="/narrow_stereo/points_filtered2" /> -->
21
                <remap from="cloud_in" to="cloud_in" />
22
​
23
        </node>
24
</launch>
Copied!
接下来，根据上一步生成点云，并根据如下命令调用Ocotmap服务：
1
roslaunch octomap_server octomap_mapping.launch
Copied!
在RVIZ中，添加如下topic以便看到Octomap生成效果：
1
/occupied_cells_vis_array
Copied!
Fig 4, topic for Octomap
与点云对应的Octomap如下图所示：
Fig 5, Generated Octomap
你可以通过rosed来调整例如Octomap分辨率以及最大范围等参数：
1
rosed octomap_server octomap_mapping.launch
Copied!
接下来，我们将讨论如何使用提供的 Navigator 来寻找路径并控制无人机飞行。
3.  使用 Navigator 寻找路径并控制飞行
Navigator 提供了一个3D空间的A* 寻路算法，它在3D空间中的效果较差，但是它是一个较好的起点。若要了解更多关于A*的信息，请参考阅读资料 [4]。
在这里，你需要将前面两部分开启（点云生成以及Octomap服务），同时不要忘记在RVIZ中添加如下两个topic，这样你可以在RVIZ中看到规划的路径以及飞机当前的位置。
Fig 6, topic for drone position
Fig 7, topic for planned path 
Navigator存放在下列目录中：
1
(GAAS_PATH)/software/Navigator
Copied!
Navigator 默认使用无人机起飞点前方70米处为终点，接下来，在执行Navigator之前，你需要首先起飞无人机：
1
python px4_mavros_run.py
Copied!
接下来，开启Navigator：
1
python Navigator.py
Copied!
一条红色的飞行路径将会出现在RVIZ之中，无人机会沿着这条路径飞行到设置好的目标终点。
Fig 8, A* planned Path
4. 路径修剪
另外一个我们需要注意的是路径修剪。A* 算法生成的原始路径由一系列航点构成，而航点的步长默认设置为0.2米，如果无人机按照0.2米为步长的路径飞行飞行任务会太过繁琐同时飞行时间也会太长，所以我们需要将路径中额外且不重要的航点删除。在此我们提供两种路径修剪方法：
1.共线判断：对于从A点到B点的一条路径，如果任意两点都共线，我们将只保留路径起点以及重点，删除中间部分；
2.bresenham3D 障碍物判断：对于从A点到B点的一条路径，按照 A 到 A+1， A 到 A+2， A 到 A+3的方式，使用bresenham3D方法进行连线，如果连线和障碍物不重合，且与障碍物距离大于某一设定好的阈值，将连线的终点从原始路径中删除，若连线与障碍物重合，保留上一条连线终点，并将新的连线起点设置为上一条连线终点。
你可以通过如下脚本查看两种算法的处理效果：
1
python (GAAS_PATH)/software/Navigator/path_optimization/path_pruning.py
Copied!
Fig 9, path pruning algorithms
在图片中，红色点组成的立方体代表了一个障碍物； 下方红色点组成的路径代表一条原始路径，其中有很多额外的航点； 绿色点组成的路径为原始路径经过共线判断后的路径，其中只保留了转角处的点；蓝色点组成的路径为原始路径经过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​