Improved grid mapping technology based on Rao-Blackwellized particle filters and the gradient descent algorithm

ABSTRACT Recently, the Rao-Blackwellized particle filter (RBPF) has been used to solve the problem of simultaneous localization and mapping (SLAM). Using the odometer information of robot to calculate the proposed distribution requires a number of sampled particles, which increases the calculation complexity in the RBPF operation. In this paper, we integrate the odometer measurement and sensor observation into the proposed distribution, effectively reducing the particle sample scale. To reduce the inconsistency in the map model caused by the cumulative error of the odometer information of robot, we applied a gradient descent algorithm to fuse the sensor data to obtain the real-time attitude angle. This combination method, based on the robot operation system (ROS), runs on a platform of self-built mobile robot equipped with a laser rangefinder. The experimental results show that this method can realize the online real-time high-precision grid map which provides a new approach for robot navigation and SLAM.


Introduction
Building maps is the most basic task on the mobile robot platform. In general, mapping is a process concerning simultaneous localization and mapping (Doucet, Freitas, Murphy, & Russel, 2013;Thrun, 2001). The robot requires continuous map upgrading to obtain its best estimated pose in the process of SLAM, which is considered as a tricky issue. The RBPF introduced by Doucet et al. (2013) breaks down the joint probability density function into two parts. In this way, simultaneous localization and mapping problems can be tackled at the same time. However, the main problem with the RBPF approach lies in complexity, measured by the number of particles needed to build an accurate map. Therefore, reducing the number of particles and constructing accurate maps are the major issues of the algorithm. In addition, cumulative errors of robot pose usually result in overlapping and distorting maps. This effect is also known as the odometer measurement error problem (Yang, Yang, & Cai, 2015).
In this article, we improve the performance of the SLAM grid map (Florian, Nico, Jeff, & Darius, 2010Georg & Dirk, 2017Matthias, Volker, & Jürgen, 2014;) construction by optimizing the odometry attitude measurement information and incorporating high-precision proposed distributions in RBPF. CONTACT Chuanjiang Wang cxjwang@163.com The odometer attitude measurement information is the source of the data in the SLAM algorithm and affects the performance of the entire system. Intuitively, the RBPF algorithm will have better performance with good attitude measurement information, so that the maps texture will be clearer. As a result, we use the gradient descent filter algorithm (Madgwick, 2010) to achieve the fusion of data poses of inertial measurement units (IMU).
The distribution of proposals is calculated by evaluating the likelihood of the most likely poses that are dependent on the particles obtained by the scan matching procedure (Giorgio, Cyrill, & Wolfram, 2007). This improved method of proposed distribution calculations can be more accurate than just considering the application of odometry models, and effectively reduce the need for sampling particle quantities. In addition, we adopted an adaptive resampling strategy to reduce the risk of particle depletion.
Our method is extensively verified through a series of system experiments. All the experiments conducted have proved that our method can effectively reduce the number of sampled particles and create more accurate metric maps. These experiments were carried out on a mobile robot platform based on the robot operation system, which is released by the Willow garage company.
Experimental data were displayed on RVIZ which is a 3D visualization tool provided by ROS.

