Abstract
Localization is the primary problem of mobile robot navigation. Monte Carlo localization based on particle filter has better accuracy and is easier to implement, but there is also the problem of particle degradation. In this paper, the iterative extended Kalman filter is optimized by the Levenberg-Marquardt optimization method. An improved particle filter algorithm based on the upon optimized iterative Kalman filter is proposed, and the importance probability density function of the particle filter is generated by the maximum posterior probability estimation of the improved iterative Kalman filter. Simulation results of the improved particle filter algorithm show that the algorithm can approximate the state posterior probability distribution more closely with fewer sampled particles under the premise of ensuring sufficient state estimation accuracy. Meanwhile, the computation is reduced and the real-time performance is enhanced. Finally, the algorithm is validated on the indoor mobile service robot. The experimental results show that the localization algorithm’s accuracy meets requirement for real-time localizing of the restaurant service robot.
1. Introduction
Localization is the primary problem of mobile robot navigation. Accurate, fast. and stable localization is the premise for mobile robot to perform navigation tasks correctly [1]. The global visual localization uses the global camera to collect the image information of the environment where the mobile robot is located. Through the computer vision algorithm, the pose of the mobile robot in the environment is obtained to realize the localization of the mobile robot [2]. In the process of global localizing by ceiling camera, the implementation of the localizing algorithm does not depend on the mobile robot’s own processor but can run on the upper computer. Meanwhile, localization multiple robots in the same space only requires a set of ceiling cameras, so it is cheaper [3]. This paper studies the localization method of mobile service robots for food delivery in restaurants. Multiple food delivery robots work simultaneously in restaurants and the working environment is relatively fixed, so it is very suitable for global visual localization using a ceiling camera.
Common global localization methods for service robots include Kalman filtering localization method [4], Markov localization method [5], and Monte Carlo localization based on particle filter algorithm (MCL) [6]. Kalman filtering localization algorithm used the recursive method to estimate the state of the linear dynamic system with Gaussian noise [7]. However, in the practical application environment, the observation equation is generally nonlinear, so it is necessary to extend the application of Kalman filter through model linearization; thus, an extended Kalman filter (EKF) is generated to solve the state estimation problem in some nonlinear environments [8]. The EKF localization algorithm is simple in structure and has certain precision, so it has been widely used. Due to model errors and noise statistical errors caused by linearization of nonlinear systems, EKF is faced with problems such as filter divergence and insufficient estimation accuracy [9]. Iterative extended Kalman filtering (IEKF) algorithm introduces the iterative filtering theory into the extended Kalman filtering method, updates the linearized observation equation by iterative calculation, compensates the truncation error caused by system linearization effectively, and makes full use of observation information to approximate the optimal estimate of state parameters [10]. IEKF localization method approximates the predictive value of robot state by using an iterative sequence, sacrificing a certain amount of computing time, so as to obtain a good performance of robot state estimation [11].
By representing the possible distribution of robot pose with grid points in the state space, Markov localization can realize the global localization of mobile robot by constantly updating the probability distribution of state space points [12]. Compared with EKF localization, Markov localization has better robustness, while EKF localization has better localization accuracy [12]. However, due to the need to calculate the reliability of all grids repeatedly, Markov localization consumes a lot of time, which leads to the poor real-time performance, and it is not conducive to practical application [13].
Monte Carlo localization is an extension of Markov method, and the basic principle of this method is to use particle filter (PF) to track the probability distribution of the robot’s possible pose. Monte Carlo localization greatly reduces memory consumption and makes effective use of robot resources. It has better localization accuracy and is easier to implement. Montemerlo et al. proposed a fast simultaneous localization and mapping (SLAM) algorithm based on Rao-Blackwellized particle filter in 2003 [14]. However, when the environmental observation noise of mobile robots is low (i.e., the environmental observation sensor is too accurate, such as the laser sensor), particle set degradation is easy to occur, which makes the SLAM algorithm diverge [15]. It is a simple and widely used method of selecting the state transfer probability density function with prior property as the importance probability density function to solve the phenomenon of particle degradation [15]. Montemerlo et al. proposed the Fast SLAM2.0 algorithm, which uses EKF to integrate the current robot’s environmental observation information into the proposal distribution’s design of the particle filter, so that the particles are concentrated in the high observation likelihood region [16]. Fast SLAM 2.0 can create a better importance density at the cost of introducing inaccuracies due to linearization, as well as making a Gaussian assumption on the form of the posterior density. Ullah et al. designs importance density function by using unscented Kalman filter and proposes an unscented particle Filter (PF-UKF) algorithm. It works well at the cost of large computation burden and when the state estimation error covariance is close to the process noise covariance [17]. The proposal distribution selected by the above algorithm does not contain the latest observation data, which also may result in the performance degradation of the algorithm. The iterated extended Kalman filter can make efficient use of the latest observation and improve the performance of particle filter. Wu et al. use the IEKF to generate proposal distribution in particle filtering framework, and an iterative extended Kalman particle filter localization algorithm (PF-IEKF) is proposed [18]. When the nonlinear system satisfies the condition of local linearization, the IEKF iterative updating sequence is equivalent to the Gauss-Newton (G-N) iterative estimation [19]. However, the G-N method has some problems in practical application, such as convergence and precision of calculation results. In order to improve the convergence and precision of the G-N algorithm, the Levenberg-Marquardt (LM) optimization method is usually used to optimize the IEKF iterative sequence to improve the convergence and precision of IEKF iterative sequence [20]. Li et al. proposed a robot vision positioning method based on iterative Kalman particle filter. Experimental results show that the accuracy and real-time performance of robot mobile navigation can meet the requirements [21].
Inspired by the Gauss-Newton optimization algorithm, this paper improves iterative extended Kalman filtering using the Levenberg-Marquardt optimization method to improve the convergence and calculation accuracy of IEKF iterative sequence. The maximum posterior probability estimation of improved IEFK is used to generate the importance probability density function of particle filter, and an improved iterative extended Kalman particle filter (PF-UIEKF) localization algorithm is proposed. Then, the effectiveness of the improved particle filter algorithm is verified by simulation. At last, it is applied to the global visual localization on the restaurant service robot and verified by experiment.
2. Modeling of the Indoor Service Robot
2.1. Hardware of the Robot
The research object of this paper is the restaurant service robot, whose task is to pick up cooked dishes and send them to the designated table autonomously. Therefore, the restaurant service robot must have good autonomous mobility and can move freely in any direction in the restaurant environment through a motion mechanism. At the same time, in order to make the restaurant service robot take the place of a human to complete the task of picking up and delivering dishes independently, the service robot must also be configured with corresponding manipulating mechanism.
As shown in Figure 1, the restaurant service robot is composed of three main parts: the robot arm, the head, and the motion mechanism. The robot motion mechanism adopts the differential drive with two driving wheels and is also equipped with two universal wheels which play a supporting role. Two robotic arms are installed on the left and right sides of the robot torso. Each robotic arm has three degrees of freedom to complete the operation of grasping the dish. To achieve the global localization of the service robot, a global camera is installed on the ceiling of the restaurant. The global vision controller receives video signals from the ceiling global camera over a wireless network. The vision processing algorithm is used to identify and measure the pose of the camera relative to the color mark installed at the top of the service robot head. At last, the global localization algorithm is run to determine the robot’s coordinates in the global coordinate system.
2.2. Kinematic Model of the Robot
In order to analyze the kinematic model of the service robot, the following assumptions are made. The two driving wheels are rigid bodies with the same size and no deformation. The line of the two wheels’ axis is perpendicular to the front and rear motion direction of the platform. The wheel face of the driving wheel is perpendicular to the ground and maintains point contact. The drive wheel only does pure rolling and will not produce axial movement. The influence of the drive wheel thickness on the movement of the moving platform is ignored.
Based on the above assumptions, as shown in Figure 2, the pose of the restaurant service robot can be expressed as , where is the projection coordinate of the central point of the driving wheel axis in the motion plane, and is the course angle of the robot. Suppose that the radius of the two driving wheels is , the axial distance is , and the rotational angular velocity of left and right wheel is and , respectively, then the localization formula of the robot based on the kinematics model can be obtained as follows:
By discretizing Equation (1) and adding the model noise of the system, the state equation of the restaurant service robot can be obtained as follows:
In Equation (2), is the model error, which is the Gaussian white noise with zero mean value. is the sampling period.
2.3. Localization of the Robot through the Global Ceiling Camera
As shown in Figure 3, the global coordinate system (GCS), robot coordinate system (RCS), and camera coordinate system (CCS) are established for the service robot firstly.
In Figure 4, denotes the origin of the camera coordinate system, denotes the height of the ceiling camera relative to the floor; denotes the height of the robot; is the coordinates of the robot in CCS; is color mark’s coordinates in CCS. According to the geometric relationship, the following equation can be obtained:
Then, the following results can be calculated as,
From Equation (4), it is obvious that the coordinates of the robot in CCS is only dependent on , which is the color mark’s coordinates in CCS. To get the (, ), the image of the monocular camera mounted on the ceiling is acquired and transmitted to the robot global vision controller through the wireless network. The image is firstly processed to remove the image distortion. Then, the color segmentation method is used to locate the robot in the image and distinguish the color mark on the head in the hue-saturation-value (HSV) color space. After distinguishing and extracting the color mark, the coordinates of the color mark in CCS can be obtained [22].
At last, the coordinates of the robot in GCS can be solved according to coordinate transformation [22], and the coordinates of the robot in GCS can be calculated as
In Equation (5), is the rotation angle of the camera coordinate system relative to the global coordinate system; and are the offset distances of the camera coordinate system relative to the global coordinate system. In order to determine the current pose of the robot, the current course angle of the service robot is also required. Gyroscope is installed in the robot, and the initial course angle of the service robot is obtained according to the gyroscope firstly. In order to overcome the influence of gyroscope cumulative error on the course angle when the robot moves for a long time or a long distance, it is necessary to correct the course angle using global vision periodically.
If the global coordinates of the service robot at two consecutive moments are obtained by the global visual localization method, and they are and , then the current course angle of the robot is
By combining Equations (5) and (6), the observation equation of service robot based on global vision after discretization can be obtained as follows:
In Equation (7), , , , and ) is the observed noise of the global ceiling camera of the robot system.
3. Improved Iterative Extended Kalman Particle Filter Localization Algorithm
3.1. Improved Iterative Extended Kalman Filtering
The robot state Equation (2) and observation Equation (7) above can be simplified as
In Equation (8), and are the state transfer function and observation function of the robot system, respectively. and are the state value and observed value of the system at time , respectively. and are the state noise and observation noise of the system, and their covariances are and , respectively. is the known input. When EKF performs nonlinear transformation, it uses Taylor series expansion and ignores higher-order terms, which will introduce truncation error and make the performance of state estimation worse. In order to reduce nonlinear error and improve the performance of nonlinear filtering, IEKF algorithm introduces the iterative filtering theory into the EKF algorithm. In the state update stage of IEKF, multistep iteration is adopted, and the observation equation is linearized for many times. The whole filtering process is repeated to achieve the optimal state estimation finally [23].
The main steps of IEKF algorithm are as follows:(1)State value prediction. Suppose the estimated state value at time is , and the known input state and the system state equation are used to obtain the estimated state value at time . Then, the Jacobian matrix of the system state equation and the error covariance matrix of the system state are calculated, respectively, to complete the state prediction process(2)Observation value acquisition. The state estimation value at time and the system observation equation were used to obtain the predicted observation value at time (3)Updating the predicted value iteratively using the observation value. Assuming that there are iterations, the Jacobian matrix and the filtered gain matrix of the system observation equation at the th iteration are calculated, respectively. Then use and to update the error covariance matrix. The difference value of the observed variable is calculated and the status is updated
It can be seen that if the updated after observation is taken as the initial value of iteration and the observation equation is linearized again, the obtained state estimation value will be more accurate. The IEKF algorithm is formed by carrying out this iterative process several times.
Bell and Cathey analyzed the relationship between the iterative sequence of IEKF and the nonlinear least squares Gauss-Newton method. When the nonlinear system satisfies the condition of local linearization, the iterative update sequence of iteratively extended Kalman filter is equivalent to Gauss-Newton iterative estimation [19]. Due to the introduction of errors such as linearization, the system’s state space equation may not be completely consistent with the actual observed data, and solutions using the Gauss-Newton method usually do not yield stable results. It also cannot guarantee the convergence of the state observation updated estimation results, and the estimated value of the covariance matrix is lower than the true value, thus affecting the effective use of the observation information.
A modified Levenberg-Marquardt method can correct the original iterative sequence by introducing a damping factor, which improves the convergence of the algorithm and the accuracy of the calculation results [24]. Therefore, in this paper, the Levenberg-Marquardt method is used to adjust the covariance matrix in the IEKF to ensure the convergence of the estimated error. The core of this method is that the damping factor is used to modify the prediction error covariance matrix during each iteration, that is, to adjust the error covariance matrix as follows:
Then, an iterative observation update is performed with a modified . The implementation steps of the improved IEKF algorithm based on the Levenberg-Marquardt optimization method are as follows:(1)Initial value setting. The calculation formula of the initial value is
where is the estimated value of initial state and is the initial state error covariance matrix(2)Prediction of state. Based on the estimated state value at time and known input , the predicted state value , Jacobian matrix , and error covariance matrix of the system at time are calculated as follows:(3)Obtain observation information. Based on and the observation equation of the system, the predicted observation value of the system can be calculated as follows:(4)Iterative updates of status. Assume that iterations are performed. For the th iteration , the following calculation is performed successively
The Jacobian matrix of the system state is calculated as
The predicted state and error covariance matrix of the system are calculated as
The modified error covariance matrix is optimized using the Levenberg-Marquardt method
The Jacobian matrix of the observation equation is calculated as
The filter gain matrix is calculated as
The updated error covariance matrix is,
The predicted value of the observed value of the system is
The system status estimated value is updated as
When the number of iterations reaches the set maximum number of iterations or the error between two consecutive iterations is less than the set minimum error value, the iteration process is stopped. The iteration result is taken as the status update value at time ,
In the above equation, “^” represents the estimated state. The subscript represents the prior estimate. The subscript is the posterior estimate.
3.2. Improved Iterative Extended Kalman Particle Filter Algorithm
Compared with the iteratively extended Kalman particle filter algorithm, the difference of the improved particle filter algorithm proposed in this paper is that, the importance probability density function of particle filter is generated by iterative Kalman filter based on L-M optimization. The importance probability density function reduces the linearization error of system. It is more consistent with the actual posterior probability distribution of the state variable. The implementation steps of the improved iteratively extended Kalman particle filter algorithm (PF-UIEKF) are as follows:(1)Initializing the particle set. The initial state and prior probability density of the robot were sampled times, and particle points were obtained(2)For , is the number of samples
For ,(i)Calculating the Jacobian matrix of the state equation using Equation (13)(ii)The improved IEKF algorithm is used to update the particle set using Equation (14)
For (a)The Jacobian matrix of the observation equation of the system is calculated using Equation (16)(b)The state covariance is updated by Equation (18)(c)Equation (17) is used to update the state gain (d)The state estimation is updated according to Equation (20)
End for(iii)Construct the importance probability density function(iv)
In Equation (22), is importance sampling density function.(v) times of sampling are conducted to generate state prediction particle set(vi)Calculating the particle weight of the sample set using the following equation
In equation (23), is the likelihood function of the observed value .
End for(3)The sample point weight is normalized as(4)To represent the degree of particle degradation, the effective particle set index in particle filter is introduced [25]
Preset a threshold value th, and . If , step 6 is entered, otherwise step 5 is entered.(5)Sequence importance resampling. Polynomial resampling is performed on the original particle set. Particles with larger sample weights are propagated, while particles with smaller sample weights are eliminated, and a new set of particles is obtained
The sample weights of the new particle set are reallocated, . At last, step 7 is entered.(6)The state estimation value of the system at time is output according to the particle set(7)Make and go to step 2, end for
3.3. Global Vision Localization Based on PF-UIEKF Algorithm
In order to reduce the navigation cost of mobile service robot in restaurant environment, the robot global localization is realized based on the restaurant environment image collected by the global ceiling camera in this paper. The localization method based on global vision has been introduced in Section 2.3 in detail, and it can realize the global localization of the robot theoretically. However, due to the influence of noise in the restaurant environment, the measured value of global vision often contains large non-Gaussian noise. It seriously affects the localization accuracy and fails to meet the mobile navigation requirements. Here, a robot global visual localization algorithm based on PF-UIEKF is proposed.
According to Equations (2) and (7), the robot system’s state model and observation model is obtained. Equations (17), (18), and (20) are used to obtain the state estimation and covariance iterative update equation of the robot’s position using IEKF based on L-M optimization. Then, the importance probability density function is generated by using the maximum posterior probability estimation of the obtained robot state, and the improved particle filter algorithm is used to obtain more accurate global coordinates of the robot. Figure 5 shows the implementation steps of the global visual localization algorithm of the restaurant service robot based on the PF-UIEKF.
4. Algorithm Verification and Experiment
4.1. Simulation and Analysis
To verify the effect of the improved iterative extended Kalman particle filter algorithm, the following standard nonlinear system model is adopted in simulation experiments, and the standard nonlinear system state equation is
And its observation equation is
The system process noise obeys the gamma distribution, and . The observed noise obeys Gaussian distribution and . The following are given parameters in a nonlinear system model: , , , and . The number of particles used in the simulation experiment is , and the observation time is s. The polynomial resampling method is used to resample the particle set. State variables were initialized again before each simulation experiment, and 100 independent simulation experiments were repeated. The output of the algorithm is the mean value of the particle set, and then, the state estimation of the nonlinear system can be expressed as
In order to explain the state estimation performance of the PF-UIEKF algorithm more intuitively, simulation experiments of various particle filtering algorithms such as standard particle filter (PF), extended Kalman particle Filter (PF-EKF), and iterative extended Kalman particle filter (PF-IEKF) were also carried out for this nonlinear model, and the performance of state estimation was compared and analyzed. Figure 6 shows posterior means estimated by four different nonlinear filtering algorithms (PF, PF-EKF, PF-IEKF, and PF-UIEKF) in an independent experiment.
In this paper, the root mean square error (RMSE) of state estimation value is used to reflect the state estimation accuracy of each particle filter algorithm, and its value can be calculated from the following formula.
Table 1 summarizes the state estimation performance data for each particle filter. The mean and variance of the RMSE of state estimation value of each particle filter algorithm are given, and the execution time of each algorithm is also given. It can be clearly seen from Table 1 that the state estimation accuracy using the PF-UIEKF algorithm is the best. Although PF-UIEKF particle filter algorithm has a longer execution time than others, the difference is not significant.
In order to analyze the state estimation performance of the improved particle filter algorithm furtherly, the number of particles used in the simulation experiment was increased to 50 and 200. Other parameters remain the same. 100 independent simulation experiments were performed on all particle filter algorithms again, and initialization was required before each simulation experiment. Tables 2 and 3 summarize the state estimation performance data for each particle filter when the number of particles is 50 and 200.
As can be seen from Table 2, when the number of particles rises from 20 to 50, the state estimation performance of each particle filter is greatly improved with the increase of the number of particles. The state estimation accuracy using the PF-UIEKF algorithm is also the best. However, the increase in the number of particles also led to the increase in the execution time of the PF-UIEKF algorithm, which reached 1.6613 s. As can be seen from Table 3, with the number of particles increasing to 200, the state estimation performance of each particle filter is still improved. However, the performance improvement of PF-UIEKF is not obvious compared with other particle filtering algorithms. At this time, PF-IEKF nearly has the same state estimation accuracy, mean, and variance of the RMSE of state estimation value as the PF-UIEKF particle filter. The estimation accuracy of these two particle filtering algorithms is the best and far better than that of other filtering algorithms. However, at this time, the execution time of the PF-UIEKF algorithm is greatly increased, which is nearly twice that of the PF-IEKF algorithm.
Compared with Table 1–3, PF-UIEKF can still get more accurate state estimation under the condition of fewer particles (), and its estimation performance is much better than that of PF-IEKF algorithm when . Moreover, the execution time of the PF-UIEKF algorithm is shorter than that of PF-IEKF when . The improved particle filter algorithm can approach the posterior probability distribution of the state more closely with fewer sampled particles under the premise of ensuring sufficient state estimation accuracy, which reduces the computation and enhances the real-time performance.
4.2. Experiments
In this paper, the following localization experiment is designed: the robot starting from the global coordinate , moving in a counterclockwise direction for one turn, and finally returning to the vicinity of the starting point. During the movement of the robot, the localization algorithm based on PF-UIEKF proposed in the paper is used to carry out on-line for real-time localization of the robot.
Firstly, the global image in Figure 7 is obtained after correcting the original image acquired by the global ceiling camera. The upper left corner of Figure 7 is the origin of the coordinate system CCS, and the origin of the coordinate system GCS is located in the upper right corner of the carpet area in Figure 7. According to the color characteristics of the service robot, an appropriate threshold is set in the HSV color space of the image by using the color segmentation method to detect the robot region in the image and the color mark above the robot head [22]. After localizing the position of the color mark of the robot’s head in the global image, the global coordinates of the service robot can be calculated according to the method in Section 2.3. Since the height of the global camera and the robot are both unchanged and known in advance (Table 4 lists the given parameters required for the global visual localization).
The current coordinates of the robot are updated every 100 ms, and the results were recorded. In order to display the experimental effect more intuitively, 55 sample points of the actual motion trajectory were extracted at certain intervals, and the robot localization results and the actual coordinate positions were displayed in Figure 8.
The experimental results show that the maximum localization error of the robot is 4.8 cm. The mean localization error was 3.74 cm. That is, the localization accuracy of the proposed global visual localizing algorithm based on PF-UIEKF can be kept within 5 cm, which fully meets the requirements of mobile navigation and localization of the restaurant service robots.
5. Conclusions
According to the global localization requirement of indoor restaurant service robot which is driven by two differential wheels, the kinematics model of the service robot and the localization model through the global ceiling camera are established in this paper firstly. Then, the iterative extended Kalman filter is optimized and modified by the Levenberg-Marquardt optimization method. A particle filter algorithm based on an improved iterative Kalman filter is proposed, and the importance probability density function of the particle filter is generated by the maximum posterior probability estimation of the improved iterative Kalman filter. A typical nonlinear system model is used to simulate the improved particle filter algorithm. The simulation results show that the particle filter algorithm can obtain higher estimation accuracy and improve the real-time performance of state estimation by using fewer particles. Finally, a method of global localization of indoor restaurant service robot based on improved iterative extended Kalman particle filter algorithm and global visual localization is proposed and verified by experiments. The experimental results show that the localizing accuracy of the proposed global localizing algorithm can be kept within 5 cm, which meets the requirements of mobile navigation and localizing of restaurant service robots.
Data Availability
The data used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare no conflict of interest.
Acknowledgments
The authors gratefully acknowledge the financial supports by the National Key Research and Development Program of China (Grant No. 2020YFC2005800, 2020YFC2005804).