# 3D Exploration and Navigation with Optimal-RRT Planners for Ground Robots in Indoor Incidents

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Methods and Algorithms

#### 2.1. RRT*—Based Path Planning in 3D Environments

#### 2.1.1. RRT* Planning on Point Clouds

#### 2.1.2. 3D Sampling Space for RRT* Planning

#### 2.1.3. Terrain Analysis

- Number of points in the sphere, $\eta $. The more points we have in the area the more accurate and reliable the representation of the surface is. In our case, we consider that a value of one point per centimetre in a plane is good density, although this value can be changed according to the particular constraints. Therefore, an upper boundary of number of points has been calculated as ${\eta}_{max}=\pi \ast sphere\_radiu{s}^{2}\ast 100$. In which the $sphere\_radius$ is presented in meters. This boundary is also used for normalization:$$\eta =(1.0-num\_points/{\eta}_{max}).$$
- Distance between the sample ${p}_{i}$ and the mean of the set of points $\overline{p}$, named as ${d}_{m}$. If these two points are not close, some areas of the region could be poorly represented by very few points in several cases:$${d}_{m}=dis{t}_{{p}_{i}-\overline{p}}/sphere\_radius.$$
- Standard deviation of the point set, $\sigma $. A high deviation could indicate that the points are dispersed along the patched region, and therefore, the probability of having voids without points could be smaller.$$\sigma =(1.0-stddev/{\sigma}_{max}).$$

- Obtaining the points around ${p}_{i}$ through nearest-neighbours search from the local point cloud.
- Calculating the pitch, roll and roughness values.
- If $\delta ,\beta $ and $\lambda $ exceed the boundaries, the terrain is considered invalid, and the remaining steps are skipped.
- If the terrain is valid, the remaining features ($\eta ,{d}_{m},\sigma ,{d}_{g}$) are calculated.
- The cost for the sample is computed according to (3), and a new node is added to the tree following the regular RRT${}^{*}$ expansion and rewiring process.

#### 2.2. 3D RRT*-Based Exploration

#### 2.2.1. Leaf Clustering

#### 2.2.2. Evaluation of Potential Frontiers

- Wall-leaf rejection.Firstly, the position of the leaf in the space is studied. The points inside a sphere of a radius equal to the inflated circumscribed sphere of the robot shape are assessed. The inflation radius has been chosen to be $0.3\phantom{\rule{3.33333pt}{0ex}}m$. The points are associated with a grid, and the normal vector to the surface formed by the points of each cell is computed. This way, we can count the number of cells of a vertical surface. If this number exceeds a pre-defined threshold, the leaf is considered that is very close to a wall and it is discarded as a possible frontier. An example is shown in Figure 5.
- Visited region rejection.Secondly, the position of the leaf is compared with the position of all the previous visited frontiers. If it is very close to a region that has been visited twice, the leaf is also discarded. To do that, the list of visited regions (with the number of visits) is provided by the counter of visited regions, which is explained in Section 2.2.3. This module plays an important role when there are frontier areas in which it is not possible to gain more information. In those cases, it is able to lead the robot to abandon the areas already explored.
- No-frontier rejection.The third step checks whether the leaf can be considered as a frontier or not. To determine that, the points in a sphere of radius 1.5 m around the leaf (this value can be configured) are counted. Moreover, the standard deviation of the points is also computed. Therefore, the lesser number of points are detected in the area where the more promising the frontier point for exploration is. The number of points ($points$) and the standard deviation ($stddev$) are normalized by choosing upper bounds ($max\_points$, $max\_stddev$) employed for the normalization. Finally, a normalized frontier cost ${F}_{cost}$ for the leaf p is computed as:$${F}_{cost}\left(p\right)=0.7\ast \frac{points\left(p\right)}{max\_points}+0.3\ast \frac{stddev\left(p\right)}{max\_stddev}.$$A threshold value ${\Delta}_{f}$ is set, so that if the frontier cost ${F}_{cost}$ is higher than the threshold, then the leaf is not considered as a frontier. An example is shown in Figure 5 with a threshold value of ${\Delta}_{f}=0.4$. The threshold evaluation determines the value of this threshold dynamically as explained in Section 2.2.4.
- Frontier evaluators.Once we have determined the set of tree leaves that are potential frontiers, the selection of the most promising frontier is performed by the frontier evaluators.We propose a novel evaluator based on a cost function, ${C}_{exp}$, that we have called
**Cost Function Exploration (CFE)**. This cost is obtained as the weighted sum of the cost of the path to the frontier $C\left({\zeta}_{{p}_{f}}\right)$, the cost of the frontier ${F}_{cost}\left({p}_{f}\right)$, and a "return" cost $R\left({p}_{f}\right)$, which penalizes the frontiers that are close to areas that the robot has already visited:$${C}_{exp}\left({p}_{f}\right)={\omega}_{c}\phantom{\rule{3.33333pt}{0ex}}C\left({\zeta}_{{p}_{f}}\right)+{\omega}_{F}\phantom{\rule{3.33333pt}{0ex}}{F}_{cost}\left({p}_{f}\right)+{\omega}_{R}\phantom{\rule{3.33333pt}{0ex}}R\left({p}_{f}\right).$$Unlike other works based on regular RRTs [21,23], our navigation cost $C\left({\zeta}_{{p}_{f}}\right)$ includes the terrain evaluation and not only the length of the path to the frontier.Finally, the aim of the return cost R is to prevent the robot from re-visiting areas where the robot already was close by. This is particularly useful in our setup, which involves tunnel scenarios and the evaluation on a local point cloud instead of the global map. In these cases, the robot is biased to explore forward the tunnel instead of going back.The return cost is calculated by computing the distance between the frontier point and the closest point of the trajectory followed by the robot so far. This cost decreases linearly from 1 to 0 in a pre-defined distance range from 0 up to 2 meters:$$R\left({p}_{f}\right)=max\_dist-d({p}_{f},{t}_{r})/max\_dist,$$We also compare this evaluator with two methods widely-used in the literature, as in Reference [22], which have been adapted to work in 3D over point clouds:

