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

本讲中我们会简要讲述如何通过双目相机以及八叉树地图实现环境感知，并通过A*实现一个路径规划。
我们建议开发者使用镜像来安装 GAAS，这样可以节省很多的时间和精力：
GAAS v0.7 Release 镜像 - x64
/handy-tools/gaas-v0.7-release-jing-xiang-x86
前面的几讲中，我们讨论了如何通过python控制无人机以及如何控制无人机在没有GPS的环境下飞行，这些内容是通往无人机自动驾驶的重要一步。本讲中，我们将讨论如何控制无人机在一个未知环境下，只通过一组双目摄像头，在Gazebo中实现路径规划，从点A飞行到点B。本讲内容可以分为如下几个部分：
基于双目摄像头的景深估计；
使用Octomap表示环境；
3D空间中的A*路径规划；
在Gazebo中实验飞行；
路径修剪。
我需要强调的是，我们提供的算法并不是最优的，你可能需要进行参数调优，甚至更换其中的某一或某几个部分的算法，才能实现较为稳定的实际飞行效果。
开始之前，记得更新本地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。我们不会讲解细节实现，但是它的工作流程如下：
基于opencv提供的StereoBM的视差图生成；
将视差图投影到相机坐标系的3D空间；
将相机坐标系的3D点云转换为无人机机身坐标系。
编译前置工作：
sudo apt-get install libpopt-dev
编译：
cd (GAAS_PATH)/software/Obstacle_Map
sh generate.sh
编译完成后，首先通过如下命令开启仿真环境：
roslaunch px4 path_planning.launch
Gazebo的仿真窗口将会弹出，并且你将看到一个3x3米的砖墙出现在你的面前，而这个砖墙就是你想在飞行过程中避开的障碍物。飞行的终点是无人机起点前方70米初。
Fig 1, Gazebo Simulation Environment
运行 Gazebo 时，如果一切正常但出现以上报错信息，该报错信息并不会影响 Gazebo 运行，可以继续进行到下一步。
如果您知道如何解决以上报错，欢迎在 GAAS GitHub 页面提交 PR，帮助我们减少不必要的报错信息。
接下来，使用：
sh run.sh
来生成相机坐标系的环境点云；使用下面的命令：
./bin/match_pointcloud_and_slam_pose2
将相机坐标系的点云转为到无人机机身坐标系。你可以通过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中可以调整的一部分参数如下：
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
Fig 4, topic for Octomap
与点云对应的Octomap如下图所示：
Fig 5, Generated Octomap
你可以通过rosed来调整例如Octomap分辨率以及最大范围等参数：
rosed octomap_server octomap_mapping.launch
接下来，我们将讨论如何使用提供的 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存放在下列目录中：
(GAAS_PATH)/software/Navigator
Navigator 默认使用无人机起飞点前方70米处为终点，接下来，在执行Navigator之前，你需要首先起飞无人机：
python px4_mavros_run.py
接下来，开启Navigator：
python Navigator.py
一条红色的飞行路径将会出现在RVIZ之中，无人机会沿着这条路径飞行到设置好的目标终点。
Fig 8, A* planned Path
4. 路径修剪
另外一个我们需要注意的是路径修剪。A* 算法生成的原始路径由一系列航点构成，而航点的步长默认设置为0.2米，如果无人机按照0.2米为步长的路径飞行飞行任务会太过繁琐同时飞行时间也会太长，所以我们需要将路径中额外且不重要的航点删除。在此我们提供两种路径修剪方法：
共线判断：对于从A点到B点的一条路径，如果任意两点都共线，我们将只保留路径起点以及重点，删除中间部分；
bresenham3D 障碍物判断：对于从A点到B点的一条路径，按照 A 到 A+1， A 到 A+2， A 到 A+3的方式，使用bresenham3D方法进行连线，如果连线和障碍物不重合，且与障碍物距离大于某一设定好的阈值，将连线的终点从原始路径中删除，若连线与障碍物重合，保留上一条连线终点，并将新的连线起点设置为上一条连线终点。
你可以通过如下脚本查看两种算法的处理效果：
python (GAAS_PATH)/software/Navigator/path_optimization/path_pruning.py
Fig 9, path pruning algorithms
在图片中，红色点组成的立方体代表了一个障碍物； 下方红色点组成的路径代表一条原始路径，其中有很多额外的航点； 绿色点组成的路径为原始路径经过共线判断后的路径，其中只保留了转角处的点；蓝色点组成的路径为原始路径经过bresenham3D 障碍物判断处理后的路径。
在Navigator 中我们按照如下逻辑进行路径修剪：
A* 生成原始路径；
对原始路径进行共线检查并生成一条新的路径；
对第二步生成的新路径进行bresenham3D 障碍物检测，剔除额外的航点，得到最终航线。
第三步的时间复杂度较高， 所以我们先进行一步共线判断再进行处理。
最后，我们可以发现经过路径修剪后的路径更加的有效且简单，为无人机提供了更好的航线。
Reading Materials
​https://courses.cs.washington.edu/courses/cse455/09wi/Lects/lect16.pdf​
​http://wiki.ros.org/octomap​
​http://wiki.ros.org/octomap_server​
​https://en.wikipedia.org/wiki/A*_search_algorithm​