Odometer attitude data optimization
The data of the odometry model is acquired from IMU and the motor encoder. IMU data can be obtained by integrating the gyroscope and accelerometer. However, the IMU information is easily affected by high noise and other environmental factors. Besides, as the system's runtime increases, the integration calculation will lead to accumulation errors. Therefore, a new algorithm is needed to fuse data for obtaining accurate attitude data.
The Kalman filtering (Arif & Ali, 2015), the Gradient descent algorithm (Madgwick, Harrison, & Vaidyanathan, 2011) and the Complementary filtering (Fourati, Manamanni, Afilal, & Handrich, 2013) algorithm is used to fuse attitude data. The Kalman filter algorithm has better accuracy and effectiveness. But this algorithm is severely limited owing to its complexity and large computation. On the other hand, the complementary filtering attitude fusion algorithm features a relatively less calculation and comparatively simpler algorithm structure. It can generate good results in low-speed motion, but there will be obvious lags in the experimental platform where the motion becomes faster. In this experimental platform, a data fusion algorithm based on quaternion gradient descent algorithm is used (Soltero, Schwager, & Rus, 2014). The algorithm simplifies the complexity of filtering and facilitates the application of practical experiments. And it offers a good solution to improve the accuracy of the motion experiment platform.
The gradient descent algorithm (Predrag & Marko, 2010) refers to looking for the minimum value of the objective function along the gradient descent direction of the objective function. In the present system, the objective function refers to the error function between the gravity acceleration g and the measured value a of the accelerometer a in the coordinate system. The quaternion describing the direction of the pose E Sq is represented by the equation The measured values of gravity acceleration and unit normalized accelerometer are assumed by the following equations: E eĝ = 0 0 1 , a b = a x a y a z T .
Then the error function f g is followed by the equation: For the above equation, ⊗ represents the compound operators (Madgwick, 2010). The conjugate of S Eq is defined by the equation: The Jacobian matrix of the error function can be obtained by the derivation of the error function, such as: Therefore, the gradient value of the error function is defined by the equation: According to the obtained error function gradient value ∇f , we can obtain the iterative equation of the target attitude quaternion: S E q ∇,t and S E q ∇,t− t represent the quaternions of the target attitude at time t and t − t, respectively, and μ t is the step length, and (∇f /||∇f ||) is the direction of the gradient.
The precondition that the gradient descent algorithm (Sebastian, Andrew, & Ravi, 2011) can track the timely attitude is that the convergence speed of the control is greater than the movement speed of the robot. The convergence speed of the gradient descent method is closely related to the control step size μ t . The larger the step size, the faster the convergence rate. In the conventional gradient algorithm, μ t is a fixed value. When the value of μ t is relatively small, the static performance of the attitude solution is better. When the value of μ t is relatively large, the dynamic performance of the attitude solution is better. Therefore, to realize a better performance of the gradient descent algorithm in the application of the highspeed and low-speed mobile platform, it is necessary to dynamically adjust the μ t in real time. The step length μ t of the gradient descent method is positively correlated with the angular velocity S Eq w,t and the sampling period t, equated as follows: α is the increasingly value of μ t . w x , w y and w z respectively indicate the angular velocity of the x, y and z axes. The module of angular velocity is defined by the following equation: Furthermore, the initial step μ 0 needs to be set for μ t , avoiding μ t = 0 in the lower speed movement conditions of the robot platform || S Eq w,t || = 0. The initial size of μ 0 is usually determined through experimental data. Therefore, the gradient descent algorithm dynamic step adjustment calculation equation can be given as follows: The attitude quaternion obtained by the differential equation is fused with the attitude quaternion obtained by the gradient descent algorithm. The fusion equation can be given as follows: The new pose quaternion S E q est,t is fused at time t. Gyro's attitude quaternion S E q w,t by differential equations at time t. γ t and (1 − γ t ) are weights applied at each attitude angle. The optimal value of γ is defined as the case where the weighted divergence of S E q w is equal to the weighted convergence of S E q ∇ . The equation can be deduced as follows: The equations are rearranged into the following equation: The (u t / t) is considered as the convergence speed of S E q ∇ . The β is considered as the divergence speed of S E q ∇ . β is usually determined by experimental data.
Since the convergence rate of the quaternion differential equation for solving the pose must be much larger than the rotation speed of the motion platform, and the sampling value is generally small, where t is about 1-5 ms, Equations (6), (9), and (13) are substituted into Equation (14): This paper selects the second-order Picasso method (Kallapur, Petersen, & Anavatti, 2009) to solve quaternion differential equations. The equation turns out to be as follows: The θ angle increment matrix is demonstrated by the following equation: θ 2 0 is expressed by the following equation: θ x , θ y , θ z are the integrals of the angular velocity w x , w y , w z of the gyroscope measurement angular velocity within the time range of (t − t, t). Therefore, Equation (14) is substituted into (15) to obtain the final formula of the gradient descent method for fused quaternion poses.
The overall flow of the gradient algorithm is shown in Figure 1.
To examine the efficiency of the calibration and fusion of the sensor attitude data by the algorithm of the IMU gesture data fusion described above, the MPU6050, a 16-bit resolution attitude sensor that includes three-axis gyroscopes and accelerometers, is commonly used in the market. Reading of attitude data in the height IIC mode of a speed up to 400 kHz will minimize unnecessary interference errors. Concreted steps will be displayed in the following: connect to the PC loaded with the ROS system via the serial port, manually hold the IMU original and display it on RVIZ. The IMU element is firstly rotated 90°a bout the z-axis, then rotated 90°again and finally rotated 180°in the opposite direction back to the initial position, as shown in Figure 2(a-d).
The pose calculation data of all the above states are collected and saved. Use MATLAB simulation software to draw waveforms for data. The data waveform is shown in Figure 3.
The four waveforms stage (1)-(4) corresponds to the four states (a)-(d) of the RVIZ test. In the above test process, we can conclude that the algorithm can make attitude tracking conducted in a more steady and effective fashion, although there are minor fluctuations in the output angle.