**Nearest Frontiers Exploration (NFE)**. This is an adaptation of the well-known Nearest Frontier approach [14] to 3D point clouds. It is based on proximity criteria by selecting the frontier with the smallest Euclidean distance to the robot ignoring the existence of obstacles.**Biggest frontier Exploration (BFE)**. It is based on size criteria. The frontier with less information (${F}_{cost}$) is selected as the goal.

#### 2.2.3. Counter of Visited Regions

#### 2.2.4. Evaluation of exploration size and frontier threshold

Algorithm 1: Size-and-threshold evaluator algorithm. |

## 3. Results

#### 3.1. Implementation and Simulations

#### 3.2. Navigation Results

#### 3.2.1. Size of the Point Cloud for Sampling Space

#### 3.2.2. Analysis of the Resolution of the Point Cloud for Traversability Analysis

#### 3.2.3. Evaluation of the Cost Function for Path Building

- Basic cost function: ${\omega}_{\delta}=0.4$, ${\omega}_{\beta}=0.4$, ${\omega}_{\lambda}=0.2$.
- Augmented cost function: ${\omega}_{\delta}=0.1$, ${\omega}_{\beta}=0.1$, ${\omega}_{\lambda}=0.1$, ${\omega}_{\eta}=0.175$, ${\omega}_{{d}_{m}}=0.175$, ${\omega}_{\sigma}=0.175$, ${\omega}_{{d}_{g}}=0.175$.

#### 3.3. Exploration Results

#### 3.3.1. Frontier Evaluators Comparison

#### 3.3.2. Frontier Threshold Evaluation

#### 3.3.3. Visited Regions Evaluation

## 4. Conclusions

## Author Contributions

## Funding

## Conflicts of Interest

## References

