Collision‐free local planner for unknown subterranean navigation

ETRI JournalVolume 43, Issue 4 p. 580-593 SPECIAL ISSUEFree Access Collision-free local planner for unknown subterranean navigation Sunggoo Jung, Sunggoo Jung orcid.org/0000-0001-7672-6885 Autonomous Flight Research Section, Electronics and Telecommunications Research Institute, Daejeon, Rep. of KoreaSearch for more papers by this authorHanseob Lee, Corresponding Author Hanseob Lee hslee89@kaist.ac.kr Unmanned Systems Research Group, KAIST, Daejeon, Rep. of Korea Correspondence Hanseob Lee, Unmanned Systems Research Group, KAIST, Daejeon, Rep. of Korea. Email: hslee89@kaist.ac.krSearch for more papers by this authorDavid Hyunchul Shim, David Hyunchul Shim Unmanned Systems Research Group, KAIST, Daejeon, Rep. of KoreaSearch for more papers by this authorAli-akbar Agha-mohammadi, Ali-akbar Agha-mohammadi JPL Robotics, NASA-JPL, California Institute of Technology, Pasadena, California, USASearch for more papers by this author Sunggoo Jung, Sunggoo Jung orcid.org/0000-0001-7672-6885 Autonomous Flight Research Section, Electronics and Telecommunications Research Institute, Daejeon, Rep. of KoreaSearch for more papers by this authorHanseob Lee, Corresponding Author Hanseob Lee hslee89@kaist.ac.kr Unmanned Systems Research Group, KAIST, Daejeon, Rep. of Korea Correspondence Hanseob Lee, Unmanned Systems Research Group, KAIST, Daejeon, Rep. of Korea. Email: hslee89@kaist.ac.krSearch for more papers by this authorDavid Hyunchul Shim, David Hyunchul Shim Unmanned Systems Research Group, KAIST, Daejeon, Rep. of KoreaSearch for more papers by this authorAli-akbar Agha-mohammadi, Ali-akbar Agha-mohammadi JPL Robotics, NASA-JPL, California Institute of Technology, Pasadena, California, USASearch for more papers by this author First published: 24 September 2021 https://doi.org/10.4218/etrij.2021-0087 Funding information: Ministry of Science and ICT, South Korea, Grant/Award Number: 2017-0-00067 AboutSectionsPDF ToolsRequest permissionExport citationAdd to favoritesTrack citation ShareShare Give accessShare full text accessShare full-text accessPlease review our Terms and Conditions of Use and check box below to share full-text version of article.I have read and accept the Wiley Online Library Terms and Conditions of UseShareable LinkUse the link below to share a full-text version of this article with your friends and colleagues. Learn more.Copy URL Share a linkShare onFacebookTwitterLinked InRedditWechat Abstract When operating in confined spaces or near obstacles, collision-free path planning is an essential requirement for autonomous exploration in unknown environments. This study presents an autonomous exploration technique using a carefully designed collision-free local planner. Using LiDAR range measurements, a local end-point selection method is designed, and the path is generated from the current position to the selected end-point. The generated path showed the consistent collision-free path in real-time by adopting the Euclidean signed distance field-based grid-search method. The results consistently demonstrated the safety and reliability of the proposed path-planning method. Real-world experiments are conducted in three different mines, demonstrating successful autonomous exploration flights in environment with various structural conditions. The results showed the high capability of the proposed flight autonomy framework for lightweight aerial robot systems. In addition, our drone performed an autonomous mission in the tunnel circuit competition (Phase 1) of the DARPA Subterranean Challenge. 1 INTRODUCTION Autonomous exploration in an unknown cluttered environment poses a set of challenging problems in many levels [1, 2]. Unavailability of GPS signals and limited lighting conditions render the perception problem extremely difficult [3]. Confined spaces between walls and low height of priory unknown environment require compact drones which cannot rely on high-performance sensor-suites [4]. These constraints require careful attention in path-planning due to the existing state estimation errors. The path with hard constraints generates energy optimal shortest path to the goal. However, it may bring about too close path to walls or obstacles, making quadrotor flying unreliable when there are control or perception errors. Besides, it requires a large amount of computation as the environmental information grows. Thus, real-time behavior decisions of the drones are not easy to realize in large-area exploration applications. To solve these limitations, this study 11 This paper describes one of the large systems of the COSTAR teamMore details are provided in [5]. proposes a three-step path generation procedure to make the generated path strongly remain in the middle of the obstacles or walls to safely explore the unknown confined areas. A grid or graph search method requires the destination. For unknown environment exploration, there are no goals or global plans. Thus, we use a frontier-based approach [6] and design a novel method that picks a receding-horizon local goal (we call it "end-point") using occupancy grid information. A high-level collision-free path is developed based on grid-search algorithm [7]. To make the path more reliable to fly, we generate the nearest occupied voxel information represented by the euclidean signed distance field (ESDF) grid map and make the novel cost function of the grid-search method which penalizes the adjacent obstacle cost from ESDF information. Finally, we introduce a method to reduce the computational burden caused by the expansion of online maps as the exploration progresses. We employ the local-map approach which has 5 m × 5 m × 5 m fixed map size to overcome this problem. Figure 1 is an example snapshot of our method during the real-world experiment. The ESDF and local replanning paths are generated within this local-map area to reduce the computational cost and achieve real-time performance. These aspects confer fully autonomous and robust exploration capability to real-world robots. This study makes the following contributions: A collision-free local planning framework is designed for small-sized (330 mm in size) aerial robots with limited computation power and low payloads. An end-point selection method that rapidly generates collision-free local-goals in local ESDF maps is developed. The algorithm performance is compared to those of the state-of-the-art local planning methods. Extensive simulation and real-world tests validate the robustness of the framework. The code is publicly released to the community. It can run on any type of (for example, ground, aerial or underwater) robot for autonomous exploration applications 22 Codes are available at our project github page: https://github.com/SunggooJung/local_planner_ROS . FIGURE 1Open in figure viewerPowerPoint Snapshot of the autonomous exploration experiment of the lightweight drone in the coal mine environment. Location: Beckley mine,West Virginia, United States The remainder of the paper is organized as follows. In Section 2, we present an overview of related works. Section 3 describes the hardware and software of the proposed framework. In Section 4, we present the details of the proposed approach. The simulations and practical implementation results are illustrated and evaluated in Section 5. Section 6 presents the conclusions and discussions. 2 RELATED WORK Considerable amount of methods have been investigated focusing on the problem of robotic exploration [6, 8]. The path planning uses mixed integer linear programming (MILP) [9, 10]. However, MILP is an NP-hard; thus, it can only be used in a finite-size planning problem. Model predictive control (MPC)-based receding-horizon navigation methods [11, 12] and urban environment exploration conceptual works [13, 14] were proposed; however, they are limited by the finite area range. Early results in unknown area exploration planner were proposed by Bircher et al. [15]. This study showed the capability of priory unknown area exploration while avoiding collisions. Kino-dynamic real-time planning using quadrotor has been presented in Allen and Pavone [16]. This study showed full-stack planning architecture; however, the state estimation is rely on the VICON motion capture system. Front-end path planning and back-end path smoothing works have been proposed in Gao et al. [17] and showed impressive results in a small range of indoor and outdoor areas. The MPC-based online path-planning work showed dynamic replanning by considering multi-object concept [18]; however, the study finished within simulation without real-world validation. One-line segment combination path smoothing approach is presented in Lai et al. [19]. This study generated the path using a two-point boundary value problem approach to reduce the computational cost. This study demonstrated reliable flight results; however, the piece-wise segment of path joint optimization is not considered. Interesting works are presented by Popovic et al. [20]. They developed an online path-planning algorithm to reduce energy costs while detecting weeds. They showed impressively reducing the energy cost by effective path-planning algorithm, however only validated in simulation. Recently, the large-area exploration problem has attracted significant attention because of DARPA subterranean challenge [21]. Sampling-base method [15], graph-based approaches, [22, 23] and learning-based approaches have been proposed [24]. The above systems require high size, weight and power (SWAP) systems making the robot relatively heavy and large. Thus, the development of a compact-sized exploration framework is an essential challenge to explore confined and cluttered spaces. Some solutions to the above have been presented such as resilient micro flyer design [25] or range based sensor array approach [26]. These demonstrate simply penetrating a small-sized tunnel or structured area, which is impractical for large-area exploration. Motivated by these studies, we discovered that the study for "lightweight autonomy" in confined and cluttered areas has not been properly investigated. This contribution aims to provide a meaningful low SWAP system alternative by combining both algorithms, the local end-point selection and the ESDF-assisted collision-free path, with fully on-board computing. 3 SYSTEM OVERVIEW In this study, which focuses on the collision-free navigation, a "350 mm in size" frame is designed. The designed concept of the system is lightweight but high performance with an acceptable flight time. The platform has four DJI Snail racing drone propulsion systems, which can lift all the sensor systems required for autonomous exploration in 12 min. We chose the PixRacer R15 autopilot with PX4 Stack for low-level flight control and NVIDIA Jetson TX2 GPU for high-level mission management. The TX2 module is mounted on a third-party carrier board (Auvidea, J120 [27]), which has RS-232 serial ports for communication with the PX4 flight controller. With the constraint of the payload limit, the drone is equipped with a two-dimensional (2D) laser scanner (Hokuyo, UST-20LX) instead of three-dimensional (3D) LiDAR, such as the popular Velodyne VLP-16. It is worth noting that in the texture-less environment, the localization using only the laser scanner can be ineffective since the features on the walls are monotonous. Thus we integrate an upward-facing Qualcomm Snapdragon Flight (QSF), which delivers on-board visual-inertial-odometry and its fisheye camera data for localization robustness. For altitude measurement, time-of-flight based 1D LiDAR is equipped (Terabee, TeraRanger-Evo). To aid and offer robust perception to vision source in the dark environment, we integrate an upward-facing high-power LED (Lume, Cube 2.0, housing case uncovered). The above sensor data are run on the NVIDIA Jetson TX2 (Ubuntu 18.04 variant and ROS Melodic). The total weight of the system, including all the sensing and processing components, and the battery is 1.43 kg. 4 PROPOSED APPROACH The proposed lightweight flight autonomy method consists of three collaborative modules: ESDF-assisted path planning, local-map generation, and end-point selection. The structure of this collaborative module setup is shown in Figure 2. First, the proposed method generates occupancy representation [28] and calculates ESDF, which allows collision-free paths. The planning module exploits the ESDF cost using the grid-search method, offering obstacle-aware safety paths. Then, to reduce the computational cost and ensure real-time performance for computing cost-constrained lightweight drones, every iteration of the path-planning generates the trajectory within 5 m × 5 m × 5 m local-map around the current robot location. Finally, the proposed method introduces the end-point selection to make the planner generates trajectory within the local-map. As the exploration progresses, the occupancy information around the robot location changes. If the end-point, the destination of the path planner, is at the obstacle, the correct path cannot be created. Thus, the occupancy data and pointcloud sensor information is used to select the obstacle-free end-point. This section discusses these methods with respect to the lightweight flight autonomy concept. FIGURE 2Open in figure viewerPowerPoint Diagram of lightweight autonomy framework. An ESDF map is developed using the mapped knowledge. ESDF-assisted planner determines good exploration paths starting at the current location to the end-point 4.1 Algorithm description Algorithm 1 outlines path-planning steps of the proposed framework. Every iteration starts by acquiring an occupancy probability map (Mprob(·)) from the simultaneous localization and mapping (SLAM) module. From the certain threshold condition (Tζ), Mprob(·) is converted to Boolean-valued voxel grid, marking flyable area by value of one (true) and fill the rest area with zero (false) (Mbool(·)). Mbool is used to generate an ESDF map representing distance information from the adjacent obstacle. The local end-point selection module receives current state ( ), LiDAR sensor data (σk). This module find best end-point from the end-point candidate Sk. Then, the passes a low-pass filter to prevent oscillation of the selected point. Finally, the planner receives the current state, selected end-point and ESDF info to generate a collision-free trajectory. It uses cost modified ESDF-assisted grid-search. The key functionalities are explained in detail in the following subsections. 4.2 Local map approach The computational complexity of the grid or graph search-based planner depends on the map resolution and size. In the fixed-horizon path-planning problem, the user usually sets the admittable map size prior to ensuring the planner performance. The unknown area exploration problem, the area to search, is not known. Thus, as the exploration goes, the computational complexity grows in a cube of grid size (O(n3), n = grid size), which is impractical for a low-cost embedded on-board processor. The proposed local-map approach is designed to reduce the computational cost by performing local planning in local-map area [sL, sW, sH] and resolution r. Thus, the time complexity remains fixed O(sLsWsH/r3) since the local-map maintains a constant size regardless of the exploration progress. The complexity only depends on the volume of the local-map when the map resolution is fixed. The local-map is updated at every 0.1 s (10 Hz); thus, this method can assure the vehicle velocity of about 50 m/s. 4.3 End-point selection As discussed above, when exploring areas fully unknown a priori, to use search-based planning method, the destination must be defined in advance. Therefore, the "end-point," the destination in local-map area must be determined based on the obstacle information from local-map window. To resolve this problem, we implement the end-point selection method using the range measurements for search-based planning in the unknown area. The range sensor gives the index and distance of the point denoted as [k, σk]. To select the end-point, we sample evenly 48 points out of 1440 point clouds from the laser scanner range sensor. The samples are at 30 intervals starting from the 1st index of the points among a total of 1440 raw points of laser scanner inputs (see Figure 3). This constitutes a set of 48 sample points surrounding the vehicle at 240 degrees (the sample field of view Ω). Next, we collect a set of points exceeding a certain distance l. Here, "l" is the design factor, meaning the planning range of the local planner. For instance, if l is set to 5 m, the local planner plans the path 5 m from current vehicle pose ξ. Then, the method determines the maximum index of this set and converts the pose of its points into a body frame coordinate system. Finally, the point is selected as the end-point (2). (1) (2) FIGURE 3Open in figure viewerPowerPoint Example illustration of the end-point selection using Equations (1) and (2) where the sampled points and total number of samples N. Figure 3 shows the end-point selection example in three different path shapes, such as straight, right open space, and four-way junction. The selected S∗ is defined as the end-point of the grid-search path. Finally, a low-pass filter is implemented to prevent end-point oscillation due to the noise of raw laser scanner input. 4.4 ESDF weighted cost function The online map from the Cartographer [28] gives the map of the environment in a regular probability grid form. With this probability, we generate a Boolean-valued voxel representation which characterizes the flyable area, defined as (3)where is the occupancy probability function of the local-map at grid point , Mbool(·) is the Boolean-value representation, and Tζ is the constant probability threshold value (which is set for 70 (range: 0 to 100) in this work). Mbool(·) is then converted to distance field using Euclidean signed distance transform [29]. For planning, we implement a grid-search method to determine the shortest path from the vehicle's current position to the selected end-point. We model the cost function using the following two terms: an ESDF term fdist, which measures the cost of being near obstacles; and a prior term fscore, which measures the distance of the path between the current position and end-point. Therefore, we present the objective as follows: (4) (5)where g(n) is the cost from start to node n, h(n) is the cost from n to goal, η(n) is the depth of the search and N is the estimated length of the path to reach the goal [30]. 4.5 Localization and mapping Underlying mapping representation of this study takes Cartographer by Google [28], which showed an outstanding performance using continuous-time SLAM by Ceres-based scan matcher and optimizer [31] and grouped probability grids called submaps. The Cartographer receives an odometry source as a secondary input to correct position drift in a featureless environment. Therefore, we employed a vision-based state estimation source from QSF as an additional measurement source. Figure 4 shows the mapping result of LiDAR-only SLAM and SLAM with a visual correction at the Beckley mine in West Virginia. Since we fused vision-based state estimation from QSF boards, which are highly integrated single-board computers, time synchronization was performed using Chrony [32] to cooperate this board with the mission computer. FIGURE 4Open in figure viewerPowerPoint Experimental comparison of localization with a visual correction (top) and LiDAR-only localization (bottom). LiDAR-only SLAM shows a mapping drift of 0.14/m due to lack of features in this underground mine environment. Location: Beckley Mine in West Virginia, United States 5 EVALUATION STUDIES To evaluate the lightweight autonomy framework, a series of simulations and experimental studies were conducted. Within the experimental field results, experiments in three different underground mine environments were performed to validate the robustness and scalability of the system. The experimental environments were a coal mine in South Korea and gold and inert coal mines in the United States. 5.1 Simulation results To evaluate and compare the proposed lightweight flight autonomy with other state-of-the-art methods, a simulation study was conducted before its real-world verification. Simulations can perform large-scale iterative tests without any cost loss. This allows us to test the stability and capability of the newly applied algorithms before conducting hardware experiments. Three simulation configurations consisting of an office, forest, and subterranean environments were utilized (see Figure 5). The office environment consisted of narrow hallway and multiple rooms. The forest environment consisted of cluttered trees and unstructured models. Finally, the subterranean environment consisted of multi-way intersections and narrow corridors with lengths of several kilometers. Examples were performed using a high-fidelity Gazebo simulator with a P x 4 flight stack [33] for flight performance evaluation. FIGURE 5Open in figure viewerPowerPoint Various simulation environments setup with custom build 350 mm lightweight drone to evaluate and fine-tune the proposed method The custom robot is developed for simulation studies assuming a quadrotor 350 mm MAV model with a laser scanner, a 2D rotating scanner up to 20 m. The local planning window is set to 5 m × 5 m × 5 m around the current vehicle pose, and the flight speed was set to 2 m/s. In all simulated tests the performance comparison is conducted with receding horizon rapidly-exploring random tree∗ (RHRRT∗) [34] and likelihood-based planning [35]. Simulation results presenting the performance of the (a) proposed planner, (b) Falco, and (c) RHRRT∗ planner are shown in Figures 6, 7, and 8. In these simulation tests, all methods use the same vehicle configuration, except for the range measurement sensor. Our planner and RHRRT∗ used a 2D laser scanner-based 2.5D mapping while Falco used the LOAM [36] based 3D mapping method due to the basic framework setting difference. Our method and Falco showed the best performance. The proposed lightweight framework successfully explored the three simulation environments. Falco also delivered reasonable performance. However, it showed staying or back-and-forth motions because it cannot define the next step that maximizes the likelihood of the drone flying to the goal. Falco further requires destination points. Because it cannot define the destination in advance (at least in the current Falco open-source package), Falco is inadequate for unknown area exploration problems. The RHRRT∗ method was excluded from the forest and subterranean simulation test because it consumes much time to generate the path. Thus, it mostly fails to obtain a meaningful result. FIGURE 6Open in figure viewerPowerPoint Compared confined office area simulation results of flight autonomy frameworks in the local planning viewpoint. The proposed lightweight flight autonomy and Falco show good performance, but RHRRT∗ cannot determine the proper path. The proposed lightweight framework, as shown in (A), successfully finds the safe path. The snapshots of the local-map and ESDF map at each position are presented. The solutions from Falco exhibit overall good performance; however, it presents wavering or staying motions for a few seconds due to trapping in local minima, as shown in (B). The RHRRT∗ could not determine the path within a reasonable amount of time because it spans the trees over the entire map FIGURE 7Open in figure viewerPowerPoint Compared forest area simulation results of flight autonomy frameworks in the local planning viewpoints. The proposed lightweight flight autonomy and Falco show good performance; however, the Falco shows wavering or staying motion in a narrow passage FIGURE 8Open in figure viewerPowerPoint Compared underground mine (subterranean) area simulation results of flight autonomy frameworks in the local planning point of view. Our planner exhibits best performance while flying over 864 m in 443 s as shown in (A). Falco shows reasonable performance; however, it gets trapped in local minima and cannot proceed further as shown in (B) Comparative analysis from simulation studies showed that the proposed lightweight autonomy framework has appropriate capability in various environments (see Table 1). In three scenarios, the exploration time of each method was tested five times in the same initial configuration. In all tests, the maximum flight speed was set to 2 m/s. Falco and the proposed planner successfully reached the destination in office and forest tests. On average, the proposed planner performs more than 10 s faster, as Falco showed a time lag while scrambling in a narrow passage. For underground tests, Falco was unable to escape from the forked road where one road was blocked. This is considered an inevitable problem because Falco proceeds by changing only the heading while looking at only the small local area. An extensive set of field experiments is presented in the next subsection to further verify the proposed framework in a real-world environment. TABLE 1. Performance comparison in each scenarios Scene Method Exploration time (s) Explored area (s) Avg Std Max Min Avg Office Falco [35] 67.17 6.00 72.05 58.36 2.08 RHRRT∗ - - - - - Proposed 40.89 1.37 42.48 39.17 3.42 Forest Falco [35] 59.83 7.74 69.29 49.89 2.50 Proposed 42.19 1.49 43.78 40.11 3.55 Subt Falco [35] - - - - - Proposed 226.00 1.13 227.43 224.56 3.48 5.2 Experiments In this section, we evaluate the performance of the proposed collision-free navigation strategy in various real-world subterranean environments. The eagle mine in the United States, California, has a long narrow straight passage. The Hawsoon mine in South Korea has a completely dark environment and round-shape structure. The Beckley mine in the United States, West Virginia, has a square-shaped structure with several pillars inside the mine. These three mines appear similar but have different characteristics. The eagle mine is a gold mine with a large amount of sand dust. Being a gold mine, eagle mine has many wall features o that are advantageous for localization but require sophisticated control through the narrow passage. Hwasoon mine is an active coal mine containing more coal dust than sand dust, but wider passageways than eagle mine. Finally, Beckley mine is an inert coal mine. The effects of dust are small because water is falling from the ceiling. This mine has a wider passageway but lower height than the other two mines, so exerts a greater ground effect during flight. All flight experiments were performed in fully autonomous mode in several test sites, which were unknown in advance. The overall system architecture is illustrated in Figure 9. FIGURE 9Open in figure viewerPowerPoint Software architecture overview of our lightweight autonomy system from sensor input to control command output 5.2.1 Test in the eagle mine In this example, the drone explores a long narrow wall shaped gold mine, located in Southern California. Here, we evaluate the proposed algorithm integration performance. The end-point selection, local-map generation, ESDF in local-map area, and the grid-search algorithm. As presented in Figure 10, the proposed lightweight autonomy framework is successfully integrated. The drone starts at the entrance of the mine and explores about 105 m in 62 s while passing a four-way junction. All environmental information is unknown before its flight test. The characteristics of this test site are presented in Table 2. FIGURE 10Open in figure viewerPowerPoint (A) Result of the first integrated experiment at the narrow gold mine environment. (B) Local-map of straight path, (C) ESDF map and consequently local-path planning results at four-way junction. This result shows the successful algorithm integration of: (1) sensing; (2) local map generation; (3) ESDF generation; (4) planning. Location: eagle mine, United States, Southern California TABLE 2. Characteristic of eagle mine Parameter Value Parameter Value Width ≤1.2 m Name Eagle mine Height ≤1.0 m Location Southern California Tunnel shape ∩ shape Characteristic Dusty, Narrow 5.2.2 Test in the Hwasoon mine In this example, we present a coal mine exploration with a two-way junction and smooth curves terrain. Unlike the square-shaped narrow long-wall gold mine example presented in the previous section, the coal mine in South Korea is round-shaped and relatively wide. The coal mine in South Korea has an uneven and bumpy wall texture because the miners widen the mine with a shovel. Besides, wooden supports are supported every 1 m to prevent the mine from collapsing. Therefore, it is useful for scan-matching with sufficient features on the wall, but it is not easy to use for vision sources because of the dust on the floor. In this experiment, the quadrotor successfully explores 182 m for 142 s along a curved tunnel using the lightweight autonomy framework. Figure 11 shows all the integrated modular framework. More specifically, at the two-way junction, the end-point selection method found that the right forked path was blocked and chose the left path. This test allowed us to evaluate long-range flight capabilities with dead-end intersections. The specification of this test mine is presented in Table 3. FIGURE 11Open in figure viewerPowerPoint Autonomous exploration results and visualization of the proposed collision-free navigation modules at the Hwasoon coal