Improved proposed distribution and adaptive resampling
The traditional RBPF algorithm (Lee, Sang, & Wan, 2011) is an effective method to solve the existential problems in simultaneous localization and mapping. The algorithm usually takes the odometry measurement model as the proposed distribution (Giorgio et al., 2007). Therefore, there are problems of large particle demand and complicated calculations. This article applies an improved algorithm. Integrating the odometry measurement information and the observation information of the laser sensor into the calculation of the proposed distribution will effectively reduce the number of required particles and reduce the uncertainty of the robot pose (Lee, Lee, Choi, & Lee, 2012). In addition, an adaptive resampling strategy (Havangi, Taghirad, Nekoui, & Teshnehlab, 2014) is applied to reduce the number of particle sampling and greatly reduce the risk of particle exhaustion.
To obtain the particles for the next iteration, sampled particles need to be obtained from the proposed distribution π. Intuitively, the closer the proposed distribution is to the target distribution, the particle filter algorithm will perform better. When the odometer movement model is considered as a proposed distribution, the proposed distribution is expressed by p  the sampled particle at time t. u t−1 denotes the odometer measurement information at time t − 1. This method simplifies the calculation of the particle filter algorithm, but its importance weight distribution has a large coverage (Giorgio et al., 2007). Therefore, it needs a sufficient sample size to cover this area. This will cause particles that are close to the meaningful regions to account for lower weights. This has led to the construction of less accurate maps by the SLAM method. The weight is calculated by the following equations: 1:t |z 1:t , u 1:t−1 ) π(x (i) 1:t |z 1:t , u 1:t−1 ) To increase the computational efficiency of the proposed distribution, the proposed distribution is transformed into a recursive equation in the following equation: π(x 1:t |z 1:t , u 1:t−1 ) = π(x t |x 1:t−1 , z 1:t , u 1:t−1 ) · π(x 1:t−1 |z 1:t−1 , u 1:t−2 ) In Equation (19) η = 1/p(z t |z 1:t−1 , u 1:t−1 ). If the proposed distribution π(x (i) t |x (i) 1:t−1 , z 1:t , u 1:t−1 ) is replaced with an odometer model, the particle weight equation is as follows (22), (23): For the sake of overcoming the above problems, the measurement information of the lidar rangefinder is integrated into the calculation of the proposed distribution p(x t |m (i) t−1 , x (i) t−1 , z t , u t−1 ). z t represents the observation information of the sensor at time t. Its observation likelihood function has a small coverage. Particles that are close to the meaningful area have a high weight. Its weight is calculated by the following equations: Another key aspect of the impact on particle filter performance is resampling. In the resampling process, particles with lower importance weights w (i) t will be replaced by those with higher weights. On the one hand, there is a need to use a limited number of particles to assess the target distribution. Resampling, on the other hand, may remove particles that have higher weights. It will lead to particle exhaustion. Therefore, the sampling time plays a decisive role in the process of resampling. In this experiment, Doucet's method (Giorgio et al., 2007) was used to calculate this sampling time. N eff is expressed by the following equation: w (i) refers to the normalized weight of particle (i). Where N eff represents the degree of dispersion of importance weights, it is considered as an important indicator of the particle set approximation posterior probability function. During the application, a sample is taken each time N eff drops below the threshold. N represents the number of particles.

