Abstract
As the basic system of the rescue robot, the SLAM system largely determines whether the rescue robot can complete the rescue mission. Although the current 2D Lidar-based SLAM algorithm, including its application in indoor rescue environment, has achieved much success, the evaluation of SLAM algorithms combined with path planning for indoor rescue has rarely been studied. This paper studies mapping and path planning for mobile robots in an indoor rescue environment. Combined with path planning algorithm, this paper analyzes the applicability of three SLAM algorithms (GMapping algorithm, Hector-SLAM algorithm, and Cartographer algorithm) in indoor rescue environment. Real-time path planning is studied to test the mapping results. To balance path optimality and obstacle avoidance, algorithm is used for global path planning, and DWA algorithm is adopted for local path planning. Experimental results validate the SLAM and path planning algorithms in simulated, emulated, and competition rescue environments, respectively. Finally, the results of this paper may facilitate researchers quickly and clearly selecting appropriate algorithms to build SLAM systems according to their own demands.
1. Introduction
Mobile robots are capable of moving around in their environment and carrying out intelligent activities autonomously, thus having extensive realistic applications, including rescue works. A key enabling technology is simultaneous localization and mapping (SLAM) which allows the robot to estimate its own position using onboard sensors and construct a map of the environment at the same time. With the SLAM technology, real-time path planning can be performed to fulfill complex manoeuvring tasks in rescue works.
SLAM-enabled mobile robots have achieved much success in various scenarios. Peng et al. [1] studied the positioning problem and implementation of SLAM for mobile robots with RGB-D cameras. Shou et al. [2] employed a Raspberry Pi module as the core controller and built a mobile robot for map construction and navigation in indoor environment. Zhang et al. [3] proposed path prediction planning based on the artificial potential field to improve obstacle avoidance. Liu et al. [4] combined the Q-learning algorithm with the deep learning algorithm for path planning, which enabled robots to make reasonable walking paths under complex environmental conditions. Yu et al. [5] applied an improved path planning algorithm to unmanned underwater survey ships, enabling quick obstacle avoidance and return to the preset route. However, these studies did not take into account the impact of the rescue environment on the SLAM algorithm. If these algorithms are directly applied to rescue robots, it may deteriorate the accuracy of path planning and even cause incorrect path planning results. At present, there are still rare systems that can combine SLAM and path planning for indoor rescue. Therefore, it is necessary to study the impact of the rescue environment on the SLAM algorithm and path planning algorithm and evaluate and select the SLAM algorithm suitable for the rescue environment.
In this paper, we evaluated the results of some commonly used SLAM algorithms in both simulation and real-world environment, tested the path planning algorithms ( algorithm and DWA algorithm), and conducted a combined experiment of mapping and path planning regarding the RoboCup competition. These experiments revealed the demerits of some algorithms and provided a benchmark for subsequent algorithm improvement.
The rest of the paper is organized as follows. Section 2 introduces the basic system structure of mobile robots; Section 3 presents the rationale of three commonly used SLAM algorithms; Section 4 briefly describes the algorithm and DWA algorithm for path planning; Section 5 provides and analyzes the experimental and simulation results; Section 6 gives the conclusion.
2. System Structure
The hardware part of the robot studied in this paper is mainly composed of motion control module, Lidar module, vision module, power module, and industrial computer module. The system structure is shown in Figure 1, and the physical map of the robot system is shown in Figure 2.
(a)
(b)
The main function of the STM32 microcontroller is to acquire and process wheel encoder data and gyroscope data. Map information, path planning, depth camera, and Lidar data are processed by the industrial computer. The industrial computer and STM32 are connected via USB cable to exchange data and instructions. The depth camera is calibrated by using a printed black and white checkerboard. The OpenCV function called by the robot operating system (ROS) is used to extract the corner information from camera images, and then internal and external parameters are obtained through calculations [6]. The industrial computer is fitted with Intel Core i5 processor, 4G memory, 128G access space, and the ubuntu16.04 system.
3. SLAM Algorithms
For a mobile robot, SLAM involves both localization and mapping in an iterative manner by continuously fusing various measurements from the onboard sensors [7, 8]. The sensor module in our system includes Lidar and depth camera to collect environmental information as well as internal measurements from the IMU. A 2D map is to be generated after processing by a mapping algorithm. Depending on the purpose of the map, different SLAM algorithms are available. For our purpose, we will focus on the task of path planning in real time. After various considerations, we decide to study in detail three most suitable SLAM algorithms: GMapping algorithm, Hector-SLAM algorithm, and Cartographer algorithm. GMapping algorithm is based on particle filter pairing algorithm, Hector-SLAM is based on scan matching algorithm, Cartographer is a scan matching algorithm with loop detection, and RGB-D algorithm is an algorithm for mapping using depth images. These several algorithms are representative and widely used algorithms.
3.1. GMapping Algorithm
The GMapping algorithm is a laser-based SLAM algorithm for grid mapping [9, 10]. This is probably the most used SLAM algorithm, currently the standard algorithm on the PR2 (a very popular mobile manipulation platform) with implementation available on openslam.org. The algorithm was initially proposed in [10], and the main idea is to use Rao–Blackwellized particle filters (RBPFs) to predict the state transition function. The algorithm is also known as the RBPF SLAM algorithm, named after the use of Rao–Blackwellized particle filters. In [11], two major improvements were made by optimizing the proposal distributions and introducing adaptive resampling, making the algorithm much more suitable for practical applications. It is then dubbed GMapping (G for grid) due to the use of grid maps.
3.1.1. RBPF
Onboard measurements include sensor data from Lidar or camera for images and odometer data from the IMU. A large number of particles are used for state transition function predictions, with each particle representing a possible position of the robot.
The sensor data are denoted by and the odometer data by for the time period from 1 to . They are used to estimate the joint posterior probability of the robot pose and the grip map of the environment, represented by . Using the Bayes’ rule, the posterior probability can be decomposed intowhere is the positioning problem, whereas is the mapping problem. The so-called importance sampling is used in the RBPF. The procedure is as follows:(i)Sampling: according to the given (previous) proposal distribution, particles from the previous generation are sampled. They are then improved by incorporating the most recent observations. Then, new particles and proposal distributions are generated.(ii)Weights: the weight of each current particle is calculated using where is the proposal distribution, which usually is a probabilistic odometry motion model.(iii)Resampling: depending on the weights, particles with smaller weights are discarded and replaced by resampled particles, but the total number of particles in the resampled particle set is unchanged.(iv)Map updating: the map update is implemented by the pose represented by each particle in combination with the current observation. To reduce the computational complexity, a recursive formula for weight update is used:where is a normalisation factor.
3.1.2. Proposal Distribution
A large number of particles will cause a large amount of calculation and memory consumption. In order to reduce the number of particles, a proposal distribution is used. Our target distribution is the best distribution of the robot state according to the data of all sensors carried by the robot. Except for the odometer model, the laser observation data is the position information of 360-degree points, which is difficult to perform Gaussian modelling. Thus, there is no direct way to sample the target distribution, and the proposal distribution is used instead of the target distribution to extract the robot pose information at the next time instant. The proposal distribution considers not only the motion (odometer) information but also the most recent observation (laser) information. This can make the proposal distribution more accurate and closer to the target distribution.
The sensor observation information is added when calculating the proposal distribution, and the sampling process is concentrated in the peak region of the likelihood function to get the optimal proposal distribution:
Then, the weights are updated according to the above weight recursion formula:
The Gaussian distribution is used to approximate the approximated peak region of the observation, and the optimal proposal distribution is obtained. The Gaussian distribution parameters, i.e., the means and covariances , are determined using sampling points:where the normalising factor is given by
3.1.3. Adaptive Resampling
Resampling may cause good particles to be removed from the filter, making the particles scarce. Therefore, it is necessary to judge the quality of the particles by the effective sampling scale standard and judging the time of resampling. The evaluation formula is as follows:where is the number of particles and is the weight of the th particle. The worse the proposal distribution estimate, the smaller the is. When , GMapping performs resampling.
3.2. Hector-SLAM Algorithm
The Hector-SLAM algorithm [12] differs from other grid-based mapping algorithms, as it does not require odometer information, but it needs laser data and a priori map. Hector-SLAM is based on the Gauss–Newton iteration formula that optimally estimates the pose of the robot as represented by the rigid body transformation from the robot to the prior map. The optimal estimation is done by optimally matching the laser data and the map in the sense that the optimal below is solved:
Here, is the value of the map at , and is the world coordinate of scan end points , which obeys the following function:
When an initial estimate of the pose is given, an updated estimate is computed by approximating using first-order Taylor expansion, and the result is as follows:where is the Hessian matrix or some approximation of it, given by
3.3. Cartographer Algorithm
When the amount of data to process becomes too large, particle-based algorithms are not applicable due to their higher computing requirements on the processor. In this case, graph optimisation algorithms are more suitable.
Google’s solution to SLAM, called Cartographer, is a graph optimisation algorithm. The Google open source code1 consists of two parts: Cartographer and Cartographer_ROS. The function of Cartographer is to process the data from Lidar, IMU, and odometers to build a map. Cartographer_ROS then acquires the sensor data through the ROS communication mechanism and converts them into the Cartographer format for processing by Cartographer, while the Cartographer processing result is released for display or storage. Impressive real-time results for solving SLAM in 2D have been described in [13] by the authors of the software.
3.4. Considering the Rescue Environment
In rescue environment, there are stairs and rugged surface which make the odometer inaccurate. It means we could not choose GMapping because it is very rely on odometer. Due to the rugged surface, IMU is also inaccurate which means Cartographer may get bad results. So, we choose Hector-SLAM.
4. Path Planning
To achieve path planning is to solve three basic problems:(1)The robot reaches the desired position(2)The obstacle avoidance and completion of the strategic task are achieved in the moving process(3)The optimal path is realized
However, in the actual environment, due to the accuracy of the robot sensor and the variability of the environment, the environmental information and location information of the map construction will be deviated [14]. Global planning in a static environment can meet the problem requirements, but to handle the deviation caused by the dynamic environment, local path planning needs to be introduced. That is to say, the local path planning pays more attention to obstacle avoidance, and the global path planning pays more attention to the shortest path. Therefore, the combination of local planning and global planning algorithms can successfully achieve accurate navigation of the robot. The global planning algorithm studied in this paper is a node-based algorithm [15, 16], and the local planning algorithm is a dynamic window algorithm (DWA) [17]. Global path planning produces a high-level plan for the robot to follow to reach the goal location. Local path planning is responsible for generating velocity commands for the mobile unit to safely move the robot toward a goal. These properties are imbedded in the plan produced by the planners, using the cost function which takes into account both distance to obstacles and distance to the path.
4.1. Global Path Planning
Based on the global path planning of the grid method, the algorithm is used to study the path planning. The algorithm follows the cost function to make the robot to directionally search for the path toward the end point. The core valuation function of the algorithm iswhere the node is abstractly understood as the next target point, represents the total valuation function of the current node , represents the actual cost of the starting point to the current point, and represents the estimated cost of the current node to the end point. The value of determines the performance of the algorithm. Typically, uses the Euclidean distance or Manhattan distance between the two points in space. In the algorithm, the Manhattan distance is used. The Manhattan distance between two points and is as follows:
4.2. Local Path Planning
This paper mainly studies the corresponding action strategy and operation of robot for path navigation in indoor environment. For this reason, the DWA algorithm is selected as the main algorithm for local path planning. The DWA algorithm requires the robot to perform numerical simulation calculations on the path of the robot within a certain speed window. Thus, it is necessary to obtain the model state expression of the robot. The two-wheeled robot based on differential drive has no velocity in the -axis direction. Since the robot is at the millisecond level in each sampling period of the program execution, the motion trajectory of the robot in the two adjacent sampling periods can be approximated as a straight line. In a period of time , the robot travels a small distance at speed , and it is at an angle to the -axis; then, the movement increments and of the robot on the -axis and the -axis can be obtained, respectively:
The robot’s movement trajectory is then given bywhere is the angular velocity of the robot.
During the speed sampling of the robot, multiple sets of trajectory velocity values are collected. To make the robot safely perform path planning, some necessary speed limits are also needed. The speed velocity value and angular velocity value of the robot change within a certain range, and the range needs to be empirically calculated according to the physical characteristics of the robot and the operating environment. The range formula is as follows:
The robot has different torque performance parameters due to different motor models. When the current speed of the robot and the angular velocity are known, the actual speed range for the next sampling time can be computed aswhere and are the maximum accelerations and and are the maximum decelerations.
When the robot is safely avoiding obstacles in navigation, the speed during the whole locally planned trajectory must be within the range of speeds given bywhere is the minimum distance from the current position to the point where the arc trajectory of and intersects the nearest obstacle.
Performing the DWA algorithm for speed selection needs to satisfy equations (19)–(21) simultaneously. On the basis of the trajectory that satisfies these speed requirements, an evaluation function is used to measure a selected trajectory and to aim the selection of the optimal trajectory. The evaluation function is as follows:where represents the angle difference between the estimated end of the route and the target; is the minimum distance from the obstacle to the planned trajectory, as explained above; indicates the moment speed evaluation; is a smoothing function; and are evaluation coefficients.
5. Experimental Results
5.1. Simulation Experiments
5.1.1. SLAM Simulation
In order to test the aforementioned algorithms, we carry out simulation experiments using the Gazebo platform to build the simulation environment shown in Figure 3. The virtual environment has real physical properties, and the simulation results have strong reference to the actual environment.
(a)
(b)
The following simulation experiment was performed according to the virtual environment. We first use Lidar as a sensor to simulate the GMapping, Hector-SLAM, and Cartographer algorithms. We then use depth camera (RGB-D) as a sensor to simulate the GMapping algorithm. Four mapping algorithms under the physical simulation platform of the ROS robot system are tested, and the simulation results are shown in Figures 4–7. The purpose of presenting these diagrams is to give an impression of the algorithms described above, which are parsed using text and formulas.
Figure 4 shows the robot simulation process using GMapping. Depth information of the Lidar is required. The robot is located in the lower left corner of simulation environment, the data collected by the Lidar is marked in red, and the established environment map is in light gray. Figure 5 shows the robot simulation process using Hector-SLAM. Figure 6 shows the robot simulation process using Cartographer. The area scanned by the Lidar changes from light gray to white until the whole map is completed.
Figure 7 shows the final grid map constructed using the RGB-D camera data. The depth data are first transformed using depthimage_to_laserscan before being applied to GMapping. The simulation results show that the constructed map is not ideal. This is because the RGB-D camera is affected by its own structure, causing a limited range of viewing angle [18]. In addition, the depth camera requires rich scene features to work. The simulated environment has smooth wall surfaces with very few scene features, making the depth camera unable to perform effective feature matching. We see that map construction cannot be successfully completed.
5.1.2. Path Planning Simulation
After obtaining the environment map, the map was loaded under the ROS framework for path planning purposes. We use the rviz package under the ROS framework for path planning and navigation simulation. The constructed map is shown in the left part of Figure 8. Here, we also see the black dot indicating an obstacle, and the cyan portion indicates the safe distance between the robot and the obstacle. The green arrow points to the target point and direction of the robot. The right part of Figure 8 shows the path planning and navigation results, as indicated by the green line. The starting position for the robot is slightly below the obstacle. We see that the simulated path not only successfully avoids the obstacle but also is a nearly straight path.
(a)
(b)
5.2. Algorithm Verification in Lab Environment
5.2.1. Emulated Rescue Experiment
In this experiment, a square wooden structure was used to splicing the actual environment in an open laboratory, as shown in Figure 9. This was built with reference to the RoboCup rescue venue to simulate an enclosed indoor rescue environment after a disaster. The aforementioned SLAM algorithms are applied, and the results are shown in Figure 10. Comparing with the simulation results, the maps constructed in the actual environment using Lidar data (GMapping, Hector-SLAM and Cartographer) are all satisfactory. In the RGB-D experiment [19, 20], the environmental features are not sufficient for depth measurements, resulting in an overlapped map. Therefore, the mapping algorithm based on the depth camera needs further optimisation.
(a)
(b)
(c)
(d)
5.2.2. Lab Office Experiment
Next, we test the SLAM algorithms in a real lab office, as shown in Figure 11. The map in Figure 12 is constructed using the Hector-SLAM algorithm. We see that not only desks are clearly identified but also the chair legs are clearly shown.
Path planning is carried out using the algorithm for global path planning and DWA algorithm for local path planning, and the results are shown in Figure 13. In the picture on the left, the navigation target point and direction of the robot are set by the green arrow. The small green patch at the bottom shows many arrows representing the particles (their positions and directions) of the starting pose of the robot. The right picture shows that, after the execution of the path planning and navigation, the robot moves to the target point, as indicated by the (smaller) green patch representing the particles of the final pose. We see that the experimental results verify the effectiveness of the path planning algorithms.
(a)
(b)
5.2.3. RoboCup Competition Test
The mapping and path planning algorithms above are used in our entry of the 2019 RoboCup competition for indoor rescue [21]. The competition venue is shown in Figure 14. The competition requirements are as follows: robot constructs the map of the competition venue by self-exploration and completes the recognition of the doll and the marking of the QR code. The robot operates by either remote control or through an autonomous exploration algorithm. In addition, the robot must avoid obstacles. The quality of the mapping result is assessed by the number of closed grids identified in the constructed grid map. Because GMapping and Cartographer need IMU to assist positioning, and in uneven terrain in the competition, IMU data will have very large errors, which leads to very large errors in these two SLAM algorithms. The RGB-D algorithm also does not perform well in flat terrain, so we use the Hector-SLAM algorithm for mapping. And we use the Hector_navigation open source software package [22] for robot self-exploration. As shown in Figure 15, due to the complex environment of the competition venue, the robot is unable to pass some obstacles, such as the 15-degree slope and stairs, so certain parts of the venue cannot be scanned by the Lidar, and the constructed map is incomplete. Due to the limitation of the robot hardware, it is not possible to finish all the mapping. But all the places reached by the robot have been well-mapped. Finally, we scored 14 points (out of 27 points) in the self-exploration session.
(a)
(b)
(a)
(b)
5.2.4. China Robot Competition
Compared with the RoboCup competition venue, the 2019 China Robot Competition venue has a larger site area and a larger slope, which means that the terrain is more complicated, as shown in Figure 16. The competition requires rescue robots to independently explore the map of the field and identify the two-dimensional code on the box in the simulated postdisaster environment. In this experiment, we continue to use the Hector_navigation open source software package for robotic exploration. The route of the robot’s autonomous navigation is shown in Figure 17. We used two servos to keep the Lidar level, which is used to automatically adjust the Lidar to a horizontal position. However, due to the fact that the competition field is not flat, which causes the robot to have large fluctuations during the movement, the Lidar is unable to adjust the pose in time. As shown in Figure 18, the yellow arrow indicates the starting point of the rescue robot, the purple line marks the movement trajectory of the rescue robot, the dark blue line represents the map constructed by the SLAM algorithm on the competition venue, and the dark blue circle with numbers represents the location of the QR code. However, the robot is navigated outside the wall. As a result, there is a positioning error. The map cannot be quickly updated for corrections, and the navigation algorithm may incorrectly navigate the robot into an obstacle. Therefore, the Hector-SLAM algorithm needs to be improved in the update speed, and the navigation algorithm should adopt a more cautious strategy according to the application of the rescue scenario. As shown in Figure 18, an unnecessary closed point appears on the periphery of the constructed map. In addition, the number in the figure is the position information of the recognised QR code.
(a)
(b)
(a)
(b)
(c)
(d)
In the above simulation experiments and field experiments for GMapping, Hector-SLAM, Cartographer, and RGB-D mapping algorithms, GMapping, Hector-SLAM, and Cartographer perform better in a flat indoor environment. The RGB-D mapping algorithm has poor mapping effect due to poor lighting conditions and lack of environmental features. Therefore, in a relatively flat indoor rescue environment, it is more appropriate to choose the first three algorithms as the basis of the SLAM system. In the RoboCup competition and the China Robot Competition, the uneven rescue environment seriously interfered with the IMU data, so the SLAM algorithm (GMapping and Cartographer) incorporating IMU data could not perform SLAM tasks normally. This causes the path planning algorithm that relies on map information to not work correctly. The Hector-SLAM algorithm, which does not rely on IMU data, can perform tasks in the competition terrain relatively correctly, but there are still problems with inability to filter wrong Lidar information and poor stability.
6. Conclusions
In this paper, the problem of indoor rescue using mobile robots was studied. Comparisons were done on the GMapping, Hector-SLAM, and Cartographer algorithms for SLAM. The path planning was done by combining the algorithm for global path planning and the DWA algorithm for local path planning. Simulation, emulation, as well as real environment experiments were conducted to compare and validate the results on map construction and path planning. In the future, further optimisation needs to be carried out in the mapping algorithms to make them more suitable to the real rescue environment.
Data Availability
Our experiment data can be found in https://github.com/9393dl/Rescue_Robot.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This work was supported by the National Natural Science Foundation of China (Grant nos. 61633014, 61803101, and U1701264).