1 Introduction

Navigating a mobile robot in a large factory amidst human–robot collaboration is difficult. The robot shall pass through narrow paths surrounded by workers and other obstacles. In such a large-scale dynamic environment, several considerations come into play. First, global path planning is required to calculate the shortest detouring path. Second, the 3-D shape of the robots, workers, and other obstacles shall be considered. Third, the global path shall be calculated in real time to reduce the waiting times of the mobile robot.

However, path planning algorithms that consider the 3-D shape of robots suffer efficiency issues. Some methods find a suitable path by sampling random robot poses, checking collision, and connecting the collision-free poses [1,2,3]. These take several seconds to calculate the path. Some sped-up methods, which are made faster by reducing the sampling number of the robot poses [4] or by sampling the poses using heuristic rules [5] and neural networks [6], still take about a second to calculate a path for a fixed manipulator in a small space. Even using parallel algorithms [7] to speed up, they take seconds to calculate results in a complex environment. In addition, some of those sped-up methods are more likely to be unable to find any feasible path. Hence, all random sampling methods and sped-up sampling methods are insufficient to adapt to mobile robots in a large and human–robot collaboration environment. Besides, the mapping of the 3-D environment [8] is also time-consuming.

The popular methods which contain global path for mobile robots are 2-D [9,10,11] or 2.5-D [12,13,14]. They down-grade the 3-D environment into 2-D or 2.5-D to reduce the computational cost to achieve real-time path planning. However, the downgrading of the 3-D environment results in losing information about the complexity of the shape of mobile robots and obstacles. In addition, some of them even lose the orientation information of the robots [13, 14]. Therefore, a mobile robot with a complex 3-D shape has difficulties in finding a feasible path to pass through narrow places. The down-grading of the environment/map in dynamical changing environment is also time-consuming [15]. They have to include a close-range local map [16] and a local path planner [17], or use a model for local navigation behavior [18] to avoid obstacles appearing suddenly in the nearby environment.

A real-time path planning method for fixed manipulators has been proposed which considers the 3-D shape of the fixed manipulator [19,20,21]. They use a prior generated probabilistic roadmap (PRM) which contains the 3-D shape information of the fixed manipulator. The PRM includes three elements: (1) nodes: key poses of robot (3-D position and 3-D orientation), (2) edges: the movements/paths between two nodes, and (3) swept volumes: 3-D discrete spaces covered by the movement of the robot for each edge. Before the real-time path planning, the 3-D swept volumes are calculated offline. In real-time path planning, a parallel version of the 3-D collision detection [19, 20] or prior calculated occupation lists [21], that detects whether obstacles (described by point cloud) are overlapped with swept volumes or not, is used to enable/disable edges of the PRM. Then, the shortest safe path can be calculated using the enabled edges of the PRM.

So far, real-time global path planning for a mobile robot with 3-D shape in a large-scale environment has not been achieved yet. To achieve that, in our research, we modify the method which is designed for fixed manipulators in a small environment to adapt to the mobile robots in a large-scale environment. First, we propose a dense template PRM for the holonomic mobile robot when the prior map is not given. Second, since the computational cost (usage of PC memory and computational time of shortest path) increases with the increasing number of nodes and edges of the PRM, it is impossible to generate a dense and large-scale PRM based on the template PRM. To solve this, we propose a hierarchical PRM. Different from other hierarchical PRMs [4, 21,22,23], our hierarchical PRM is built by three independent PRMs that are combined online before the real-time path planning. This reduces memory usage that would otherwise be required to save every sub-PRM for the whole large space. First, we generate a first-layer hub-PRM which is a low density but large-scale PRM. Then, before the real-time path planning, we combine the hub-PRM with two sub-PRMs, which are two prior generated template PRMs around the robot pose (start pose) and the goal pose. Finally, during real-time path planning, we first calculate a hub-path, and then two sub-paths are calculated to connect the robot pose to the hub-path and the hub-path to the goal pose. The single global path is the path of the three paths. We do not calculate the globally shortest path at once by combining three independent PRMs to a single PRM because the rewriting of the PRM to the memory takes time. Besides, comparing to the hub-PRM, the sub-PRMs are too small to have an obvious difference between the globally shortest path and three combined shortest paths.