- Menna, M.; Gianni, M.; Ferri, F.; Pirri, F. Real-time autonomous 3D navigation for tracked vehicles in rescue environments. In Proceedings of the 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, Chicago, IL, USA, 14–18 September 2014; pp. 696–702. [Google Scholar]
- Kruijff, G.J.M.; Tretyakov, V.; Linder, T.; Pirri, F.; Gianni, M.; Papadakis, P.; Pizzoli, M.; Sinha, A.; Emanuele, P.; Corrao, S.; et al. Rescue Robots at Earthquake-Hit Mirandola, Italy: A Field Report. Proceeding of the IEEE International Symposium on Safety, Security and Rescue Robotics (SSRR), College Station, TX, USA, 5–8 November 2012; pp. 1–8. [Google Scholar] [CrossRef] [Green Version]
- Tsitsimpelis, I.; Taylor, C.J.; Lennox, B.; Joyce, M.J. A review of ground-based robotic systems for the characterization of nuclear environments. Prog. Nucl. Energy
**2019**, 111, 109–124. [Google Scholar] [CrossRef] - Papadakis, P. Terrain traversability analysis methods for unmanned ground vehicles: A survey. Eng. Appl. Artif. Intell.
**2013**, 26, 1373–1385. [Google Scholar] [CrossRef] [Green Version] - Garrido, S.; Malfaz, M.; Blanco, D. Application of the fast marching method for outdoor motion planning in robotics. Rob. Auton. Syst.
**2013**, 61, 106–114. [Google Scholar] [CrossRef] [Green Version] - Liu, M.; Siegwart, R. Navigation on point-cloud-A Riemannian metric approach. In Proceedings of the 2014 IEEE International Conference on Robotics and Automation (ICRA), Hong Kong, China, 31 May–7 June 2014; pp. 4088–4093. [Google Scholar] [CrossRef]
- Liu, M. Robotic online path planning on point cloud. IEEE Trans. Cybern.
**2016**, 46, 1217–1228. [Google Scholar] [CrossRef] [PubMed] - Hornung, A.; Wurm, K.M.; Bennewitz, M.; Stachniss, C.; Burgard, W. OctoMap: An efficient probabilistic 3D mapping framework based on octrees. Auton. Robot.
**2013**, 34, 189–206. [Google Scholar] [CrossRef] [Green Version] - Song, S.; Jo, S. Online inspection path planning for autonomous 3D modeling using a micro-aerial vehicle. In Proceedings of the 2017 IEEE International Conference on Robotics and Automation (ICRA), Singapore, 29 May–3 June 2017; pp. 6217–6224. [Google Scholar] [CrossRef]
- Oleynikova, H.; Taylor, Z.; Fehr, M.; Nieto, J.; Siegwart, R. Voxblox: Building 3D Signed Distance Fields for Planning. arXiv
**2016**, arXiv:1611. [Google Scholar] - Ramos, F.; Ott, L. Hilbert maps: Scalable continuous occupancy mapping with stochastic gradient descent. Int. J. Rob. Res.
**2016**, 35, 1717–1730. [Google Scholar] [CrossRef] - Philipp Krüsi, P.; Furgale, P.; Bosse, M.; Siegwart, R. Driving on Point Clouds: Motion Planning, Trajectory Optimization, and Terrain Assessment in Generic Nonplanar Environments. J. Field Rob.
**2016**, 34, 940–984. [Google Scholar] [CrossRef] - Lavalle, S.M. Rapidly-Exploring Random Trees: A New Tool for Path Planning. Technical Report; In CiteSeerX; 1998. [Google Scholar]
- Yamauchi, B. A Frontier-Based Approach for Autonomous Exploration. In Proceedings of the IEEE International Symposium on Computational Intelligence, Robotics and Automation, Monterey, CA, USA, 10–11 July 1997; pp. 146–151. [Google Scholar]
- Witting, C.; Fehr, M.; Bähnemann, R.; Oleynikova, H.; Siegwart, R. History-Aware Autonomous Exploration in Confined Environments Using MAVs. In Proceedings of the 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2018, Madrid, Spain, 1–5 October 2018; 2018; 2018, pp. 1–9. [Google Scholar] [CrossRef] [Green Version]
- Song, S.; Jo, S. Surface-Based Exploration for Autonomous 3D Modeling. In Proceedings of the 2018 IEEE International Conference on Robotics and Automation (ICRA), Brisbane, QLD, Australia, 21–25 May 2018; pp. 1–8. [Google Scholar] [CrossRef]
- Dang, T.; Papachristos, C.; Alexis, K. Visual Saliency-Aware Receding Horizon Autonomous Exploration with Application to Aerial Robotics. In Proceedings of the 2018 IEEE International Conference on Robotics and Automation (ICRA), Brisbane, QLD, Australia, 21–25 May 2018; pp. 2526–2533. [Google Scholar] [CrossRef]
- Dornhege, C.; Kleiner, A. A Frontier-Void-Based Approach for Autonomous Exploration in 3D. Adv. Rob.
**2013**, 27, 459–468. [Google Scholar] [CrossRef] [Green Version] - Colas, F.; Mahesh, S.; Liu, M.; Siegwart, R. 3D Path Planning and Execution for Search and Rescue Ground Robots. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Tokyo, Japan, 3–7 November 2013; pp. 722–727. [Google Scholar]
- Kantaros, Y.; Schlotfeldt, B.; Atanasov, N.; Pappas, G.J. Asymptotically Optimal Planning for Non-Myopic Multi-Robot Information Gathering. Proceedings of Robotics: Science and Systems (RSS), Freiburg, Germany, 22–26 June 2019. [Google Scholar] [CrossRef]
- Umari, H.; Mukhopadhyay, S. Autonomous robotic exploration based on multiple rapidly-exploring randomized trees. In Proceedings of the 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, 24–28 September 2017; pp. 1396–1402. [Google Scholar] [CrossRef]
- Pimentel, J.M.; Alvim, M.S.; Campos, M.F.M.; Macharet, D.G. Information-Driven Rapidly-Exploring Random Tree for Efficient Environment Exploration. J. Intell. Rob. Syst.
**2018**, 91, 313–331. [Google Scholar] [CrossRef] - Bircher, A.; Kamel, M.; Alexis, K.; Oleynikova, H.; Siegwart, R. Receding horizon path planning for 3D exploration and surface inspection. Auton. Robot.
**2018**, 42, 291–306. [Google Scholar] [CrossRef] - Vallvé, J.; Andrade-Cetto, J. Potential Information Fields for Mobile Robot Exploration. Robot. Auton. Syst.
**2015**, 69, 68–79. [Google Scholar] [CrossRef] [Green Version] - Wang, D.; Duan, Y.; Wang, J. Environment exploration and map building of mobile robot in unknown environment. Int. J. Simul. Proc. Model.
**2015**, 10, 241–252. [Google Scholar] [CrossRef] - Savkin, A.; Li, H. A safe area search and map building algorithm for a wheeled mobile robot in complex unknown cluttered environments. Robotica
**2017**, 36, 1–23. [Google Scholar] [CrossRef] - Karaman, S.; Frazzoli, E. Sampling-based algorithms for optimal motion planning. Int. J. Rob. Res.
**2011**, 30, 846–894. [Google Scholar] [CrossRef] - Pérez-Higueras, N.; Caballero, F.; Merino, L. Teaching Robot Navigation Behaviors to Optimal RRT Planners. Int. J. Soc. Robot.
**2018**, 10, 235–249. [Google Scholar] [CrossRef] - Koubaa, A. (Ed.) Robot Operating System (ROS): The Complete Reference (Volume 3). In Studies in Computational Intelligence; Springer: Berlin/Heidelberg, Germany, 2018; Volume 778. [Google Scholar] [CrossRef] [Green Version]
- Pomerleau, F.; Colas, F.; Siegwart, R.; Magnenat, S. Comparing ICP variants on real-world data sets. Auton. Robot.
**2013**, 34, 133–148. [Google Scholar] [CrossRef] - Pomerleau, F.; Magnenat, S.; Colas, F.; Liu, M.; Siegwart, R. Tracking a Depth Camera: Parameter Exploration for Fast ICP. In Proceedings of the 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, San Francisco, CA, USA, 25–30 September 2011; pp. 3824–3829. [Google Scholar]