Experiments
The odometer attitude data fusion algorithm and the improved RBPF algorithm described above were tested on a robot mobile platform based on an ROS system. For the sake of proving the effectiveness of the algorithm described above, the experiment was implemented in a series of different environments. The map generated by the above methods will not appear distorted and distorted. There will not be any obvious inconsistency between the map model and the actual environment.
In the environment of a real-world data set, a relatively consistent and accurate map can be established.
To reduce the weight of the robot's mobile platform and enhance its flexibility, a minicomputer equipped with the ROS system was chosen, as shown in Figure 4. The microcomputer is a raspberry PI 3B with a 1.2-core CPU based on the ARM A53 processor. The microcomputer is equipped with wireless WIFI and USB communication interfaces, providing convenience for remote control and data transmission. In the robot mobile platform system, the Raspberry PI Microcomputer serves as the brain for data reception, processing, and behavioural commands. The mobile platform is equipped with a laser radar range finder to acquire environmental data, as shown in Figure 5. The STM32 microcontroller controls the movement of the geared motor with encoder, the integration of the odometer attitude data, and transfers the data to the upper raspberry function via the USB interface, which plays the role of cerebellum in the entire system.
In this paper, the mobile robot system is divided into a human-machine interaction layer, algorithm implementation layer and motion control layer according to functional modules. The structure of the system is shown in Figure 6.
We use a second ROS-mounted notebook computer to communicate with the microcomputer through the  network protocol SSH command. This will facilitate the observation of real-time map data information and manipulation of the microcomputer Raspberry Pi. In other words, the data transmission between the notebook computer and the microcomputer is performed via WIFI. After the notebook computer is connected to the microcomputer, node information is run to start the entire experiment. * ssh rikirobot@robot // Connect to the master on the notebook virtual side * roscore // Open the overall system Master * rosrun robot_serial_node // Transport protocol execution node * rosrun slam_gmappingmapping //Mapping node * rosrun robot_base// Motion control node * rosrun rviz rviz // Visual tools.
The entire system consisting of a notebook computer and a Raspberry Pi only allows the existence of a node manager Master at the same time. Therefore, the master must be fixed on the running system of the Raspberry Pi. Otherwise, the entire system cannot be initiated. The notebook computer only plays the role of display and remote control in this system. The robot_serial_node is  a USB serial data transmission protocol node to ensure correct data transmission. The slam_gampping node is an implementation of the above-described improved RBPF algorithm. The robot_base node is a receiver and motion control node of odometer attitude fusion information. The RVIZ node is a visual graphical tool node.
The compositional environment for this experiment was carried out in the indoor environment of an office of the Robot Research Center of the university, covering an area of approximately 8 × 8 m. The mapping map of the environment is shown in Figure 7. The size of the grid selected is 1 × 1 m in RVIZ. Figure 8 shows the map constructed by the traditional RBPF algorithm. Because only the odometer model is used as the distribution of the proposal, the error is relatively large as the system running time increases. As a result, the constructed map appears to be significantly distorted (for example, q 1 , q 2 are marked). The movement path of the Figure 8. The office map constructed using odometer information without the gradient descent algorithm and conventional particle filtering was used.
robot is: 1 → 2 → 3 → 4 . However, after applying the above-mentioned odometer attitude data fusion algorithm and improved RBPF algorithm, the map constructed by the mobile platform of the robot obviously improves the distortion phenomenon in the traditional method environment mapping map. And a clearer texture map is built (as shown in Figure 9, s 1 , s 2 marked. The robot path is a → b → c → d). Table 1 shows the number of particles and the average time required for the mapping to be constructed on the mobile robot platform after using the traditional particle filter algorithm and the improved algorithms. It can be seen from the comparison results that after optimizing the odometer attitude data and the improved RBPF, the number of particles required by the robot mobile platform in time of constructing a map is significantly Figure 9. The office map constructed using the gradient-descent algorithm-optimized odometer information and particle filtering that improves the proposed distribution.  reduced. And the time required for map construction has also been greatly reduced.
To further demonstrate the effectiveness of applying the gradient descent method and the improved proposed distribution for mapping, we set up the robot mobile platform in the complex environment of a largescale laboratory at our university and conducted experiments. The real environment map of a large laboratory is shown in Figure 10. The map of Robot Research Center Laboratory can be seen as following Figure 11. Figures 12  and 13 show an enlarged environment map of M 1 shown as Figure 11.
As can be seen from Figure 12, the map shows significant distortion (q 3 are marked). However, the problems were significantly improved by using the optimization algorithm from both the odometer and the particle filter  The laboratory map constructed using odometer information without the gradient descent algorithm and conventional particle filtering was used. Figure 13. The office map constructed using the gradientdescent algorithm-optimized odometer information and particle filtering that improves the proposed distribution.
(such as s 3 in Figure 13). Comparing s 4 and q 4 (such as in Figures 12 and 13), Figure 13 has a clearer texture, after applying the gradient descent method and the improved RBPF algorithm.  Table 2 shows the number of particles and the average time required for the mapping in a complex lab environment. It can be seen from the comparison results that after increasing the area of environment, the number of particles and the average time required for the mapping are significantly increased. However, the proportion of growth of our method is significantly smaller than that of traditional particle filter that without using the improved proposed distribution and the gradient descent algorithm in mapping.
Through the above experiment and particle number analysis, we can conclude that after the traditional RBPF algorithm incorporates an observation model with relatively high odometer accuracy, a highly accurate proposed distribution that is closer to the target distribution can be calculated. This can greatly reduce the number of particles used. In addition, an adaptive re-sample strategy was applied to reduce the number of particle sampling and improve the precision of map construction as well as the efficiency of map construction. Finally, based on the open source code Grid mapping, the optimized algorithm is integrated into the code. The test and experimental result analysis on the self-built ROS robot mobile platform were also completed.

Conclusion
In this paper, we systematically optimize odometers and particle filters for grid map construction. On the one hand, we use the gradient descent algorithm to fuse the odometer attitude data. On the other hand, we use the sensor measurement information and odometry information to calculate the more accurate proposed distribution in the improved RBPF algorithm. This allows us to absorb particles in a more accurate way, which can greatly reduce the number of required particle samples. In addition, an adaptive resampling strategy is also applied, which can greatly reduce unnecessary sampling operations and avoid the risk of particle exhaustion. Finally, the above analysis method was tested and evaluated on the data acquired by a mobile robot platform equipped with an ROS system equipped with a laser range finder. The algorithm was transplanted to mobile platform with ROS for a series of experiments. The higher the precision map, the fewer the number of particles and the shorter average time required exhibit the effectiveness of the algorithm.

Disclosure statement
No potential conflict of interest was reported by the authors.