We carry out a real-world experiment demonstrating a holonomic mobile robot using this method to navigate itself in real time to avoid unknown obstacles indoors. Also, to prove the capability in the large-scale environment, a simulation experiment is carried out and we compare the proposed method with a general 2-D path planning method.

The originality and main contributions of this research are:

  1. 1.

    We propose a real-time global path planning method for mobile robots with complex 3-D shapes in a large-scale environment by modifying a state-of-the-art 3-D path planning method which is for manipulators in a small space.

  2. 2.

    We propose a template PRM for the holonomic mobile robot when the prior map is not given.

  3. 3.

    We propose a hierarchical PRM which combines a hub-PRM and two sub-PRMs for large-scale environments.

2 Design of the PRM

2.1 Probabilistic roadmap

Fig. 1
figure 1

Three elements of the PRM. a The two nodes represents two robot poses (position and orientation). b The edge represents the movement/path between two nodes. c The swept volume represents the space that the robot passes through when moving through the path of the edge

Full size image

The probabilistic roadmap (PRM) contains three elements: nodes, edges, and swept volumes, as shown in Fig. 1. The nodes are sampled poses of the mobile robot in the environment (Fig. 1a). Two nodes are connected by an edge or edges (Fig. 1b). Each edge contains two elements: (1) a given path between two nodes, (2) a cost for the robot moving between those two nodes. The cost of the edges is the distance between the two robot poses of the nodes. The swept volumes (Fig. 1c) are generated by simulating the movement of the mobile robot to achieve the paths of the edges.

2.2 Template PRM for the holonomic robots

We propose a dense template PRM for holonomic robots when a prior map is not given. We presume the holonomic mobile robot moves in such an environment with flat ground without slopes.

First, we sample the nodes. A 2-D illustration of the sampled nodes is shown in Fig. 2. The positions are sampled in a discrete grid environment. The resolution (distance between two positions) is r which is a parameter given by the user. At each sampled position, we place eight nodes with different rotations (yaw angle = \(0^\circ\), \(45^\circ\), \(90^\circ\), \(135^\circ\), \(180^\circ\), \(225^\circ\), \(270^\circ\), or \(315^\circ\)). It means eight nodes have the same positions but have different directions.

Fig. 2
figure 2

A 2-D illustration of the sampling of the nodes of the template PRM. The positions of nodes are sampled as a grid. Eight nodes with different rotations have the same position

Full size image
Fig. 3
figure 3

Ten edges for each node of the template PRM. a The two edges of the rotation movement. The node can (1) turn left \(45^\circ\) or (2) turn right \(45^\circ\). b The eight edges (3)–(10) of the linear movement. The node in the center connects its eight nearby nodes that share the same orientation

Full size image

Second, we generate edges. We restrict the robot to only move from the current node to its neighboring nodes. Besides, the holonomic mobile robot has only two basic motions: (1) move linearly without rotation and (2) rotate \(45^\circ\) without linear movement. It means the holonomic mobile robot cannot move linearly and rotate at the same time. The purpose of these restrictions is to decrease the number of edges. The result of this is shown in Fig. 3, that each node connects to ten nodes: (1) turn left \(45^\circ\), (2) turn right \(45^\circ\), and (3)–(10) move to the eight neighboring nodes directly. Since the two nodes are close to each other, the paths of the edges are a straight line.

Figure 4 shows an example of the template PRM with a square field. O-XYZ is the inertia coordinate frame. After the generation of the edges that are blue lines in Fig. 4, the swept volume can then be generated using the edges and the CAD model of the mobile robot. It takes about 3 h to generate the swept volume of a PRM with 80000 nodes and 770432 edges using a computer (Dell Precision Tower) with Intel Xeon W-2155 CPU and 32GB memory.