**Figure 2.**Pointclouds examples. Left: local point cloud used as sampling space for the RRT${}^{*}$ planner in red color. Right: local point cloud employed by the RRT${}^{*}$ for traversability analysis (multicolor).

**Figure 5.**Example of wall-leaf rejection and frontier rejection. The tree leaves in red colour have been discarded as possible frontiers by the wall-leaf detection module. The leaves in yellow color have been accepted as frontiers by the no-frontier rejection module.

**Figure 6.**Scenarios for evaluation. Left: tunnel environment with obstacles for navigation (navigation map). Center and right: environments for evaluation of the exploration and navigation (exploration maps 1 and 2 respectively).

**Figure 7.**Comparison of pointcloud resolution for traversability versus size of RRT${}^{*}$ tree and path costs in $5\phantom{\rule{3.33333pt}{0ex}}secs$ of planning in the tunnel scenario.

**Figure 9.**Percentage of ground truth area covered by the proposed approaches. Left: map 1. Right: map 2.

**Figure 10.**Frontiers detected (yellow squares) according to different values of frontier threshold ${\Delta}_{f}$.

**Figure 11.**Adaptation of the planning radius and frontier threshold during the exploration of the maps 1 and 2 using the CFE approach.

**Figure 12.**Number of regions discarded for each exploration iteration of the maps 1 and 2 using the CFE approach.

