在浏览PX4开发手册过程中看见里面提到的一种障碍规避算法——3DVFH+。该算法在之前做本科毕设时候有所接触，不过当时是进行了2D的Matlab仿真，down个代码改改障碍物形状，飞机当做质点考虑，没有深入了解这个算法在实际中该怎么应用。今天找到了论文的原文《3DVFH+:Real-Time Three_Dimensional Obstacle Avoidance Using an Octomap》，从头到尾仔细看了看，发现比较适合无人机在室内导航中做障碍规避，尤其是在低速时的无人机状态可以简化为(x,y,z,yaw)时。更方便的 是这个算法被集成到ROS的功能包中，不过程序还没来得及看，Github上有源码，可以直接下载浏览。下面给出原文和一些自己的理解。
在之前解决三维高密度障碍的方法中多数是从3D地图中建立2D地图，或者是有多层高度的2D地图，在实际三维空间的飞行中，这些方法效果不佳。本文提出使用了基于2D的VFH+和octomap表示三维环境的方法。该方法从octomap中生成一个2D极坐标直方图来指引机器人运动。并且计算量极小，实时性很好。
1.机器人对环境中的密集障碍物的定位可以通过laser或者depth camera实现，并且用octomap表示。
2.目前2D机器人在3D环境中导航通过建立2.5D高度地图或者2D地图来规避障碍。解决3D的路径规划使用A或D，但这两种方法耗费计算资源。
3.作者的方法为基于2DVFH+，从octomap中生成一个2D极坐标直方图来进行3D导航。（贴出原文）
简单说就是之前的导航算法针对的都是2D运动机器人，3D环境使用A和D计算太慢。作者将自己的算法分成五个部分进行说明。
Octomap是一种表示3D环境的有效方法。只要传感器能给出深度信息（点云信息）就可以生成，基本单位是voxel（体素），voxel的大小也可称为分辨率，体积越大计算量越小但是精度越大。voxel定义网上有很多，我截个图直观的展示一下，然后附带着原文简要说一下octomap的表示方法。
octomap框架是一个基于八叉树结构的3D占用网格映射，每个体素（也称节点）都是一个正方体。每个体素可以包含8或0个子节点，当八个子节点均被占用时，便可缩成一个父节点，全部空闲时同理。传感器中有一些不确定信息，octomap的叶子体素中包含一个概率，也就是octomap有一个环境的概率表示，这个值来表示体素占据或者空闲的概率。八叉树框架没有直接包含体素的位置信息，但是位置可以在八叉树下降时计算得到，根体素有一个固定的位置，子体素的数量决定了子体素的位置。
机器人的传感器感知范围有限，并且考虑到精度和计算载荷，在机器人运动过程中，仅对一个小边界范围内进行地图探索。里面还提到一些简化运算的技巧，不展开谈。
将体素中得到的信息添加到2D原始极坐标直方图中，一个有效的体素由两个角度决定。
然后对体素进行处理，一些不符合要求的体素别剔，对剩下的有效的体素进行位置（距离和角度）计算。具体数学过程就是基本的几何知识，不难理解。图四中还考虑到飞机的安全包络，机体半径等因素影响。
算法对范围内体素的权重进行了分配，根据欧氏距离和被占有的概率，体素的权重值在第四步的二值极坐标直方图有很大作用。
这部分讲到了机器人在运动过程中的动力学问题，有时候需要调整航向进行转弯的机动，这些动作和机器人当前状态有关。当然在无人机速度不快的时候，我们可以将其运动分为旋转和平移两个独立的部分。后半部分主要讲了转弯半径和爬升的机动策略。
此部分减少了原直方图的信息来生成一种新的直方图。
在2D二值化极坐标直方图中进行窗口扫描，如果空隙大于飞机包络，记作一条可选路径。对于多条候选路径，本文给出了一个代价函数来对路径进行选择。
我的理解，第一个因素是看各条候选路径“绕远量”，第二个因素看各条候选路径的“旋转代价”，第三个因素可以理解为“机动代价”，对于双轮车和慢速无人机，我们可以先转向再行驶，因此第二三个因素貌似是等价的。当然速度过快的无人机就不能作此简化。
实验给了一个gazebo的仿真，没有实际的测试。后面内容略掉。
准备以后拿飞机上结合自己的任务试一试，有结果继续更。