Fig. 4
figure 4

An example of the template PRM. A 10[m]\(\times\)10[m] square with the resolution of 1[m]

Full size image

2.3 The hierarchical PRM

We propose a hierarchical PRM to reduce the computational cost for the large-scale environment. The proposed hierarchical PRM contains three PRMs: (1) A large-scale but sparse hub-PRM, (b) A dense front sub-PRM around the current robot pose, and (3) A dense rear sub-PRM around the goal pose. A 2-D illustration of the hierarchical PRM is shown in Fig. 5.

Fig. 5
figure 5

A 2-D illustration of the hierarchical PRM. a The red big dots and lines are the nodes and edges of the hub-PRM. The blue area is the prior known obstacles on the prior given map. The black area is the new observed obstacles. The blue dot is the current robot position. The green big dot is the goal position. b Around the big blue dot (robot position), the gray small dots and lines are nodes and edges of sub-PRM (color figure online)

Full size image

First, we explain the generation of the hub-PRM. If a prior map is not given, we use the template PRM as the hub-PRM, which is explained in Sect. 2.2. If the prior map is given, we increase the efficiency by removing unsafe nodes and edges of the template PRM. We sample the nodes in the same way as the template PRM. If the swept volume of the node has collisions with any obstacles, the node is removed. Then the edges are generated with the same method as the template PRM. Since the PRM is generated with knowledge of prior given obstacles, you can observe that the hub-PRM has overlapped areas with new obstacles in Fig. 5a. The overlapped edges will be removed in real-time by collision checking explained in the next section.

Then, we explain the generation of the two sub-PRMs. One sub-PRM is named the front sub-PRM, shown in Fig. 5b. The other is named the rear sub-PRM. Both of them are generated based on the template PRM. There are many grids in the hub-PRM since the hub-PRM is sampled as a grid. The front sub-PRM is a square grid which is located inside the grid of the hub-PRM in which the current robot pose is located. The corners of the square have the same positions as the nodes of the hub-PRM. Similarly, the rear sub-PRM is located inside the hub-PRM grid around the goal position.

3 Real-Time path planning

3.1 Algorithm

The algorithm for the proposed method consists of six steps as depicted in Fig. 6.

  1. I.

    Acquiring the point-cloud data: In the first step, the real-time point cloud information of the obstacles is obtained by LiDARs.

  2. II.

    Mapping the point cloud: The obtained real-time point cloud information is mapped using the Octomap technique [24].

  3. III.

    Collision checking: Any edges whose swept volume would contain a point from the mapped point cloud is removed from consideration. The collision checking can accept the real-time point cloud instead of the mapped point cloud.

  4. IV.

    Path planning: Using the collision-free edges of the PRM, the shortest path (the set of edges) from the current robot pose to the goal pose is calculated by A* searching algorithm.

  5. V.

    Path following: The mobile robot follows the path.

  6. VI.

    Path collision checking and replanning: While the mobile robot follows the path, the new mapped point cloud is obtained and a monitor module is checking whether the swept volume of the shortest path (the set of edges) overlaps with the new mapped point cloud or not. If a collision is found, the mobile robot stops and replans the path.

Fig. 6
figure 6

The algorithmic structure of the system: (I) obtain point cloud data (II) mapping of the point cloud, (III) collision checking, (IV) path planning, (V) path following control of the robot, and (VI) collision checking for the current path

Full size image

3.2 Real-time collision checking

Since the hub-PRM and two sub-PRMs have overlapped areas with new observed obstacles, we disable the edges whose corresponding paths pass through the new observed obstacles using the collision checking in real time as mentioned in the algorithm before.

The collision checking is to check whether any points of the point cloud are located inside the swept volumes or not. If any points are located inside the swept volumes of an edge, the edge is disabled and is not used in path planning. Since the collision checking takes time, we apply a parallel version of accelerated collision checking [19, 20]. The accelerator (additional hardware) is produced by Realtime Robotics, Inc. for robot manipulators.