Strict | Medium | Relaxed | |
---|---|---|---|

pitch | 0.7 | 0.87 | 1.3 |

roll | 0.7 | 0.87 | 1.3 |

roughness | 0.2 | 0.8 | 3.0 |

${\mathit{\omega}}_{\mathit{c}}=0.305\phantom{\rule{3.33333pt}{0ex}}[0,1]$ | ${\mathit{\omega}}_{\mathit{F}}=0.39\phantom{\rule{3.33333pt}{0ex}}[0,1]$ | ${\mathit{\omega}}_{\mathit{R}}=0.305\phantom{\rule{3.33333pt}{0ex}}[0,1]$ |

$min\_size=5.0\phantom{\rule{3.33333pt}{0ex}}m$ | $max\_size=13.0\phantom{\rule{3.33333pt}{0ex}}m$ | $size\_inc=2.0\phantom{\rule{3.33333pt}{0ex}}m$ |

${\Delta}_{f}=0.4\phantom{\rule{3.33333pt}{0ex}}[0,1]$ | $thresh\_inc=0.1\phantom{\rule{3.33333pt}{0ex}}[0,1]$ | $time\_inc=0.8\phantom{\rule{3.33333pt}{0ex}}seg$ |

$\kappa =0.35\phantom{\rule{3.33333pt}{0ex}}[0,1]$ | $\u03f5=0.85\phantom{\rule{3.33333pt}{0ex}}[0,1]$ | ${d}_{reg}=1.5\phantom{\rule{3.33333pt}{0ex}}m.$ |

Dist. (m) | CFE | NFE | BFE |
---|---|---|---|

Map 1 | $74.08\pm 5.84$ | $74.59\pm 11.80$ | $79.10\pm 12.84$ |

Map 2 | $87.63\pm 3.01$ | $86.30\pm 9.95$ | $99.98\pm 5.01$ |

CFE | NFE | BFE | ||
---|---|---|---|---|

${\Delta}_{f}=0.4$ | $Exp.(\%)$ | $97.00\pm 2.51$ | $94.76\pm 4.47$ | $90.41\pm 7.14$ |

$Dist.\left(m\right)$ | $74.08\pm 5.84$ | $74.59\pm 11.80$ | $79.10\pm 12.84$ | |

${\Delta}_{f}=0.6$ | $Exp.(\%)$ | $90.01\pm 6.47$ | $75.36\pm 3.55$ | $92.63\pm 1.23$ |

$Dist.\left(m\right)$ | $72.07\pm 2.89$ | $41.09\pm 2.33$ | $95.42\pm 7.90.$ |

© 2019 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Pérez-Higueras, N.; Jardón, A.; Rodríguez, Á.; Balaguer, C.
3D Exploration and Navigation with Optimal-RRT Planners for Ground Robots in Indoor Incidents. *Sensors* **2020**, *20*, 220.
https://doi.org/10.3390/s20010220

**AMA Style**

Pérez-Higueras N, Jardón A, Rodríguez Á, Balaguer C.
3D Exploration and Navigation with Optimal-RRT Planners for Ground Robots in Indoor Incidents. *Sensors*. 2020; 20(1):220.
https://doi.org/10.3390/s20010220

**Chicago/Turabian Style**

Pérez-Higueras, Noé, Alberto Jardón, Ángel Rodríguez, and Carlos Balaguer.
2020. "3D Exploration and Navigation with Optimal-RRT Planners for Ground Robots in Indoor Incidents" *Sensors* 20, no. 1: 220.
https://doi.org/10.3390/s20010220