2D Lidar-Based SLAM and Path Planning for Indoor Rescue Using Mobile Robots

,


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 Qlearning 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 A * 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. erefore, 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 (A * algorithm and DWA algorithm), and conducted a combined experiment of mapping and path planning regarding the RoboCup competition. ese experiments revealed the demerits of some algorithms and provided a benchmark for subsequent algorithm improvement. e 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 A * algorithm and DWA algorithm for path planning; Section 5 provides and analyzes the experimental and simulation results; Section 6 gives the conclusion.

System Structure
e 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. e system structure is shown in Figure 1, and the physical map of the robot system is shown in Figure 2.
e 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. e industrial computer and STM32 are connected via USB cable to exchange data and instructions. e depth camera is calibrated by using a printed black and white checkerboard. e 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]. e industrial computer is fitted with Intel Core i5 processor, 4G memory, 128G access space, and the ubuntu16.04 system.

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]. e 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. ese several algorithms are representative and widely used algorithms.

GMapping Algorithm.
e GMapping algorithm is a laser-based SLAM algorithm for grid mapping [9,10]. is 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. e algorithm was initially proposed in [10], and the main idea is to use Rao-Blackwellized particle filters (RBPFs) to predict the state transition function. e 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.

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. e sensor data are denoted by (z 1:t � z 1 , z 2 , . . . , z t ) and the odometer data by (u 1:t � u 1 , u 2 , . . . , u t−1 ) for the time period from 1 to t.
ey are used to estimate the joint posterior probability p(x 1:t , m|z 1:t , u 1:t−1 ) of the robot pose (x 1:t � x 1 , x 2 , . . . , x t ) and the grip map of the environment, represented by m. Using the Bayes' rule, the posterior probability can be decomposed into p x 1:t , m|z 1:t , u 1:t−1 � p x 1:t |z 1:t , u 1:t−1 p m|x 1:t , z 1:t , where p(x 1:t |z 1:t , u 1:t−1 ) is the positioning problem, whereas p(m|x 1:t , z 1:t ) is the mapping problem. e so-called importance sampling is used in the RBPF. e procedure is as follows: (i) Sampling: according to the given (previous) proposal distribution, particles (x (i) t−1 ) from the previous generation are sampled. ey are then improved by incorporating the most recent observations. en, new particles (x (i) t ) and proposal distributions are generated.
(ii) Weights: the weight w (i) t of each current particle x (i) t is calculated using where π(·) is the proposal distribution, which usually is a probabilistic odometry motion model. Journal of Advanced Transportation (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.

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. us, 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. e proposal distribution considers not only the motion (odometer) information but also the most recent observation (laser) information. is can make the proposal distribution more accurate and closer to the target distribution.  Figure 1: System structure of our mobile robot. "STM32 robot control module" gets information of motors from "encoder" and motion from "gyroscope" to control motors and transfer the motion and encoder information to "IPC" which is a microcomputer. en, "IPC" gets information from Lidar, "STM32 robot control module," and RGB-D camera.
(a) (b) Figure 2: Key modules of the robot. (1) "RGB-D," used to get RGB image and depth image; (2) "lidar," used to get 2D lidar point cloud; (3) "STM32 controller," used to control the movement of the car; (4) "gyroscope," used to get the attitude information; (5) "WI-FI," used to contact with the host computer; (6) "power supply system"; and (7) "motor and encoder," used to get the velocity feedback of the car movement. e 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: en, the weights are updated according to the above weight recursion formula: e Gaussian distribution is used to approximate the approximated peak region of the observation, and the optimal proposal distribution is obtained. e Gaussian distribution parameters, i.e., the means μ (i) t and covariances (i) t , are determined using K sampling points: where the normalising factor η (i) is given by 3.1.3. Adaptive Resampling. Resampling may cause good particles to be removed from the filter, making the particles scarce. erefore, it is necessary to judge the quality of the particles by the effective sampling scale standard and judging the time of resampling. e evaluation formula is as follows: where N is the number of particles and w (i) is the weight of the ith particle. e worse the proposal distribution estimate, the smaller the N eff is. When N eff < (1/2)N, GMapping performs resampling.

Hector-SLAM Algorithm.
e 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 ξ � [p x , p y , ψ] T from the robot to the prior map. e optimal estimation is done by optimally matching the laser data and the map in the sense that the optimal ξ * below is solved: Here, M(S i (ξ)) is the value of the map at S i (ξ), and S i (ξ) is the world coordinate of scan end points s i � (s i,x , x i,y ) T , which obeys the following function: When an initial estimate of the pose ξ is given, an updated estimate ξ + Δξ is computed by approximating M(S i (ξ + Δξ)) using first-order Taylor expansion, and the result is as follows: where H 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.
e Google open source code1 consists of two parts: Cartographer and Cartogra-pher_ROS. e 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.

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.

Path Planning
To achieve path planning is to solve three basic problems: (1) e robot reaches the desired position (2) e obstacle avoidance and completion of the strategic task are achieved in the moving process (3) e 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. at 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. erefore, the combination of local planning and global planning algorithms can successfully achieve accurate navigation of the robot. e global planning algorithm studied in this paper is a node-based A * 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. ese 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.

Global Path
Planning. Based on the global path planning of the grid method, the A * algorithm is used to study the path planning. e A * algorithm follows the cost function to make the robot to directionally search for the path toward the end point. e core valuation function of the A * algorithm is where the node n is abstractly understood as the next target point, f(n) represents the total valuation function of the current node n, g(n) represents the actual cost of the starting point to the current point, and h(n) represents the estimated cost of the current node to the end point. e value of h(n) determines the performance of the algorithm. Typically, h(n) uses the Euclidean distance or Manhattan distance between the two points in space. In the A * algorithm, the Manhattan distance is used. e Manhattan distance between two points (x 1 , y 1 ) and (x 2 , y 2 ) is as follows:

Local Path Planning.
is 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. e DWA algorithm requires the robot to perform numerical simulation calculations on the path of the robot within a certain speed window. us, it is necessary to obtain the model state expression of the robot. e twowheeled robot based on differential drive has no velocity in the y-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 v, and it is at an angle θ t to the x-axis; then, the movement increments Δx and Δy of the robot on the x-axis and the y-axis can be obtained, respectively: e robot's movement trajectory is then given by where ω 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. e 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. e range formula is as follows: e robot has different torque performance parameters due to different motor models. When the current speed of the robot v c and the angular velocity ω c are known, the actual speed range for the next sampling time can be computed as

Journal of Advanced Transportation
where _ v a and _ ω a are the maximum accelerations and _ v d and _ ω d are the maximum decelerations.
When the robot is safely avoiding obstacles in navigation, the speed (v, ω) during the whole locally planned trajectory must be within the range of speeds given by where dis(v, ω) is the minimum distance from the current position to the point where the arc trajectory of v and w 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. e evaluation function is as follows: where head(v, ω) represents the angle difference between the estimated end of the route and the target; dis(v, ω) is the minimum distance from the obstacle to the planned trajectory, as explained above; vel(v, ω) indicates the moment speed evaluation; σ(·) is a smoothing function; and α, β, c > 0 are evaluation coefficients.

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. e virtual environment has real physical properties, and the simulation results have strong reference to the actual environment. e 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. e 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. e 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. e 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. e depth data are first transformed using depthimage_to_laserscan before being applied to GMapping. e simulation results show that the constructed map is not ideal. is 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. e 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.

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. e 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. e green arrow points to the target point and direction of the robot. e right part of Figure 8 shows the path planning and navigation results, as indicated by the green line. e 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.

Emulated Rescue Experiment.
In this experiment, a 1.25 m × 1.25 m square wooden structure was used to splicing the actual environment in an open laboratory, as shown in Figure 9.
is was built with reference to the RoboCup rescue venue to simulate an enclosed indoor rescue environment after a disaster. e 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. erefore, the mapping algorithm based on the depth camera needs further optimisation.

Lab Office Experiment.
Next, we test the SLAM algorithms in a real lab office, as shown in Figure 11. e 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 A * 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. e small green patch at the bottom shows many arrows representing the particles (their positions and directions) of the starting pose of the robot. e right picture shows that, after the execution of the path planning and navigation, the robot moves to the target

RoboCup Competition
Test. e mapping and path planning algorithms above are used in our entry of the 2019 RoboCup competition for indoor rescue [21]. e competition venue is shown in Figure 14.
e competition re-     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.

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. e 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. e 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. e map cannot be quickly updated for corrections, and the navigation algorithm may incorrectly navigate the robot into an obstacle.  Journal of Advanced Transportation erefore, 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.
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. e RGB-D mapping algorithm has poor mapping effect due to poor lighting conditions and lack of environmental features. erefore, 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. is causes the path planning algorithm that relies on map information to not work correctly.
e 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.

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. e path planning was done by combining the A * 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.

Conflicts of Interest
e authors declare that they have no conflicts of interest.