3.3 Path planning

The generation of the global path is divided into four steps: (1) generation of the hub-path, (2) generation of the front sub-path, (3) generation of the rear sub-path, and (4) combination of the three paths. The generation of the three paths is shown in Fig. 7.

First, we enable/disable the edges of the three PRMs using the collision checking. Secondly, we find the nearest node to the current robot pose in the hub-PRM and mark it as the start node of the hub-path. Then we find the nearest node to the goal pose as the end node of the hub-path. Based on the hub-PRM, the shortest hub-path is calculated, shown in Fig. 7a. Then, the front sub-path, which is the shortest path from the current robot pose to the start node of the path found in the hub-PRM, is calculated. And the rear sub-path, which is the shortest path from the end node to the goal pose, is calculated, shown in Fig. 7b. Finally, the global path is generated by combining three paths.

If path planning fails, replanning is carried out by changing the start node and the end node to the next nearest nodes to the robot pose and the goal pose. If replanning fails after using all possible combinations of the four start nodes and the four end nodes that are nearest to the current robot pose and goal pose, we think no feasible path exists. Then, we stop the replanning procedure and stop the robot.

Fig. 7
figure 7

A 2-D illustration of the path. a The hub-path: the bold red line illustrates the hub-path, the red dots and lines are the nodes and edges of the hub-PRM, the blue dot is the position of the current robot pose, the green dot is the position of goal pose. b The sub-paths: the blue line is the front sub-path, the green line is the rear sub-path (color figure online)

Full size image

4 Experiments

4.1 Real-world experiment

The performance of the system is tested by asking a holonomic mobile robot with a 3-D complex shape to pass through the prior unknown field where the pipes with different heights are placed. The mobile robot will replan the path when new obstacles appear.

Fig. 8
figure 8

The configuration of the mobile robot with a complex shape. a Physical configuration of the mobile robot with a 0.7[m] beam. b The configuration of sensors

Full size image
Fig. 9
figure 9

The experiment field. a The field in the simulation. b The physical field of physical experiment. The setting of the physical field is the same as the simulation

Full size image

The configuration of the holonomic mobile robot is shown in Fig. 8. The size of the robot is length 0.7[m], width 0.5[m], and height 0.8[m]. The base of the mobile robot is SEED-mover. The accelerator (hardware) is installed inside the desktop PC (Dell Precision Tower with a CPU of Intel Xeon W-2155 and a 32Gb memory) in the middle of the mobile robot. A 0.6[m] high from the ground and 0.7[m] long beam is installed on the mobile robot to provide a complex shape. To obtain the 3-D point cloud information, a 3-D LiDAR Velodyne VLP-16 is mounted on the top of the mobile robot. And two 2-D LiDARs (Hokuyo 30lx) are placed on the bottom of the robot to detect the suddenly appearing obstacles at the foot of the robot in the blind spot of the VLP-16. For localization of the robot, the 2-D point cloud information from two 2-D LiDARs is also used for 2-D SLAM. To increase the accuracy of the 2-D SLAM, we apply the particle filter to fuse the visual odometry data using RealSense T265 and 2-D SLAM data using 2-D LiDARs.

Fig. 10
figure 10

The experiment result. The black arrows in a–c are the paths. The red arrows in d are the robot trajectory. The bottoms of the arrows are the positions of the robot. The directions of the arrows are the orientations. a The first path at t=0[s]. b The replanned path at t=3[s]. c The replanned path at t=4[s]. d The recorded trajectory of the mobile robot when the mobile robot reached the goal at t=12[s]

Full size image

