避障算法之3DVFH+

目录

一、3DVFH+论文翻译

摘要

1.引言

2.相关工作

3.八叉树地图

4.3DVFH+

4.1 第一步：八叉图探索

4.2 第二步：二维基础极直方图(2D Primary Polar Histogram)

4.3 第三步：物理参量(Physical Characterstics)

4.4 第四步：二维二进制极直方图(2D Binary Polar Histogram)

4.5 第五步：路径检测与选择(Path detection and selection)

---

博主平时玩ROS+PX4的避障功能包(avoidance)的时候，对其避障原理颇为好奇...据查用的是本blog的主角——3D-VFH+算法。先附上论文链接:3D-VFH+ ，值得一提的是论文发表于2014年，彼时并没有ORB-SLAM、LSD-SLAM之属。

---

一、3DVFH+论文翻译

摘要

本文提出了一种新的实时三维避障算法3D-VFH+。该方法基于2D VFH+避障算法并且使用了八叉树(octomap)框架来表现三维环境。算法将从八叉图生成2D极坐标直方图并用于生成机器人的运动。该方法可实时运行，速度可达到300us计算一次机器人运动。

1.引言

话不多说吧，原文比较简短。提到了基于激光雷达的SLAM方法(如：FastSlam、GraphSlam)和基于RGB-D相机的SLAM算法，这两种传感器可以获取深度，可以简单粗暴地对未知环境构建3D地图同时对机器人进行定位，基本等于废话。总结一点就是，作者当时(2014年V-SLAM尚未流行)针对的就是采用可以获取深度信息的SLAM方案如何进行避障，对标传统的A*和D* Lite（姑且这么翻译）之流，作者指出以上算法的计算代价昂贵，实时性差。

2.相关工作

1. VFH+算法：该算法可以用于在二维平面上移动的机器人的路径规划。VFH+能够实时生成一条平滑的轨迹，并且将尺寸和转向速度等物理参量考虑在内。
2. D* Lite算法：Hrabar采用D* Lite进行概率性道路地图规划，重规划需要耗时0.15s。
3. A*算法：Maier等人使用了部分八叉图（机器人垂直方向的尺寸）然后将其投影成一个2D地图，接着采用A*算法进行二维路径规划。Nieuwenhuisen等人提出了一个局部多分辨率路径规划系统，采用了多分辨率栅格地图用于局部路径规划，并且用全局规划器global planner产生一条到下一目标的新路径，同样使用的A*算法。

3.八叉树地图

略，详情参见高翔的视觉SLAM十四讲。

4.3DVFH+

3DVFH+旨在针对3D环境进行路径规划，逼格满满呀...当给定机器人在3D环境中的位姿的时候，算法采用的八叉树去求解障碍物的位置。算法分为五步去计算机器人的新的运动。

4.1 第一步：八叉图探索

机器人在一个很大的空间内运动时显然受限于计算能力不可能去考虑所有的体素(voxel)。因而八叉图探索阶段仅仅考虑无人机周围一定范围(a bounding box around the robot)的体素，自然而然地该立方体以机器人的中心(VCP)为中心点，取固定的长宽高。总而言之，一句话就是确定SLAM求出的Octomap中那些voxel是VCP周围立方体内的体素而需要加以考虑，其余则可忽略；八叉树由于树形结构可以很快的把一个分支的不需要考虑的体素全忽略，因而效率较高。

4.2 第二步：二维主极直方图(2D Primary Polar Histogram)

这一步就是将需要考虑的体素添加到二维主极直方图中，活跃的体素通过两个角来获取，如下Fig.2所示。角是通过体素位置与VCP来求得的，将每个体素求得的权重（先挖个坑？）填入二维主极直方图。

首先，活跃的体素元胞将通过在立方体内取一个球面的方式得到减少，这通过计算该元胞与VCP之间的欧式距离来实现。当欧式距离大于约束求替班的时候，该元胞将被忽略。（这不就是第一步吗，该段都是废话。）

接着，算法计算两个角度 方位角azimuth angle  和 海拔角elevation angle  用于生成二维主极直方图，如Fig.3。

方位角  ：同样被用于VFH+算法，计算如下。通过计算体素的最大与最小角度来判断是否处于二维主极直方图的同一个元胞内。

海拔角 ：海拔角的计算不同于VFH+.先计算p(Fig3.左边)，然后计算 。

---

重点来了。

通过在二维主极直方图中用去扩大所有活动体素的大小以补偿机器人的尺寸大小带来的影响。扩大因子将机器人半径、安全半径和体素尺寸考虑在内。扩大体素以便算法可以计算出该体素所在的最大与最小角度，如下所示。

体素与VCP之间的最小距离通过欧氏距离减去扩大尺度来得到。

接下来便是计算权重了。一个体素的权重通过欧氏距离和占据概率来计算。如下所示。

常量a和b的值不重要，重要的是相对值，如下。

权重将在第四步中用于获取二维二进制极直方图。

4.3 第三步：物理参量(Physical Characterstics)

先不看，吃饭去

4.4 第四步：二维二进制极直方图(2D Binary Polar Histogram)

也很简单。就是根据两个阈值h和l进行截断操作，高于h则为1，低于l则为0；中间部分就取相邻点的值。(这样似乎不太妥当吧！！万一接连好几个都处于中间值咋办。)

4.5 第五步：路径检测与选择(Path detection and selection)

通过一个滑窗来对二维二进制极直方图进行检测与选择，并选择最小路径权重的路径。滑窗将把所有元素均为0的窗口标记为可通过。这样仅有大的开口可以被作为可通过路径穿过。根据极坐标的性质，当窗口在二维图的水平方向两端时，将把另一侧的对接用于计算。当窗口子二维图垂直方向的两端时，将同时检查与之相差180度方位角的元胞。

接着，将计算三个路径权重，并且将其结合成一个路径权重用于候选方向选择。如式子19。

第一个路径权重基于目标角度kt与候选方向的差v来计算，第二个权重通过机器人旋转角theta与候选方向之间的差v来计算，第三个权重是先前选择方向与候选方向之间的差。作者将其三个稀疏u分别设为 (5,2,2)。三个系数可用于调整选择方向的倾向性，比如倾向于做攀升运动还是水平转向运动。

5.结果

这里我不多bb了，有兴趣看原文吧。