Recently, researchers have tried to solve the computational intensive three-dimensional obstacle avoidance by creating a 2D map from a 3D map or by creating a 2D map with multiple altitude levels. When a robot can move in a three-dimensional space, these techniques are no longer su cient. This paper proposes a new algorithm for realtime three-dimensional obstacle avoidance. This algorithm is based on the 2D VFH+ obstacle avoidance algorithm and uses the octomap framework to represent the three-dimensional environment. The algorithm will generate a 2D Polar Histogram from this octomap which will be used to generate a robot motion. The results show that the robot is able to avoid 3D obstacles in real-time. The algorithm is able to calculate a new robot motion with an average time of 300 s.
Robotic applications such as airborne or underwater robots need to avoid
obstacles in a 3D environment. These robots need to create a 3D map from the
environment to locate obstacles. This can, for example, be accomplished by using
a 3D laser scan Simultaneous Localization and Mapping (SLAM) algorithm [
Researchers [5{7] have tried to solve the 3D obstacle avoidance problem with
a 2D robot by creating a 2.5D height map from the original 3D map or by
projecting the 3D map on a 2D map. This technique can only be used if the robot
can move in 2D. When the robot can move in 3D, researchers [
The 3DVFH+ algorithm is based on the 2D VFH+ algorithm [
This research paper is organised in the following order. To begin with, the related researches will be described in Section 2. Then in Section 3, we will discuss the octomap framework. Section 4 will describe the multiple stages of the 3DVFH+ algorithm. Section 5 follows with an analysis of the results of this research. Next, we will come to a conclusion in Section 6. Finally, Section 7 proposes future improvements to the 3DVFH+ algorithm. 2
Ulrich et al. [
Hrabar [
Burgard et al. [
Maier et al. [
Nieuwenhuisen et al. [
Robot applications often use a probabilistic representation of the environment because input sensors introduce noise and therefore uncertainty into the system. The octomap leaf voxels contains a probabilistic number. This allows the octomap to have a probabilistic representation of the environment. This likelihood will be used to determine if te voxel is occupied or if voxel is free.
The octomap framework does not directly include information on the voxel location. The location can be calculated when descending the octomap tree. The root voxel has a xed location and the number of the child voxel that is explored determines the location of the child voxel. 4
3DVFH+ is an enhanced version of the two-dimensional VFH+ algorithm [
First Stage: Octomap Exploring
The octomap data structure makes use of the Octree data structure (see
Figure 1). When the robot moves around in a large environment it is not possible to
explore all the voxels due to computational limits. So the Octomap's exploring
stage will only research voxels that lie within a bounding box around the robot.
This bounding box has a size of ws ws ws (width, height and depth) and
contains the Vehicle Center Point (VCP) as centroid. The VCP is the center point
of the robot and the whole robot will be represented by this point. When a voxel
lies within the bounding box, the voxel is an active cell Ci;j ;k with i; j; k
coordinates within the active region Ca. The exploring stage only needs to nd voxels
that lie within the boundaries of the bounding box. The algorithm needs to use
the location of the voxels to determine which voxels need further exploration or
which voxels can be ignored. But the location of the voxels is not implemented
in the octomap data structure to reduce memory overhead [
Second Stage: 2D Primary Polar Histogram The second stage will add the information from the voxel (which has been found by the rst stage) into the 2D primary polar histogram. This histogram (as shown in Figure 2) is a polar histogram where the location of an active voxel is determined by two angles. These angles are determined by the position of the voxel and the VCP. A weight that is calculated based on the voxel will be inserted into the 2D primary polar histogram with the two angles as coordinates.
First, the list of active cells will further be reduced by making a bounding sphere within the bounding box. This can be accomplished by calculating the euclidean distance di;j;k between the VCP and each voxel. When the euclidean distance is larger then the radius of the bounding sphere di;j;k > ws=2, the active cell will be ignored. A boundary sphere is necessary to create a rotation independent 2D polar histogram.
Next, the algorithm will calculate the two angles to determine the coordinates within the 2D primary polar histogram Hp. These angles are the azimuth angle z (x-axis of the 2D primary polar histogram) and the elevation angle e (y-axis of the 2D primary polar histogram). These angles are shown in Figure 3 li,j,k
d i,j,k λ i,j,k rr+s+v
VCP
The azimuth angle z is calculated by using (1) which is also used in the
VFH+ algorithm [
z = oor
arctan 1 xi yi x0 y0 ! (1)
The elevation angle needs to be calculated in a di erent way (see (2) and (3)) because we need the elevation angle independent of the azimuth angle. This can be implemented by calculating p based on the x and y coordinates of the VCP and the voxel, see Figure 3.
e = oor 1 arctan zi p z0 ! p = p(xi x0)2 + (yi y0)2 (2) this results in The previous calculations used the VCP and the center of the voxel to calculate the two angles. The size of the robot is implemented by enlarging all the active voxels in the 2D primary polar histogram with rr+s+v to compensate for the robots size, see Figure 4. This enlargement factor is calculated by adding the radius of the robot rr, the safety radius rs and the voxel size rv. The voxels are enlarged so the algorithm can calculate the maximum and minimum angle in which the voxel lies, see (4). (3) (4) (5) (6) (7) The minimum distance between the voxel and the VCP can be calculated by subtracting the enlargement from the euclidean distance, see (5). li;j;k = di;j;k rr+s+v Now the algorithm has calculated which cells in the 2D primary polar histogram are in uenced by the voxel, the algorithm needs to calculate the weight that the voxel will add to the 2D primary histogram. The weight of a voxel is calculated based on the euclidean distance li;j;k and their occupancy certainty oi;j;k, see (6).
Hzp;e =
X i;j;k2Ca >>>: 0 8 >> (oi;j;k)2(a > < bli;j;k) if e 2 [ e and z 2 [ z otherwise ; e +
] ; z + ] i;j;k = oor 1 arcsin rr+s+v di;j;k ! Two constants a and b are calculated by using (7) to obtain a balanced a and b. The value of these constants is unimportant, only the relative size or fraction is important.
a b ws 2 The weight of a voxel will be used during stage 4 to obtain the 2D binary polar histogram. After the second stage, the third stage will calculate and add new information into the 2D primary polar histogram based on the physical characteristics of the robot and location of the voxel. When the robot has a certain heading retrieved by the RGBD-SLAM algorithm and speed, it cannot change direction instantly,
Δxr Δxl VCP rr Δyr dr
Voxel
(8)
(9)
(10)
(11)
so it will need to turn. When the robot is moving slowly, this stage will be ignored
because it can change its direction instantly. The robot's turning trajectory
depends on the robot's forward velocity, turning speed and the climbing speed.
The turning speed and robot velocity are implemented in the VFH+ algorithm
[
First, the two center points of the turning circle need to be calculated (see
Figure 5). This can be achieved in the same way as in the original VFH+
algorithm [
xr = rr sin xl = rl sin
yr = rr cos yl = rl cos Secondly, the algorithm needs to check every active cell Ci;j;k to see if it lies on the turning circle. This will be achieved by calculating the distance dl;r between the centre of the turning circle and the voxel, see (10). To detect if the voxel lies on the turning circle, the algorithm will compare this distance with the safety range rr+s+v and the diameter of the turning circle rl;r, see (11). The function x(i) will calculate the x distance between the voxel and the VCP. dr = p( xr dl = p( xl x(i))2 + ( yr x(i))2 + ( yl y(j))2 y(j))2 dr < (rr + rr+s+v) dl < (rl + rr+s+v) Thirdly, the algorithm also needs to take the climbing motion of the robot into account. The algorithm will calculate which cells within the 2D masked polar histogram are blocked by the voxel. This will be achieved by calculating for all the following e angles after the object which z angle is blocked by the voxel. First, the climbing motion constant f needs to be calculated. This is achieved by calculating the turning distance t to the obstacle, see (12). Next, the climbing motion constant is calculated with the altitude di erence zi z0 and the turning distance, see (13). t Next, a new unreachable altitude z z needs to be calculated for every z angle after voxel. The turning distance t z, see (14) to the next z needs to be calculated to determine the unreachable altitude, see (15).
Finally, the unreachable elevation angle e will be calculated using the altitude di erence and the euclidean distance l z, see (16) and by using (17). l z = pr4 2:r2: cos(270
2: z) 1 e = arctan( z z
) l z These calculations can be extended so that the algorithm will calculate unreachable cells with a given forward speed and maximum climb speed. t = f = (17) (18) 4.4
Fourth Stage: 2D Binary Polar Histogram The previous stages will generate a 2D primary polar histogram based on the voxels of the octomap. The fourth stage will reduce the information further by creating a 2D binary polar histogram based on the 2D primary polar histogram. This will be accomplished by comparing every cell in the 2D primary polar histogram with two thresholds low and high. When a value is higher than high the point will be 1 in the 2D binary polar histogram. When a value is lower than low the point will be 0 in the 2D binary polar histogram. If a point lies between the two thresholds the value next to the point will be used in the 2D binary polar histogram. The two thresholds allow the algorithm to distinguish real obstacles and measurement errors.
Because the 3DVFH+ algorithm uses a 2D polar histogram, the thresholds high and low need to change when using a di erent elevation angle e. The cells of the 2D polar histogram do not have the same size (as shown in Figure 2) so to compensate for this, di erent thresholds are required for di erent elevation angles, see (18). The size of these thresholds depends on the robot, the robot's speed, the window size of stage ve, the octomap resolution and the bounding sphere size.
8 1 Hbz; e = < 0
p : H z 1; p
p if H z; e >
p if H z; e < otherwise ehigh elow
Fifth stage: Path detection and selection The fth stage searches for available paths in the 2D binary polar histogram and selects the path with the lowest path weight. To determine which paths are available the algorithm will detect openings in the 2D binary polar Histogram by moving a window around the 2D binary polar histogram. This window will mark the path passable, if all the elements in the window are equal to 0. This way only large openings will be threaded as available paths. When implementing this window the algorithms needs to take the polar properties of the histogram into account. When the window crosses the boundaries of the histogram, the window will use elements that are connected by the rules of a 2D polar histogram.
When the window crosses the upper or lower boundary of the histogram (see black markings in Figure 6) the cells that need to be checked are located in the same elevation angle but 180 degrees rotated over the azimuth angle. When the window crosses the left or right border the window will check the cells on the other side of the histogram.
Next, three path weights will be calculated and combined into a single path
weight for the candidate direction, see (19). The rst path weight will be
calculated based on the di erence between the target angle kt and the candidate
direction v. The second path weight is the di erence between the rotation of
the robot and the candidate direction v. The last path weight is the di erence
between the previous selected direction ki 1 and the candidate direction v. The
variable is used to select which of the three path weight has a larger impact
on the nal path weight. We used for our goal-oriented robot 1 = 5, 2 = 2
and 3 = 2 as proposed by Ulrich et al. [
The function (v1; v2) will calculate the di erence between the two elevation angles and the two azimuth angles. From these di erences the function will generate a weight based on the two calculated angle di erences. ki = 1: (v; kt) + 2: (v; ) + 3: (v; ki 1) (19)
When the path weight of all the candidate directions is calculated the algorithm can select the direction with the lowest weight. The chosen direction will be converted into a robot motion. The conversion can easily be implemented by making a decision tree that will generate a robot motion based on the coordinates of the calculated direction. 5
In this section we will evaluate the 3DVFH+ algorithm. The requirements for
the 3DVFH+ algorithm are that it needs to avoid obstacles in a 3D environment
and perform these calculations in real time. These results are generated by using
the Robot Operating System (ROS) [
3D Obstacle Avoidance Result
The 3DVFH+ algorithm is a 3D obstacle avoidance algorithm, so to test the
algorithm we simulated a quad-copter in gazebo. The quad-copter needed to y
from point A to point B and from point B to point C without bumping into
obstacles (as shown in Figure 7). For this simulations, the dataset FR-079
corridor [
The left image in Figure 7 shows how the robot will go from point A to point B and from point B to point C. The right in Figure 7 image shows how the robot meets an obstacle O and will avoid it by a combination of ying over the obstacle and turning to avoid the wall. 5.2
Performance Result This section will describe the performance of the 3DVFH+ histogram is measured by using the same dataset and the same intermediate points as in the previous section, see Figure 7. For these experiments we used a standard laptop with an Intel i7-3720QM processor with a NVIDIA GeForce GT 650M. The simulations were executed on a di erent machines to ensure that the simulation software did not in uence the results. When performing the simulation the robot was ying around at a low speed, so the third stage was ignored. The third stage did not impact the performance measurements. The following parameters has the most in uences to the performance of the algorithm: the bounding box size, the moving window size, the thresholds of stage four, the robot speed, and the octomap resolution. Due to the increasing complexity and the amount of voxels, the performance will raise or slow down.
We found that the average time that the algorithm needs to calculate a new robot motion is 326 s. As a result, that the algorithm is capable of calculating on average a new robot motion 3061 times per second. The rst three stages mainly determine the speed of the algorithm because these stage speed depends on the number of voxels within the bounding sphere. Based on multiple experiments we found that it takes 300 ns for every voxel to calculate the 2D primary polar histogram. The speed of the fourth and fth stage are negligible with a speed of 0.2 s for every robot motion. In practice the algorithm is limited to the other algorithms that the robot uses to calculate its pose and the octomap. 6
The 3DVFH+ algorithm is a real-time three-dimensional obstacle avoidance algorithm that uses an octomap to determine the obstacles locations. The algorithm can determine the location of these obstacles in real-time because the algorithm will only take obstacles into account that are located close to the robot. From the location of the obstacles the algorithm will make a 2D primary polar histogram based on the pose of the robot and location of the obstacles. Next, the algorithm will take the physical capability of the robot into account. In this 2D binary polar histogram the algorithm will nd multiple paths, give them a path weight and determine the path with the lowest path weight. This path will be used to calculate a motion for the robot. By using these techniques the algorithm is able to calculate the robot motion real time in 3 dimensions. 7
Before the 3DVFH+ algorithm can be used, the algorithm needs to be con gured based on the robot's size, robot's speed, octomap's resolution and multiple levels of thresholds to generate a 2D binary polar histogram. These parameters are chosen empirically. In future work this process could be automated.
In this system the thresholds that generate a 2D binary polar histogram are static. But when the robot is moving with a di erent speed, di erent thresholds could and should be used.