The configuration of the experiment field is shown in Fig. 9. The size of the field is 5[m]\(\times\)7[m]. Several pipes of different heights are placed in the field. The height of the lower pipes is 0.5[m], which does not collide with the long beam of the mobile robot. The height of higher pipes is 1.0[m], which is possible to collide with the long beam. The distance between the pipes is 1.0[m]. Some walls have a height of 1.2[m] to block the sensor of the mobile robot. So the robot should sense the obstacles in real time and replan to avoid the suddenly appearing obstacles. Because the experiment field is small, to increase the total driving length, we place the start point and end point off-centered and build a field that the robot moves as an ‘S’ shape.

Considering the limited room space of the experiment field, the PRM is a scaled-down dense PRM that keeps a large number of nodes for a large-scale environment. The hub-PRM is a 10[m]\(\times\)10[m] template PRM with a resolution \(r=0.1\)[m]. The two sub-PRMs are 0.1[m]\(\times\)0.1[m] template PRM with a resolution \(r=0.001\)[m]. All three PRMs have 80000 nodes and 770432 edges. Such a dense configuration of nodes and edges is equivalent to the configuration of a large-scale environment that the hub-PRM is 1000[m]\(\times\)1000[m] with a resolution \(r=10\)[m] and the sub-PRM is 10[m]\(\times\)10[m] with a resolution \(r=0.1\)[m]. We think the resolution \(r=0.1\)[m] is dense enough for our robot.

4.2 Real-world experiment results

The robot took 12[s] to move from the start pose to the goal pose without touching any obstacles. The robot moved smoothly without any obvious stopping. The calculated global paths of the mobile robot are shown in Fig. 10a–c. The robot calculated the path three times at t=0[s], t=3[s], and t=4[s] when the new observed obstacle appeared on the current path. The recorded trajectory of the mobile robot is shown in Fig. 10d.

Then we report the computational time of the paths. The first global path took 115[ms] (hub-path: 37[ms], front sub-path 43[ms], and rear sub-path 35[ms]). The second global path took 112[ms] (hub-path: 41[ms], front sub-path 33[ms], and rear sub-path 38[ms]). The third global path took 122[ms] (hub-path: 54[ms], front sub-path 31[ms], and rear sub-path 37[ms]).

4.3 Simulation experiment

A simulation experiment is carried out for comparison between the proposed method and a general 2-D path planning method in a large-scale environment. In the simulation, the robot should avoid two moving obstacles. One is a global obstacle that opens or closes the two possible passes which are far away from each other. The other is a local obstacle that moves fast so that the robot should recalculate the path in real-time to avoid it.

Fig. 11
figure 11

The simulation experiment field

Full size image

The field of the simulation is shown in Fig. 11. The Gazebo simulator is used to build the field. The field is a 100[m]\(\times\)100[m] square surrounded by walls. In the middle of the field, a wall with two gates (the width of each gate is 2[m]) traverses the field into two parts. The robot stops and the robot is asked to calculate the path which navigates the robot moving from one part of the field to the other. A door (global obstacle) moves left and right to open or close each gate with a moving speed 3[m/s]. So the door forces the robot to recalculate the global path repeatedly to pass through different gates. Another small moving obstacle whose width is 1[m] is placed in front of the robot as the local obstacle. It moves left and right repeatedly with the moving speed 5[m/s]. We assume that the robot has a very precise sensor and obtains point cloud information with a 0.025[m] resolution inside of a visible region of the robot in the simulation environment. Comparing the size of the robot (length 0.7[m], width 0.5[m], height 0.8[m]) and the resolution of the PRM of the proposed method and the costmap of the compared method ’move base’ (0.1[m]) to the sensor resolution, the resolution of 0.025[m] is precise enough for our experiment.

The 2-D path planning method for comparison is ’move base’ [25] which is the most common method for ROS (Robot Operation System). It is a method using both global path planning and local path planning. First, the ’move base’ generates two 2-D costmap (global costmap and local costmap) [25]. The costmap is a 2-D map of the occupancy grid. The occupied grid is generated by drawing circles using the inflation radius around mapped obstacles. The inflation radius is obtained by projecting the 3-D shape of the robot into 2-D. Second, the ’move base’ calculates the global path using the global costmap from the robot pose to the goal pose. And then the local path is calculated using the local costmap from the robot pose to the global path. A*-based planner is used for the global path. TEB local planner [26] is used for the local path.

About the robot, different from the real-world experiment, the robot in the simulation experiment does not equip the additional long beam (The robot CG without additional beam is illustrated in Fig. 1a). The reason is that the global path planning of the 2-D method is difficult to apply to such a complex 3-D robot shape and environment. The compared method ‘move base’ can be only applied to a 2-D shape of the mobile robot and a 2-D environment. If we use ‘move base’ for the complex shape of the robot (the mobile robot with a long beam), the projected 2-D region that the robot should be included will be too big. And the width of the gates in the simulation would be unrealistically big. To apply the same robot to both proposed and compared methods, we have to remove the long beam and do not consider the 3-D shape of the robot for a fair comparison.

The setting of the proposed method is that the hub-PRM is 100[m]\(\times\)100[m] with a resolution of 1[m], the sub-PRM is 1[m]\(\times\)1[m] with a resolution of 0.01[m]. The setting of the compared method is that the global costmap is 100[m]\(\times\)100[m] with a resolution of 0.1[m], the local costmap is 10[m]\(\times\)10[m] with a resolution of 0.1[m]. The resolution of the proposed method and the compared method is hard to be set as the same value. We select the resolution 0.1[m] for the costmap because the compared method has good performance at that level of granularity. If the resolution of the costmap is 0.01[m], the same resolution as the proposed method’s sub-PRM, it is impossible to update the costmap in seconds. If the resolution of the costmap is 1[m], the same resolution as the hub-PRM, the 2-D shape of the robot will be too rough to pass the gate (2[m] width).

4.4 Simulation experiment results

Fig. 12
figure 12

The calculated paths of simulation experiment at a moment

Full size image

The paths at a moment are shown in Fig. 12. The black line is the path of the proposed method. The blue line is the global path of the compared method. The short red line is the local path of the compared method. It can be observed that the blue line is passing through the black global obstacle. It means the compared method could not calculate the global path in real-time to avoid the global obstacle. We can find that the proposed method avoided two obstacles with a single global path. On the other hand, the compared method avoided the local obstacle in real-time by the local path. But the global path of the compared method did not avoid the global obstacle in real time. We record the computational time of global paths when a gate closes and the monitor of the proposed method detects the collision of the current hub-path. The proposed method took about 0.15[s] to calculate/return the global path. On the other hand, the computational time of the global path of the compared method is from 0.7[s] to 3[s]. In conclusion, the compared method returned the global path slower than the proposed method. The reason is that the compared method spends time to generate the 2-D costmap. When the left gate opened and the right gate closed, the compared method needed time to remove the occupied grid of the opened gate. Before the occupied grid is cleared out, the global planner cannot find any feasible path. If the global planner of the compared method cannot find any feasible path, the local planner also cannot calculate the local path. For safety, the robot shall stop when the local path cannot be calculated. So the mobile robot of the proposed method has less waiting time.

5 Conclusion

This paper presents a method of real-time path planning that considers the 3-D shape of the mobile robot and the environment in a large-scale environment. A template PRM is proposed for a holonomic mobile robot when the prior map is not given. And a hierarchical PRM, which combines a hub-PRM and two sub-PRMs, is proposed for a large-scale environment. We applied an accelerator for checking the collision and calculating the shortest path on the hierarchical PRM. A real-world experiment is carried out in a scaled-down environment using a physical holonomic mobile robot. The experiment result shows that the proposed method calculates a feasible path for a mobile robot with a 3-D complex shape in a complex environment. And the computational time of the path is about 100[ms] which means it is possible to avoid dynamic obstacles in real time. In a simulation experiment, a comparison experiment shows that the proposed method returns the global path faster than a general 2-D path planning method. About the future works, we are planning to apply the proposed method to a mobile manipulator which has higher DoF and a more complex shape.